Modules/ObstaclesLocator/GT2005ObstaclesLocator.cpp

Go to the documentation of this file.

/**
* @file GT2005ObstaclesLocator.cpp
*
* This file contains a class for obstacle localization.
* @author Jan Hoffmann
* @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
*/

#include "GT2005ObstaclesLocator.h"
#include "Tools/Debugging/DebugDrawings.h"
#include "Tools/Math/Geometry.h"
#include "Tools/FieldDimensions.h"
#include "Tools/Math/Matrix2x2.h"
#include "Tools/Debugging/GenericDebugData.h"
#include "Platform/SystemCall.h"

GT2005ObstaclesLocator::GT2005ObstaclesLocator(const ObstaclesLocatorInterfaces& interfaces)
: ObstaclesLocator(interfaces), headTiltClipPSD(-.3)
{
  usePSD = true;
  useLinesPercept = false;
  useObstaclesPercept = true;
  useAging = true;
  
  sectorWidth = pi2/(double)ObstaclesModel::numOfSectors;

  for(int i = 0; i < ObstaclesModel::numOfSectors; i++)
  {
    obstacles[i].x = 0;
    obstacles[i].y = 0;
    obstacleTypes[i] = ObstaclesPercept::unknown;
  }

  for( int i = 0; i < 2; ++i)
  {
    angleToSeenGoal[i] = 0;
    distanceToSeenGoal[i] = 0;
    timeOfSeenGoal[i] = ULONG_MAX;
  }

}


void GT2005ObstaclesLocator::execute()
{
  odometry = odometryData - lastOdometry;
  lastOdometry = odometryData;

  MODIFY("obstacles locator:odometry", odometry);

  moveObstaclesByOdometry();

//  if(usePSD) addPSDPercept();
  if(useObstaclesPercept) addObstaclesPercept();
  if(useLinesPercept) addLinesPercept();

  setObstaclesModel(true);
  obstaclesModel.setFrameNumber(obstaclesPercept.frameNumber);


  determineFreePartsOfGoals();
  determineNextFreeTeammate();
  determineSeenGoals();
  DEBUG_DRAWING_FINISHED(sketch);
}

void GT2005ObstaclesLocator::moveObstaclesByOdometry()
{
  // move representatives by odometry
/*  if (odometry.translation.x == 0 &&
      odometry.translation.y == 0 &&
      odometry.rotation == 0) return;
*/

  Vector2<double> c1(cos(odometry.rotation),-sin(odometry.rotation));
  Vector2<double> c2(sin(odometry.rotation),cos(odometry.rotation));
  Matrix2x2<double> R(c1, c2);

  Vector2<double> movedObstacles[ObstaclesModel::numOfSectors];
  unsigned long movedTimestamps[ObstaclesModel::numOfSectors];
  ObstaclesPercept::ObstacleType movedObstacleTypes[ObstaclesModel::numOfSectors];
  int numOfObstacles = 0;

  int i;  

  for(i = 0; i < ObstaclesModel::numOfSectors; i++)
  {
    if (
      (obstacles[i].x != 0 || obstacles[i].y != 0) &&
      (!useAging ||
       SystemCall::getTimeSince(timestamps[i]) < timeAfterWhichObstacleIsForgotten)
       )
    {
      movedObstacles[numOfObstacles] = R*obstacles[i];
      movedObstacles[numOfObstacles] -= odometry.translation;
      movedObstacleTypes[numOfObstacles] = obstacleTypes[i];
      movedTimestamps[numOfObstacles] = timestamps[i];
      numOfObstacles++;
    }
    obstacles[i].x = obstacles[i].y = 0;
    obstacleTypes[i] = ObstaclesPercept::unknown;
  }

  for(i = 0; i < numOfObstacles; i++)
  {
    int targetSector = ObstaclesModel::getSectorFromAngle(movedObstacles[i].angle());
    double sqrDistance = movedObstacles[i]*movedObstacles[i];

    if
      (
        sqrDistance >= 10.0 &&
//        sqrDistance < ObstaclesModel::maxDistance*ObstaclesModel::maxDistance &&
        (
         (obstacles[targetSector].x == 0 && obstacles[targetSector].y == 0) || 
         sqrDistance <= obstacles[targetSector]*obstacles[targetSector]
        )
      )
    {
      obstacles[targetSector] = movedObstacles[i];
      obstacleTypes[targetSector] = movedObstacleTypes[i];
      timestamps[targetSector] = movedTimestamps[i];
    }
  }
}

