GT2005: Processes/CMD/Motion.cpp Source File

00001 /** 
00002 * @file Motion.cpp
00003 * Implementation of class Motion
00004 */
00005 #include "Motion.h"
00006  
00007 #include "Platform/Sensors.h"
00008 #include "Tools/RobotConfiguration.h"
00009 #include "Tools/Debugging/Stopwatch.h"
00010 
00011 /*
00012 * that's very fine - with this blocking sender
00013 * the robot can call for a new set of data and
00014 * decide itself when motion should be started
00015 */
00016 Motion::Motion () :
00017 INIT_DEBUGGING,
00018 INIT_RECEIVER_SENSORDATA(false),
00019 INIT_RECEIVER(PackageCognitionMotion,false),
00020 INIT_SENDER(Trace, false),
00021 INIT_RECEIVER(Trace, false),
00022 
00023 INIT_SENDER_MOTORCOMMANDS(true),
00024 INIT_SENDER_SOUNDDATA(false),
00025 INIT_SENDER(PackageMotionCognition,false),
00026 INIT_SENDER(OdometryData,false),
00027 
00028 gameSpeed(1.0),
00029 headIsBlockedBySpecialActionOrWalk(false),
00030 motionControlExecuteSlowMotion(1),
00031 traceStored(false)
00032 {
00033 #ifndef _WIN32
00034   setTrace(theTraceSender.getShared());
00035 #endif
00036   frameNumber = 0;
00037   debugIn.setSize(200000);
00038 //****for GT2005WalkingEngine****
00039   debugOut.setSize(20000); //2000
00040   
00041   SensorDataProcessorInterfaces sensorDataProcessorInterfaces(    
00042     theSensorDataBufferReceiver,theSensorDataBufferReceiver.frame[0].frameNumber,
00043     thePackageMotionCognitionSender.motionInfo,
00044     bodyPercept, bodyPosture, cameraMatrix, psdPercept);
00045   pSensorDataProcessor = new SensorDataProcessorSelector(moduleHandler, sensorDataProcessorInterfaces);
00046   // PRINT("Motion:: SensorDataProcessor created");
00047   
00048 
00049   HeadControlInterfaces headControlInterfaces(
00050     frameNumber,
00051     theSensorDataBufferReceiver, 
00052     cameraMatrix,
00053     theOdometryDataSender,
00054     bodyPosture,
00055     thePackageCognitionMotionReceiver.landmarksPercept,
00056     thePackageCognitionMotionReceiver.ballModel,
00057     thePackageCognitionMotionReceiver.robotPose,
00058     thePackageCognitionMotionReceiver.landmarksState,
00059     thePackageCognitionMotionReceiver.robotState,
00060     thePackageCognitionMotionReceiver.motionRequest,
00061     thePackageMotionCognitionSender.motionInfo,
00062     thePackageCognitionMotionReceiver.headControlMode,
00063     thePackageCognitionMotionReceiver.obstaclesModel,
00064     headIsBlockedBySpecialActionOrWalk,
00065     thePackageCognitionMotionReceiver.gameControlData,
00066     headMotionRequest,
00067     theMotorCommandsSender.pidData
00068   );
00069   pHeadControl = new HeadControlSelector(moduleHandler, headControlInterfaces);
00070   // PRINT("Motion:: HeadControl created");
00071   
00072   LEDControlInterfaces ledControlInterfaces(
00073     frameNumber,
00074     thePackageCognitionMotionReceiver.ledRequest,
00075     thePackageCognitionMotionReceiver.wLanStatus, 
00076     theMotorCommandsSender.ledValue);
00077   pLEDControl = new LEDControlSelector(moduleHandler, ledControlInterfaces);
00078   // PRINT("Motion:: LEDControl created");
00079   
00080   MotionControlInterfaces motionControlInterfaces(
00081     frameNumber,
00082     thePackageCognitionMotionReceiver.motionRequest, headMotionRequest, theSensorDataBufferReceiver, bodyPosture,
00083     thePackageCognitionMotionReceiver.invKinWalkingParameters,
00084     thePackageCognitionMotionReceiver.gt2004WalkingParameters,
00085     thePackageCognitionMotionReceiver.walkParameterTimeStamp,
00086     theSensorDataBufferReceiver.receivedNew(),
00087     theMotorCommandsSender.jointDataBuffer,
00088     theMotorCommandsSender.pidData,
00089     theOdometryDataSender,
00090     thePackageMotionCognitionSender.motionInfo,
00091     headIsBlockedBySpecialActionOrWalk,
00092   //****for GT2005WalkingEngine****
00093   thePackageCognitionMotionReceiver.gt2005Parameters,
00094   thePackageMotionCognitionSender.gt2005DebugData);
00095   pMotionControl = new MotionControlSelector(moduleHandler, motionControlInterfaces);
00096   // PRINT("Motion:: MotionControl created");
00097   
00098   SoundControlInterfaces soundControlInterfaces(
00099     thePackageCognitionMotionReceiver.soundRequest, theSoundDataSender);
00100   pSoundControl = new SoundControlSelector(moduleHandler, soundControlInterfaces);
00101   // PRINT("Motion:: SoundControl created");
00102 #ifndef NDEBUG
00103   replace = false;
00104 #endif
00105   
00106 }
00107 
00108 Motion::~Motion()
00109 {
00110   delete pSensorDataProcessor;
00111   delete pHeadControl;
00112   delete pLEDControl;
00113   delete pMotionControl;
00114   delete pSoundControl;
00115 }
00116 
00117 void Motion::init()
00118 {
00119 #ifndef _WIN32
00120   theTraceSender.send();
00121 #endif
00122 }
00123 
00124 int Motion::main() 
00125 {
00126   DEBUG_RESPONSE("Processes:Motion - reset odometry data", 
00127     theOdometryDataSender.rotation = 0; 
00128     theOdometryDataSender.translation.x = 0; 
00129     theOdometryDataSender.translation.y = 0;
00130   );
00131   if(theSensorDataBufferReceiver.receivedNew())
00132   {
00133     frameNumber = theSensorDataBufferReceiver.frame[0].frameNumber;
00134     DEBUG_RESPONSE("Processes:Motion - frameNumberInfo",
00135       OUTPUT(idText, text, frameNumber << 
00136       "(received sensorData: " << 
00137       theSensorDataBufferReceiver.numOfFrames << 
00138       " frames)" ); );
00139     // without STOP_TIME
00140     pSensorDataProcessor->execute();
00141   }
00142   else
00143   {
00144     frameNumber++;
00145     DEBUG_RESPONSE("Processes:Motion - frameNumberInfo",
00146       OUTPUT(idText, text, frameNumber);
00147     );
00148   }
00149 
00150   INFO(sendJointData,idJointData,bin,theMotorCommandsSender.jointDataBuffer.frame[0]);
00151   DEBUG_RESPONSE("send representation:motion:jointData", OUTPUT(idJointData,bin,theMotorCommandsSender.jointDataBuffer.frame[0]));
00152 
00153   DEBUG_RESPONSE("send representation:motion:joint and sensor data (from motion)", 
00154 
00155     static bool send = false;
00156     if (theSensorDataBufferReceiver.frame[0].data[SensorData::backF] > 10 )
00157       send = true;
00158     if (theSensorDataBufferReceiver.frame[0].data[SensorData::backR] > 10 ) 
00159       send = false;
00160 
00161     if(send)
00162     {
00163       OUTPUT(idSensorData,bin,theSensorDataBufferReceiver.frame[0]);
00164       OUTPUT(idJointData,bin,theMotorCommandsSender.jointDataBuffer.frame[0]);
00165     }   
00166     );
00167 
00168 
00169 
00170 #ifndef _WIN32
00171   if (
00172     SystemCall::getTimeSince(thePackageCognitionMotionReceiver.timeStamp) > 2000
00173     &&
00174     frameNumber > 3000 // don't swing if the Motion process is called the first time
00175     )
00176   {
00177     thePackageCognitionMotionReceiver.motionRequest.motionType = MotionRequest::specialAction;
00178     thePackageCognitionMotionReceiver.motionRequest.specialActionRequest.specialActionType = SpecialActionRequest::swing;
00179     OUTPUT(idText, text, "frameNumber: " << frameNumber << " Motion: no packages from Cognition for 2000ms");
00180     if(!traceStored && &theTraceReceiver.getShared())
00181     {
00182       OutTextFile stream("/MS/cognit.log");
00183       stream << theTraceReceiver.getShared();
00184       traceStored = true;
00185     }
00186   }
00187 
00188   if (thePackageCognitionMotionReceiver.motionRequest.updateRP)
00189   {
00190     if((lastFrameRobotPoseWasUpdated != thePackageCognitionMotionReceiver.robotPose.frameNumber) &&
00191     (frameNumber != thePackageCognitionMotionReceiver.robotPose.frameNumber))
00192     {
00193       int frameDifference = (int)(frameNumber - 
00194         thePackageCognitionMotionReceiver.robotPose.frameNumber);
00195 
00196        // OUTPUT(idText, text, "+ new rp! " << frameDifference << ", " << thePackageCognitionMotionReceiver.robotPose.frameNumber);
00197 
00198       thePackageCognitionMotionReceiver.robotPose += 
00199         (theOdometryDataSender - odometryHistory.getEntry(frameDifference + 1));
00200     } 
00201     else
00202     {
00203       thePackageCognitionMotionReceiver.robotPose += (theOdometryDataSender - lastOdometry);  
00204     }  
00205   lastOdometry = theOdometryDataSender;
00206   odometryHistory.add(theOdometryDataSender);
00207   lastFrameRobotPoseWasUpdated = thePackageCognitionMotionReceiver.robotPose.frameNumber;
00208   }
00209 
00210   // OUTPUT(idText, text, "[" << frameNumber << "] od: " << theOdometryDataSender.rotation << " rp: " << thePackageCognitionMotionReceiver.robotPose.rotation);
00211 #endif
00212 
00213   STOP_TIME_ON_REQUEST(headControl,pHeadControl->execute(););
00214   NSTOP_TIME_ON_REQUEST(headControl,pHeadControl->execute(););
00215   getPlayer().setTeamColor(thePackageCognitionMotionReceiver.teamColor);
00216   pLEDControl->execute();
00217  
00218   MODIFY("motion:motionRequest", thePackageCognitionMotionReceiver.motionRequest);
00219   MODIFY("motion:headMotionRequest", headMotionRequest);
00220 
00221   // slow down the motion request if requested
00222   thePackageCognitionMotionReceiver.motionRequest.walkRequest.walkParams.translation.x *= gameSpeed;
00223   thePackageCognitionMotionReceiver.motionRequest.walkRequest.walkParams.translation.y *= gameSpeed;
00224   thePackageCognitionMotionReceiver.motionRequest.walkRequest.walkParams.rotation *= gameSpeed;
00225   
00226   if(frameNumber % motionControlExecuteSlowMotion == 0){
00227     
00228     STOP_TIME_ON_REQUEST(motionControl,pMotionControl->execute(););
00229     NSTOP_TIME_ON_REQUEST(motionControl,pMotionControl->execute(););
00230     thePackageMotionCognitionSender.motionInfo.executedMotionRequest.setFrameNumber(frameNumber);
00231     
00232   }
00233 
00234   DEBUG_RESPONSE("Processes:Motion - tail shows quality of localisation",
00235     /* tail shows quality of localisation */  
00236     double maxStandardDeviation = 500;
00237     const long tailRangePan = 2094400;
00238     const long tailRangeTilt = 1134500;
00239     
00240     double relativeStandardDeviation = sqrt(thePackageCognitionMotionReceiver.robotPose.getPositionVariance())/maxStandardDeviation;
00241       if (relativeStandardDeviation > 1) 
00242           relativeStandardDeviation = 1;
00243     theMotorCommandsSender.jointDataBuffer.frame[0].data[JointData::tailTilt] = 1134500-(long)(tailRangeTilt*relativeStandardDeviation);
00244     theMotorCommandsSender.jointDataBuffer.frame[0].data[JointData::tailPan] = (-1047200 + (long)(tailRangePan*relativeStandardDeviation));
00245   );
00246   
00247   theMotorCommandsSender.send();
00248   
00249   INFO(sendMotionInfo, idMotionInfo, bin, thePackageMotionCognitionSender.motionInfo);
00250   DEBUG_RESPONSE("send representation:motion:motionInfo", OUTPUT(idMotionInfo, bin, thePackageMotionCognitionSender.motionInfo));
00251 
00252   INFO(sendBodyPosture, idBodyPosture, bin, bodyPosture);
00253   DEBUG_RESPONSE("send representation:motion:bodyPosture", OUTPUT(idBodyPosture, bin, bodyPosture));
00254   
00255 #ifndef WIN32
00256   if(theSoundDataSender.requestedNew())
00257 #endif
00258   {
00259     STOP_TIME_ON_REQUEST(soundControl,pSoundControl->execute(););
00260     NSTOP_TIME_ON_REQUEST(soundControl,pSoundControl->execute(););
00261     if (theSoundDataSender.isInUse) theSoundDataSender.send();            
00262   }
00263 
00264   DEBUG_RESPONSE("automated requests:StreamSpecification", OUTPUT(idStreamSpecification, bin, getStreamHandler()));
00265 
00266   // speed up the executed motion request again
00267   thePackageMotionCognitionSender.motionInfo.executedMotionRequest.walkRequest.walkParams.translation.x /= gameSpeed;
00268   thePackageMotionCognitionSender.motionInfo.executedMotionRequest.walkRequest.walkParams.translation.x /= gameSpeed;
00269   thePackageMotionCognitionSender.motionInfo.executedMotionRequest.walkRequest.walkParams.rotation /= gameSpeed;
00270   thePackageMotionCognitionSender.timeStamp = SystemCall::getCurrentSystemTime();
00271 
00272   theOdometryDataSender.send();
00273   thePackageMotionCognitionSender.send();
00274   theDebugSender.send();
00275   
00276   return 0;
00277 }
00278 
00279 
00280 bool Motion::handleMessage(InMessage& message)
00281 {
00282   switch (message.getMessageID())
00283   {
00284   case idDebugDataChangeRequest:
00285     return debugDataTable.processChangeRequest( message.bin);
00286   case idOdometryData:
00287     message.bin >> theOdometryDataSender;
00288     return true;
00289   case idHeadMotionRequest:
00290     message.bin >> headMotionRequest;
00291     return true;
00292   case idPIDData:
00293     message.bin >> theMotorCommandsSender.pidData;
00294     return true;
00295   case idGameSpeed:
00296     message.bin >> gameSpeed;
00297     return true;
00298   case idMotionControlSlowMotion:
00299     {
00300       double newMCSLowMo;
00301       message.bin >> newMCSLowMo;
00302       if(newMCSLowMo>=1){
00303         motionControlExecuteSlowMotion = (unsigned  int)newMCSLowMo;
00304       }
00305     }
00306     return true;
00307   case idBodyOffsets:
00308     getRobotConfiguration().handleMessage(message);
00309     return true;
00310   case idXabsl2DebugRequest:
00311     return pHeadControl->handleMessage(message);
00312   case idGT2005Request:
00313     return pMotionControl->handleMessage(message);
00314   case idOdometryScale:
00315     pMotionControl->handleMessage(message);
00316     return true;
00317   default:
00318     return Process::handleMessage(message);
00319   }
00320 }
00321 
00322 MAKE_PROCESS(Motion);