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)
{
}


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

  // "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.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);
}

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),
    fromDegrees(45),
    angleWithMinDistance,
    ObstaclesPercept::opponent) < ((int)Geometry::distanceTo(robotPose.getPose(),ballModel.seen)) + 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::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();
}

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

/*
* Change Log
* 
* $Log: ObstaclesSymbols.cpp,v $
* Revision 1.3  2004/06/23 13:55:26  juengel
* Added enum elements for obstacles.collision-side
*
* Revision 1.2  2004/06/23 11:02:26  goehring
* Xabsl symbol collision-side added
*
* Revision 1.1  2004/06/16 15:14:29  risler
* added obstacles symbols
* added symbol obstacles.opponent-close-to-ball
*
*
*/

---