void GT2005ObstaclesLocator::setObstaclesModel(bool addWorldModel)
{
  int i;
  double distance;

  ObstaclesModel buffer;

  // set sector distances
  for(i = 0; i < ObstaclesModel::numOfSectors; i++)
  {
    distance = obstacles[i].abs();

    // if the player is the goalie, don't avoid own penalty area!
    if (getPlayer().getPlayerNumber() != Player::one)
    //if (addWorldModel)
    {
      if (FieldDimensions::distanceToOwnPenaltyArea(robotPose) > 0)
      {
        double distToPenaltyArea = field.getDistanceToOwnPenaltyArea(robotPose + Pose2D(ObstaclesModel::getAngleOfSector(i)));
        if (distToPenaltyArea > 50 && distToPenaltyArea < ObstaclesModel::maxDistance)
          if ((distToPenaltyArea < distance) || (distance == 0))
            distance = distToPenaltyArea;
      }
    }

    if(distance > 0)
    {
      buffer.distance[i] = (int)distance;
      buffer.obstacleType[i] = obstacleTypes[i];
    }
    else
    {
      buffer.distance[i] = ObstaclesModel::maxDistance;
      buffer.obstacleType[i] = ObstaclesPercept::unknown;
    }
  }



  // remove near peaks to make it more robust against 
  // noise!
  for(i = 0; i < ObstaclesModel::numOfSectors; i++)
  {
    // if the two neighbouring sectors have a distance
    // that is 200 mm greater than the current one, 
    // perform smoothing:
    if ((buffer.distance[i] - buffer.distance[(i+1)%ObstaclesModel::numOfSectors] < -200) &&
        (buffer.distance[i] - buffer.distance[(i+ObstaclesModel::numOfSectors-1)%ObstaclesModel::numOfSectors]) < -200)
    {
      obstaclesModel.distance[i] = (buffer.distance[(i+1)%ObstaclesModel::numOfSectors] +
        buffer.distance[(i-1)%ObstaclesModel::numOfSectors])/2;
    }
    else
    {
      obstaclesModel.distance[i] = buffer.distance[i];
    }
    //obstaclesModel.distance[i] = buffer.distance[i];
    obstaclesModel.obstacleType[i] = buffer.obstacleType[i];
  }

  // calculate corridor

  /* the following is pretty useless !! - thinks JH 

  
 
  */
  double closest = ObstaclesModel::maxDistance; 
  for(i = ObstaclesModel::getSectorFromAngle(-pi_2); i < ObstaclesModel::getSectorFromAngle(pi_2); i++)
  {
    if ((obstacles[i].x < closest) && (obstacles[i].x > 10)
      && (obstacles[i].y < 90) && (obstacles[i].y > -90))
      closest = obstacles[i].x;
  }
  obstaclesModel.corridorInFront = (int )closest;

  obstaclesModel.bodyPSD = psdPercept[psdPercept.numOfPercepts - 1].body;
}

