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

# Modules/BehaviorControl/SLBenchmark.cpp

Go to the documentation of this file.

    00001 #include "SLBenchmark.h"
    00002
    00003 #ifdef APERIOS1_3_2
    00004   #include <ERA201D1.h>
    00005 #endif
    00006
    00007 #include "Platform/GTAssert.h"
    00008
    00009 #define INITIAL_STATE 0x00
    00010
    00011 using namespace std;
    00012
    00013 #define min(a,b) (a<b?a:b)
    00014
    00015 SLBenchmark::SLBenchmark(const BehaviorControlInterfaces& interfaces) :
    00016 BehaviorControl (interfaces)
    00017
    00018 {
    00019   OUTPUT (idText, text, "wlan measure behavior loaded");
    00020
    00021   targets[0] = Vector3<double> (-2000.0, -1800.0, pi_2);
    00022   targets[1] = Vector3<double> (-2000.0, -1800.0, 0.0);
    00023   targets[2] = Vector3<double> ( 1000.0, -1500.0, 0.0);
    00024   targets[3] = Vector3<double> ( 2450.0, -0500.0, pi_4);
    00025   targets[4] = Vector3<double> ( 1500.0,  1000.0, 0.0);
    00026   targets[5] = Vector3<double> (-0500.0, -1500.0, -pi);
    00027   targets[6] = Vector3<double> (-2000.0,  0500.0, -pi);
    00028
    00029   index = 0;
    00030
    00031   state = INITIAL_STATE;
    00032
    00033   GenerateNewTarget ();
    00034 };
    00035
    00036 #define TRANS_ERROR 100
    00037 #define ROT_ERROR (pi / 8.0)
    00038
    00039 void SLBenchmark::execute()
    00040 {
    00041   currentPose = gtCamWorldState.getRobotPose ();
    00042
    00043   Vector2<int> lastPoint = Vector2<int> ((int) currentPose.translation.x, (int) currentPose.translation.y);
    00044
    00045   for (int i = index - 1; i < 6; i++)
    00046   {
    00047     LINE (selfLocatorField, lastPoint.x, lastPoint.y, (int)targets[i].x, (int)targets[i].y, 10, Drawings::ps_solid, Drawings::red);
    00048
    00049     lastPoint.x = (int) targets[i].x;
    00050     lastPoint.y = (int) targets[i].y;
    00051   };
    00052
    00053   motionRequest.motionType = motionRequest.stand;
    00054
    00055   motionRequest.walkRequest.walkParams.rotation = 0;
    00056   motionRequest.walkRequest.walkParams.translation.x = 0;
    00057   motionRequest.walkRequest.walkParams.translation.y = 0;
    00058
    00059   headControlMode.headControlMode = HeadControlMode::searchAuto;
    00060
    00061   if (currentPose.translation.x == 0.0 && currentPose.translation.y == 0.0)
    00062     return;
    00063
    00064   switch (state)
    00065   {
    00066   case 0x00:
    00067     {
    00068       if (GoToNextTarget () <= TRANS_ERROR)
    00069         state++;
    00070
    00071       break;
    00072     }
    00073   case 0x01:
    00074     {
    00075       if (RotateToNextTarget () <= ROT_ERROR)
    00076         state++;
    00077
    00078       break;
    00079     }
    00080   case 0x02:
    00081     {
    00082       if (index < 7)
    00083       {
    00084         GenerateNewTarget ();
    00085
    00086         state = INITIAL_STATE;
    00087       }
    00088       else
    00089         state++;
    00090     }
    00091   case 0x03:
    00092     {
    00093       //finished
    00094
    00095       break;
    00096     }
    00097   }
    00098
    00099   double angle = gtCamWorldState.getRobotPose ().rotation + motionRequest.walkRequest.walkParams.translation.angle ();
    00100
    00101   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);
    00102
    00103   lastOdometry = odometryData;
    00104
    00105   return;
    00106 };
    00107
    00108 void SLBenchmark::GenerateNewTarget ()
    00109 {
    00110 #ifdef RANDOM_TARGET
    00111   targetPosition.x = (((double)rand () / (double)RAND_MAX * 4000.0) - 2000);
    00112   targetPosition.y = ((double)rand () / (double)RAND_MAX * 3000.0) - 1500;
    00113
    00114   targetRotation = ((double)rand () / (double)RAND_MAX * pi2) - pi;
    00115 #else
    00116   targetPosition.x = targets[index].x;
    00117   targetPosition.y = targets[index].y;
    00118   targetRotation = targets[index].z;
    00119
    00120   index++;
    00121 #endif
    00122
    00123   cout << "ziel : " << targetPosition.x << " , " << targetPosition.y << " , " << targetRotation << endl << flush;
    00124
    00125   OUTPUT (idText, text, "ziel : " << targetPosition.x << " , " << targetPosition.y << " , " << targetRotation);
    00126 }
    00127
    00128 double SLBenchmark::GoToNextTarget ()
    00129 {
    00130   Vector2<double> currentPosition;
    00131   Vector2<double> wayLeft;
    00132
    00133   currentPosition.x = gtCamWorldState.getRobotPose ().translation.x;
    00134   currentPosition.y = gtCamWorldState.getRobotPose ().translation.y;
    00135
    00136   double currentRotation = gtCamWorldState.getRobotPose ().rotation;
    00137
    00138   wayLeft.x = targetPosition.x - currentPosition.x;
    00139   wayLeft.y = targetPosition.y - currentPosition.y;
    00140
    00141   double distance = wayLeft.abs ();
    00142
    00143   if (distance > TRANS_ERROR)
    00144   {
    00145       double angleToDestination = Geometry::angleTo(gtCamWorldState.getRobotPose (),targetPosition);
    00146
    00147       motionRequest.motionType = motionRequest.walk;
    00148
    00149       motionRequest.walkRequest.walkParams.rotation = 0;
    00150       motionRequest.walkRequest.walkParams.translation.x = cos (angleToDestination)* min ((double)400.0, (double)distance);
    00151       motionRequest.walkRequest.walkParams.translation.y = sin (angleToDestination)* min ((double)400.0, (double)distance);
    00152
    00153       ARROW (selfLocatorField, currentPose.translation.x, currentPose.translation.y, cos (currentPose.rotation) * 200, sin (currentPose.rotation) * 200, 10, Drawings::ps_solid, Drawings::orange);
    00154       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);
    00155
    00156       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;
    00157   }
    00158   else
    00159     cout << "ziel erreicht : " << wayLeft.x << " , " << wayLeft.y  << endl << flush;
    00160
    00161   return distance;
    00162 };
    00163
    00164 double SLBenchmark::RotateToNextTarget ()
    00165 {
    00166   double rotationLeft = targetRotation - gtCamWorldState.getRobotPose ().rotation;
    00167
    00168   if (fabs (rotationLeft) > ROT_ERROR)
    00169   {
    00170     if (rotationLeft > pi)
    00171       rotationLeft -= pi2;
    00172
    00173     if (rotationLeft < pi)
    00174       rotationLeft += pi2;
    00175
    00176     motionRequest.motionType = motionRequest.walk;
    00177
    00178     motionRequest.walkRequest.walkParams.rotation = rotationLeft;
    00179     motionRequest.walkRequest.walkParams.translation.x = 0;
    00180     motionRequest.walkRequest.walkParams.translation.y = 0;
    00181
    00182     if (motionRequest.walkRequest.walkParams.rotation > pi)
    00183       motionRequest.walkRequest.walkParams.rotation = pi_4;
    00184     if (motionRequest.walkRequest.walkParams.rotation < -pi)
    00185       motionRequest.walkRequest.walkParams.rotation = -pi_4;
    00186   }
    00187
    00188   return fabs (rotationLeft);
    00189 };
    00190
    00191 bool SLBenchmark::FullTurn ()
    00192 {
    00193   Pose2D delta = odometryData - lastOdometry;
    00194
    00195   rotationAngle += delta.rotation;
    00196
    00197   if (rotationAngle < 4.0 * pi)
    00198   {
    00199     motionRequest.motionType = motionRequest.walk;
    00200
    00201     motionRequest.walkRequest.walkParams.rotation = pi_2;
    00202     motionRequest.walkRequest.walkParams.translation.x = 0;
    00203     motionRequest.walkRequest.walkParams.translation.y = 0;
    00204
    00205     return false;
    00206   }
    00207
    00208   return true;
    00209 }