Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/HeadControl/GT2005HeadControl/GT2005HeadControlSymbols.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file Modules/HeadControl/GT2005HeadControl/GT2005HeadControlSymbols.cpp
    00003 *
    00004 * Implementation of class GT2005HeadControlSymbols.
    00005 *
    00006 * @author Martin Lötzsch
    00007 */
    00008
    00009 #include "GT2005HeadControl.h"
    00010
    00011 GT2005HeadControlSymbols::GT2005HeadControlSymbols(const HeadControlInterfaces& interfaces,
    00012                                                    GT2005HeadControl& headControl,
    00013                                                    GT2005BasicBehaviorDirectedScanForLandmarks& msh05BasicBehaviorDirectedScanForLandmarks)
    00014 : HeadControlInterfaces(interfaces), headControl(headControl), msh05BasicBehaviorDirectedScanForLandmarks(msh05BasicBehaviorDirectedScanForLandmarks)
    00015 {
    00016   lastSeenBeaconIndex = 0;
    00017 }
    00018
    00019
    00020 void GT2005HeadControlSymbols::registerSymbols(Xabsl2Engine& engine)
    00021 {
    00022   engine.registerDecimalInputSymbol("position-in-walk-cycle",this,
    00023     (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getPositionInWalkCycle);
    00024
    00025   engine.registerDecimalInputSymbol("ball.time-since-last-seen",this,
    00026     (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getBallTimeSinceLastSeen);
    00027
    00028   engine.registerDecimalInputSymbol("ball.seen.distance",this,
    00029     (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getSeenDistance);
    00030
    00031   engine.registerDecimalInputSymbol("ball.consecutive-seen-time",this,
    00032     (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getBallConsecutivelySeenTime);
    00033   engine.registerDecimalInputSymbol("ball.time-since-last-seen-consecutively",this,
    00034     (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getTimeSinceLastSeenConsecutively);
    00035
    00036   engine.registerDecimalInputSymbol("ball.communicated-ball-distance",this,
    00037     (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getCommunicatedBallDistance);
    00038
    00039   engine.registerDecimalInputSymbol("ball.relative-speed-x",this,
    00040     (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getRelativeBallSpeedX);
    00041   engine.registerDecimalInputSymbol("ball.relative-speed-y",this,
    00042     (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getRelativeBallSpeedY);
    00043   engine.registerDecimalInputSymbol("ball.speed-abs",this,
    00044     (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getBallSpeedAbs);
    00045
    00046   engine.registerDecimalInputSymbol("time-since-last-seen-beacon",this,
    00047     (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getTimeSinceLastSeenABeacon);
    00048   engine.registerDecimalInputSymbol("time-between-last-beacons",this,
    00049     (double (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getTimeBetweenSeen2LastBeacons);
    00050
    00051
    00052   engine.registerBooleanInputSymbol("body-PSD-detect-ball",this,
    00053     (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getBodyPSDDetectBall);
    00054
    00055   engine.registerBooleanInputSymbol("head-is-blocked",&headIsBlockedBySpecialActionOrWalk);
    00056   engine.registerBooleanInputSymbol("last-head-path-is-finished",this,
    00057     (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getLastHeadPathIsFinished);
    00058   engine.registerBooleanInputSymbol("set-joints-is-close-to-destination",this,
    00059     (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getSetJointsIsCloseToDestination);
    00060   engine.registerBooleanInputSymbol("set-joints-max-pan-reached",this,
    00061     (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getSetJointsMaxPanReached);
    00062   engine.registerBooleanInputSymbol("next-landmark-is-within-reach",this,
    00063     (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getNextLandmarkIsWithinReach);
    00064
    00065   engine.registerBooleanInputSymbol("releasing-ball-left-is-possible",this,
    00066     (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getReleasingBallLeftIsPossible);
    00067
    00068   engine.registerBooleanInputSymbol("releasing-ball-right-is-possible",this,
    00069     (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getReleasingBallRightIsPossible);
    00070   engine.registerBooleanInputSymbol("set-joints-is-near-destination",this,
    00071     (bool (Xabsl2FunctionProvider::*)())&GT2005HeadControlSymbols::getSetJointsIsCloseToDestination);
    00072
    00073
    00074
    00075   engine.registerEnumeratedInputSymbol("head-control-mode",(const int*)&headControlMode.headControlMode);
    00076   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","search-auto",HeadControlMode::searchAuto);
    00077   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","search-for-ball",HeadControlMode::searchForBall);
    00078   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","catch-ball",HeadControlMode::catchBall);
    00079   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","catch-ball-high",HeadControlMode::catchBallHigh);
    00080
    00081   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","release-caught-ball-when-turning-left", HeadControlMode::releaseCaughtBallWhenTurningLeft);
    00082   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","release-caught-ball-when-turning-right", HeadControlMode::releaseCaughtBallWhenTurningRight);
    00083   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","search-for-ball-left", HeadControlMode::searchForBallLeft);
    00084   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","search-for-ball-right", HeadControlMode::searchForBallRight);
    00085   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","directed-scan-for-obstacles", HeadControlMode::directedScanForObstacles);
    00086
    00087   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","hold-ball", HeadControlMode::holdBall);
    00088   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","none", HeadControlMode::none);
    00089   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-left", HeadControlMode::lookLeft);
    00090   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-right", HeadControlMode::lookRight);
    00091   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","scan-for-obstacles", HeadControlMode::scanForObstacles);
    00092   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","search-for-landmarks", HeadControlMode::searchForLandmarks);
    00093   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","search-for-landmarks-head-low", HeadControlMode::searchForLandmarksHeadLow);
    00094   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-at-blue-pink-landmark", HeadControlMode::lookAtBluePinkLandmark);
    00095   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-straight-ahead", HeadControlMode::lookStraightAhead);
    00096   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-toward-opponent-goal", HeadControlMode::lookTowardOpponentGoal);
    00097   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-between-feet", HeadControlMode::lookBetweenFeet);
    00098   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-to-stars", HeadControlMode::lookToStars);
    00099   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","snap-at-finger", HeadControlMode::snapAtFinger);
    00100   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-parallel-to-ground", HeadControlMode::lookParallelToGround);
    00101   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","direct", HeadControlMode::direct);
    00102   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","stay-as-forced", HeadControlMode::stayAsForced);
    00103   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","watch-origin", HeadControlMode::watchOrigin);
    00104   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","calibrate-head-speed", HeadControlMode::calibrateHeadSpeed);
    00105
    00106   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","real-slow-scan", HeadControlMode::realSlowScan);
    00107   engine.registerEnumeratedInputSymbolEnumElement("head-control-mode","look-between-feet-for-carried-ball", HeadControlMode::lookBetweenFeetForCarriedBall);
    00108 }
    00109
    00110
    00111
    00112 void GT2005HeadControlSymbols::update()
    00113 {
    00114   // calculate the last seen beacon index
    00115   lastSeenBeaconIndex = headControl.getLastSeenBeaconIndex();
    00116 }
    00117
    00118
    00119 double GT2005HeadControlSymbols::getBallTimeSinceLastSeen()
    00120 {
    00121   return SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen);
    00122 }
    00123
    00124 double GT2005HeadControlSymbols::getBallConsecutivelySeenTime()
    00125 {
    00126   return ballModel.seen.timeUntilSeenConsecutively
    00127     - ballModel.seen.timeWhenFirstSeenConsecutively;
    00128 }
    00129
    00130 double GT2005HeadControlSymbols::getTimeSinceLastSeenConsecutively()
    00131 {
    00132   return SystemCall::getTimeSince(ballModel.seen.timeUntilSeenConsecutively);
    00133 }
    00134
    00135 double GT2005HeadControlSymbols::getSeenDistance()
    00136 {
    00137   return ballModel.seen.positionRobot.abs();
    00138   //return Geometry::distanceTo(robotPose.getPose(),ballModel.seen.positionField);
    00139 }
    00140
    00141 double GT2005HeadControlSymbols::getCommunicatedBallDistance()
    00142 {
    00143   return (ballModel.communicated.positionField-robotPose.translation).abs();
    00144 }
    00145
    00146 double GT2005HeadControlSymbols::getRelativeBallSpeedX()
    00147 {
    00148   return ballModel.seen.speedRobot.x;
    00149   /*return (ballModel.seen.speed.x * cos(robotPose.rotation) -
    00150           ballModel.seen.speed.y * sin(robotPose.rotation));*/
    00151 }
    00152
    00153 double GT2005HeadControlSymbols::getRelativeBallSpeedY()
    00154 {
    00155   return ballModel.seen.speedRobot.y;
    00156   /*return (ballModel.seen.speed.x * sin(robotPose.rotation) -
    00157           ballModel.seen.speed.y * cos(robotPose.rotation));*/
    00158 }
    00159
    00160 double GT2005HeadControlSymbols::getBallSpeedAbs()
    00161 {
    00162   return ballModel.seen.speedRobot.abs();
    00163 }
    00164
    00165 double GT2005HeadControlSymbols::getTimeSinceLastSeenABeacon()
    00166 {
    00167   return (double) SystemCall::getTimeSince(headControl.getTimeOfLastSeenBeacon(lastSeenBeaconIndex));
    00168 }
    00169
    00170 double GT2005HeadControlSymbols::getTimeBetweenSeen2LastBeacons()
    00171 {
    00172   return (double) headControl.getTimeBetweenSeen2LastBeacons(lastSeenBeaconIndex);
    00173 }
    00174
    00175 bool GT2005HeadControlSymbols::getBodyPSDDetectBall()
    00176 {
    00177   return robotState.getSomethingInFrontOfChest();
    00178 }
    00179
    00180 bool GT2005HeadControlSymbols::getLastHeadPathIsFinished()
    00181 {
    00182   return headControl.headPathPlanner.isLastPathFinished();
    00183 }
    00184
    00185 bool GT2005HeadControlSymbols::getSetJointsIsCloseToDestination()
    00186 {
    00187   return (headControl.setJointsIsCloseToDestination > 15);
    00188 }
    00189
    00190 bool GT2005HeadControlSymbols::getSetJointsMaxPanReached()
    00191 {
    00192   return headControl.setJointsMaxPanReached;
    00193 }
    00194
    00195 bool GT2005HeadControlSymbols::getNextLandmarkIsWithinReach()
    00196 {
    00197   return msh05BasicBehaviorDirectedScanForLandmarks.nextLandmarkIsWithinReach;
    00198 }
    00199
    00200 bool GT2005HeadControlSymbols::getReleasingBallLeftIsPossible()
    00201 {
    00202   return (
    00203     motionInfo.positionInWalkCycle > 0.27 &&
    00204     motionInfo.positionInWalkCycle < 0.35);
    00205 }
    00206
    00207 bool GT2005HeadControlSymbols::getReleasingBallRightIsPossible()
    00208 {
    00209   return (
    00210     motionInfo.positionInWalkCycle > 0.27 &&
    00211     motionInfo.positionInWalkCycle < 0.35);
    00212 }
    00213
    00214 double GT2005HeadControlSymbols::getPositionInWalkCycle()
    00215 {
    00216   return motionInfo.positionInWalkCycle;
    00217 }
    00218