void GT2005ObstaclesLocator::addPSDPercept()
{
  for (int i = 0; i < psdPercept.numOfPercepts; i++)
  {
    if (psdPercept[i].neckTilt > headTiltClipPSD && psdPercept[i].isValid)
    {
      // latest measurement
      Vector2<double> psdPoint(psdPercept[i].x, psdPercept[i].y);
      int sector = ObstaclesModel::getSectorFromAngle(psdPoint.angle());
      
       // point is on the ground  
      if(psdPercept[i].z < 90)
      {    
        double sqrDistance = obstacles[sector] * obstacles[sector];
        // only update if the point seen is farther away
        if (sqrDistance > 0 &&
            sqrDistance < psdPoint * psdPoint)
        {
          obstacles[sector].x = psdPoint.x;
          obstacles[sector].y = psdPoint.y;
          timestamps[sector] = SystemCall::getCurrentSystemTime();
        }
      }
      // point is an obstacle
      else if (!psdPercept[i].tooFarAway) 
      {
        double angleToPoint = normalize(psdPoint.angle());
        double angleToBall = Geometry::angleTo(robotPose.getPose(), ballModel.seen.positionField);

        if(toDegrees(normalize(angleToPoint - angleToBall)) < 10 &&
          toDegrees(normalize(angleToPoint - angleToBall)) > -10 && 
          SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen) < 1000)
        {
          DOT(sketch, 20, 10, Drawings::green, Drawings::green);
        }
        else
        {
          obstacles[sector].x = psdPoint.x;
          obstacles[sector].y = psdPoint.y;
          timestamps[sector] = SystemCall::getCurrentSystemTime();
        }
      }
      // if nothing in this direction, perform reset
      else 
      {
        obstacles[sector].x = 0;
        obstacles[sector].y = 0;
      }
    }
  }
}

void GT2005ObstaclesLocator::addLinesPercept()
{
  int i;
  for(i = 0; i < linesPercept.points[LinesPercept::redRobot].numberOfPoints; i++)
  {
    Vector2<double> obstaclePoint(linesPercept.points[LinesPercept::redRobot].pointsOnField[i].x, linesPercept.points[LinesPercept::redRobot].pointsOnField[i].y);
    addObstaclePoint(obstaclePoint, limit);
  }
  for(i = 0; i < linesPercept.points[LinesPercept::blueRobot].numberOfPoints; i++)
  {
    Vector2<double> obstaclePoint(linesPercept.points[LinesPercept::blueRobot].pointsOnField[i].x, linesPercept.points[LinesPercept::blueRobot].pointsOnField[i].y);
    addObstaclePoint(obstaclePoint, limit);
  }
  //for(i = 0; i < linesPercept.points[LinesPercept::border].numberOfPoints; i++)
  //{
  //  Vector2<double> obstaclePoint(linesPercept.points[LinesPercept::border].pointsOnField[i].x, linesPercept.points[LinesPercept::border].pointsOnField[i].y);
  //  addObstaclePoint(obstaclePoint, extend);
  //}
}

void GT2005ObstaclesLocator::addObstaclesPercept()
{
  int i;
  obstaclesModel.currentlySeenStartSector = obstaclesModel.numOfSectors;
  obstaclesModel.currentlySeenEndSector = 0;
  for(i = 0; i < obstaclesPercept.numberOfSegments ; i++)
  {
    const ObstaclesPercept::Segment& s = obstaclesPercept.segments[i];
//    addObstaclePoint(obstaclePoint, limit);
  
//    if(
//      Geometry::distance(
//      obstaclesPercept.farPoints[i], 
//      obstaclesPercept.farPoints[i-1]) < 300 
//      &&
//      Geometry::distance(
//      obstaclesPercept.farPoints[i], 
//      obstaclesPercept.farPoints[i+1]) < 300 
//      )
    addObstaclePoints(
      Vector2<int>(int(s.nearPointOnField.x), int(s.nearPointOnField.y)), 
      Vector2<int>(int(s.farPointOnField.x), int(s.farPointOnField.y)), 
      s.farPointIsOnImageBorder,
      s.obstacleType);
  }
}

