Modules/HeadControl/GT2005HeadControl/GT2005HeadControlSymbols.cpp

Go to the documentation of this file.

/** 
* @file Modules/HeadControl/GT2005HeadControl/GT2005HeadControlSymbols.cpp
*
* Implementation of class GT2005HeadControlSymbols.
*
* @author Martin Lötzsch
*/

#include "GT2005HeadControl.h"

GT2005HeadControlSymbols::GT2005HeadControlSymbols(const HeadControlInterfaces& interfaces,
                                                   GT2005HeadControl& headControl,
                                                   GT2005BasicBehaviorDirectedScanForLandmarks& msh05BasicBehaviorDirectedScanForLandmarks)
: HeadControlInterfaces(interfaces), headControl(headControl), msh05BasicBehaviorDirectedScanForLandmarks(msh05BasicBehaviorDirectedScanForLandmarks)
{
  lastSeenBeaconIndex = 0;
}


void GT2005HeadControlSymbols::registerSymbols(Xabsl2Engine& engine)
{
  engine.registerDecimalInputSymbol("position-in-walk-cycle",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getPositionInWalkCycle);

  engine.registerDecimalInputSymbol("ball.time-since-last-seen",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getBallTimeSinceLastSeen);

  engine.registerDecimalInputSymbol("ball.seen.distance",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getSeenDistance);

  engine.registerDecimalInputSymbol("ball.consecutive-seen-time",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getBallConsecutivelySeenTime);
  engine.registerDecimalInputSymbol("ball.time-since-last-seen-consecutively",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getTimeSinceLastSeenConsecutively);

  engine.registerDecimalInputSymbol("ball.communicated-ball-distance",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getCommunicatedBallDistance);

  engine.registerDecimalInputSymbol("ball.relative-speed-x",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getRelativeBallSpeedX);
  engine.registerDecimalInputSymbol("ball.relative-speed-y",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getRelativeBallSpeedY);
  engine.registerDecimalInputSymbol("ball.speed-abs",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getBallSpeedAbs);
  
  engine.registerDecimalInputSymbol("time-since-last-seen-beacon",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getTimeSinceLastSeenABeacon);
  engine.registerDecimalInputSymbol("time-between-last-beacons",this,
    (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getTimeBetweenSeen2LastBeacons); 
  
  
  engine.registerBooleanInputSymbol("body-PSD-detect-ball",this,
    (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getBodyPSDDetectBall);

  engine.registerBooleanInputSymbol("head-is-blocked",&headIsBlockedBySpecialActionOrWalk);
  engine.registerBooleanInputSymbol("last-head-path-is-finished",this,
    (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getLastHeadPathIsFinished);
  engine.registerBooleanInputSymbol("set-joints-is-close-to-destination",this,
    (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getSetJointsIsCloseToDestination);
  engine.registerBooleanInputSymbol("set-joints-max-pan-reached",this,
    (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getSetJointsMaxPanReached);
  engine.registerBooleanInputSymbol("next-landmark-is-within-reach",this,
    (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getNextLandmarkIsWithinReach);

  engine.registerBooleanInputSymbol("releasing-ball-left-is-possible",this,
    (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getReleasingBallLeftIsPossible);

  engine.registerBooleanInputSymbol("releasing-ball-right-is-possible",this,
    (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getReleasingBallRightIsPossible);
  engine.registerBooleanInputSymbol("set-joints-is-near-destination",this,
    (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getSetJointsIsCloseToDestination);
 


  engine.registerEnumeratedInputSymbol("head-control-mode",(const int*)&headControlMode.headControlMode);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","search-auto",HeadControlMode::searchAuto);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","search-for-ball",HeadControlMode::searchForBall);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","catch-ball",HeadControlMode::catchBall);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","catch-ball-high",HeadControlMode::catchBallHigh);

  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","release-caught-ball-when-turning-left", HeadControlMode::releaseCaughtBallWhenTurningLeft);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","release-caught-ball-when-turning-right", HeadControlMode::releaseCaughtBallWhenTurningRight);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","search-for-ball-left", HeadControlMode::searchForBallLeft);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","search-for-ball-right", HeadControlMode::searchForBallRight);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","directed-scan-for-obstacles", HeadControlMode::directedScanForObstacles);

  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","hold-ball", HeadControlMode::holdBall);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","none", HeadControlMode::none);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-left", HeadControlMode::lookLeft);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-right", HeadControlMode::lookRight);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","scan-for-obstacles", HeadControlMode::scanForObstacles);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","search-for-landmarks", HeadControlMode::searchForLandmarks);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","search-for-landmarks-head-low", HeadControlMode::searchForLandmarksHeadLow);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-at-blue-pink-landmark", HeadControlMode::lookAtBluePinkLandmark);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-straight-ahead", HeadControlMode::lookStraightAhead);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-toward-opponent-goal", HeadControlMode::lookTowardOpponentGoal);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-between-feet", HeadControlMode::lookBetweenFeet);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-to-stars", HeadControlMode::lookToStars);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","snap-at-finger", HeadControlMode::snapAtFinger);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-parallel-to-ground", HeadControlMode::lookParallelToGround);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","direct", HeadControlMode::direct);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","stay-as-forced", HeadControlMode::stayAsForced);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","watch-origin", HeadControlMode::watchOrigin);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","calibrate-head-speed", HeadControlMode::calibrateHeadSpeed);

  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","real-slow-scan", HeadControlMode::realSlowScan);
  engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-between-feet-for-carried-ball", HeadControlMode::lookBetweenFeetForCarriedBall);
}



