Representations/Cognition/RobotPose.h

Go to the documentation of this file.

/**
 * @file RobotPose.h
 *
 * The file contains the definition of the class RobotPose.
 *
 * @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
 */

#ifndef __RobotPose_h_
#define __RobotPose_h_

#include "Tools/Streams/InOut.h"
#include "Tools/Math/Pose2D.h"

/**
 * @class RobotPose
 * A Pose2D with validity.
 */
class RobotPose : public Pose2D
{
  public:
    double validity; /**< The validity of the robot pose. */
    double positionVariance; /**< The variance of the particle distribution from which the pose was generated. */
    Pose2D greatestUncertainty; /**< direction of greatest uncertainty from the (x,y) average of all particles */
    bool directionOfGreatestUncertaintyExists; /**< if particle distribution is symmetrical, this is false, otherwise its set to true*/

  public:
    /**
     * Constructor.
     */
    RobotPose() {validity = 0; frameNumber = 0; directionOfGreatestUncertaintyExists = false;}

    const RobotPose& operator=(const RobotPose& other)
    {
      (Pose2D&) *this = (const Pose2D&) other;
      validity = other.validity;
      positionVariance = other.positionVariance;
      greatestUncertainty = other.greatestUncertainty;
      directionOfGreatestUncertaintyExists = other.directionOfGreatestUncertaintyExists;
      frameNumber = other.frameNumber;
      timestamp = other.timestamp;
      return *this;
    }

    /**
     * Cast Contructor.
     */
    RobotPose (const Pose2D& otherPose) {
      (Pose2D&) *this = otherPose;
      validity=0;
      positionVariance=0; 
      directionOfGreatestUncertaintyExists = false;
      greatestUncertainty = Pose2D();
    }

    const RobotPose& operator=(const Pose2D& other)
    {
      (Pose2D&) *this = other;
      //don't touch validity and frameNumber here!
      return *this;
    }

    /**
     * The function sets the current pose.
     * @param p The pose.
     */
    void setPose(const Pose2D& p) {
      (Pose2D&) *this = p;
    }

    /**
     * The function sets the validity of the current pose.
     * @param v The validity.
     */
    void setValidity(const double v) {validity = v;}

    /**
     * The function sets the validity of the current pose.
     * @param v The validity.
     */
    void setPositionVariance(const double v) {positionVariance = v;}

  /**
     * The function sets the frame number of the current pose.
     */
    void setFrameNumber(unsigned long frameNumber) {this->frameNumber = frameNumber;}

  /**
     * The function sets the timestamp of the current pose.
     */
    void setTimestamp(unsigned long timestamp) {this->timestamp = timestamp;}

  
  /**
     * The function returns the current pose.
     * @return The pose.
     * @attention this function is obsolete and should not be used any more
     * @attention instead this object IS a pose instead of owning one
     */
    const Pose2D getPose() const {
      return (const Pose2D&) *this;
    }

    /**
     * The function returns the validity of the current pose.
     * @return The validity.
     */
    const double& getValidity() const {return validity;}

    /**
     * The function returns the variance of the current particle distribuiton of the robot pose.
     * @return The variance.
     */
    const double& getPositionVariance() const {return positionVariance;}

    /**
     * The function returns the standard deviation of the current particle distribuiton of the robot pose.
     * @return The standard-deviation.
     */
    double getPositionStandardDeviation() const; 
    
    /**
     * The function returns the position, where the robot will be 
     * if it walks with the specified translation and rotation for the specified time.
     * @param movementPerSec The movement with x- and y-translation in mm/s and rotation in rad/s
     * @param time The time in sec how long the robot moves
     * @param result The target pose.
     */
    void calculateMovement(const Pose2D & movementPerSec, double time, RobotPose & result) const;


  /**< The frame number when perceived. */
  unsigned long frameNumber;

  /**< The time when the position is calculated. */
  unsigned long timestamp;

  /**< Speed measured by perceived distance to goal. */
  double speedbyDistanceToGoal;

/**< The distance to the field border */
  double distanceToBorder;

  /**< The angle to the field border */
  double angleToBorder;

  friend In& operator>>(In& stream,RobotPose& robotPose);
  friend Out& operator<<(Out& stream, const RobotPose& robotPose);
};

/**
 * Streaming operator that reads a robot pose from a stream.
 * @param stream The stream from which is read.
 * @param robotPose The robot pose object.
 * @return The stream.
 */ 
In& operator>>(In& stream,RobotPose& robotPose);
 
/**
 * Streaming operator that writes a robot pose to a stream.
 * @param stream The stream to write on.
 * @param robotPose The robot pose object.
 * @return The stream.
 */ 
Out& operator<<(Out& stream, const RobotPose& robotPose);


#endif //__RobotPose_h_

---