void GT2005ObstaclesLocator::addObstaclePoint
(
 const Vector2<double>& obstaclePoint,
 UpdateMode mode
 )
{
  double angleToPoint = normalize(obstaclePoint.angle());

  //Vision based obstacles percept does not contain balls.
/*
  double angleToBall = Geometry::angleTo(robotPose.getPose(), ballModel.seen);
  if(toDegrees(normalize(angleToPoint - angleToBall)) < 10 &&
    toDegrees(normalize(angleToPoint - angleToBall)) > -10 && 
    SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen) < 1000)
  {
    DOT(sketch, 10, 10, Drawings::red, Drawings::red);
    return;
  }
*/ 

  int sector = ObstaclesModel::getSectorFromAngle(angleToPoint);

  if ( sector >= ObstaclesModel::numOfSectors || ObstaclesModel::numOfSectors < 0 ) 
  {
    sector = 0;
    return;

    OUTPUT ( idText, text, "sector >= ObstaclesModel::numOfSectors" );
  }
  double sqrDistanceInModel = obstacles[sector] * obstacles[sector];
  double sqrDistanceToPoint = obstaclePoint * obstaclePoint;

  if(sqrt(sqrDistanceToPoint) < 150)
  {
    DOT(sketch, 30, 10, Drawings::blue, Drawings::blue);
    return;
  }

  switch(mode)
  {
  case overwrite:
      obstacles[sector].x = obstaclePoint.x;
      obstacles[sector].y = obstaclePoint.y;
      timestamps[sector] = SystemCall::getCurrentSystemTime();
    break;
  case extend:
    // only update if the new point seen is farther away than the old one
    if(sqrDistanceInModel < sqrDistanceToPoint)
    {
      obstacles[sector].x = obstaclePoint.x;
      obstacles[sector].y = obstaclePoint.y;
      timestamps[sector] = SystemCall::getCurrentSystemTime();
    }
    break;
 case limit:
    // only update if the new point is closer than the old one
    if(
      (obstacles[sector].x == 0 && obstacles[sector].y == 0) ||
       sqrDistanceToPoint < sqrDistanceInModel)
    {
      obstacles[sector].x = obstaclePoint.x;
      obstacles[sector].y = obstaclePoint.y;
      timestamps[sector] = SystemCall::getCurrentSystemTime();
    }
    break;
  }
}

void GT2005ObstaclesLocator::addObstaclePoints
(
 const Vector2<int>& nearPoint,
 const Vector2<int>& farPoint,
 bool farPointIsOnImageBorder,
 ObstaclesPercept::ObstacleType obstacleType
 )
{
  double angleToPoints = normalize(farPoint.angle());
  int sector = ObstaclesModel::getSectorFromAngle(angleToPoints);

  obstaclesModel.currentlySeenStartSector = min(obstaclesModel.currentlySeenStartSector,sector);
  obstaclesModel.currentlySeenEndSector = max(obstaclesModel.currentlySeenEndSector,sector);
  
  if ( sector >= ObstaclesModel::numOfSectors || ObstaclesModel::numOfSectors < 0 ) 
  {
    sector = 0;
    return;
    OUTPUT ( idText, text, "sector >= ObstaclesModel::numOfSectors" );
  }
  double sqrDistanceInModel = obstacles[sector] * obstacles[sector];
  double sqrDistanceToNearPoint = nearPoint * nearPoint;
  double sqrDistanceToFarPoint = farPoint * farPoint;

  if(sqrDistanceToNearPoint > sqrDistanceInModel && sqrDistanceInModel != 0) return;
  else if(!farPointIsOnImageBorder)
  {
    obstacles[sector].x = farPoint.x;
    obstacles[sector].y = farPoint.y;
    if(obstacleType != ObstaclesPercept::unknown || (fabs(sqrDistanceToFarPoint - sqrDistanceInModel) > 500)) 
      obstacleTypes[sector] = obstacleType;
    timestamps[sector] = SystemCall::getCurrentSystemTime();
  }
  else if(sqrDistanceInModel < sqrDistanceToFarPoint && sqrDistanceInModel != 0)
  {
    obstacles[sector].x = farPoint.x;
    obstacles[sector].y = farPoint.y;
    timestamps[sector] = SystemCall::getCurrentSystemTime();
  }
}


