Modules/RobotStateDetector/GT2005RobotStateDetector.cpp

Go to the documentation of this file.

/**
* @file GT2005RobotStateDetector.cpp
*
* Implementation of class GT2005RobotStateDetector
*
* @author Max Risler
*/

#include "GT2005RobotStateDetector.h"
#include "Platform/SystemCall.h"

GT2005RobotStateDetector::GT2005RobotStateDetector(const RobotStateDetectorInterfaces& interfaces)
: RobotStateDetector(interfaces)
{
  int i,j,k;
  
  for (i = 0; i < BodyPercept::numOfSwitches;switchDown[i++]=false);
  
  for (i = 0; i < 6; i++)
  {
    timeSinceLastCollision[i] = 0;
    consecutiveCollisionTime[i] = 0;
  }
   
  remindCollision = 750;

  for (j = 0; j < maxFrameHistory; j++)
  {
    for (k = 0; k < robotSides; k++)
    {
      numOfCollisions[j][k] = false;
    }  
  }

  /****/
  for (i = 0; i < BodyPercept::numOfSwitches;i++)
  {
    buttonPressed[i]=false;
    buttonTime[i]=0;
    buttonPressedTime[i]=0;
    buttonDuration[i]=0;
  }
  anyBackButtonPressed=false;
  anyBackButtonTime=0;
  anyBackButtonPressedTime=0;
  anyBackButtonDuration=0;
  setNewButtonStatus();
  /****/
}

void GT2005RobotStateDetector::execute()
{
  robotState.setFrameNumber(bodyPercept.frameNumber);
  
  calculateCollisionState();
  
  robotState.setState(
    (RobotState::States)bodyPercept.getState());
  
  robotState.setMouthState(
    (RobotState::MouthStates)bodyPercept.getMouthState());
  
  robotState.setSomethingInFrontOfChest(bodyPercept.getBodyPSDHighValue());
  robotState.setDistanceToSIFOC(bodyPercept.getBodyPSDDistance());
  if(bodyPercept.getBodyPSDHighValue())
  {
    robotState.setTimeWhenSomethingWasInFrontOfChestLast(SystemCall::getCurrentSystemTime());
  }

  robotState.acceleration=bodyPercept.acceleration;
  
  int i;
  for (i = 0; i < BodyPercept::numOfSwitches; i++) 
  {
    if ((bodyPercept.getSwitches() & (1<<i)) > 0)
    {
      if (!switchDown[i]) 
      {
        // switch pressed
        switchDown[i] = true;
        switchDownTime[i] = SystemCall::getCurrentSystemTime();
        
        /***/
        buttonPressed[i]=true;  //Button pressed
        buttonTime[i]=SystemCall::getCurrentSystemTime(); //Aktuelle Zeit
        buttonPressedTime[i]=0; //Later: CurrentTime-buttonTime, buttonPressedTime started
        buttonDuration[i]=0;
        
        switch(i)
        {
        case BodyPercept::backMiddle:
        case BodyPercept::backBack:
        case BodyPercept::backFront:
          {
            if (anyBackButtonPressed==false)
            {
              anyBackButtonPressed=true;
              anyBackButtonTime=SystemCall::getCurrentSystemTime();
              anyBackButtonPressedTime=0;
              anyBackButtonDuration=0;
            }
            break;
          }
        }
        setNewButtonStatus();
        /***/
        
      }
    }
    else
    { 
      if (switchDown[i]) 
      {
        // switch released
        switchDown[i] = false;
        
        /***/
        buttonPressed[i]=false;  //Button released
        buttonPressedTime[i]=SystemCall::getCurrentSystemTime()-buttonTime[i]; //how long the button was pressed
        buttonTime[i]=SystemCall::getCurrentSystemTime(); //when the button was pressed
        buttonDuration[i]=0; // And it starts again
        if (anyBackButtonPressed==true)
        {
          if ((buttonPressed[BodyPercept::backMiddle]==false)&&(buttonPressed[BodyPercept::backFront]==false)&&(buttonPressed[BodyPercept::backBack]==false))
          {
            anyBackButtonPressed=false;
            anyBackButtonPressedTime=SystemCall::getCurrentSystemTime()-anyBackButtonTime;
            anyBackButtonTime=SystemCall::getCurrentSystemTime();
            anyBackButtonDuration=0;
          }
        }
        setNewButtonStatus();
        /***/
      }
    }
  }
  /****/
  for (i = 0; i < BodyPercept::numOfSwitches; i++)  // update my duration time
  {
    buttonDuration[i]=SystemCall::getTimeSince(buttonTime[i]);
    /*  
    OUTPUT(idText, text, "ButtonDuration: " << buttonDuration[i]);
    OUTPUT(idText, text, "ButtonTime: " << buttonTime[i]);
    OUTPUT(idText, text, "ButtonPressed: " << buttonPressed[i]);
    OUTPUT(idText, text, "ButtonPressedTime: " << buttonPressedTime[i]);
    */  
  }
  anyBackButtonDuration=SystemCall::getTimeSince(anyBackButtonTime);
  /*
  OUTPUT(idText, text, "anyBackButtonDuration: " << anyBackButtonDuration);
  OUTPUT(idText, text, "--------------- ");
  */
  setNewButtonStatus();
  /****/

  DEBUG_RESPONSE("Processes:Cognition - activate button-pressed-messages",
    if(robotState.getButtonPressed(BodyPercept::head) &&
      robotState.getButtonDuration(BodyPercept::head) < 10
      )
    {
      OUTPUT(idText, text, "head-button-pressed");
    }

    if(robotState.getAnyBackButtonPressed() &&
      robotState.getAnyBackButtonDuration() < 10
      )
    {
      OUTPUT(idText, text, "back-button-pressed");
    }

    );

}

