Modules/BehaviorControl/MotionRatingBehaviorControl/MotionRatingBehaviorControl.cpp

Go to the documentation of this file.

/**
* @file MotionRatingBehaviorControl.cpp
* 
* Implementation of class MotionRatingBehaviorControl.
*
* @author Dimitri Idessis
* @author Eduardo Tsen
* @author Sebastian Petters
* @author Dirk Thomas
*/

#include "MotionRatingBehaviorControl.h"

MotionRatingBehaviorControl::MotionRatingBehaviorControl(BehaviorControlInterfaces& interfaces)
: BehaviorControl(interfaces)
{
  showMessages = false;

  lastMessage = 0;
  pingTimeout = 3000;
  reset();
  path = 0;
  pathOffset = 0;
  startlocalise = 0;
  startrotate = 0;
  state = sleep;

  distTolerance = 100.0;
  angleTolerance = 0.1;

}

MotionRatingBehaviorControl::~MotionRatingBehaviorControl()
{
}

bool MotionRatingBehaviorControl::handleMessage(InMessage& msg)
{
  if ( msg.getMessageID() != idMotionRatingBehaviorMessage ) return false;

  MotionRatingBehaviorMessage message;
  msg.bin >> message;

  Vector2<double> v;

  switch ( message.type )
  {
    case MotionRatingBehaviorMessage::init:
      OUTPUT(idText,text,"BEHA: 'init' received");
      path = message.data;
      paramSetId = 0;
      walkCount = 0;
      currentWalk = 0;

      track.start.translation = message.start;
      track.end.translation = message.end;
      v = track.end.translation - track.start.translation;
      track.start.rotation = normalize ( v.angle() );
      v = track.start.translation - track.end.translation;
      track.end.rotation = normalize ( v.angle() );

      setPoints();
      startlocalise = SystemCall::getCurrentSystemTime();
      state = localise;
      break;
    case MotionRatingBehaviorMessage::request:
      OUTPUT(idText,text,"BEHA: 'request' received");
      paramSetId = message.data;
      walkCount = message.walkCount;
      currentWalk = 0;
      setPoints();
      startlocalise = SystemCall::getCurrentSystemTime();
      state = localise;
      break;
    case MotionRatingBehaviorMessage::abort:
      paramSetId = 0;
      state = wait;
      break;
    default:
      OUTPUT ( idText, text, "unhandled message received" );
  }

  MotionRatingBehaviorMessage outmessage;

  outmessage.type = MotionRatingBehaviorMessage::ping;
  outmessage.state = encodeState();
  outmessage.paramSetId = paramSetId;
  outmessage.data = 0;
  outmessage.currentSpeed = 0.0;
  outmessage.averageSpeed = 0.0;
  outmessage.currentWalk = currentWalk; // + 1;
  outmessage.walkCount = walkCount;
  outmessage.remainingPower = SystemCall::getRemainingPower();

  OUTPUT ( idMotionRatingBehaviorMessage, bin, outmessage );

  lastMessage = SystemCall::getCurrentSystemTime();

  return true;
}


