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

# Modules/BehaviorControl/MotionRatingBehaviorControl/MotionRatingBehaviorControl.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file MotionRatingBehaviorControl.cpp
    00003 *
    00004 * Implementation of class MotionRatingBehaviorControl.
    00005 *
    00006 * @author Dimitri Idessis
    00007 * @author Eduardo Tsen
    00008 * @author Sebastian Petters
    00009 * @author Dirk Thomas
    00010 */
    00011
    00012 #include "MotionRatingBehaviorControl.h"
    00013
    00014 MotionRatingBehaviorControl::MotionRatingBehaviorControl(BehaviorControlInterfaces& interfaces)
    00015 : BehaviorControl(interfaces)
    00016 {
    00017   showMessages = false;
    00018
    00019   lastMessage = 0;
    00020   pingTimeout = 3000;
    00021   reset();
    00022   path = 0;
    00023   pathOffset = 0;
    00024   startlocalise = 0;
    00025   startrotate = 0;
    00026   state = sleep;
    00027
    00028   distTolerance = 100.0;
    00029   angleTolerance = 0.1;
    00030
    00031 }
    00032
    00033 MotionRatingBehaviorControl::~MotionRatingBehaviorControl()
    00034 {
    00035 }
    00036
    00037 bool MotionRatingBehaviorControl::handleMessage(InMessage& msg)
    00038 {
    00039   if ( msg.getMessageID() != idMotionRatingBehaviorMessage ) return false;
    00040
    00041   MotionRatingBehaviorMessage message;
    00042   msg.bin >> message;
    00043
    00044   Vector2<double> v;
    00045
    00046   switch ( message.type )
    00047   {
    00048     case MotionRatingBehaviorMessage::init:
    00049       OUTPUT(idText,text,"BEHA: 'init' received");
    00050       path = message.data;
    00051       paramSetId = 0;
    00052       walkCount = 0;
    00053       currentWalk = 0;
    00054
    00055       track.start.translation = message.start;
    00056       track.end.translation = message.end;
    00057       v = track.end.translation - track.start.translation;
    00058       track.start.rotation = normalize ( v.angle() );
    00059       v = track.start.translation - track.end.translation;
    00060       track.end.rotation = normalize ( v.angle() );
    00061
    00062       setPoints();
    00063       startlocalise = SystemCall::getCurrentSystemTime();
    00064       state = localise;
    00065       break;
    00066     case MotionRatingBehaviorMessage::request:
    00067       OUTPUT(idText,text,"BEHA: 'request' received");
    00068       paramSetId = message.data;
    00069       walkCount = message.walkCount;
    00070       currentWalk = 0;
    00071       setPoints();
    00072       startlocalise = SystemCall::getCurrentSystemTime();
    00073       state = localise;
    00074       break;
    00075     case MotionRatingBehaviorMessage::abort:
    00076       paramSetId = 0;
    00077       state = wait;
    00078       break;
    00079     default:
    00080       OUTPUT ( idText, text, "unhandled message received" );
    00081   }
    00082
    00083   MotionRatingBehaviorMessage outmessage;
    00084
    00085   outmessage.type = MotionRatingBehaviorMessage::ping;
    00086   outmessage.state = encodeState();
    00087   outmessage.paramSetId = paramSetId;
    00088   outmessage.data = 0;
    00089   outmessage.currentSpeed = 0.0;
    00090   outmessage.averageSpeed = 0.0;
    00091   outmessage.currentWalk = currentWalk; // + 1;
    00092   outmessage.walkCount = walkCount;
    00093   outmessage.remainingPower = SystemCall::getRemainingPower();
    00094
    00095   OUTPUT ( idMotionRatingBehaviorMessage, bin, outmessage );
    00096
    00097   lastMessage = SystemCall::getCurrentSystemTime();
    00098
    00099   return true;
    00100 }
    00101
    00102
    00103 void MotionRatingBehaviorControl::execute()
    00104 {
    00105   /////////////////////////////
    00106     char message[100];
    00107     //if ( showMessages ) sprintf ( message,"(%.2f,%.2f) -> %.2f\r\n\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation);
    00108     //if ( showMessages ) OUTPUT(idText,text, message );
    00109   /////////////////////////////
    00110
    00111   headControlMode.headControlMode = HeadControlMode::searchForLandmarks;
    00112
    00113   bool notify = false;
    00114
    00115   switch (state)
    00116   {
    00117   case sleep:
    00118     // sleep and wait for start message
    00119     if ( showMessages ) sprintf ( message,"sleep (at %.2f,%.2f,%.2f)\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation);
    00120     playDead();
    00121     break;
    00122   case localise:
    00123     // stand until localisation is ok before positioning
    00124     {
    00125       bool localized = doLocalise();
    00126       if ( SystemCall::getTimeSince(startlocalise) > 1000 && localized )
    00127       {
    00128         state = prepare;
    00129       }
    00130     }
    00131     break;
    00132   case prepare:
    00133     // go to starting point
    00134     if ( showMessages ) sprintf ( message,"prepare (at %.2f,%.2f,%.2f) [rotate to %.2f]\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation,startpoint.rotation);
    00135     if ( gotoPoint ( startpoint, true ) )
    00136     {
    00137       notify = true;
    00138       startlocalise = SystemCall::getCurrentSystemTime();
    00139       state = ( paramSetId != 0 ) ? localisepre : wait;
    00140     }
    00141     // abort if localisation lost
    00142     if ( robotPose.getValidity() < 0.1 )
    00143     {
    00144       stand();
    00145       state = localise;
    00146     }
    00147     break;
    00148   case wait:
    00149     // stand and wait for parameters
    00150     headControlMode.headControlMode = HeadControlMode::lookStraightAhead;
    00151     if ( showMessages ) sprintf ( message,"wait (at %.2f,%.2f,%.2f)\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation);
    00152     stand();
    00153     state = ( paramSetId != 0 ) ? localise : wait;
    00154     break;
    00155   case localisepre:
    00156     {
    00157       // stand until localisation is ok before testing
    00158       if ( showMessages ) sprintf ( message,"localisepre (at %.2f,%.2f,%.2f)\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation);
    00159       bool localized = doLocalise();
    00160       if ( SystemCall::getTimeSince(startlocalise) > 1000 && localized )
    00161       {
    00162         notify = true;
    00163         p1 = robotPose.translation;
    00164         currentWalk++;
    00165         state = walk;
    00166         starttime = SystemCall::getCurrentSystemTime();
    00167       }
    00168     }
    00169     break;
    00170   case walk:
    00171     // test a parameter set
    00172     if ( showMessages ) sprintf ( message,"walk (at %.2f,%.2f,%.2f) [from %.2f,%.2f,%.2f to %.2f,%.2f,%.2f]\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation,startpoint.translation.x,startpoint.translation.y,startpoint.rotation,endpoint.translation.x,endpoint.translation.y,endpoint.rotation);
    00173     if ( gotoPoint ( endpoint.translation, false ) || ( SystemCall::getTimeSince(starttime) > 20000 ))
    00174     {
    00175       stand();
    00176       state = stop;
    00177     }
    00178     // abort if localisation lost
    00179     if ( robotPose.getValidity() < 0.1 )
    00180     {
    00181       stand();
    00182       state = stop;
    00183     }
    00184     break;
    00185   case stop:
    00186     // stop and wait until robot has stopped
    00187     if ( showMessages ) sprintf ( message,"stop (at %.2f,%.2f,%.2f)\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation);
    00188     stand();
    00189     if ( motionInfo.executedMotionRequest.motionType == MotionRequest::stand )
    00190     {
    00191       endtime = SystemCall::getCurrentSystemTime();
    00192       startlocalise = SystemCall::getCurrentSystemTime();
    00193       state = localiseaft;
    00194     }
    00195     break;
    00196   case localiseaft:
    00197     {
    00198       // stand until localisation is ok after testing
    00199       if ( showMessages ) sprintf ( message,"localiseaft (at %.2f,%.2f,%.2f)\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation);
    00200       bool localized = doLocalise();
    00201       if ( SystemCall::getTimeSince(startlocalise) > 1000 && localized )
    00202       {
    00203         p2 = robotPose.translation;
    00204         state = report;
    00205       }
    00206     }
    00207     break;
    00208   case report:
    00209     // measure speed and report results
    00210     if ( showMessages ) sprintf ( message,"report (at %.2f,%.2f,%.2f)\r\n",robotPose.translation.x,robotPose.translation.y,robotPose.rotation);
    00211     {
    00212       double distance = (p2 - p1).abs();
    00213       unsigned long time = (endtime - starttime);
    00214
    00215       totalDistance += distance;
    00216       totalTime += time;
    00217
    00218       /*
    00219       char message[50];
    00220       sprintf ( message, "current: t: %.2fs d: %.2fm v: %.2fm/s",
    00221         time/1000.0,
    00222         distance/1000.0,
    00223         100.0*(distance/time)
    00224       );
    00225       OUTPUT(idText,text, message );
    00226       sprintf ( message, "total: t: %.2fs d: %.2fm v: %.2fm/s",
    00227         totalTime/1000.0,
    00228         totalDistance/1000.0,
    00229         100.0*(totalDistance/totalTime)
    00230       );
    00231       OUTPUT(idText,text, message );
    00232       */
    00233
    00234       //currentWalk++;
    00235       bool finished = walkCount <= currentWalk;
    00236
    00237       pathOffset = 1 - pathOffset;
    00238       setPoints();
    00239       state = prepare;
    00240
    00241       MotionRatingBehaviorMessage message;
    00242       message.remainingPower = SystemCall::getRemainingPower();
    00243       message.type = MotionRatingBehaviorMessage::report;
    00244       message.state = encodeState();
    00245       message.paramSetId = paramSetId;
    00246       message.currentSpeed = distance / time;
    00247       message.averageSpeed = totalDistance / totalTime;
    00248       message.currentWalk = currentWalk; // + 1;
    00249       message.walkCount = walkCount;
    00250       message.finished = finished;
    00251       message.distance=distance;
    00252       message.time=time;
    00253
    00254       OUTPUT ( idMotionRatingBehaviorMessage, bin, message );
    00255       lastMessage = SystemCall::getCurrentSystemTime();
    00256
    00257       if ( finished ) {
    00258         reset();
    00259         // state = wait;
    00260       }
    00261     }
    00262     break;
    00263   default:
    00264     OUTPUT(idText,text,"unknown state");
    00265     break;
    00266   }
    00267   // debug output current state and position and optionally target info
    00268   if ( showMessages ) OUTPUT(idText,text, message );
    00269
    00270   if ( notify || ( SystemCall::getTimeSince(lastMessage) > pingTimeout ) )
    00271   {
    00272     MotionRatingBehaviorMessage message;
    00273     message.type = MotionRatingBehaviorMessage::ping;
    00274     message.state = encodeState();
    00275     message.paramSetId = paramSetId;
    00276     message.currentWalk = currentWalk; // + 1;
    00277     message.walkCount = walkCount;
    00278     message.data = 0;
    00279     message.remainingPower = SystemCall::getRemainingPower();
    00280
    00281     OUTPUT ( idMotionRatingBehaviorMessage, bin, message );
    00282
    00283     lastMessage = SystemCall::getCurrentSystemTime();
    00284   }
    00285
    00286 }
    00287
    00288 void MotionRatingBehaviorControl::reset()
    00289 {
    00290   paramSetId = 0;
    00291   currentWalk = 0;
    00292   walkCount = 0;
    00293   totalDistance = 0.0;
    00294   totalTime = 0;
    00295   paused = false;
    00296 }
    00297
    00298 void MotionRatingBehaviorControl::setPoints()
    00299 {
    00300   startpoint = ( pathOffset == 0 ) ? track.start : track.end;
    00301   endpoint = ( pathOffset == 0 ) ? track.end : track.start;
    00302 }
    00303
    00304 bool MotionRatingBehaviorControl::gotoPoint ( Pose2D pose, bool safe )
    00305 {
    00306   char message[50];
    00307
    00308
    00309   Vector2<double> v = Pose2D( -robotPose.rotation ) * ( pose.translation - robotPose.translation );
    00310
    00311   if ( showMessages ) sprintf ( message, "Vektor v: (%.2f,%.2f)",v.x,v.y);
    00312   if ( showMessages ) OUTPUT(idText,text, message );
    00313
    00314   double dist = v.abs();
    00315   double theta = normalize ( v.angle() );
    00316   double phi = normalize ( pose.getAngle() - robotPose.getAngle() );
    00317
    00318   if ( showMessages ) sprintf ( message, "dist: %.2f",dist);
    00319   if ( showMessages ) OUTPUT(idText,text, message );
    00320   if ( showMessages ) sprintf ( message, "theta: %.2f",theta);
    00321   if ( showMessages ) OUTPUT(idText,text, message );
    00322   if ( showMessages ) sprintf ( message, "phi: %.2f",phi);
    00323   if ( showMessages ) OUTPUT(idText,text, message );
    00324
    00325
    00326
    00327   double rotationDist = 600;
    00328   double t = 1.0;
    00329   if ( dist < rotationDist && dist > distTolerance ) t = ( dist - distTolerance ) / ( rotationDist - distTolerance );
    00330   if ( dist <= distTolerance ) t = 0.0;
    00331   double alpha = t * theta + (1 - t) * phi;
    00332
    00333   if ( showMessages ) sprintf ( message, "alpha: %.2f\r\n",alpha);
    00334   if ( showMessages ) OUTPUT(idText,text, message );
    00335
    00336   setWalkMotionRequest ( dist, theta, alpha, safe );
    00337
    00338   return dist < distTolerance && fabs ( phi ) < angleTolerance;
    00339 }
    00340
    00341 bool MotionRatingBehaviorControl::gotoPoint ( Vector2<double> point, bool safe )
    00342 {
    00343   char message[50];
    00344
    00345   Vector2<double> v = Pose2D( -robotPose.rotation ) * ( point - robotPose.translation );
    00346
    00347   if ( showMessages ) sprintf ( message, "Vektor v: (%.2f,%.2f)",v.x,v.y);
    00348   if ( showMessages ) OUTPUT(idText,text, message );
    00349
    00350   double dist = v.abs();
    00351   double theta = normalize ( v.angle() );
    00352
    00353   if ( showMessages ) sprintf ( message, "dist: %.2f",dist);
    00354   if ( showMessages ) OUTPUT(idText,text, message );
    00355   if ( showMessages ) sprintf ( message, "theta: %.2f",theta);
    00356   if ( showMessages ) OUTPUT(idText,text, message );
    00357
    00358   setWalkMotionRequest ( walkMaxForwardSpeed, theta, theta, safe );
    00359
    00360   return dist < distTolerance;
    00361 }
    00362
    00363 void MotionRatingBehaviorControl::setWalkMotionRequest ( double distance, double theta, double phi, bool safe )
    00364 {
    00365   double speed = distance > walkMaxForwardSpeed ? walkMaxForwardSpeed : distance;
    00366
    00367   if ( phi >  walkMaxRotationSpeed ) phi =  walkMaxRotationSpeed;
    00368   if ( phi < -walkMaxRotationSpeed ) phi = -walkMaxRotationSpeed;
    00369
    00370   motionRequest.motionType = MotionRequest::walk;
    00371   motionRequest.walkRequest.walkType = safe ? WalkRequest::normal : WalkRequest::normal;
    00372   motionRequest.walkRequest.walkParams.rotation = phi;
    00373   motionRequest.walkRequest.walkParams.translation.x = cos ( theta ) * speed;
    00374   motionRequest.walkRequest.walkParams.translation.y = sin ( theta ) * speed;
    00375 }
    00376
    00377 bool MotionRatingBehaviorControl::stand()
    00378 {
    00379   motionRequest.motionType = MotionRequest::stand;
    00380   return (motionInfo.executedMotionRequest.motionType==MotionRequest::stand);
    00381 }
    00382
    00383 void MotionRatingBehaviorControl::playDead()
    00384 {
    00385   motionRequest.motionType = MotionRequest::playDead;
    00386 }
    00387
    00388
    00389 bool MotionRatingBehaviorControl::doLocalise()
    00390 {
    00391   /*
    00392   char message[50];
    00393   sprintf ( message, "pos: x:%.2f, y:%.2f phi:%.2f s:%.4f",
    00394     robotPose.translation.x,
    00395     robotPose.translation.y,
    00396     robotPose.rotation,
    00397     robotPose.getValidity()
    00398   );
    00399   OUTPUT(idText,text, message );
    00400   */
    00401   //headControlMode.headControlMode = HeadControlMode::optimizerMode;
    00402   if (!stand()) return false;
    00403   return robotPose.getValidity() > 0.90;
    00404 }
    00405
    00406 MotionRatingBehaviorMessage::States MotionRatingBehaviorControl::encodeState()
    00407 {
    00408   switch (state)
    00409   {
    00410     case sleep:
    00411       return MotionRatingBehaviorMessage::sleeping;
    00412     case wait:
    00413       return MotionRatingBehaviorMessage::waiting;
    00414     case localise:
    00415     case prepare:
    00416       return ( paramSetId != 0 ) ? MotionRatingBehaviorMessage::testing : MotionRatingBehaviorMessage::preparing;
    00417     case localisepre:
    00418     case walk:
    00419     case stop:
    00420     case localiseaft:
    00421     case report:
    00422       return MotionRatingBehaviorMessage::testing;
    00423     default:
    00424       return MotionRatingBehaviorMessage::unknown;
    00425   }
    00426 }
    00427
    00428
    00429 //bool MotionRatingBehaviorControl::goTo ( Pose2D target /*Vector2<double> point */)
    00430 /*{
    00431   if ((target.translation-robotPose.translation).abs()<distTolerance)
    00432   {
    00433    return true;
    00434   }
    00435   else
    00436   {
    00437         double theta=robotPose.rotation-target.rotation;
    00438         if (fabs(theta)<angleTolerance)
    00439         {
    00440           theta=0.0;
    00441         }
    00442         motionRequest.motionType = MotionRequest::walk;
    00443         motionRequest.walkRequest.walkType = WalkRequest::normal;
    00444         motionRequest.walkRequest.walkParams = Pose2D(theta,walkMaxForwardSpeed,0);
    00445     return false;
    00446   }
    00447 }
    00448
    00449 bool MotionRatingBehaviorControl::rotateTo ( double angle )
    00450 {
    00451       if (startrotate==0) startrotate=SystemCall::getCurrentSystemTime();
    00452       double theta=robotPose.rotation-angle;
    00453       if (fabs(theta)<angleTolerance
    00454         || ( SystemCall::getTimeSince(startlocalise) > 10000 ))
    00455       {
    00456         startrotate=0;
    00457         return true;
    00458       }
    00459       else
    00460       {
    00461         motionRequest.motionType = MotionRequest::walk;
    00462         motionRequest.walkRequest.walkType = WalkRequest::normal;
    00463         //if (fabs(theta)>pi_2) theta=(theta<0.0)?-pi_2:pi_2;
    00464         motionRequest.walkRequest.walkParams = Pose2D(theta,0,0);
    00465         return false;
    00466       }
    00467
    00468 }*/
    00469
    00470
    00471
    00472 /*
    00473 * Change log :
    00474 *
    00475 * $Log: MotionRatingBehaviorControl.cpp,v $
    00476 * Revision 1.1  2004/03/17 16:18:49  thomas
    00477 * added preversion of motion optimisation with behaviour, selflocator, headcontrol and robotcontrol dialog
    00478 *
    00479 *
    00480 */