void GT2005ObstaclesLocator::determineFreePartsOfGoals()
{
  for(int i = 0; i < 2; i++)
  {
    const ObstaclesPercept::FreePartOfGoal& f = obstaclesPercept.freePartOfGoal[i];
    if (f.wasDetermined)
    {
      double seenLeft = normalize(f.directionAsAngle + f.widthAsAngle / 2.0);
      double seenRight = normalize(f.directionAsAngle - f.widthAsAngle / 2.0); 
      double oldLeft = normalize(angleToFreePartOfGoal[i] + widthOfFreePartOfGoal[i] / 2.0);
      double oldRight = normalize(angleToFreePartOfGoal[i] - widthOfFreePartOfGoal[i] / 2.0);
      double newLeft = oldLeft;
      double newRight = oldRight;
      bool updated = false;

      if
      (
        f.leftEndSeen &&
        (f.widthAsAngle > widthOfFreePartOfGoal[i] || 
         (normalize(oldRight - seenLeft) < 0 && normalize(seenLeft - oldLeft) < 0)
        )
      )
      {
        newLeft = seenLeft;
        if (newLeft < oldRight) newRight = seenRight;
        updated = true;
      }
      if
      (
        f.rightEndSeen &&
        (f.widthAsAngle > widthOfFreePartOfGoal[i]|| 
         (normalize(oldRight - seenLeft) < 0 && normalize(seenLeft - oldLeft) < 0)
        )
      )
      {
        newRight = seenRight;
        if (newRight > oldLeft) newLeft = seenLeft;
        updated = true;
      }
      if (updated)
      {
        obstaclesModel.lastTimeFreePartOfGoalWasDetermined[i] = SystemCall::getCurrentSystemTime();
        widthOfFreePartOfGoal[i] = normalize(newLeft - newRight);
        angleToFreePartOfGoal[i] = normalize(newRight + widthOfFreePartOfGoal[i] / 2.0);
        distanceToFreePartOfGoal[i] = f.distanceOnField;

        obstaclesModel.angleToFreePartOfGoalWasDetermined[i] = true;
      }
    }
    else if(SystemCall::getTimeSince(obstaclesModel.lastTimeFreePartOfGoalWasDetermined[i]) < 500)
    {
      angleToFreePartOfGoal[i] = normalize(angleToFreePartOfGoal[i] - odometry.getAngle());
      obstaclesModel.angleToFreePartOfGoalWasDetermined[i] = true;
    }
    else
    {
      angleToFreePartOfGoal[i] = 0.0;
      widthOfFreePartOfGoal[i] = 0.0;
      distanceToFreePartOfGoal[i] = 1000.0;

      obstaclesModel.angleToFreePartOfGoalWasDetermined[i] = false;
    }
    obstaclesModel.angleToFreePartOfGoal[i] = angleToFreePartOfGoal[i];
    obstaclesModel.widthOfFreePartOfGoal[i] = widthOfFreePartOfGoal[i];
    obstaclesModel.distanceToFreePartOfGoal[i] = distanceToFreePartOfGoal[i];
  }
}

