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;


  /** 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;

  /**
  * 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};
  bool angleToFreePartOfGoalWasDetermined[2];
  double angleToFreePartOfGoal[2];
  double widthOfFreePartOfGoal[2];
  double distanceToFreePartOfGoal[2];

  bool angleToNextFreeTeammateWasDetermined;
  double angleToNextFreeTeammate;
  double distanceToNextFreeTeammate;

  /**
  * 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_

/*
 * Change log :
 * 
 * $Log: ObstaclesModel.h,v $
 * Revision 1.3  2004/07/02 15:57:00  thomas
 * fix comment of function-signature
 *
 * Revision 1.2  2004/06/16 14:43:51  risler
 * added obstacles symbols
 * added symbol obstacles.opponent-close-to-ball
 *
 * Revision 1.1.1.1  2004/05/22 17:25:15  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.7  2004/03/16 14:00:21  juengel
 * Integrated Improvments from "Günne"
 * -ATH2004ERS7Behavior
 * -ATHHeadControl
 * -KickSelectionTable
 * -KickEditor
 *
 * Revision 1.6  2004/03/10 14:16:33  risler
 * body psd value added to PSDPercept and ObstaclesModel
 *
 * Revision 1.2  2004/03/15 17:11:40  hoffmann
 * - added ATH2004HeadControl
 * - added ATH2004LEDControl
 * - headmotiontester shows "tilt2"
 * - motion process updates odometry while no new robotPose is received, added to motion request
 * - some ui adjustments
 * - added member function to "field" to find out if robot is in own penalty area for use in the obstacles locator
 *
 * Revision 1.5  2004/03/01 11:45:49  juengel
 * Moved enum ObstacleType to class ObstaclesPercept.
 *
 * Revision 1.4  2004/02/28 13:56:13  juengel
 * Added ObstacleType.
 * Reduced number of sectors to 60.
 *
 * Revision 1.3  2004/01/19 14:55:23  dueffert
 * all frameNumbers (and not only some of them) are unsigned long now
 *
 * Revision 1.2  2003/11/14 19:02:25  goehring
 * frameNumber added
 *
 * Revision 1.1  2003/10/07 10:07:01  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.2  2003/07/03 18:13:35  dassler
 * Adding two Methods:
 * getAngleOfNextFreeSectorLeft
 * getAngleOfNextFreeSectorRight
 *
 * Copy of getAngleOfNextFreeSector
 *
 * Revision 1.1.1.1  2003/07/02 09:40:22  cvsadm
 * created new repository for the competitions in Padova from the 
 * tamara CVS (Tuesday 2:00 pm)
 *
 * removed unused solutions
 *
 * Revision 1.18  2003/06/29 08:40:17  roefer
 * Warnings removed
 *
 * Revision 1.17  2003/06/27 15:10:15  juengel
 * Added getAngleOfLargeGapInRange2 which uses minimum instead of sum.
 *
 * Revision 1.16  2003/06/27 13:17:57  jhoffman
 * work on obstacle avoider challenge,
 * added headcontrolmode,
 * added method needed to determine empty space,
 * updatet drawing method to reflect actual corridor size
 *
 * Revision 1.15  2003/06/26 12:35:25  juengel
 * getAngleOfLargeGapInRange has search direction.
 *
 * Revision 1.14  2003/06/21 12:55:41  juengel
 * Added getPercentageOfLowDistanceObstaclesInRange.
 *
 * Revision 1.13  2003/06/20 20:13:01  juengel
 * Renamed some methods.
 *
 * Revision 1.12  2003/06/20 13:59:17  juengel
 * Added minimalDistanceInRange.
 *
 * Revision 1.11  2003/06/19 12:10:43  juengel
 * Added getBestAngle
 *
 * Revision 1.10  2003/06/18 18:30:43  juengel
 * no message
 *
 * Revision 1.9  2003/06/17 20:01:22  juengel
 * Renamed getNextFreeSector to getAngleOfNextFreeSector.
 *
 * Revision 1.8  2003/06/05 08:14:28  juengel
 * Added getDistanceInCorridor and getNextFreeSector.
 *
 * Revision 1.7  2003/06/03 15:50:04  risler
 * added getDistanceInDirection
 * minor optimizations
 *
 * Revision 1.6  2003/05/26 08:29:57  juengel
 * Added angleToFreePartOfGoal and angle to nextFreeTeammate.
 *
 * Revision 1.5  2003/05/15 00:56:48  risler
 * added functions for sector to angle mapping to ObstaclesModel
 * sector to angle mapping unified
 * added rule AvoidObstacles
 *
 * Revision 1.4  2003/05/14 13:08:38  risler
 * removed DefaultObstaclesLocator
 * renamed MicroSectorsObstaclesLocator to DefaultObstaclesLocator
 * ObstaclesModel contains increased number of sectors
 * DefaultObstaclesLocator clean up
 *
 * Revision 1.3  2003/04/14 16:01:41  loetzsch
 * ATH after GermanOpen CVS merge:
 * added corridor to obstacle model
 *
 * Revision 1.2  2003/04/10 14:14:47  Jan Hoffmann
 * added corridor to obstacle model
 *
 * Revision 1.1.1.1  2003/04/09 14:22:16  loetzsch
 * started Aibo Team Humboldt's GermanOpen CVS
 *
 * Revision 1.2  2003/03/11 11:29:20  juengel
 * Added data and constructor
 *
 * Revision 1.1  2003/03/10 13:52:30  juengel
 * Added ObstaclesModel
 *
 */

---