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

# Modules/BehaviorControl/CommonXabsl2Symbols/KickSelectionSymbols.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file KickSelectionSymbols.cpp
    00003 *
    00004 * Implementation of class KickSelectionSymbols.
    00005 *
    00006 * @author Martin Lötzsch
    00007 */
    00008
    00009 #include "KickSelectionSymbols.h"
    00010
    00011
    00012 KickSelectionSymbols::KickSelectionSymbols(BehaviorControlInterfaces& interfaces,
    00013                                            const char* fileName)
    00014 : BehaviorControlInterfaces(interfaces)
    00015 {
    00016   kickSelectionTable.load(fileName);
    00017 }
    00018
    00019 void KickSelectionSymbols::registerSymbols(Xabsl2Engine& engine)
    00020 {
    00021   engine.registerDecimalInputFunction("retrieve-kick",this,
    00022     (double (Xabsl2FunctionProvider::*)())&KickSelectionSymbols::retrieveKick);
    00023   engine.registerDecimalInputFunctionParameter("retrieve-kick","retrieve-kick.angle",angle);
    00024   engine.registerDecimalInputFunctionParameter("retrieve-kick","retrieve-kick.angle-width",angleWidth);
    00025   engine.registerDecimalInputFunctionParameter("retrieve-kick","retrieve-kick.table-id",tableID);
    00026
    00027   engine.registerDecimalInputFunction("intentional-kick-possible",this,
    00028     (double (Xabsl2FunctionProvider::*)())&KickSelectionSymbols::intentionalKickPossible);
    00029   engine.registerDecimalInputFunctionParameter("intentional-kick-possible","intentional-kick-possible.angle",intentionalAngle);
    00030   engine.registerDecimalInputFunctionParameter("intentional-kick-possible","intentional-kick-possible.table-id",intentionalTable);
    00031
    00032   engine.registerDecimalInputSymbol("intentional-kick.xoffset",&intentionalKickOffset.x);
    00033   engine.registerDecimalInputSymbol("intentional-kick.yoffset",&intentionalKickOffset.y);
    00034   engine.registerDecimalInputSymbol("intentional-kick.kick",&intentionalKick);
    00035
    00036   engine.registerDecimalInputSymbol("intentional-kick.walktotargetx",this,
    00037     (double (Xabsl2FunctionProvider::*)())&KickSelectionSymbols::intentionalKickTargetX);
    00038   engine.registerDecimalInputSymbol("intentional-kick.walktotargety",this,
    00039     (double (Xabsl2FunctionProvider::*)())&KickSelectionSymbols::intentionalKickTargetY);
    00040
    00041 }
    00042
    00043 double KickSelectionSymbols::retrieveKick()
    00044 {
    00045   DEBUG_RESPONSE("KickSelectionSymbols:retrieveKick:useSeenBall",
    00046     if (SystemCall::getTimeSince(ballModel.seen.timeUntilSeenConsecutively) < 200
    00047       && ballModel.seen.timeUntilSeenConsecutively - ballModel.seen.timeWhenFirstSeenConsecutively > 150)
    00048     {
    00049       return kickSelectionTable.retrieveKick(Geometry::distanceTo(robotPose.getPose(),ballModel.seen.positionField)
    00050         * cos(Geometry::angleTo(robotPose.getPose(),ballModel.seen.positionField)),
    00051         Geometry::distanceTo(robotPose.getPose(),ballModel.seen.positionField)
    00052         * sin(Geometry::angleTo(robotPose.getPose(),ballModel.seen.positionField)),
    00053         angle, angleWidth, (KickSelectionTable::KickSelectionTableID)(int)tableID
    00054         );
    00055     }
    00056     return KickSelectionTable::nothing;
    00057   );
    00058
    00059   if (   ballModel.ballWasSeen
    00060       && ballModel.propagated.getPositionValidity() > 0.2)
    00061   {
    00062     return kickSelectionTable.retrieveKick
    00063       (ballModel.propagated.positionRobot.x,
    00064        ballModel.propagated.positionRobot.y,
    00065        angle,
    00066        angleWidth,
    00067        (KickSelectionTable::KickSelectionTableID)(int)tableID
    00068        );
    00069
    00070   }
    00071   else
    00072     return KickSelectionTable::nothing;
    00073 }
    00074
    00075 double KickSelectionSymbols::intentionalKickPossible(){
    00076   double maxDistance=400;
    00077   if (SystemCall::getTimeSince(ballModel.seen.timeUntilSeenConsecutively) < 200
    00078     && ballModel.seen.timeUntilSeenConsecutively - ballModel.seen.timeWhenFirstSeenConsecutively > 150)
    00079   {
    00080     intentionalKickOffset=(kickSelectionTable.searchKickInNeighbourhood(Geometry::distanceTo(robotPose.getPose(),ballModel.seen.positionField)
    00081       * cos(Geometry::angleTo(robotPose.getPose(),ballModel.propagated.positionField)),
    00082       Geometry::distanceTo(robotPose.getPose(),ballModel.propagated.positionField)
    00083       * sin(Geometry::angleTo(robotPose.getPose(),ballModel.propagated.positionField)),
    00084       intentionalAngle, (KickSelectionTable::KickSelectionTableID)(int)intentionalTable, robotPose, maxDistance, intentionalKick));
    00085
    00086     OUTPUT (idText,text,"IntentionalKick:"<<intentionalKickOffset.x<<" "<<intentionalKickOffset.y);
    00087     if (intentionalKickOffset.abs() <= maxDistance) return 1; else return 0;
    00088
    00089   }
    00090   else
    00091   {
    00092     return 0;
    00093   }
    00094 }
    00095
    00096 double KickSelectionSymbols::intentionalKickXOffset(){
    00097   return intentionalKickOffset.x;
    00098 }
    00099 double KickSelectionSymbols::intentionalKickYOffset(){
    00100   return intentionalKickOffset.y;
    00101 }
    00102
    00103 double KickSelectionSymbols::intentionalKickKick(){
    00104   return intentionalKick;
    00105 }
    00106
    00107 double KickSelectionSymbols::intentionalKickTargetX(){
    00108   return Geometry::relative2FieldCoord(robotPose,intentionalKickOffset.x*3,intentionalKickOffset.y*3).x;
    00109 }
    00110 double KickSelectionSymbols::intentionalKickTargetY(){
    00111   return Geometry::relative2FieldCoord(robotPose,intentionalKickOffset.x*3,intentionalKickOffset.y*3).y;
    00112 }
    00113
    00114
    00115 bool KickSelectionSymbols::handleMessage(InMessage& message)
    00116 {
    00117   if(message.getMessageID() == idKickSelectionTable)
    00118   {
    00119     message.text >> kickSelectionTable;
    00120     return true;
    00121   }
    00122   else return false;
    00123 }
    00124