Modules/RobotStateDetector/GT2004RobotStateDetector.cpp

Go to the documentation of this file.

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

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

GT2004RobotStateDetector::GT2004RobotStateDetector(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 GT2004RobotStateDetector::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();
  /****/
}

void GT2004RobotStateDetector::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 GT2004RobotStateDetector::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;
  }

  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 GT2004RobotStateDetector::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 GT2004RobotStateDetector::setCollisionOnLeftSide(bool collision)
{
  numOfCollisions[collisionPercept.frameNumber%maxFrameHistory][GT2004RobotStateDetector::left] = collision;
}

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



/*
* Change log :
* 
* $Log: GT2004RobotStateDetector.cpp,v $
* Revision 1.1  2004/07/10 00:18:31  spranger
* renamed (readded) for coderelease
*
* Revision 1.6  2004/06/23 11:02:26  goehring
* Xabsl symbol collision-side added
*
* Revision 1.5  2004/06/16 17:07:33  cesarz
* Moved body PSD calculations
*
* Revision 1.4  2004/06/08 18:35:18  juengel
* Added robot-state.time-since-something-was-in-front-of-chest.
*
* Revision 1.3  2004/05/25 12:36:51  tim
* added body PSD data
*
* Revision 1.2  2004/05/24 14:14:26  juengel
* New button evaluation.
*
* Revision 1.1.1.1  2004/05/22 17:20:41  cvsadm
* created new repository GT2004_WM
*
* Revision 1.8  2004/05/11 08:00:19  juengel
* Fixed VC6 compiler errors.
*
* Revision 1.7  2004/05/11 06:59:05  brueckne
* added new way to know how long a button is/was pressed/released (not ready yet)
*
* Revision 1.6  2004/03/08 02:11:44  roefer
* Interfaces should be const
*
* Revision 1.5  2004/02/10 11:12:31  dueffert
* acceleration added
*
* Revision 1.4  2004/01/24 11:32:32  juengel
* Added ERS7 switches (head, backFront, backBack).
*
* Revision 1.3  2003/11/14 19:02:26  goehring
* frameNumber added
*
* Revision 1.2  2003/10/16 12:58:40  goehring
* remindCollision reduced for CollisionDetection
*
* Revision 1.1  2003/10/06 14:10:14  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.3  2003/09/26 21:23:20  loetzsch
* renamed class JointState to CollisionPercept
*
* Revision 1.2  2003/07/06 12:05:31  schumann
* added foreleg opening angle for ball challenge
*
* Revision 1.1.1.1  2003/07/02 09:40:24  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.10  2003/06/21 13:07:36  goehring
* CollisionStateSymbol modelling changed
*
* Revision 1.9  2003/06/20 16:57:46  goehring
* CollisionDetectorValues improved
*
* Revision 1.8  2003/06/20 14:50:07  risler
* numOfSwitches added
*
* Revision 1.7  2003/06/20 13:27:19  risler
* added tailLeft tailRight messages
*
* Revision 1.6  2003/06/20 10:30:37  goehring
* calculateCollisionState implemented
*
* Revision 1.5  2003/04/01 22:40:44  cesarz
* added mouth states
*
* Revision 1.4  2003/03/24 18:39:46  loetzsch
* message backPressedAndTailUp and backPressedAndTailDown removed
*
* Revision 1.3  2003/01/30 11:29:24  juengel
* Added tailPosition to bodyPercept
*
* Revision 1.2  2002/09/12 09:45:58  juengel
* continued change of module/solution mechanisms
*
* Revision 1.1  2002/09/10 15:36:15  cvsadm
* Created new project GT2003 (M.L.)
* - Cleaned up the /Src/DataTypes directory
* - Removed challenge related source code
* - Removed processing of incoming audio data
* - Renamed AcousticMessage to SoundRequest
*
* Revision 1.4  2002/08/30 14:33:38  dueffert
* removed unused includes
*
* Revision 1.3  2002/05/15 10:52:42  risler
* no message
*
* Revision 1.2  2002/05/15 08:09:10  risler
* added button down messages
*
* Revision 1.1.1.1  2002/05/10 12:40:15  cvsadm
* Moved GT2002 Project from ute to tamara.
*
* Revision 1.6  2002/03/28 16:55:58  risler
* RobotStateDetector receives BodyPercept instead of PerceptCollection
* added switch duration in RobotStateDetector
*
* Revision 1.5  2002/03/28 15:21:40  risler
* removed output
*
* Revision 1.4  2002/03/28 15:19:20  risler
* implemented switch messages in RobotStateDetector
*
* Revision 1.3  2002/02/23 16:33:07  risler
* finished GetupEngine
*
* Revision 1.2  2001/12/10 17:47:06  risler
* change log added
*
*/

---