diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp index b94129315..332133940 100644 --- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp @@ -307,8 +307,15 @@ HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::~jointTrajectoryActionObj void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc() { - // finish interpolation + ros::Time tm_on_execute = ros::Time::now(); + #ifdef USE_PR2_CONTROLLERS_MSGS + // start postponed joint trajectory + if (joint_trajectory_server->isActive() && interpolationp == false && pr2_traj_start_tm <= tm_on_execute) + { + onJointTrajectory(postponed_pr2_traj); + } + // finish interpolation if (joint_trajectory_server->isActive() && interpolationp == true && parent->m_service0->isEmpty() == true) { pr2_controllers_msgs::JointTrajectoryResult result; @@ -317,6 +324,12 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc() ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @proc joint_trajectory_server->setSucceeded()"); } #endif + // start postponed joint trajectory + if (follow_joint_trajectory_server->isActive() && interpolationp == false && traj_start_tm <= tm_on_execute) + { + onJointTrajectory(postponed_traj); + } + // finish interpolation if (follow_joint_trajectory_server->isActive() && interpolationp == true && parent->m_service0->isEmpty() == true) { control_msgs::FollowJointTrajectoryResult result; @@ -326,8 +339,6 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc() ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @proc follow_joint_trajectory_server->setSucceeded()"); } - ros::Time tm_on_execute = ros::Time::now(); - // FIXME: need to set actual informatoin, currently we set dummy information trajectory_msgs::JointTrajectoryPoint commanded_joint_trajectory_point, error_joint_trajectory_point; commanded_joint_trajectory_point.time_from_start = tm_on_execute - traj_start_tm; @@ -504,7 +515,6 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory( parent->m_service0->playPattern(angles, rpy, zmp, duration); } } - traj_start_tm = ros::Time::now(); interpolationp = true; } @@ -514,7 +524,16 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectoryAct { ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryActionGoal"); pr2_controllers_msgs::JointTrajectoryGoalConstPtr goal = joint_trajectory_server->acceptNewGoal(); - onJointTrajectory(goal->trajectory); + pr2_traj_start_tm = goal->trajectory.header.stamp; + if (pr2_traj_start_tm <= ros::Time::now()) + { + onJointTrajectory(goal->trajectory); + } + else + { + // trajectory is postponed as it is in future + postponed_pr2_traj = goal->trajectory; + } } #endif @@ -522,7 +541,16 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onFollowJointTraject { ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onFollowJointTrajectoryActionGoal()"); control_msgs::FollowJointTrajectoryGoalConstPtr goal = follow_joint_trajectory_server->acceptNewGoal(); - onJointTrajectory(goal->trajectory); + traj_start_tm = goal->trajectory.header.stamp; + if (traj_start_tm <= ros::Time::now()) + { + onJointTrajectory(goal->trajectory); + } + else + { + // trajectory is postponed as it is in future + postponed_traj = goal->trajectory; + } } #ifdef USE_PR2_CONTROLLERS_MSGS diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h index 875745991..6a5018069 100644 --- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h +++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h @@ -102,6 +102,11 @@ class HrpsysJointTrajectoryBridge : public RTC::DataFlowComponentBase std::string groupname; std::vector joint_list; bool interpolationp; +#ifdef USE_PR2_CONTROLLERS_MSGS + trajectory_msgs::JointTrajectory postponed_pr2_traj; + ros::Time pr2_traj_start_tm; +#endif + trajectory_msgs::JointTrajectory postponed_traj; ros::Time traj_start_tm; public: diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index 03c8be9fa..01533c00e 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -218,7 +218,6 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory OpenHRP::dSequenceSequence rpy, zmp; m_service0->playPattern(angles, rpy, zmp, duration); } - traj_start_tm = ros::Time::now(); interpolationp = true; } @@ -226,14 +225,26 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory #ifdef USE_PR2_CONTROLLERS_MSGS void HrpsysSeqStateROSBridge::onJointTrajectoryActionGoal() { pr2_controllers_msgs::JointTrajectoryGoalConstPtr goal = joint_trajectory_server.acceptNewGoal(); - onJointTrajectory(goal->trajectory); + pr2_traj_start_tm = goal->trajectory.header.stamp; + if ( pr2_traj_start_tm <= ros::Time::now() ) { + onJointTrajectory(goal->trajectory); + } else { + // trajectory is postponed as it is in future + postponed_pr2_traj = goal->trajectory; + } } #endif void HrpsysSeqStateROSBridge::onFollowJointTrajectoryActionGoal() { control_msgs::FollowJointTrajectoryGoalConstPtr goal = follow_joint_trajectory_server.acceptNewGoal(); follow_action_initialized = true; - onJointTrajectory(goal->trajectory); + traj_start_tm = goal->trajectory.header.stamp; + if ( traj_start_tm <= ros::Time::now() ) { + onJointTrajectory(goal->trajectory); + } else { + // trajectory is postponed as it is in future + postponed_traj = goal->trajectory; + } } #ifdef USE_PR2_CONTROLLERS_MSGS @@ -490,6 +501,12 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id) updateSensorTransform(sensor_tf_stamp); // transform depends on joint angles, not sensor values #ifdef USE_PR2_CONTROLLERS_MSGS + // start postponed joint trajectory + if ( joint_trajectory_server.isActive() && + interpolationp == false && pr2_traj_start_tm <= tm_on_execute ) { + onJointTrajectory(postponed_pr2_traj); + } + // finish interpolation if ( joint_trajectory_server.isActive() && interpolationp == true && m_service0->isEmpty() == true ) { pr2_controllers_msgs::JointTrajectoryResult result; @@ -497,6 +514,12 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id) interpolationp = false; } #endif + // start postponed joint trajectory + if ( follow_joint_trajectory_server.isActive() && + interpolationp == false && traj_start_tm <= tm_on_execute ) { + onJointTrajectory(postponed_traj); + } + // finish interpolation if ( follow_joint_trajectory_server.isActive() && interpolationp == true && m_service0->isEmpty() == true ) { control_msgs::FollowJointTrajectoryResult result; diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h index 3e80a2891..41a61378e 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h @@ -87,6 +87,11 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl void clock_cb(const rosgraph_msgs::ClockPtr& str) {}; bool follow_action_initialized; +#ifdef USE_PR2_CONTROLLERS_MSGS + trajectory_msgs::JointTrajectory postponed_pr2_traj; + ros::Time pr2_traj_start_tm; +#endif + trajectory_msgs::JointTrajectory postponed_traj; ros::Time traj_start_tm; boost::mutex tf_mutex;