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

# Modules/BehaviorControl/GT2005BehaviorControl/GT2005BasicBehaviors/GT2005WalkCalibrationBasicBehaviors.cpp

Go to the documentation of this file.

    00001
    00002 #include "Tools/Math/PIDsmoothedValue.h"
    00003 #include "GT2005WalkCalibrationBasicBehaviors.h"
    00004 #include "Tools/Math/Geometry.h"
    00005 #include "Tools/Math/Matrix2x2.h"
    00006 #include "Tools/Math/Common.h"
    00007
    00008 #include "Tools/Debugging/DebugDrawings2.h"
    00009 #include "Tools/Debugging/Debugging.h"
    00010
    00011 GT2005WalkCalibrationMainBehavior::GT2005WalkCalibrationMainBehavior(BehaviorControlInterfaces& interfaces, Xabsl2ErrorHandler& errorHandler)
    00012 : Xabsl2BasicBehavior("walk-calibration", errorHandler),
    00013 BehaviorControlInterfaces(interfaces)
    00014
    00015 {
    00016   registerParameter("walk-calibration.initialize", initializeParameter);
    00017   registerParameter("walk-calibration.rotation", rotationParameter);
    00018
    00019   result = resultUnready;
    00020   state = stateNotInitialized;
    00021   tempPoseCount = 0;
    00022   startPoseCalculated = false;
    00023 }
    00024
    00025 void GT2005WalkCalibrationMainBehavior::removeExtreme(bool *pBadIndex, bool removeMaximum)
    00026 {
    00027     int extremeIndex;
    00028     double extremeSpeed;
    00029     double tempSpeed;
    00030
    00031     //repeat this 4 times (2 min, 2 max)
    00032     extremeIndex = -1;
    00033
    00034     if(removeMaximum)
    00035       extremeSpeed = 0.0;
    00036     else
    00037       extremeSpeed = 1000000;
    00038
    00039
    00040     for(int i = 0; i < tempPoseCount; i++)
    00041     {
    00042       if(!pBadIndex[i])
    00043       {
    00044         tempSpeed = pow(tempPose[i].translation.x, 2) + pow(tempPose[i].translation.y, 2);
    00045         if(removeMaximum && tempSpeed > extremeSpeed)
    00046         {
    00047           extremeSpeed = tempSpeed;
    00048           extremeIndex = i;
    00049         }
    00050         else if(!removeMaximum && tempSpeed < extremeSpeed)
    00051         {
    00052           extremeSpeed = tempSpeed;
    00053           extremeIndex = i;
    00054         }
    00055       }
    00056     }
    00057     if(extremeIndex >= 0)
    00058       pBadIndex[extremeIndex] = true;
    00059 }
    00060
    00061
    00062 void GT2005WalkCalibrationMainBehavior::execute()
    00063 {
    00064   if(initializeParameter)
    00065     Initialize();
    00066   else
    00067   {
    00068     bool performMeasure = true;
    00069
    00070 #ifdef _WIN32
    00071     camRobotPose = robotPose;
    00072     camTimestamp = SystemCall::getCurrentSystemTime();
    00073     robotNotDetectedCount = 0;
    00074
    00075 #else
    00076     camRobotPose = gtCamWorldState.getRobotPose(robotNumber, robotColor);
    00077     camTimestamp = gtCamWorldState.getTimestamp();
    00078     if((camRobotPose.translation.x == 0.0) &&
    00079       (camRobotPose.translation.y == 0.0) &&
    00080       (camRobotPose.rotation == 0.0))
    00081       robotNotDetectedCount++;
    00082     else
    00083       robotNotDetectedCount = 0;
    00084 #endif
    00085
    00086     // robot not detected by gtCam
    00087     if(robotNotDetectedCount > 0)
    00088     {
    00089       state = stateMeasureStart;
    00090       result = (robotNotDetectedCount >= 10) ? resultInteraction : resultReposition;
    00091       performMeasure = false;
    00092       OUTPUT(idText, text, "GTWalkCalibration: Robot not recognized or worldstate missing (" << robotNotDetectedCount << ". times)");
    00093     }
    00094
    00095     // dog is outside camera range or field
    00096     Vector2<double> temp = camRobotPose.translation;
    00097     if(fabs(temp.y) > 1900 || temp.x > 2400 || temp.x < 0)
    00098     {
    00099       result = resultReposition;
    00100       state = stateMeasureStart;
    00101       startPoseBadCount++;
    00102       performMeasure = false;
    00103       OUTPUT(idText, text, "GTWalkCalibration: Robot out of measure arrea (" << startPoseBadCount << ". times)");
    00104     }
    00105
    00106     if(performMeasure)
    00107       Measure();
    00108
    00109     //catch too many bad start positions in a row
    00110     if(startPoseBadCount >= 5)
    00111     {
    00112       OUTPUT(idText, text, "start pose was bad " << startPoseBadCount << " times in a row, requesting manual interaction");
    00113       state = stateMeasureStart;
    00114       result = resultInteraction;
    00115     }
    00116
    00117     //calculation of start position
    00118     if(result == resultReposition)
    00119     {
    00120       if(tempPoseCount > 0)
    00121       {
    00122         Pose2D mid;
    00123
    00124         for(int i=0;i< tempPoseCount; i++)
    00125         {
    00126           mid.translation.x += tempPose[i].translation.x;
    00127           mid.translation.y += tempPose[i].translation.y;
    00128           mid.rotation += tempPose[i].rotation;
    00129         }
    00130         mid.translation.x /= (double)tempPoseCount;
    00131         mid.translation.y /= (double)tempPoseCount;
    00132         mid.rotation /= (double)tempPoseCount;
    00133
    00134         startPose = calculateStartPose(mid);
    00135         startPoseCalculated = true;
    00136         //TODO also repeat on line change??
    00137       }
    00138       else
    00139       {
    00140         startPose = calculateStartPose(measurePoints[currentPoint].controlled);
    00141         startPoseCalculated = false;
    00142       }
    00143       startPoseAngle = toDegrees(startPose.rotation);
    00144     }
    00145   }
    00146 }
    00147
    00148 void GT2005WalkCalibrationMainBehavior::Initialize()
    00149 {
    00150   OUTPUT(idText, text, "GTWalkCalibration: Initializing");
    00151   state = stateMeasureStart;
    00152   measureStatus = before;
    00153   robotColor = getPlayer().getTeamColor();
    00154   robotNumber = getPlayer().getPlayerNumber();
    00155   currentTable = 0;
    00156   currentPoint = 0;
    00157   currentRotation = 0;
    00158   startOffset = 2000.0;
    00159   measureDuration = 4500.0; //4500.0 for normal plane calibartion 9500.0 for rotation only calibration
    00160   stopOffset = 500.0;
    00161
    00162   /*measurePoints[0].controlled = Pose2D(-1.6666 * pi, 0, 0);
    00163   measurePoints[1].controlled = Pose2D(-1.3000 * pi, 0, 0);
    00164   measurePoints[2].controlled = Pose2D(-1.0000 * pi, 0, 0);
    00165   measurePoints[3].controlled = Pose2D(-0.8333 * pi, 0, 0);
    00166   measurePoints[4].controlled = Pose2D(-0.6400 * pi, 0, 0);
    00167   measurePoints[5].controlled = Pose2D(-0.4000 * pi, 0, 0);
    00168   measurePoints[6].controlled = Pose2D(-0.3000 * pi, 0, 0);
    00169   measurePoints[7].controlled = Pose2D(-0.2000 * pi, 0, 0);
    00170   measurePoints[8].controlled = Pose2D(-0.1500 * pi, 0, 0);
    00171   measurePoints[9].controlled = Pose2D(-0.1000 * pi, 0, 0);
    00172   measurePoints[10].controlled = Pose2D(0.0000 * pi, 0, 0);
    00173   measurePoints[11].controlled = Pose2D(0.1000 * pi, 0, 0);
    00174   measurePoints[12].controlled = Pose2D(0.1500 * pi, 0, 0);
    00175   measurePoints[13].controlled = Pose2D(0.2000 * pi, 0, 0);
    00176   measurePoints[14].controlled = Pose2D(0.3000 * pi, 0, 0);
    00177   measurePoints[15].controlled = Pose2D(0.4000 * pi, 0, 0);
    00178   measurePoints[16].controlled = Pose2D(0.6400 * pi, 0, 0);
    00179   measurePoints[17].controlled = Pose2D(0.8333 * pi, 0, 0);
    00180   measurePoints[18].controlled = Pose2D(1.0000 * pi, 0, 0);
    00181   measurePoints[19].controlled = Pose2D(1.3000 * pi, 0, 0);
    00182   measurePoints[20].controlled = Pose2D(1.6666 * pi, 0, 0);//*/
    00183
    00184
    00185   double measurePointsX[] = {-300, -250, -200, -150, -125, -100, -50, 0, 50, 100, 125, 150, 200, 250, 300};
    00186   double measurePointsY[] = {-300, -250, -200, -150, -100, -50, 0, 50, 100, 150, 200, 250, 300};
    00187   for(int i = 0; i < 15; i++)
    00188   {
    00189     for(int j = 0; j < 13; j++)
    00190     {
    00191       measurePoints[i * 13 + j].controlled = Pose2D(rotationParameter / 180.0 * pi, measurePointsX[i], measurePointsY[j]); //0.87266
    00192     }
    00193   }
    00194
    00195   InBinaryFile file("_Measure.bin");
    00196   if(file.exists())
    00197   {
    00198     Pose2D controlled;
    00199     for(int i = 0; i < measureSize; i++)
    00200     {
    00201       if(file.eof())
    00202         break;
    00203
    00204       file >> controlled;
    00205
    00206       if(controlled != measurePoints[i].controlled)
    00207       {
    00208         OUTPUT(idText, text, "controlled Points mismatch, cannot resume ");
    00209         currentPoint = 0;
    00210         break;
    00211       }
    00212
    00213       if(file.eof())
    00214         break;
    00215
    00216       file >> measurePoints[i].measured;
    00217       currentPoint = i + 1;
    00218     }
    00219     if(currentPoint > 0)
    00220       OUTPUT(idText, text, "GTWalkCalibration: resuming at measure #" << currentPoint);
    00221
    00222     sprintf(filename, "od_%i%s.dat", (int)fabs(rotationParameter), rotationParameter < 0 ? "m" : "");
    00223   }
    00224
    00225   center.x = 1200.0;
    00226   center.y = 0.0;
    00227
    00228   startPoseBadCount = 0;
    00229   measureCount = 0;
    00230   initializationTime = SystemCall::getCurrentSystemTime();
    00231   result = resultReady;
    00232 }
    00233
    00234
    00235
    00236 void GT2005WalkCalibrationMainBehavior::Measure()
    00237 {
    00238   switch(state)
    00239   {
    00240   case stateMeasureStart:
    00241     {
    00242       motionRequest.motionType = MotionRequest::walk;
    00243       motionRequest.walkRequest.walkType = WalkRequest::normal;
    00244       motionRequest.walkRequest.walkParams = Pose2D(0, 0, 0);
    00245
    00246       measureStartTime = camTimestamp + startOffset;
    00247       measureStopTime = measureStartTime + measureDuration;
    00248       rotationTemp = 0;
    00249       tempPoseCount = 0;
    00250
    00251       double timePerMeasure, timeLeft;
    00252       if(measureCount > 0)
    00253         timePerMeasure = (SystemCall::getCurrentSystemTime() - initializationTime) / ((double)measureCount);
    00254       else
    00255         timePerMeasure = 0.0;
    00256
    00257       timeLeft = (measureSize - currentPoint) * timePerMeasure;
    00258       OUTPUT(idText, text, "PWR: " << SystemCall::getRemainingPower() << "%, ETA: " << timeLeft / 1000.0 / 60.0 << "min");
    00259
    00260       OUTPUT(idText, text, "GTWalkCalibration: starting measure #" << currentPoint);
    00261       state = stateMeasure;
    00262       measureStatus = before;
    00263       result = resultMeasure;
    00264
    00265       //clear qSum
    00266       qSum = 0.0;
    00267       qCount = 0;
    00268
    00269       break;
    00270     }
    00271   case stateMeasure:
    00272     {
    00273       Pose2D request;
    00274
    00275       motionRequest.motionType = MotionRequest::walk;
    00276       motionRequest.walkRequest.walkType = WalkRequest::normal;
    00277       request = measurePoints[currentPoint].controlled;
    00278       //hack for calibration
    00279       request.translation.x /= 1.5; // 0..450 -> 0..300
    00280       request.translation.y /= 1.5; // 0..450 -> 0..300
    00281       request.rotation *= 1.5; // 0..200 -> 0..300
    00282       motionRequest.walkRequest.walkParams = request;
    00283
    00284       if(measureStatus == measuring)
    00285       {
    00286         double qRms, qDist;
    00287
    00288         //stdaweichung abstand zur kamera updaten
    00289         qCount++;
    00290         qDist = (center - camRobotPose.translation).abs();
    00291         qSum += qDist * qDist;
    00292         qRms = sqrt(qSum/(double)qCount);
    00293
    00294         //if we got the startpose from controlled values
    00295         //and qRms is too high and we have a measueMent yet
    00296         if(!startPoseCalculated && qRms > 800.0 && tempPoseCount > 0)
    00297         {
    00298           result = resultReposition;
    00299           state = stateMeasureStart;
    00300           startPoseBadCount++;
    00301           OUTPUT(idText, text, "start positioning was too bad(" << startPoseBadCount << "), repeating measure");
    00302           break;
    00303         }
    00304         //if qRms is far too high
    00305         if(qRms > 1000.0)
    00306         {
    00307           result = resultReposition;
    00308           state = stateMeasureStart;
    00309           startPoseBadCount++;
    00310           OUTPUT(idText, text, "we are too far off(" << startPoseBadCount <<") !!");
    00311           break;
    00312         }
    00313       }
    00314
    00315       if(camTimestamp >= measureStartTime && measureStatus == before)
    00316       {
    00317         translationStartPoint = camRobotPose.translation;
    00318         rotationStartAngle = camRobotPose.rotation;
    00319         tempTimeStart = measureStartTime = camTimestamp;
    00320
    00321         tempTime = measureStartTime + 400;
    00322         measureStatus = measuring;
    00323
    00324         // vorsichtshalber auch mal komplett messen
    00325         measureStartTime0 = measureStartTime;
    00326         translationStartPoint0 = translationStartPoint;
    00327         rotationStartAngle0 = rotationStartAngle;
    00328       }
    00329       else if(camTimestamp >= tempTime && measureStatus == measuring)
    00330       {
    00331         translationStopPoint = camRobotPose.translation;
    00332         rotationStopAngle = camRobotPose.rotation;
    00333         tempTimeStop = camTimestamp;
    00334
    00335         if(tempTimeStart == tempTimeStop)
    00336         {
    00337           OUTPUT(idText, text, "GTWalkCalibration: measureStartTime = measureStopTime");
    00338           state = stateMeasureStart;
    00339           result = resultInteraction;
    00340           break;
    00341         }
    00342
    00343         double angle = rotationStartAngle; //(rotationStopAngle + rotationStartAngle) / 2;
    00344         Vector2<double> robotx, roboty;
    00345         double a, b;
    00346         robotx.x = cos(angle);
    00347         robotx.y = sin(angle);
    00348         roboty.x = -robotx.y;
    00349         roboty.y = robotx.x;
    00350         a = ((translationStopPoint.x - translationStartPoint.x) * roboty.y
    00351           + (translationStartPoint.y - translationStopPoint.y) * roboty.x)
    00352           / (robotx.x * roboty.y - robotx.y * roboty.x);
    00353         b = (translationStopPoint.y - translationStartPoint.y - a * robotx.y) / roboty.y;
    00354         tempPose[tempPoseCount].translation.x = a / ((tempTimeStop - tempTimeStart) / 1000);
    00355         tempPose[tempPoseCount].translation.y = b / ((tempTimeStop - tempTimeStart) / 1000);
    00356         tempPose[tempPoseCount].rotation = normalize(rotationStopAngle - rotationStartAngle) / ((tempTimeStop - tempTimeStart) / 1000);
    00357
    00358         OUTPUT(idText, text, "GTWalkCalibration: rot(" << tempPoseCount << "): " << tempPose[tempPoseCount].rotation);
    00359
    00360         // set new startPointParameters
    00361         translationStartPoint = camRobotPose.translation;
    00362         rotationStartAngle = camRobotPose.rotation;
    00363         tempTimeStart = camTimestamp;
    00364         tempTime = tempTimeStart + 400;
    00365
    00366         // dann wolln mer mal nen neues Häppchen messen
    00367         if(tempPoseCount < 49) tempPoseCount++;
    00368       }
    00369       if(camTimestamp >= measureStopTime && measureStatus == measuring)
    00370       {
    00371         // vorsichtshalber auch mal komplett messen
    00372         measureStopTime0 = camTimestamp;
    00373         translationStopPoint0 = camRobotPose.translation;
    00374         rotationStopAngle0 = camRobotPose.rotation;
    00375
    00376         state = stateMeasureFinished;
    00377         result = resultMeasure;
    00378         measureStatus = end;
    00379         break;
    00380       }
    00381       break;
    00382     }
    00383   case stateMeasureFinished:
    00384     {
    00385       bool badIndex[measureSize];
    00386
    00387       OUTPUT(idText, text, " ");
    00388       OUTPUT(idText, text, "GTWalkCalibration: finished measure #" << currentPoint);
    00389
    00390       for(int i = 0 ;i < measureSize; i++)
    00391         badIndex[i] = false;
    00392
    00393       removeExtreme(badIndex, true);
    00394       removeExtreme(badIndex, true);
    00395       removeExtreme(badIndex, false);
    00396       removeExtreme(badIndex, false);
    00397
    00398       measurePoints[currentPoint].measured = Pose2D(0, 0, 0);
    00399       int dieNackteWahrheit = 0;
    00400       for(int i = 0; i < tempPoseCount; i++)
    00401       {
    00402         if (badIndex[i])
    00403         {
    00404           OUTPUT(idText, text, "discarded index " << i);
    00405         }
    00406         else
    00407         {
    00408           measurePoints[currentPoint].measured.translation.x += tempPose[i].translation.x;
    00409           measurePoints[currentPoint].measured.translation.y += tempPose[i].translation.y;
    00410           measurePoints[currentPoint].measured.rotation += tempPose[i].rotation;
    00411           dieNackteWahrheit++;
    00412         }
    00413
    00414       }
    00415       if(dieNackteWahrheit > 0)
    00416       {
    00417         // hier setze ich für die rotationsmessung mal die translation auf 0
    00418         measurePoints[currentPoint].measured.translation.x /= dieNackteWahrheit;
    00419         measurePoints[currentPoint].measured.translation.y /= dieNackteWahrheit;
    00420         measurePoints[currentPoint].measured.rotation /= dieNackteWahrheit;
    00421       }
    00422       else
    00423       {
    00424         OUTPUT(idText, text, "Illegal value. Repeat measure.");
    00425         result = resultReposition;
    00426         state = stateMeasureStart;
    00427         break;
    00428       }
    00429       //      OUTPUT(idText, text, "indizes: " << tempDeleteMin[0] << "  " << tempDeleteMin[1] << "  " << tempDeleteMax[0] << "  " << tempDeleteMax[1]);
    00430       OUTPUT(idText, text, "count: " << dieNackteWahrheit);
    00431
    00432       OUTPUT(idText, text, "Controlled: " << measurePoints[currentPoint].controlled.translation.x << ", " << measurePoints[currentPoint].controlled.translation.y << ", " << measurePoints[currentPoint].controlled.rotation);
    00433       OUTPUT(idText, text, "Measured: " << measurePoints[currentPoint].measured.translation.x << ", " << measurePoints[currentPoint].measured.translation.y << ", " << measurePoints[currentPoint].measured.rotation);
    00434       if(qCount > 0)
    00435         OUTPUT(idText, text, "Last rms error: " << sqrt(qSum/(double)qCount));
    00436       OUTPUT(idText, text, "");
    00437
    00438       //      if(fabs(normalize(rotationStartAngle - rotationStopAngle) / 3) < 0.25)//0.125)
    00439       currentPoint++;
    00440       measureCount++;
    00441       //      else
    00442       //        OUTPUT(idText, text, "Repeat Measure, because Robot rotated too much");
    00443
    00444
    00445       if(currentPoint < measureSize)
    00446       {
    00447         // write so far measured points to file
    00448         OutBinaryFile file("_Measure.bin");
    00449         for(int i = 0; i < currentPoint; i++) //currentPoint is already incremented
    00450           file << measurePoints[i].controlled << measurePoints[i].measured;
    00451
    00452         OUTPUT(idText, text, "GT2005 Walk-Calibration done measure #" << currentPoint - 1);
    00453
    00454         state = stateMeasureStart;
    00455         result = resultReposition;
    00456         startPoseBadCount = 0;
    00457       }
    00458       else
    00459       {
    00460         OutBinaryFile deleteFile("_Measure.bin"); // shrink file to zer0 size
    00461         OutBinaryFile file(filename);
    00462         for(int i = 0; i < measureSize; i++)
    00463           file << measurePoints[i].controlled << measurePoints[i].measured;
    00464
    00465         OUTPUT(idText, text, "GT2005 Walk-Calibration finished");
    00466         state = stateCalibrationFinished;
    00467         result = resultUnready;
    00468       }
    00469       break;
    00470     }
    00471    case stateCalibrationFinished:
    00472     OUTPUT(idText, text, "calibration finished.");
    00473     result = resultUnready;
    00474     state = stateCalibrationFinished;
    00475     return;
    00476
    00477   case stateNotInitialized:
    00478     OUTPUT(idText, text, "not yet initialized.");
    00479     result = resultUnready;
    00480     state = stateNotInitialized;
    00481     return;
    00482
    00483   default:
    00484     OUTPUT(idText, text, "unknown state " << state);
    00485     result = resultUnready;
    00486     return;
    00487   }
    00488 }
    00489
    00490
    00491 double GT2005WalkCalibrationMainBehavior::minDistanceToBorder(Vector2<double>& t)
    00492 {
    00493   double dist;
    00494
    00495   if(t.x < 0.0 || t.x > 2400.0 || fabs(t.y) > 1900.0)
    00496     return 0.0;
    00497
    00498   dist = fabs(2400.0 - t.x);
    00499   dist = min(dist, t.x);
    00500   dist = min(dist, 1900.0 - fabs(t.y));
    00501
    00502   return dist;
    00503 }
    00504
    00505
    00506 Pose2D GT2005WalkCalibrationMainBehavior::calculateStartPose(const Pose2D& base)
    00507 {
    00508   const int count = 9; //zeitwert
    00509   double dist1, dist2, dist3;
    00510   double maxDist1 = 0, minDist2 = 10000;
    00511
    00512   Pose2D bestPose;
    00513   Pose2D pose[count];
    00514
    00515   for(double r = -pi; r <= pi; r += pi / 40.0)
    00516   {
    00517     for(int x = 100; x <= 2300; x += 200)
    00518     {
    00519       for(int y = -1800; y <= 1800; y += 200)
    00520       {
    00521         pose[0].rotation = r;
    00522         pose[0].translation.x = x;
    00523         pose[0].translation.y = y;
    00524         dist1 = 10000.0;
    00525         dist2 = 0.0;
    00526
    00527         for(int i = 1; i < count; i++)
    00528           pose[i] = pose[i - 1] + base;
    00529
    00530         //abstand zum border
    00531         for(int i = 0; i < count; i++)
    00532           dist1 = min(dist1, minDistanceToBorder(pose[i].translation));
    00533
    00534         //stdabweichung abstand zur kamera, erste beiden punkte ignorieren dabei
    00535         if(count > 2)
    00536         {
    00537           for(int i = 2; i < count; i++)
    00538           {
    00539             double dist = (pose[i].translation - center).abs();
    00540             dist2 += sqrt(dist*dist);
    00541           }
    00542           dist2 /= (double)(count - 2);
    00543         }
    00544
    00545         //abstand zum startpunkt
    00546         dist3 = (pose[0].translation - camRobotPose.translation).abs();
    00547         dist2 += dist3 * 0.5; //dist3 halb gewichten
    00548
    00549         if((dist1 > maxDist1)||
    00550           (dist1 == maxDist1 && dist2 < minDist2))
    00551         {
    00552           maxDist1 = dist1;
    00553           minDist2 = dist2;
    00554           bestPose = pose[0];
    00555         }
    00556       }
    00557     }
    00558   }
    00559
    00560   pose[0] = bestPose;
    00561   for(int i = 0; i < count; i++)
    00562   {
    00563     if(i == 0)
    00564     {
    00565       NCIRCLE("GT2005WalkCalibration:walkpath", pose[i].translation.x, pose[i].translation.y, 15,1,Drawings::ps_solid,Drawings::blue);
    00566     }
    00567     else
    00568     {
    00569       pose[i] = pose[i - 1] + base;
    00570       NCIRCLE("GT2005WalkCalibration:walkpath", pose[i].translation.x, pose[i].translation.y, 15,1,Drawings::ps_solid,Drawings::white);
    00571     }
    00572   }
    00573   NCROSS("GT2005WalkCalibration:walkpath", center.x, center.y, 50, 10, Drawings::ps_solid, Drawings::black);
    00574
    00575   return bestPose;
    00576 }
    00577
    00578
    00579
    00580
    00581 void GT2005WalkCalibrationMainBehavior::registerSymbols(Xabsl2Engine& engine)
    00582 {
    00583   engine.registerDecimalInputSymbol("walk-calibration.result", &result);
    00584   engine.registerDecimalInputSymbol("walk-calibration.start.x", &startPose.translation.x);
    00585   engine.registerDecimalInputSymbol("walk-calibration.start.y", &startPose.translation.y);
    00586   engine.registerDecimalInputSymbol("walk-calibration.start.angle", &startPoseAngle);
    00587 }
    00588
    00589 void GT2005WalkCalibrationBasicBehaviors::registerBasicBehaviors(Xabsl2Engine& engine)
    00590 {
    00591   engine.registerBasicBehavior(gt2005WalkCalibrationMainBehavior);
    00592   gt2005WalkCalibrationMainBehavior.registerSymbols(engine);
    00593 }
    00594 //TODO bessere borderbeststimmung bzw abstand(stdabweichung) zum mittelpunkt
    00595 //TODO flag setzten ob startPos geschätzt oder gemessen
    00596 //TODO abbrechen wenn geschätzt und rms > 1000 zwischendurch