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

# Processes/CMD/Motion.cpp

Go to the documentation of this 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);