void GT2005HeadControlSymbols::update()
{
  // calculate the last seen beacon index
  lastSeenBeaconIndex = headControl.getLastSeenBeaconIndex();
}


double GT2005HeadControlSymbols::getBallTimeSinceLastSeen()
{
  return SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen);
}

double GT2005HeadControlSymbols::getBallConsecutivelySeenTime()
{
  return ballModel.seen.timeUntilSeenConsecutively 
    - ballModel.seen.timeWhenFirstSeenConsecutively;
}

double GT2005HeadControlSymbols::getTimeSinceLastSeenConsecutively()
{
  return SystemCall::getTimeSince(ballModel.seen.timeUntilSeenConsecutively);
}

double GT2005HeadControlSymbols::getSeenDistance()
{
  return ballModel.seen.positionRobot.abs();
  //return Geometry::distanceTo(robotPose.getPose(),ballModel.seen.positionField);
}

double GT2005HeadControlSymbols::getCommunicatedBallDistance()
{
  return (ballModel.communicated.positionField-robotPose.translation).abs();
}

double GT2005HeadControlSymbols::getRelativeBallSpeedX()
{
  return ballModel.seen.speedRobot.x;
  /*return (ballModel.seen.speed.x * cos(robotPose.rotation) -
          ballModel.seen.speed.y * sin(robotPose.rotation));*/
}

double GT2005HeadControlSymbols::getRelativeBallSpeedY()
{
  return ballModel.seen.speedRobot.y;
  /*return (ballModel.seen.speed.x * sin(robotPose.rotation) -
          ballModel.seen.speed.y * cos(robotPose.rotation));*/
}

double GT2005HeadControlSymbols::getBallSpeedAbs()
{
  return ballModel.seen.speedRobot.abs();
}

double GT2005HeadControlSymbols::getTimeSinceLastSeenABeacon()
{
  return (double) SystemCall::getTimeSince(headControl.getTimeOfLastSeenBeacon(lastSeenBeaconIndex));
}

double GT2005HeadControlSymbols::getTimeBetweenSeen2LastBeacons()
{
  return (double) headControl.getTimeBetweenSeen2LastBeacons(lastSeenBeaconIndex);
}

bool GT2005HeadControlSymbols::getBodyPSDDetectBall()
{
  return robotState.getSomethingInFrontOfChest();
}

bool GT2005HeadControlSymbols::getLastHeadPathIsFinished()
{
  return headControl.headPathPlanner.isLastPathFinished();
}

bool GT2005HeadControlSymbols::getSetJointsIsCloseToDestination()
{
  return (headControl.setJointsIsCloseToDestination > 15);
}

bool GT2005HeadControlSymbols::getSetJointsMaxPanReached()
{
  return headControl.setJointsMaxPanReached;
}

bool GT2005HeadControlSymbols::getNextLandmarkIsWithinReach()
{
  return msh05BasicBehaviorDirectedScanForLandmarks.nextLandmarkIsWithinReach;
}

bool GT2005HeadControlSymbols::getReleasingBallLeftIsPossible()
{
  return (
    motionInfo.positionInWalkCycle > 0.27 && 
    motionInfo.positionInWalkCycle < 0.35);
}

bool GT2005HeadControlSymbols::getReleasingBallRightIsPossible()
{
  return (
    motionInfo.positionInWalkCycle > 0.27 && 
    motionInfo.positionInWalkCycle < 0.35);
}

double GT2005HeadControlSymbols::getPositionInWalkCycle()
{
  return motionInfo.positionInWalkCycle;
}

---