Representations/Cognition/ObstaclesModel.h

Go to the documentation of this file.

/**
 * @file ObstaclesModel.h
 * 
 * Declaration of class ObstaclesModel
 * @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
 */ 

#ifndef __ObstaclesModel_h_
#define __ObstaclesModel_h_


#include "Tools/Streams/InOut.h"
#include "Tools/Math/Common.h"
#include "Tools/Debugging/Debugging.h"

#include "Representations/Perception/ObstaclesPercept.h"

/**
 * A class describing the distance to obstacles around the robot,
 */
class ObstaclesModel
{
public:
  ObstaclesModel();
  ~ObstaclesModel();

  /** copies another ObstaclesModel to this one */
  void operator = (const ObstaclesModel& other);

  enum Directions{ back, backRight, right, frontRight, front, frontLeft, left, backLeft, numberOfDirections};
  enum SearchDirections{searchLeft, searchLeftAndRight, searchRight};
  enum { numOfSectors = 90 , maxDistance = 1500};

  /**
   * return the center angle of a given sector
   */
  static double getAngleOfSector(int sector)
  {
//    if(sector < numOfSectors && 0 <= sector)
//    {
      return -pi + ((double)sector + 0.5) * pi2 / numOfSectors;
//    }
//    else
//    {
//      OUTPUT(idText, text, "sector out of range");
//      return 0;
//    }
  }

  /** the frame number when perceived */
  unsigned long frameNumber;

  void setFrameNumber(unsigned long frameNumber) {this->frameNumber = frameNumber;}
  
  /** the distance to the closest obstacle in an angular sector */
  int distance[numOfSectors];

  /** */
  ObstaclesPercept::ObstacleType obstacleType[numOfSectors];

  /** the distance to the closest obstacle in a rectangular corridor in front of the robot */
  int corridorInFront;

  /** currently seen begining sector and end sector */
  int currentlySeenStartSector;
  int currentlySeenEndSector;


  /** returns the distance to the closest obstacle
   * in the corridor specified by angle and width */
  double getDistanceInCorridor(double angle, double width) const;

  /** returns the angle of the closest free sector with angle sizeOfSector 
   * relative to angle. Free means free up to minDistance */
  double getAngleOfNextFreeSector(double sizeOfSector, double angle, int minDistance) const;

  /** returns the angle of the closest free with angle sizeOfSector 
   * relative to angle. Free means free up to minDistance */
  double getAngleOfNextFreeSectorLeft(double sizeOfSector, double angle, int minDistance) const;
  double getAngleOfNextFreeSectorRight(double sizeOfSector, double angle, int minDistance) const;

  /** Calculates the angle with the farsest distance to an obstacle between the front direction and the angle.
   * Returns the distance.
   */
//  int getBestAngleBetweenFrontAndDestination(double sizeOfSector, double angle, double& bestAngle) const;

  /** 
   * returns the distance to the closest obstacle
   * in one of the major directions defined above */
  int getDistanceInMajorDirection(Directions direction) const;

  /**
   * return the distance to the closest obstacle
   * in a given direction and a given opening angle */
  int getDistanceInDirection(double direction, double openingAngle) const;
  /**
   * return the distance of an sector to the closest obstacle
   * in a given direction */

  int getSectorDistanceInDirection(double direction) const;
  /**
  * sums up all distances in a sector specified by direction and opening angle;
  * the maxDist(ance) specifies what the maximum distance should be when
  * adding up (you don't want really big single values to distort the sum) */
  double getTotalFreeSpaceInSector(double direction, double openingAngle, double maxDist) const;

  /** Calculates the angle with the closest distance to an obstacle.
   * Returns the distance.
   */
  int getMinimalDistanceInRange(double centerAngle, double openingAngle, double& angleWithMinimalDistance) const;

  /** Calculates the angle with the closest distance to an obstacle of a certain obstacle type.
   * Returns the distance.
   */
  int getMinimalDistanceInRange(double centerAngle, double openingAngle, double& angleWithMinimalDistance, ObstaclesPercept::ObstacleType obstacleType) const;

  /** Calculates the percentage of sectors with obstacles that are closer than the specified distance in the specified range.
   */
  double getPercentageOfLowDistanceObstaclesInRange(double centerAngle, double openingAngle, int maxDistance) const;

  /** Calculates the angle with the farsest distance to an obstacle.
   * Returns the angle.
   */
  double getAngleOfLargeGapInRange(double centerAngle, double openingAngle, SearchDirections searchDirection) const;

  /** Calculates the angle with the farsest distance to an obstacle.
   * Returns the angle.
   */
  double getAngleOfLargeGapInRange2(double centerAngle, double openingAngle, SearchDirections searchDirection) const;

  /**
   * return the corresponding sector to a given angle
   */
  static int getSectorFromAngle(double angle)
  { 
    return (int)((normalize(angle) + pi) / pi2 * numOfSectors);
  }

  enum {ownGoal = 0, opponentGoal = 1};
  double angleToFreePartOfGoal[2];
  double widthOfFreePartOfGoal[2];
  double distanceToFreePartOfGoal[2]; // NOT SET BY GoalRecognizer DDD2005 version
  unsigned long lastTimeFreePartOfGoalWasDetermined[2];
  bool angleToFreePartOfGoalWasDetermined[2];

  bool angleToNextFreeTeammateWasDetermined;
  double angleToNextFreeTeammate;
  double distanceToNextFreeTeammate;

  /** angle to seen goal, updated by vision and odometry*/
  double angleToSeenGoal[2];

  /** distance to seen goal, updated by vision and odometry*/
  double distanceToSeenGoal[2];

  /** something is definitely wrong with the seen goal (occurs only when not seeing the goal)*/
  bool trustSeenGoal[2]; /* kicks in when the other goal is seen and the angle to the goal is also in the image */


  /** time since last seen goal*/
  unsigned long timeOfSeenGoal[2];


  /**
  * last value of the body psd sensor (if present)
  */
  double bodyPSD;
};

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


#endif //__ObstaclesModel_h_

---