Modules/HeadControl/GT2004HeadControl/GT2004HeadControlSymbols.cpp

Go to the documentation of this file.

/** 
* @file GT2004HeadControlSymbols.cpp
*
* Implementation of class GT2004HeadControlSymbols.
*
* @author Martin Lötzsch
*/

#include "GT2004HeadControl.h"

GT2004HeadControlSymbols::GT2004HeadControlSymbols(const HeadControlInterfaces& interfaces,
                                                   GT2004HeadControl& headControl,
                                                   GT2004BasicBehaviorDirectedScanForLandmarks& gt2004BasicBehaviorDirectedScanForLandmarks)
: HeadControlInterfaces(interfaces), headControl(headControl), gt2004BasicBehaviorDirectedScanForLandmarks(gt2004BasicBehaviorDirectedScanForLandmarks)
{
  lastSeenBeaconIndex = 0;
}


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

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

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

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

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

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

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

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

  engine.registerBooleanInputSymbol("releasing-ball-right-is-possible",this,
    (bool (Xabsl2FunctionProvider::*)())&GT2004HeadControlSymbols::getReleasingBallRightIsPossible);
  engine.registerBooleanInputSymbol("set-joints-is-near-destination",this,
    (bool (Xabsl2FunctionProvider::*)())&GT2004HeadControlSymbols::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","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);

}



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


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

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

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

double GT2004HeadControlSymbols::getSeenDistance()
{
  return Geometry::distanceTo(robotPose.getPose(),ballModel.seen);
}

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

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

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

double GT2004HeadControlSymbols::getBallSpeedAbs()
{
  return ballModel.seen.speed.abs();
}

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

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

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

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

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

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

bool GT2004HeadControlSymbols::getNextLandmarkIsWithinReach()
{
  return gt2004BasicBehaviorDirectedScanForLandmarks.nextLandmarkIsWithinReach;
}

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

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

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

/*
* Change Log
* 
* $Log: GT2004HeadControlSymbols.cpp,v $
* Revision 1.19  2004/07/01 18:21:16  dassler
* Added BasicBehavior and HeadControlMode:
* search-for-ball-left
* search-for-ball-right
* Test is needed
*
* Revision 1.18  2004/06/28 14:00:04  jhoffman
* - scan back to ball slower
* - directed scan briefly stops at landmarks
* - input symbol set-joints-is-close-to-destination implemented and added to behavior
* - merged marcs changes into track-ball-strict
*
* Revision 1.17  2004/06/28 09:46:57  dassler
* introduced some more headcontrol symbols
* time-since-last-seen-beacon
* time-between-last-beacons
*
* Revision 1.16  2004/06/27 15:37:45  dassler
* introduced ball speed to headcontrol
*
* Revision 1.15  2004/06/23 17:31:46  loetzsch
* improved
*
* Revision 1.14  2004/06/23 12:46:40  loetzsch
* added "ball.time-since-last-seen-consecutively"
*
* Revision 1.13  2004/06/18 18:28:39  dassler
* introduced basic-behavior:
* BeginBallSearchAtBallPositionSeen
* BeginBallSearchAtBallPositionCommunicated
* BeginBallSearchAtBallPositionPropagated
*
* track-ball modified and build in ball-just-lost
*
* Revision 1.12  2004/06/16 18:14:00  jhoffman
* - search-for-landmarks-bug removed (setjointsdirect now sets "headPathPlanner.last...")
*
* Revision 1.11  2004/06/15 16:29:56  jhoffman
* check in for practice match
*
* Revision 1.10  2004/06/14 20:12:10  jhoffman
* - numerous changes and additions to headcontrol
* - cameraInfo default constructor now creates ERS7 info
* - debug drawing "headcontrolfield" added
*
* Revision 1.9  2004/06/11 16:31:59  juengel
* Added symbol positionInWalkCycle.
*
* Revision 1.8  2004/05/26 18:36:03  loetzsch
* cleanup in the head control interfaces
*
* Revision 1.7  2004/05/26 12:25:24  juengel
* Added release-ball modes.
*
* Revision 1.6  2004/05/25 16:54:29  goehring
* catch-ball registered
*
* Revision 1.5  2004/05/25 13:12:06  tim
* changed body PSD stuff
*
* Revision 1.4  2004/05/25 10:29:49  tim
* added body PSD symbols
*
* Revision 1.3  2004/05/23 20:27:57  loetzsch
* some improvements with the head control
*
* Revision 1.2  2004/05/22 22:52:03  juengel
* Renamed ballP_osition to ballModel.
*
* Revision 1.1.1.1  2004/05/22 17:19:24  cvsadm
* created new repository GT2004_WM
*
* Revision 1.2  2004/05/20 13:32:28  roefer
* Warnings removed
*
* Revision 1.1  2004/05/18 13:40:00  loetzsch
* registered symbols and basic behaviors for GT2004HeadControl,
* renamed some states and basic behaviors
*
*/

---