void GT2005RobotStateDetector::setNewButtonStatus()
{
  robotState.setButtonPressed(buttonPressed);
  robotState.setButtonTime(buttonTime);
  robotState.setButtonPressedTime(buttonPressedTime);
  robotState.setButtonDuration(buttonDuration);
  robotState.setAnyBackButtonPressed(anyBackButtonPressed);
  robotState.setAnyBackButtonTime(anyBackButtonTime);
  robotState.setAnyBackButtonPressedTime(anyBackButtonPressedTime);
  robotState.setAnyBackButtonDuration(anyBackButtonDuration);
}

void GT2005RobotStateDetector::calculateCollisionState()
{
  // collisionTime  1. Component 0 fl, 1 fr, 2 hl, 3 hr, 4 head, 5 anyone of them
  //                2. Component two previous collision times
  
  
  long time = SystemCall::getCurrentSystemTime();
  
  //setze alle states auf 0!!!
  
  robotState.setCollisionFrontLeft(false);
  robotState.setCollisionFrontRight(false);
  robotState.setCollisionHindLeft(false);
  robotState.setCollisionHindRight(false);
  robotState.setCollisionHead(false);
  robotState.setCollisionAggregate(false);
  
  
  if ((time - timeSinceLastCollision[0]) >  remindCollision)
  {
    consecutiveCollisionTime[0] = time;
    robotState.setConsecutiveCollisionTimeFrontLeft(0);
  }
  
  if ((time - timeSinceLastCollision[1]) >  remindCollision)
  {
    consecutiveCollisionTime[1] = time;
    robotState.setConsecutiveCollisionTimeFrontRight(0);
  }
  
  if ((time - timeSinceLastCollision[2]) >  remindCollision)
  {
    consecutiveCollisionTime[2] = time;
    robotState.setConsecutiveCollisionTimeHindLeft(0);
  }
  
  if ((time - timeSinceLastCollision[3]) >  remindCollision)
  {
    consecutiveCollisionTime[3] = time;
    robotState.setConsecutiveCollisionTimeHindRight(0);
  }
  
  if ((time - timeSinceLastCollision[4]) >  remindCollision)
  {
    consecutiveCollisionTime[4] = time;
    robotState.setConsecutiveCollisionTimeHead(0);
  }
  
  if ((time - timeSinceLastCollision[5]) >  remindCollision)
  {
    consecutiveCollisionTime[5] = time;
    robotState.setConsecutiveCollisionTimeAggregate(0);
  }
  
  if (collisionPercept.getCollisionFrontLeft())
  {
    robotState.setCollisionFrontLeft(true);
    if ((time-timeSinceLastCollision[0]) > remindCollision)
    {
      consecutiveCollisionTime[0] = time; // first collision
    }
    robotState.setConsecutiveCollisionTimeFrontLeft(time-consecutiveCollisionTime[0]);
    timeSinceLastCollision[0] = time; //last collision
  }
  
  if (collisionPercept.getCollisionFrontRight()) 
  {
    robotState.setCollisionFrontRight(true);
    if ((time-timeSinceLastCollision[1]) > remindCollision)
    {
      consecutiveCollisionTime[1] = time;
    }
    robotState.setConsecutiveCollisionTimeFrontRight(time-consecutiveCollisionTime[1]);
    timeSinceLastCollision[1] = time;
  }
  
  if (collisionPercept.getCollisionHindLeft()) 
  {
    robotState.setCollisionHindLeft(true);
    if ((time-timeSinceLastCollision[2]) > remindCollision)
    {
      consecutiveCollisionTime[2] = time;
    }
    robotState.setConsecutiveCollisionTimeHindLeft(time-consecutiveCollisionTime[2]);
    timeSinceLastCollision[2] = time;
  }
  
  if (collisionPercept.getCollisionHindRight()) 
  {
    robotState.setCollisionHindRight(true);
    if ((time-timeSinceLastCollision[3]) > remindCollision)
    {
      consecutiveCollisionTime[3] = time;
    }
    robotState.setConsecutiveCollisionTimeHindRight(time-consecutiveCollisionTime[3]);
    timeSinceLastCollision[3] = time;
  }
  
  if (collisionPercept.getCollisionHead()) 
  {
    robotState.setCollisionHead(true);
    if ((time-timeSinceLastCollision[4]) > remindCollision)
    {
      consecutiveCollisionTime[4] = time;
    }
    robotState.setConsecutiveCollisionTimeHead(time-consecutiveCollisionTime[4]);
    timeSinceLastCollision[4] = time;
  }
  
  if (collisionPercept.getCollisionAggregate()) 
  {
    robotState.setCollisionAggregate(true);
    if ((time-timeSinceLastCollision[5]) > remindCollision)
    {
      consecutiveCollisionTime[5] = time;
    }
    robotState.setConsecutiveCollisionTimeAggregate(time-consecutiveCollisionTime[5]);
    timeSinceLastCollision[5] = time;
  }

  robotState.setOdometryDisturbance(collisionPercept.getOdometryDisturbance());

  if (collisionPercept.getCollisionFrontLeft() || collisionPercept.getCollisionHindLeft())
  {
    setCollisionOnLeftSide(true);
  }
  else 
  {
    setCollisionOnLeftSide(false);
  }

  if (collisionPercept.getCollisionFrontRight() || collisionPercept.getCollisionHindRight())
  {
    setCollisionOnRightSide(true);
  }
  else 
  {
    setCollisionOnRightSide(false);
  }

  robotState.setCollisionSide(getCollisionSide());
}

  /** Did collision occur at front left leg */
int GT2005RobotStateDetector::getCollisionSide() 
{
    int l = 0;
    int r = 0;

    for (int i = 0; i < maxFrameHistory; i++)
    {
      if (numOfCollisions[i][left])  {++l;}
      if (numOfCollisions[i][right]) {++r;}
    }

    if (l > r) {return left;}
    else if (l < r) {return right;}
    else return middle;
}

void GT2005RobotStateDetector::setCollisionOnLeftSide(bool collision)
{
  numOfCollisions[collisionPercept.frameNumber%maxFrameHistory][GT2005RobotStateDetector::left] = collision;
}

void GT2005RobotStateDetector::setCollisionOnRightSide(bool collision)
{
  numOfCollisions[collisionPercept.frameNumber%maxFrameHistory][GT2005RobotStateDetector::right] = collision;
}

---