Modules/BehaviorControl/GT2005BehaviorControl/GT2005BasicBehaviors/GT2005PotentialFieldBasicBehaviors.cpp

Go to the documentation of this file.

/** 
* @file GT2005PotentialFieldBasicBehaviors.cpp
*
* Implementation of basic behaviors defined in potential-field-basic-behaviors.xml.
*
* @author <a href="mailto:timlaue@tzi.de">Tim Laue</a>
*/

#include "GT2005PotentialFieldBasicBehaviors.h"
#include "Tools/FieldDimensions.h"



GT2005PotentialFieldBasicBehaviorSupport::GT2005PotentialFieldBasicBehaviorSupport(
                                            const BehaviorControlInterfaces& interfaces,
                                            Xabsl2ErrorHandler& errorHandler):
            GT2005PotentialFieldBasicBehavior(interfaces, errorHandler, "potential-field-support","GT2005/support.pfc")
{
  registerParameter("potential-field-support.x", x);
  registerParameter("potential-field-support.y", y);
  registerParameter("potential-field-support.max-speed", maxSpeed);
}


void GT2005PotentialFieldBasicBehaviorSupport::execute()
{
  Pose2D destination(0.0,x,y);
  Pose2D dummy;
  potentialfields.execute(robotPose, ballModel, playerPoseCollection, 
                          obstaclesModel, teamMessageCollection, destination, dummy, 1, result);
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;
  
  Vector2<double> relVec(-robotPose.translation + destination.translation);
  double distanceToPoint(relVec.abs());
  if(fabs(result.motion.speed) > 1.0) result.motion.speed = 1.0;
  motionRequest.walkRequest.walkParams.translation.x = maxSpeed * result.motion.speed * result.motion.pos.x;
  motionRequest.walkRequest.walkParams.translation.y = maxSpeed * result.motion.speed * result.motion.pos.y;
  motionRequest.walkRequest.walkParams.rotation = result.motion.rotation;
  if((distanceToPoint < 50.0) && fabs(toDegrees(result.motion.rotation)) < 15)
  {
    motionRequest.motionType = MotionRequest::stand;
  }
  else if(distanceToPoint < 50.0)
  {
    motionRequest.walkRequest.walkParams.translation.x = 0.0;
    motionRequest.walkRequest.walkParams.translation.y = 0.0; 
  }
  else if(distanceToPoint < 300.0)
  {
    motionRequest.walkRequest.walkParams.translation *= distanceToPoint/300.0;
  }
}


GT2005PotentialFieldBasicBehaviorOffensiveSupport::GT2005PotentialFieldBasicBehaviorOffensiveSupport(
                                            const BehaviorControlInterfaces& interfaces,
                                            Xabsl2ErrorHandler& errorHandler):
            GT2005PotentialFieldBasicBehavior(interfaces, errorHandler, "potential-field-offensive-support","GT2005/offsupp.pfc")
{
  registerParameter("potential-field-offensive-support.x", x);
  registerParameter("potential-field-offensive-support.y", y);
  registerParameter("potential-field-offensive-support.max-speed", maxSpeed);
}


void GT2005PotentialFieldBasicBehaviorOffensiveSupport::execute()
{
  Pose2D destination(0.0,x,y);
  //Compute a position to rotate to:
  Pose2D destination2;
  Vector2<double> knownBallPos(ballModel.getKnownPosition(BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted));
  Vector2<double> knownBallPosRobotRelative(Geometry::vectorTo(robotPose, knownBallPos));
  Vector2<double> rP(robotPose.translation);
  //double relAngleToBall(Vector2<double>(knownBallPos-rP).angle());
  Vector2<double> absGoalVec((double)xPosOpponentGoal,0.0);
  //double relAngleToGoal((absGoalVec-rP).angle());
 /* if(fabs( relAngleToBall - relAngleToGoal) < pi/2)
  {
    double desAngle(((relAngleToBall + relAngleToGoal) /2) - (fabs(relAngleToBall) < fabs(relAngleToGoal) ? relAngleToBall:relAngleToGoal));
    destination2.translation = Geometry::vectorTo(robotPose, Vector2<double>(rP.x+cos(desAngle),rP.y+sin(desAngle)));
  }

  else
  {
     destination2.translation = knownBallPosRobotRelative;
  }*/
  destination2.translation = knownBallPos;
  //Execute potential field behavior:
  potentialfields.execute(robotPose, ballModel, playerPoseCollection, 
                          obstaclesModel, teamMessageCollection, destination, destination2, 2, result);
  //Set motion request:
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;
  
  Vector2<double> relVec(-robotPose.translation + destination.translation);
  double distanceToPoint(relVec.abs());
  if(fabs(result.motion.speed) > 1.0) result.motion.speed = 1.0;
  motionRequest.walkRequest.walkParams.translation.x = maxSpeed * result.motion.speed * result.motion.pos.x;
  motionRequest.walkRequest.walkParams.translation.y = maxSpeed * result.motion.speed * result.motion.pos.y;
  motionRequest.walkRequest.walkParams.rotation = result.motion.rotation;
  if((distanceToPoint < 50.0) && fabs(toDegrees(result.motion.rotation)) < 15)
  {
    motionRequest.motionType = MotionRequest::stand;
  }
  else if(distanceToPoint < 50.0)
  {
    motionRequest.walkRequest.walkParams.translation.x = 0.0;
    motionRequest.walkRequest.walkParams.translation.y = 0.0; 
  }
  else if(distanceToPoint < 300.0)
  {
    motionRequest.walkRequest.walkParams.translation *= distanceToPoint/300.0;
  }
}


GT2005PotentialFieldBasicBehaviorAvoidRobots::GT2005PotentialFieldBasicBehaviorAvoidRobots(
                                              const BehaviorControlInterfaces& interfaces,
                                              Xabsl2ErrorHandler& errorHandler):
            GT2005PotentialFieldBasicBehavior(interfaces, errorHandler, "potential-field-avoid-robots","GT2005/avoidbot.pfc")
{
  registerParameter("potential-field-avoid-robots.x", x);
  registerParameter("potential-field-avoid-robots.y", y);
  registerParameter("potential-field-avoid-robots.max-speed", maxSpeed);
}


void GT2005PotentialFieldBasicBehaviorAvoidRobots::execute()
{
  Pose2D destination(0.0,x,y);
  Pose2D dummy;
  potentialfields.execute(robotPose, ballModel, playerPoseCollection, 
                          obstaclesModel, teamMessageCollection, destination, dummy, 1, result);
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;
  
  Vector2<double> relVec(-robotPose.translation + destination.translation);
  double distanceToPoint(relVec.abs());
  if(fabs(result.motion.speed) > 1.0) result.motion.speed = 1.0;
  motionRequest.walkRequest.walkParams.translation.x = maxSpeed * result.motion.speed * result.motion.pos.x;
  motionRequest.walkRequest.walkParams.translation.y = maxSpeed * result.motion.speed * result.motion.pos.y;
  motionRequest.walkRequest.walkParams.rotation = result.motion.rotation;
  if((distanceToPoint < 50.0) && fabs(toDegrees(result.motion.rotation)) < 15)
  {
    motionRequest.motionType = MotionRequest::stand;
  }
  else if(distanceToPoint < 50.0)
  {
    motionRequest.walkRequest.walkParams.translation.x = 0.0;
    motionRequest.walkRequest.walkParams.translation.y = 0.0; 
  }
  else if(distanceToPoint < 300.0)
  {
    motionRequest.walkRequest.walkParams.translation *= distanceToPoint/300.0;
  }
}

---