void MotionRatingBehaviorControl::execute()
{
  /////////////////////////////
    char message[100];
    //if ( showMessages ) sprintf ( message,"(%.2f,%.2f) -> %.2f\r\n\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation);
    //if ( showMessages ) OUTPUT(idText,text, message );
  /////////////////////////////

  headControlMode.headControlMode = HeadControlMode::searchForLandmarks;

  bool notify = false;

  switch (state)
  {
  case sleep:
    // sleep and wait for start message
    if ( showMessages ) sprintf ( message,"sleep (at %.2f,%.2f,%.2f)\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation);
    playDead();
    break;
  case localise:
    // stand until localisation is ok before positioning
    {
      bool localized = doLocalise();
      if ( SystemCall::getTimeSince(startlocalise) > 1000 && localized )
      {
        state = prepare;
      }
    }
    break;
  case prepare:
    // go to starting point
    if ( showMessages ) sprintf ( message,"prepare (at %.2f,%.2f,%.2f) [rotate to %.2f]\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation,startpoint.rotation);
    if ( gotoPoint ( startpoint, true ) )
    {
      notify = true;
      startlocalise = SystemCall::getCurrentSystemTime();
      state = ( paramSetId != 0 ) ? localisepre : wait;
    }
    // abort if localisation lost
    if ( robotPose.getValidity() < 0.1 )
    {
      stand();
      state = localise;
    }
    break;
  case wait:
    // stand and wait for parameters
    headControlMode.headControlMode = HeadControlMode::lookStraightAhead;
    if ( showMessages ) sprintf ( message,"wait (at %.2f,%.2f,%.2f)\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation);
    stand();
    state = ( paramSetId != 0 ) ? localise : wait;
    break;
  case localisepre:
    {
      // stand until localisation is ok before testing
      if ( showMessages ) sprintf ( message,"localisepre (at %.2f,%.2f,%.2f)\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation);
      bool localized = doLocalise();
      if ( SystemCall::getTimeSince(startlocalise) > 1000 && localized )
      {
        notify = true;
        p1 = robotPose.translation;
        currentWalk++;
        state = walk;
        starttime = SystemCall::getCurrentSystemTime();
      }
    }
    break;
  case walk:
    // test a parameter set
    if ( showMessages ) sprintf ( message,"walk (at %.2f,%.2f,%.2f) [from %.2f,%.2f,%.2f to %.2f,%.2f,%.2f]\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation,startpoint.translation.x,startpoint.translation.y,startpoint.rotation,endpoint.translation.x,endpoint.translation.y,endpoint.rotation);
    if ( gotoPoint ( endpoint.translation, false ) || ( SystemCall::getTimeSince(starttime) > 20000 ))
    {
      stand();
      state = stop;
    }
    // abort if localisation lost
    if ( robotPose.getValidity() < 0.1 )
    {
      stand();
      state = stop;
    }
    break;
  case stop:
    // stop and wait until robot has stopped
    if ( showMessages ) sprintf ( message,"stop (at %.2f,%.2f,%.2f)\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation);
    stand();
    if ( motionInfo.executedMotionRequest.motionType == MotionRequest::stand )
    {
      endtime = SystemCall::getCurrentSystemTime();
      startlocalise = SystemCall::getCurrentSystemTime();
      state = localiseaft;
    }
    break;
  case localiseaft:
    {
      // stand until localisation is ok after testing
      if ( showMessages ) sprintf ( message,"localiseaft (at %.2f,%.2f,%.2f)\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation);
      bool localized = doLocalise();
      if ( SystemCall::getTimeSince(startlocalise) > 1000 && localized )
      {
        p2 = robotPose.translation;
        state = report;
      }
    }
    break;
  case report:
    // measure speed and report results
    if ( showMessages ) sprintf ( message,"report (at %.2f,%.2f,%.2f)\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation);
    {
      double distance = (p2 - p1).abs();
      unsigned long time = (endtime - starttime);

      totalDistance += distance;
      totalTime += time;

      /*
      char message[50];
      sprintf ( message, "current: t: %.2fs d: %.2fm v: %.2fm/s",
        time/1000.0,
        distance/1000.0,
        100.0*(distance/time)
      );
      OUTPUT(idText,text, message );
      sprintf ( message, "total: t: %.2fs d: %.2fm v: %.2fm/s",
        totalTime/1000.0,
        totalDistance/1000.0,
        100.0*(totalDistance/totalTime)
      );
      OUTPUT(idText,text, message );
      */

      //currentWalk++;
      bool finished = walkCount <= currentWalk;

      pathOffset = 1 - pathOffset;
      setPoints();
      state = prepare;
      
      MotionRatingBehaviorMessage message;
      message.remainingPower = SystemCall::getRemainingPower();
      message.type = MotionRatingBehaviorMessage::report;
      message.state = encodeState();
      message.paramSetId = paramSetId;
      message.currentSpeed = distance / time;
      message.averageSpeed = totalDistance / totalTime;
      message.currentWalk = currentWalk; // + 1;
      message.walkCount = walkCount;
      message.finished = finished;
      message.distance=distance;
      message.time=time;

      OUTPUT ( idMotionRatingBehaviorMessage, bin, message );
      lastMessage = SystemCall::getCurrentSystemTime();

      if ( finished ) {
        reset();
        // state = wait;
      }
    }
    break;
  default:
    OUTPUT(idText,text,"unknown state");
    break;
  }
  // debug output current state and position and optionally target info
  if ( showMessages ) OUTPUT(idText,text, message );

  if ( notify || ( SystemCall::getTimeSince(lastMessage) > pingTimeout ) )
  {
    MotionRatingBehaviorMessage message;
    message.type = MotionRatingBehaviorMessage::ping;
    message.state = encodeState();
    message.paramSetId = paramSetId;
    message.currentWalk = currentWalk; // + 1;
    message.walkCount = walkCount;
    message.data = 0;
    message.remainingPower = SystemCall::getRemainingPower();

    OUTPUT ( idMotionRatingBehaviorMessage, bin, message );

    lastMessage = SystemCall::getCurrentSystemTime();
  }

}

void MotionRatingBehaviorControl::reset()
{
  paramSetId = 0;
  currentWalk = 0;
  walkCount = 0;
  totalDistance = 0.0;
  totalTime = 0;
  paused = false;
}

void MotionRatingBehaviorControl::setPoints()
{
  startpoint = ( pathOffset == 0 ) ? track.start : track.end;
  endpoint = ( pathOffset == 0 ) ? track.end : track.start;
}

bool MotionRatingBehaviorControl::gotoPoint ( Pose2D pose, bool safe )
{
  char message[50];


  Vector2<double> v = Pose2D( -robotPose.rotation ) * ( pose.translation - robotPose.translation );
    
  if ( showMessages ) sprintf ( message, "Vektor v: (%.2f,%.2f)",v.x,v.y);
  if ( showMessages ) OUTPUT(idText,text, message );

  double dist = v.abs();
  double theta = normalize ( v.angle() );
  double phi = normalize ( pose.getAngle() - robotPose.getAngle() );

  if ( showMessages ) sprintf ( message, "dist: %.2f",dist);
  if ( showMessages ) OUTPUT(idText,text, message );
  if ( showMessages ) sprintf ( message, "theta: %.2f",theta);
  if ( showMessages ) OUTPUT(idText,text, message );
  if ( showMessages ) sprintf ( message, "phi: %.2f",phi);
  if ( showMessages ) OUTPUT(idText,text, message );



  double rotationDist = 600;
  double t = 1.0;
  if ( dist < rotationDist && dist > distTolerance ) t = ( dist - distTolerance ) / ( rotationDist - distTolerance );
  if ( dist <= distTolerance ) t = 0.0;
  double alpha = t * theta + (1 - t) * phi;

  if ( showMessages ) sprintf ( message, "alpha: %.2f\r\n",alpha);
  if ( showMessages ) OUTPUT(idText,text, message );

  setWalkMotionRequest ( dist, theta, alpha, safe );

  return dist < distTolerance && fabs ( phi ) < angleTolerance;
}

bool MotionRatingBehaviorControl::gotoPoint ( Vector2<double> point, bool safe )
{
  char message[50];

  Vector2<double> v = Pose2D( -robotPose.rotation ) * ( point - robotPose.translation );

  if ( showMessages ) sprintf ( message, "Vektor v: (%.2f,%.2f)",v.x,v.y);
  if ( showMessages ) OUTPUT(idText,text, message );

  double dist = v.abs();
  double theta = normalize ( v.angle() );

  if ( showMessages ) sprintf ( message, "dist: %.2f",dist);
  if ( showMessages ) OUTPUT(idText,text, message );
  if ( showMessages ) sprintf ( message, "theta: %.2f",theta);
  if ( showMessages ) OUTPUT(idText,text, message );

  setWalkMotionRequest ( walkMaxForwardSpeed, theta, theta, safe );

  return dist < distTolerance;
}

void MotionRatingBehaviorControl::setWalkMotionRequest ( double distance, double theta, double phi, bool safe )
{
  double speed = distance > walkMaxForwardSpeed ? walkMaxForwardSpeed : distance;

  if ( phi >  walkMaxRotationSpeed ) phi =  walkMaxRotationSpeed;
  if ( phi < -walkMaxRotationSpeed ) phi = -walkMaxRotationSpeed;

  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = safe ? WalkRequest::normal : WalkRequest::normal;
  motionRequest.walkRequest.walkParams.rotation = phi;
  motionRequest.walkRequest.walkParams.translation.x = cos ( theta ) * speed;
  motionRequest.walkRequest.walkParams.translation.y = sin ( theta ) * speed;
}

bool MotionRatingBehaviorControl::stand()
{
  motionRequest.motionType = MotionRequest::stand;
  return (motionInfo.executedMotionRequest.motionType==MotionRequest::stand);
}

void MotionRatingBehaviorControl::playDead()
{
  motionRequest.motionType = MotionRequest::playDead;
}


bool MotionRatingBehaviorControl::doLocalise()
{
  /*
  char message[50];
  sprintf ( message, "pos: x:%.2f, y:%.2f phi:%.2f s:%.4f",
    robotPose.translation.x,
    robotPose.translation.y,
    robotPose.rotation,
    robotPose.getValidity()
  );
  OUTPUT(idText,text, message );
  */
  //headControlMode.headControlMode = HeadControlMode::optimizerMode;
  if (!stand()) return false;
  return robotPose.getValidity() > 0.90;
}

MotionRatingBehaviorMessage::States MotionRatingBehaviorControl::encodeState()
{
  switch (state)
  {
    case sleep:
      return MotionRatingBehaviorMessage::sleeping;
    case wait:
      return MotionRatingBehaviorMessage::waiting;
    case localise:
    case prepare:
      return ( paramSetId != 0 ) ? MotionRatingBehaviorMessage::testing : MotionRatingBehaviorMessage::preparing;
    case localisepre:
    case walk:
    case stop:
    case localiseaft:
    case report:
      return MotionRatingBehaviorMessage::testing;
    default:
      return MotionRatingBehaviorMessage::unknown;
  }
}


//bool MotionRatingBehaviorControl::goTo ( Pose2D target /*Vector2<double> point */)
/*{
  if ((target.translation-robotPose.translation).abs()<distTolerance)
  {
   return true;
  }
  else
  {
        double theta=robotPose.rotation-target.rotation;
        if (fabs(theta)<angleTolerance)
        {
          theta=0.0;
        }
        motionRequest.motionType = MotionRequest::walk;
        motionRequest.walkRequest.walkType = WalkRequest::normal;
        motionRequest.walkRequest.walkParams = Pose2D(theta,walkMaxForwardSpeed,0);
    return false;
  }
}

bool MotionRatingBehaviorControl::rotateTo ( double angle )
{
      if (startrotate==0) startrotate=SystemCall::getCurrentSystemTime();
      double theta=robotPose.rotation-angle;
      if (fabs(theta)<angleTolerance 
        || ( SystemCall::getTimeSince(startlocalise) > 10000 ))
      {
        startrotate=0;
        return true;
      }
      else
      {
        motionRequest.motionType = MotionRequest::walk;
        motionRequest.walkRequest.walkType = WalkRequest::normal;
        //if (fabs(theta)>pi_2) theta=(theta<0.0)?-pi_2:pi_2;
        motionRequest.walkRequest.walkParams = Pose2D(theta,0,0);
        return false;
      }

}*/



/*
* Change log :
* 
* $Log: MotionRatingBehaviorControl.cpp,v $
* Revision 1.1  2004/03/17 16:18:49  thomas
* added preversion of motion optimisation with behaviour, selflocator, headcontrol and robotcontrol dialog
*
*
*/

---