Representations/Cognition/RobotPose.cpp

Go to the documentation of this file.

/**
* @file RobotPose.cpp
*
* contains the implementation of the class RobotPose
*/

#include "RobotPose.h"

double RobotPose::getPositionStandardDeviation() const
{
  return sqrt(positionVariance);
}

void RobotPose::calculateMovement(const Pose2D & movementPerSec, double time, RobotPose & result) const
{
  Vector2<double> speedVector(movementPerSec.translation.x,movementPerSec.translation.y);
  double speed = speedVector.abs();
  double diffBetweenOrientationAndSpeedVectorDirection = atan2(speedVector.y,speedVector.x);
  result = *this;
  result.rotation += diffBetweenOrientationAndSpeedVectorDirection;
  Pose2D movement; // the transition will be calculated in a system alligned with the speed vector
  if (movementPerSec.rotation == 0.0)
  {
    movement.rotation = 0;
    movement.translation.x = speed * time;
    movement.translation.y = 0;
  }
  else
  {
    movement.rotation = movementPerSec.rotation * time;
    movement.translation.x = speed/movementPerSec.rotation * sin(movement.rotation);
    movement.translation.y = speed/movementPerSec.rotation * (1.0 - cos(movement.rotation));
  }
  result += movement; // this automatically rotates the movement translation vector into the right orientation and so on... 
  result.rotation -= diffBetweenOrientationAndSpeedVectorDirection;
}

In& operator>>(In& stream,RobotPose& robotPose)
{

  STREAM_REGISTER_BEGIN_EXT(robotPose);
  STREAM_BASE_EXT( stream, (Pose2D&) robotPose);
  STREAM_EXT(stream, robotPose.validity);
  STREAM_EXT(stream, robotPose.positionVariance);
  STREAM_EXT(stream, robotPose.frameNumber);
  STREAM_EXT(stream, robotPose.timestamp);
  STREAM_EXT(stream, robotPose.speedbyDistanceToGoal);
  STREAM_EXT(stream, robotPose.directionOfGreatestUncertaintyExists);
  STREAM_EXT(stream, robotPose.greatestUncertainty);
  STREAM_REGISTER_FINISH();
/*
  stream.read(&robotPose,sizeof(RobotPose));
*/
  return stream;
}

Out& operator<<(Out& stream, const RobotPose& robotPose)
{
  STREAM_REGISTER_BEGIN_EXT(robotPose);
  STREAM_BASE_EXT( stream, (Pose2D&) robotPose);
  STREAM_EXT(stream, robotPose.validity);
  STREAM_EXT(stream, robotPose.positionVariance);
  STREAM_EXT(stream, robotPose.frameNumber);
  STREAM_EXT(stream, robotPose.timestamp);
  STREAM_EXT(stream, robotPose.speedbyDistanceToGoal);
  STREAM_EXT(stream, robotPose.directionOfGreatestUncertaintyExists);
  STREAM_EXT(stream, robotPose.greatestUncertainty);
  STREAM_REGISTER_FINISH();
/*
  stream.write(&robotPose,sizeof(RobotPose));
*/
  return stream;
}

---