void GT2005ObstaclesLocator::determineNextFreeTeammate()
{
  int distanceToNextTeammate = 10000;
  double angleToNextTeammate = 0;

  bool foundTeammate = false;

  for(int i = 0; i < playerPoseCollection.numberOfOwnPlayers; i++)
  {
    PlayerPose currentPose = playerPoseCollection.getOwnPlayerPose(i);
    int currentDistance = (int)((currentPose.getPose().translation - robotPose.translation).abs());
    double currentAngle = normalize(Geometry::angleTo(robotPose, currentPose.getPose().translation));

    int targetSector = ObstaclesModel::getSectorFromAngle(currentAngle);
    int distanceOfTargetSector = (int)obstacles[targetSector].abs();

    if(currentDistance < distanceToNextTeammate && 
      (
      distanceOfTargetSector == 0 ||
      distanceOfTargetSector + 200 > distanceToNextTeammate
      )
      )
    {
      foundTeammate = true;
      angleToNextTeammate = currentAngle;
      distanceToNextTeammate = currentDistance;
    }
  }
  if(foundTeammate)
  {
    obstaclesModel.angleToNextFreeTeammateWasDetermined = true;
    obstaclesModel.angleToNextFreeTeammate = angleToNextTeammate;
    obstaclesModel.distanceToNextFreeTeammate = distanceToNextTeammate;
  }
  else
  {
    obstaclesModel.angleToNextFreeTeammateWasDetermined = false;
  }
}


