Modules/BehaviorControl/SLBenchmark.cpp

Go to the documentation of this file.

#include "SLBenchmark.h"

#ifdef APERIOS1_3_2
  #include <ERA201D1.h>
#endif

#include "Platform/GTAssert.h"

#define INITIAL_STATE 0x00

using namespace std;

#define min(a,b) (a<b?a:b)

SLBenchmark::SLBenchmark(const BehaviorControlInterfaces& interfaces) :
BehaviorControl (interfaces)

{
  OUTPUT (idText, text, "wlan measure behavior loaded");

  targets[0] = Vector3<double> (-2000.0, -1800.0, pi_2);
  targets[1] = Vector3<double> (-2000.0, -1800.0, 0.0);
  targets[2] = Vector3<double> ( 1000.0, -1500.0, 0.0);
  targets[3] = Vector3<double> ( 2450.0, -0500.0, pi_4);
  targets[4] = Vector3<double> ( 1500.0,  1000.0, 0.0);
  targets[5] = Vector3<double> (-0500.0, -1500.0, -pi);
  targets[6] = Vector3<double> (-2000.0,  0500.0, -pi);

  index = 0;

  state = INITIAL_STATE;

  GenerateNewTarget ();
};

#define TRANS_ERROR 100
#define ROT_ERROR (pi / 8.0)

void SLBenchmark::execute()
{
  currentPose = gtCamWorldState.getRobotPose ();

  Vector2<int> lastPoint = Vector2<int> ((int) currentPose.translation.x, (int) currentPose.translation.y);

  for (int i = index - 1; i < 6; i++)
  {
    LINE (selfLocatorField, lastPoint.x, lastPoint.y, (int)targets[i].x, (int)targets[i].y, 10, Drawings::ps_solid, Drawings::red);

    lastPoint.x = (int) targets[i].x;
    lastPoint.y = (int) targets[i].y;
  };

  motionRequest.motionType = motionRequest.stand;

  motionRequest.walkRequest.walkParams.rotation = 0;
  motionRequest.walkRequest.walkParams.translation.x = 0;
  motionRequest.walkRequest.walkParams.translation.y = 0;

  headControlMode.headControlMode = HeadControlMode::searchAuto;

  if (currentPose.translation.x == 0.0 && currentPose.translation.y == 0.0)
    return;

  switch (state)
  {
  case 0x00:
    {
      if (GoToNextTarget () <= TRANS_ERROR)
        state++;

      break;
    }
  case 0x01:
    {
      if (RotateToNextTarget () <= ROT_ERROR)
        state++;

      break;
    }
  case 0x02:
    {
      if (index < 7)
      {
        GenerateNewTarget ();

        state = INITIAL_STATE;
      }
      else
        state++;
    }
  case 0x03:
    {
      //finished

      break;
    }
  }

  double angle = gtCamWorldState.getRobotPose ().rotation + motionRequest.walkRequest.walkParams.translation.angle ();

  ARROW (selfLocatorField, (int) currentPose.translation.x, (int) currentPose.translation.y, (int) (currentPose.translation.x + cos (angle) * 200.0), (int) (currentPose.translation.y + sin (angle) * 200.0), 10, Drawings::ps_solid, Drawings::blue);

  lastOdometry = odometryData;

  return;
};

void SLBenchmark::GenerateNewTarget ()
{
#ifdef RANDOM_TARGET
  targetPosition.x = (((double)rand () / (double)RAND_MAX * 4000.0) - 2000);
  targetPosition.y = ((double)rand () / (double)RAND_MAX * 3000.0) - 1500;

  targetRotation = ((double)rand () / (double)RAND_MAX * pi2) - pi;
#else
  targetPosition.x = targets[index].x;
  targetPosition.y = targets[index].y;
  targetRotation = targets[index].z;

  index++;
#endif

  cout << "ziel : " << targetPosition.x << " , " << targetPosition.y << " , " << targetRotation << endl << flush;

  OUTPUT (idText, text, "ziel : " << targetPosition.x << " , " << targetPosition.y << " , " << targetRotation);
}

double SLBenchmark::GoToNextTarget ()
{
  Vector2<double> currentPosition;
  Vector2<double> wayLeft;

  currentPosition.x = gtCamWorldState.getRobotPose ().translation.x;
  currentPosition.y = gtCamWorldState.getRobotPose ().translation.y;

  double currentRotation = gtCamWorldState.getRobotPose ().rotation;

  wayLeft.x = targetPosition.x - currentPosition.x;
  wayLeft.y = targetPosition.y - currentPosition.y;

  double distance = wayLeft.abs ();

  if (distance > TRANS_ERROR)
  {
      double angleToDestination = Geometry::angleTo(gtCamWorldState.getRobotPose (),targetPosition);

      motionRequest.motionType = motionRequest.walk;

      motionRequest.walkRequest.walkParams.rotation = 0;
      motionRequest.walkRequest.walkParams.translation.x = cos (angleToDestination)* min ((double)400.0, (double)distance);
      motionRequest.walkRequest.walkParams.translation.y = sin (angleToDestination)* min ((double)400.0, (double)distance);

      ARROW (selfLocatorField, currentPose.translation.x, currentPose.translation.y, cos (currentPose.rotation) * 200, sin (currentPose.rotation) * 200, 10, Drawings::ps_solid, Drawings::orange);
      ARROW (selfLocatorField, currentPose.translation.x, currentPose.translation.y, currentPose.translation.x + motionRequest.walkRequest.walkParams.translation.x, currentPose.translation.y + motionRequest.walkRequest.walkParams.translation.y, 10, Drawings::ps_solid, Drawings::pink);

      cout << "laufe : " << gtCamWorldState.getRobotPose ().translation.x << " , " << gtCamWorldState.getRobotPose ().translation.y << " , " << wayLeft.x << " , " << wayLeft.y << " , " << motionRequest.walkRequest.walkParams.translation.x << " , " << motionRequest.walkRequest.walkParams.translation.y << endl << flush;
  }
  else
    cout << "ziel erreicht : " << wayLeft.x << " , " << wayLeft.y  << endl << flush;

  return distance;
};

double SLBenchmark::RotateToNextTarget ()
{
  double rotationLeft = targetRotation - gtCamWorldState.getRobotPose ().rotation;

  if (fabs (rotationLeft) > ROT_ERROR)
  {
    if (rotationLeft > pi)
      rotationLeft -= pi2;

    if (rotationLeft < pi)
      rotationLeft += pi2;

    motionRequest.motionType = motionRequest.walk;

    motionRequest.walkRequest.walkParams.rotation = rotationLeft;
    motionRequest.walkRequest.walkParams.translation.x = 0;
    motionRequest.walkRequest.walkParams.translation.y = 0;

    if (motionRequest.walkRequest.walkParams.rotation > pi)
      motionRequest.walkRequest.walkParams.rotation = pi_4;
    if (motionRequest.walkRequest.walkParams.rotation < -pi)
      motionRequest.walkRequest.walkParams.rotation = -pi_4;
  }

  return fabs (rotationLeft);
};

bool SLBenchmark::FullTurn ()
{
  Pose2D delta = odometryData - lastOdometry;

  rotationAngle += delta.rotation;

  if (rotationAngle < 4.0 * pi)
  {
    motionRequest.motionType = motionRequest.walk;

    motionRequest.walkRequest.walkParams.rotation = pi_2;
    motionRequest.walkRequest.walkParams.translation.x = 0;
    motionRequest.walkRequest.walkParams.translation.y = 0;

    return false;
  }

  return true;
}

---