Modules/BehaviorControl/CommonXabsl2Symbols/ObstaclesSymbols.cpp

Go to the documentation of this file.

/**
* @file ObstaclesSymbols.cpp
*
* Implementation of class ObstaclesSymbols.
*
* @author Matthias Jüngel
*/

#include "ObstaclesSymbols.h"

ObstaclesSymbols::ObstaclesSymbols(const BehaviorControlInterfaces& interfaces)
: BehaviorControlInterfaces(interfaces)
{
  for(int i = 0; i < sizeOfArray; i++)
  {
    lastValuesLeft[i] = 0;
    lastValuesRight[i] = 0;
    lastValuesMiddle[i] = 0;
  }

}


void ObstaclesSymbols::registerSymbols(Xabsl2Engine& engine)
{
  // "obstacles.collision-front-left"
  engine.registerBooleanInputSymbol("obstacles.collision-front-left",this,
    (bool (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getCollisionFrontLeft);
  // "obstacles.collision-front-right
  engine.registerBooleanInputSymbol("obstacles.collision-front-right",this,
    (bool (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getCollisionFrontRight);
  // "obstacles.collision-hind-left"
  engine.registerBooleanInputSymbol("obstacles.collision-hind-left",this,
    (bool (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getCollisionHindLeft);
  // "obstacles.collision-hind-right"
  engine.registerBooleanInputSymbol("obstacles.collision-hind-right",this,
    (bool (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getCollisionHindRight);
  // "obstacles.collision-head"
  engine.registerBooleanInputSymbol("obstacles.collision-head",this,
    (bool (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getCollisionHead);
  // "obstacles.collision-aggregate"
  engine.registerBooleanInputSymbol("obstacles.collision-aggregate",this,
    (bool (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getCollisionAggregate);
  // "obstacles.collision-front-left"
  engine.registerDecimalInputSymbol("obstacles.odometry-disturbance",this,
    (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getOdometryDisturbance);

  // "obstacles.consecutive-collision-time-front-left"
  engine.registerDecimalInputSymbol("obstacles.consecutive-collision-time-front-left",this,
    (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getConsecutiveCollisionTimeFrontLeft);
  // "obstacles.collision-consecutive-front-time-left"
  engine.registerDecimalInputSymbol("obstacles.consecutive-collision-time-front-right",this,
    (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getConsecutiveCollisionTimeFrontRight);
  // "obstacles.collision-consecutive-front-time-left"
  engine.registerDecimalInputSymbol("obstacles.consecutive-collision-time-hind-left",this,
    (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getConsecutiveCollisionTimeHindLeft);
  // "obstacles.collision-consecutive-front-time-left"
  engine.registerDecimalInputSymbol("obstacles.consecutive-collision-time-hind-right",this,
    (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getConsecutiveCollisionTimeHindRight);
  // "obstacles.collision-consecutive-front-time-left"
  engine.registerDecimalInputSymbol("obstacles.consecutive-collision-time-head",this,
    (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getConsecutiveCollisionTimeHead);
  // "obstacles.collision-consecutive-front-time-left"
  engine.registerDecimalInputSymbol("obstacles.consecutive-collision-time-aggregate",this,
    (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getConsecutiveCollisionTimeAggregate);

  engine.registerDecimalInputSymbol("obstacles.min-distance-left",this,
    (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getMinDistanceLeft);
  engine.registerDecimalInputSymbol("obstacles.min-distance-right",this,
    (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getMinDistanceRight);
  engine.registerDecimalInputSymbol("obstacles.min-distance-middle",this,
    (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getMinDistanceMiddle);


  engine.registerEnumeratedInputSymbol("obstacles.collision-side",this,
    (int (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getCollisionSide);

  engine.registerEnumeratedInputSymbolEnumElement("obstacles.collision-side","left",1);
  engine.registerEnumeratedInputSymbolEnumElement("obstacles.collision-side","middle",0);
  engine.registerEnumeratedInputSymbolEnumElement("obstacles.collision-side","right",2);


  // "obstacles.robot-is-stuck"
  engine.registerBooleanInputSymbol("obstacles.robot-is-stuck", &robotIsStuck);

  // "obstacles.close"
  engine.registerBooleanInputSymbol("obstacles.close", &obstaclesAreClose);

  // "obstacles.opponents-close-to-ball"
  engine.registerBooleanInputSymbol("obstacles.opponents-close-to-ball", &opponentsCloseToBall);

  engine.registerBooleanInputSymbol("obstacles.obstacle-front",this,
    (bool (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getObstacleInFront);
  engine.registerDecimalInputSymbol("obstacles.obstacle-front-left",this,
    (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getNearestObstacleFrontLeft);
  engine.registerDecimalInputSymbol("obstacles.obstacle-front-right",this,
    (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getNearestObstacleFrontRight);

}

void ObstaclesSymbols::update()
{
  // robot is stuck
  if(obstaclesModel.getPercentageOfLowDistanceObstaclesInRange(0, pi_2, 300) > 0.2)
  {
    robotIsStuck = true;
    obstaclesAreClose = true;
  }
  else
  {
    robotIsStuck = false;
    obstaclesAreClose = (obstaclesModel.getPercentageOfLowDistanceObstaclesInRange(0, pi_2, 500) > 0.15);
  }

  // opponents close to ball
  double angleWithMinDistance;
  opponentsCloseToBall = (obstaclesModel.getMinimalDistanceInRange(
    Geometry::angleTo(robotPose.getPose(), ballModel.seen.positionField),
    fromDegrees(45),
    angleWithMinDistance,
    ObstaclesPercept::opponent) < ((int)Geometry::distanceTo(robotPose.getPose(),ballModel.seen.positionField)) + 1000);
}

bool ObstaclesSymbols::getCollisionFrontLeft()
{
  return robotState.getCollisionFrontLeft();
}

bool ObstaclesSymbols::getCollisionFrontRight()
{
  return robotState.getCollisionFrontRight();
}

bool ObstaclesSymbols::getCollisionHindLeft()
{
  return robotState.getCollisionHindLeft();
}

bool ObstaclesSymbols::getCollisionHindRight()
{
  return robotState.getCollisionHindRight();
}

bool ObstaclesSymbols::getCollisionHead()
{
  return robotState.getCollisionHead();
}

bool ObstaclesSymbols::getCollisionAggregate()
{
  return robotState.getCollisionAggregate();
}

double ObstaclesSymbols::getOdometryDisturbance()
{
  return robotState.getOdometryDisturbance();
}

double ObstaclesSymbols::getConsecutiveCollisionTimeFrontLeft()
{
  return robotState.getConsecutiveCollisionTimeFrontLeft();
}

double ObstaclesSymbols::getConsecutiveCollisionTimeFrontRight()
{
  return robotState.getConsecutiveCollisionTimeFrontRight();
}

double ObstaclesSymbols::getConsecutiveCollisionTimeHindLeft()
{
  return robotState.getConsecutiveCollisionTimeHindLeft();
}

double ObstaclesSymbols::getConsecutiveCollisionTimeHindRight()
{
  return robotState.getConsecutiveCollisionTimeHindRight();
}

double ObstaclesSymbols::getConsecutiveCollisionTimeHead()
{
  return robotState.getConsecutiveCollisionTimeHead();
}

double ObstaclesSymbols::getConsecutiveCollisionTimeAggregate()
{
  return robotState.getConsecutiveCollisionTimeAggregate();
}

 

double ObstaclesSymbols::getMinDistanceLeft()
{
  double x;
  
  for(int i = sizeOfArray-1; i > 0; i--)
  {
    lastValuesLeft[i] = lastValuesLeft[i-1];
  }

  lastValuesLeft[0] = obstaclesModel.getMinimalDistanceInRange(pi/4, pi/4, x);

  double durchschnitt = 0; 

  for(int i = 0; i < sizeOfArray; i++)
  {
    durchschnitt = durchschnitt + lastValuesLeft[i];
  }

  durchschnitt = durchschnitt / sizeOfArray;

  return durchschnitt;
  
}

double ObstaclesSymbols::getMinDistanceRight()
{
  double x;

  for(int i = sizeOfArray-1; i > 0; i--)
  {
    lastValuesRight[i] = lastValuesRight[i-1];
  }

  lastValuesRight[0] = obstaclesModel.getMinimalDistanceInRange(-pi/4, pi/4, x);

  double durchschnitt = 0; 

  for(int i = 0; i < sizeOfArray; i++)
  {
    durchschnitt = durchschnitt + lastValuesRight[i];
  }

  durchschnitt = durchschnitt / sizeOfArray;

  return durchschnitt;
  
}

double ObstaclesSymbols::getMinDistanceMiddle()
{
  return obstaclesModel.corridorInFront;
}

int ObstaclesSymbols::getCollisionSide()
{
  return robotState.getCollisionSide();
}

bool ObstaclesSymbols::getObstacleInFront()
{
  if (this->obstaclesModel.getDistanceInDirection(0,pi/4)<800)
  {
    if (this->obstaclesModel.getDistanceInDirection(0,pi/4)<400) // very near obstacle
      return (this->obstaclesModel.getPercentageOfLowDistanceObstaclesInRange(0,pi/4,400) > 0.3); //should not be too thin! (could be a line)
    else 
      return (this->obstaclesModel.getPercentageOfLowDistanceObstaclesInRange(0,pi/4,800) > 0.1);
  }
  else
    return false;
  //~ return (this->obstaclesModel.getDistanceInDirection(0,pi/4)<800); 
}

double ObstaclesSymbols::getNearestObstacleFrontRight()
{
  double angle=0;
  while (angle > -pi/2 && obstaclesModel.getDistanceInDirection(angle,pi/8)<800){
    angle-=pi/16;
  }
  return fabs(angle);
//  return (this->obstaclesModel.getDistanceInDirection(-pi/4,pi/4));
}
double ObstaclesSymbols::getNearestObstacleFrontLeft()
{
  double angle=0;
  while (angle < pi/2 && obstaclesModel.getDistanceInDirection(angle,pi/8)<800){
    angle+=pi/16;
  }
  return fabs(angle);
//  return (this->obstaclesModel.getDistanceInDirection(pi/4,pi/4));
}

---