void GT2005ObstaclesLocator::determineSeenGoals()
{
  NDECLARE_DEBUGDRAWING( "obstacles locator:determined seen goal angle and distance", "drawingOnField", "draws the seen goal angle and distance as determined by obstacleslocator");
  NDECLARE_DEBUGDRAWING( "obstacles locator:seen goal angle", "drawingOnField", "draws the seen goal angle and distance as determined by obstacleslocator");
  // update for seen goals
  bool notSeenGoals[2] = { true, true};
  for( int i = 0; i < landmarksPercept.numberOfGoals; ++i)
  {
    const Goal& goal = landmarksPercept.goals[i];
    colorClass ownGoalColor = (getPlayer().getTeamColor()==Player::blue) ? skyblue : yellow;
    ObstaclesPercept::WhoseGoal index = (landmarksPercept.goals[i].color==ownGoalColor) ? ObstaclesPercept::ownGoal : ObstaclesPercept::opponentGoal; 

    distanceToSeenGoal[index] = obstaclesModel.distanceToSeenGoal[index] = goal.distance;
    double expectedWidthOfGoalAngle = fabs(atan2( 400, distanceToSeenGoal[index])); // half of this angle is sufficient
    double widthOfDetectedGoalAngle = fabs( goal.x.max - goal.x.min)/2; // half of the width
    
    if( goal.isOnBorder( goal.x.min) && !goal.isOnBorder(goal.x.max))
    {
      obstaclesModel.angleToSeenGoal[index] = goal.angle + widthOfDetectedGoalAngle - expectedWidthOfGoalAngle;
      angleToSeenGoal[index] = obstaclesModel.angleToSeenGoal[index];
    }
    else if( !goal.isOnBorder( goal.x.min) && goal.isOnBorder(goal.x.max))
    {
      obstaclesModel.angleToSeenGoal[index] = goal.angle - widthOfDetectedGoalAngle + expectedWidthOfGoalAngle;
      angleToSeenGoal[index] = obstaclesModel.angleToSeenGoal[index];
    }
    else
    {
      obstaclesModel.angleToSeenGoal[index] = goal.angle;
      angleToSeenGoal[index] = obstaclesModel.angleToSeenGoal[index];
    }

    NCOMPLEX_DRAWING(
      "obstacles locator:determined seen goal angle and distance",
      {
        Pose2D leftPost =robotPose + Pose2D( angleToSeenGoal[index]) + Pose2D( distanceToSeenGoal[index], tan( expectedWidthOfGoalAngle) * distanceToSeenGoal[index]);
        Pose2D rightPost =robotPose + Pose2D( angleToSeenGoal[index]) + Pose2D( distanceToSeenGoal[index], - tan( expectedWidthOfGoalAngle) * distanceToSeenGoal[index]);
        NLINE(
          "obstacles locator:determined seen goal angle and distance",
          leftPost.translation.x,
          leftPost.translation.y,
          rightPost.translation.x,
          rightPost.translation.y,
          10,
          Drawings::ps_solid, 
          Drawings::orange);

        leftPost =robotPose + Pose2D( goal.angle) + Pose2D( distanceToSeenGoal[index], tan( widthOfDetectedGoalAngle) * distanceToSeenGoal[index]);
        rightPost =robotPose + Pose2D( goal.angle) + Pose2D( distanceToSeenGoal[index], - tan( widthOfDetectedGoalAngle) * distanceToSeenGoal[index]);
        NLINE(
          "obstacles locator:determined seen goal angle and distance",
          leftPost.translation.x,
          leftPost.translation.y,
          rightPost.translation.x,
          rightPost.translation.y,
          10,
          Drawings::ps_solid, 
          Drawings::yellow);
      }
    );

  
    timeOfSeenGoal[index] = obstaclesModel.timeOfSeenGoal[index] = SystemCall::getCurrentSystemTime();
    notSeenGoals[index]= false;
    obstaclesModel.trustSeenGoal[index] = true;
  }
  
  // update for not seen goals

  double angleToLeftOpponentGoalPost  = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGroundline,yPosLeftGoal));
  double angleToRightOpponentGoalPost = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGroundline,yPosRightGoal));

  if(angleToLeftOpponentGoalPost < angleToRightOpponentGoalPost)
  {
    angleToLeftOpponentGoalPost += pi2;
  }
  double angleToOpponentGoal = angleToLeftOpponentGoalPost + angleToRightOpponentGoalPost / 2.0;

  double angleToLeftOwnGoalPost  = Geometry::angleTo(robotPose, Vector2<double>(xPosOwnGroundline,yPosLeftGoal));
  double angleToRightOwnGoalPost = Geometry::angleTo(robotPose, Vector2<double>(xPosOwnGroundline,yPosRightGoal));

  if(angleToLeftOwnGoalPost < angleToRightOwnGoalPost)
  {
    angleToLeftOwnGoalPost += pi2;
  }
  double angleToOwnGoal = angleToLeftOwnGoalPost + angleToRightOwnGoalPost / 2.0;

  CameraInfo cameraInfo(getRobotConfiguration().getRobotDesign());
  for( int i = 0; i < 2; ++i)
  {
    if( notSeenGoals[i])
    {
      Pose2D relOffset = Pose2D( 
        cos(angleToSeenGoal[i]) * distanceToSeenGoal[i],
        sin(angleToSeenGoal[i]) * distanceToSeenGoal[i]);

      Pose2D absPoint = robotPose + relOffset;

      Drawings::Color color = (i == ObstaclesPercept::ownGoal ? ((getPlayer().getTeamColor()==Player::blue) ? Drawings::skyblue : Drawings::yellow) : (((getPlayer().getTeamColor()==Player::blue) ? Drawings::yellow : Drawings::skyblue)));

      NCIRCLE(
        "obstacles locator:determined seen goal angle and distance",
        absPoint.translation.x,
        absPoint.translation.y,
        30,
        30,
        Drawings::ps_solid, 
        Drawings::gray
        );
      relOffset = relOffset - odometry;
      
      absPoint = robotPose + relOffset;

      NCIRCLE(
        "obstacles locator:determined seen goal angle and distance",
        absPoint.translation.x,
        absPoint.translation.y,
        20,
        20,
        Drawings::ps_solid, 
        color
        );

      distanceToSeenGoal[i] = obstaclesModel.distanceToSeenGoal[i] = relOffset.translation.abs();
      double angle = 0;
      if( relOffset.translation.x != 0 || relOffset.translation.y != 0)
        angle  = atan2(  relOffset.translation.y, relOffset.translation.x);
      angleToSeenGoal[i] = obstaclesModel.angleToSeenGoal[i] = angle;

      // calc trust
      if( fabs(normalize(angleToSeenGoal[i] - (i==ObstaclesPercept::ownGoal?angleToOwnGoal:angleToOpponentGoal))) > pi_2)
      {
        obstaclesModel.trustSeenGoal[i] = false;
      }
      if( obstaclesModel.trustSeenGoal[i])
      {        
        for(int j = 0; j < landmarksPercept.numberOfGoals; ++j)
        {
          if(
            (angleToSeenGoal[i] < landmarksPercept.goals[j].angle + (cameraInfo.openingAngleWidth / 2.0)
            && angleToSeenGoal[i] > landmarksPercept.goals[j].angle - (cameraInfo.openingAngleWidth / 2.0))
            )
          {
            obstaclesModel.trustSeenGoal[i] = false;
            break;
          }
        }
      }
    }
  }

  if( !notSeenGoals[ObstaclesModel::opponentGoal])
  {
        
        Drawings::Color color = (getPlayer().getTeamColor()==Player::blue) ? Drawings::yellow : Drawings::skyblue;
        Pose2D leftPost = robotPose + Pose2D( distanceToSeenGoal[ ObstaclesModel::opponentGoal], -400);
        Pose2D rightPose = robotPose + Pose2D( distanceToSeenGoal[ ObstaclesModel::opponentGoal], 400);
        NLINE(
          "obstacles locator:determined seen goal angle and distance",
          leftPost.translation.x,
          leftPost.translation.y,
          leftPost.translation.x,
          leftPost.translation.y,
          10,
          Drawings::ps_solid, 
          color
        );
    }
    NCOMPLEX_DRAWING(
      "obstacles locator:seen goal angle",
    {
  
      for( int i = 0; i < 2;++i)
      {
        Drawings::Color color = (i == ObstaclesPercept::ownGoal ? ((getPlayer().getTeamColor()==Player::blue) ? Drawings::skyblue : Drawings::yellow) : (((getPlayer().getTeamColor()==Player::blue) ? Drawings::yellow : Drawings::skyblue)));

        Pose2D relOffset = Pose2D( 
          cos(angleToSeenGoal[i]) * distanceToSeenGoal[i],
          sin(angleToSeenGoal[i]) * distanceToSeenGoal[i]);
        Pose2D absPoint = robotPose + relOffset;
        NLINE(
          "obstacles locator:seen goal angle",
          robotPose.translation.x,
          robotPose.translation.y,
          absPoint.translation.x,
          absPoint.translation.y,
          10,
          Drawings::ps_solid, 
          color
        );
      }
    }
  );

  MODIFY( "obstacles locator:angleToSeenGoal[0]", angleToSeenGoal[0]);
  MODIFY( "obstacles locator:angleToSeenGoal[1]", angleToSeenGoal[1]);
  MODIFY( "obstacles locator:angleToSeenGoal[1]", timeOfSeenGoal[0]);
  MODIFY( "obstacles locator:angleToSeenGoal[1]", timeOfSeenGoal[1]);
  MODIFY( "obstacles locator:distanceToSeenGoal[0]", distanceToSeenGoal[0]);
  MODIFY( "obstacles locator:distanceToSeenGoal[1]", distanceToSeenGoal[1]);
}

bool GT2005ObstaclesLocator::handleMessage(InMessage& message)
{
  bool handled = false;

  switch(message.getMessageID())
  {
  case idGenericDebugData:
    GenericDebugData d;
    message.bin >> d;
    if(d.id == GenericDebugData::defaultObstaclesLocator)
    {
      OUTPUT(idText,text,"generic debug message handled by module DefaultObstaclesLocator");

      headTiltClipPSD = d.data[0];
      switch((int)d.data[1])
      {
      case 0: usePSD = true; useLinesPercept = false; useAging = false;
        break;
      case 1: usePSD = false; useLinesPercept = true; useAging = false;
        break;
      case 2: usePSD = false; useLinesPercept = true; useAging = true;
        break;
      case 3: usePSD = true; useLinesPercept = true; useAging = false;
        break;
      case 4: usePSD = true; useLinesPercept = true; useAging = true;
        break;
      };
    }
    handled = true;
    break;

  }
  return handled;
}
 

---