Modules/BehaviorControl/CommonXabsl2Symbols/KickSelectionSymbols.cpp

Go to the documentation of this file.

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

#include "KickSelectionSymbols.h"


KickSelectionSymbols::KickSelectionSymbols(BehaviorControlInterfaces& interfaces, 
                                           const char* fileName)
: BehaviorControlInterfaces(interfaces)
{
  kickSelectionTable.load(fileName);
}

void KickSelectionSymbols::registerSymbols(Xabsl2Engine& engine)
{
  engine.registerDecimalInputFunction("retrieve-kick",this,
    (double (Xabsl2FunctionProvider::*)())&KickSelectionSymbols::retrieveKick);
  engine.registerDecimalInputFunctionParameter("retrieve-kick","retrieve-kick.angle",angle);
  engine.registerDecimalInputFunctionParameter("retrieve-kick","retrieve-kick.angle-width",angleWidth);
  engine.registerDecimalInputFunctionParameter("retrieve-kick","retrieve-kick.table-id",tableID);

  engine.registerDecimalInputFunction("intentional-kick-possible",this,
    (double (Xabsl2FunctionProvider::*)())&KickSelectionSymbols::intentionalKickPossible);
  engine.registerDecimalInputFunctionParameter("intentional-kick-possible","intentional-kick-possible.angle",intentionalAngle);
  engine.registerDecimalInputFunctionParameter("intentional-kick-possible","intentional-kick-possible.table-id",intentionalTable);

  engine.registerDecimalInputSymbol("intentional-kick.xoffset",&intentionalKickOffset.x);
  engine.registerDecimalInputSymbol("intentional-kick.yoffset",&intentionalKickOffset.y);
  engine.registerDecimalInputSymbol("intentional-kick.kick",&intentionalKick);

  engine.registerDecimalInputSymbol("intentional-kick.walktotargetx",this,
    (double (Xabsl2FunctionProvider::*)())&KickSelectionSymbols::intentionalKickTargetX);
  engine.registerDecimalInputSymbol("intentional-kick.walktotargety",this,
    (double (Xabsl2FunctionProvider::*)())&KickSelectionSymbols::intentionalKickTargetY);
  
}

double KickSelectionSymbols::retrieveKick()
{
  DEBUG_RESPONSE("KickSelectionSymbols:retrieveKick:useSeenBall",
    if (SystemCall::getTimeSince(ballModel.seen.timeUntilSeenConsecutively) < 200
      && ballModel.seen.timeUntilSeenConsecutively - ballModel.seen.timeWhenFirstSeenConsecutively > 150)
    {
      return kickSelectionTable.retrieveKick(Geometry::distanceTo(robotPose.getPose(),ballModel.seen.positionField)
        * cos(Geometry::angleTo(robotPose.getPose(),ballModel.seen.positionField)),
        Geometry::distanceTo(robotPose.getPose(),ballModel.seen.positionField)
        * sin(Geometry::angleTo(robotPose.getPose(),ballModel.seen.positionField)),
        angle, angleWidth, (KickSelectionTable::KickSelectionTableID)(int)tableID
        );
    }
    return KickSelectionTable::nothing;
  );

  if (   ballModel.ballWasSeen
      && ballModel.propagated.getPositionValidity() > 0.2)
  {
    return kickSelectionTable.retrieveKick
      (ballModel.propagated.positionRobot.x,
       ballModel.propagated.positionRobot.y,
       angle, 
       angleWidth, 
       (KickSelectionTable::KickSelectionTableID)(int)tableID
       );

  }
  else
    return KickSelectionTable::nothing;
}

double KickSelectionSymbols::intentionalKickPossible(){
  double maxDistance=400;
  if (SystemCall::getTimeSince(ballModel.seen.timeUntilSeenConsecutively) < 200
    && ballModel.seen.timeUntilSeenConsecutively - ballModel.seen.timeWhenFirstSeenConsecutively > 150)
  {
    intentionalKickOffset=(kickSelectionTable.searchKickInNeighbourhood(Geometry::distanceTo(robotPose.getPose(),ballModel.seen.positionField)
      * cos(Geometry::angleTo(robotPose.getPose(),ballModel.propagated.positionField)),
      Geometry::distanceTo(robotPose.getPose(),ballModel.propagated.positionField)
      * sin(Geometry::angleTo(robotPose.getPose(),ballModel.propagated.positionField)),
      intentionalAngle, (KickSelectionTable::KickSelectionTableID)(int)intentionalTable, robotPose, maxDistance, intentionalKick));

    OUTPUT (idText,text,"IntentionalKick:"<<intentionalKickOffset.x<<" "<<intentionalKickOffset.y);
    if (intentionalKickOffset.abs() <= maxDistance) return 1; else return 0;

  }
  else
  {
    return 0;
  }
}

double KickSelectionSymbols::intentionalKickXOffset(){
  return intentionalKickOffset.x;
}
double KickSelectionSymbols::intentionalKickYOffset(){
  return intentionalKickOffset.y;
}

double KickSelectionSymbols::intentionalKickKick(){
  return intentionalKick;
}

double KickSelectionSymbols::intentionalKickTargetX(){
  return Geometry::relative2FieldCoord(robotPose,intentionalKickOffset.x*3,intentionalKickOffset.y*3).x;
}
double KickSelectionSymbols::intentionalKickTargetY(){
  return Geometry::relative2FieldCoord(robotPose,intentionalKickOffset.x*3,intentionalKickOffset.y*3).y;
}


bool KickSelectionSymbols::handleMessage(InMessage& message)
{
  if(message.getMessageID() == idKickSelectionTable)
  {
    message.text >> kickSelectionTable;
    return true;
  }
  else return false;
}

---