bool MotionPlayer::init(robotcontrol::RobotModel* model) { if (! robotcontrol::MotionModule::init(model)) return false; fs::path p((ros::package::getPath("launch") + "/motions")); if (! loadMotionFiles(p)) return false; m_model = model; m_state_prone = m_model->registerState("prone"); m_state_supine = m_model->registerState("supine"); m_state_init = m_model->registerState("init"); m_state_relaxed = m_model->registerState("relaxed"); initMotions(); m_cmdPositions.resize(model->numJoints()); m_cmdEffort.resize(model->numJoints()); /* * Change the initial Pose of the Robot from standing (zero-positions) to * a sitting pose */ motion_player::PlayMotion initPose; initPose.request.name = "INIT_POSE"; handlePlay(initPose.request, initPose.response); return true; }
// Handle messages for this resource. UtlBoolean MprFromStream::handleMessage(MpFlowGraphMsg& rMsg) { UtlBoolean bHandled = FALSE ; switch (rMsg.getMsg()) { case SOURCE_RENDER: bHandled = handleRender((MpStreamFeeder*) rMsg.getPtr1()) ; break ; case SOURCE_PLAY: bHandled = handlePlay((MpStreamFeeder*) rMsg.getPtr1()) ; break ; case SOURCE_REWIND: bHandled = handleRewind((MpStreamFeeder*) rMsg.getPtr1()) ; break ; case SOURCE_PAUSE: bHandled = handlePause((MpStreamFeeder*) rMsg.getPtr1()) ; break ; case SOURCE_STOP: bHandled = handleStop((MpStreamFeeder*) rMsg.getPtr1()) ; break ; case SOURCE_DESTROY: bHandled = handleDestroy((MpStreamFeeder*) rMsg.getPtr1()) ; break ; default: bHandled = MpResource::handleMessage(rMsg); break; } return bHandled ; }
bool MotionPlayer::isTriggered() { if (isPlaying) return true; if (model()->state() == m_state_relaxed && m_isInitialized) { m_isInitialized = false; motion_player::PlayMotion init_pose; init_pose.request.name = "INIT_POSE"; handlePlay(init_pose.request, init_pose.response); } if(model()->state() == m_state_init && ! m_isInitialized) { m_isInitialized = true; std::string initMotionName = "INIT"; motion_player::PlayMotion initMotion; initMotion.request.name = initMotionName; handlePlay(initMotion.request, initMotion.response); } if (model()->state() == m_state_prone || model()->state() == m_state_supine) { m_isInitialized = false; robotcontrol::FadeTorqueGoal goal; goal.torque = 1.0; m_torqueAct.cancelAllGoals(); m_torqueAct.sendGoal(goal); motion_player::PlayMotion getUp; if (model()->state() == m_state_prone) getUp.request.name = "PRONE_GETUP"; else getUp.request.name = "SUPINE_GETUP"; handlePlay(getUp.request, getUp.response); } return false; }