Start joint trajectory at assigned timing

start-jsk · Jul 12, 2018 · 3d7c61e · 3d7c61e
1 parent f347672
commit 3d7c61e
Show file tree

Hide file tree

Showing 4 changed files with 70 additions and 9 deletions.
diff --git 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,15 +524,33 @@ 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
 
 void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onFollowJointTrajectoryActionGoal()
 {
   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
@@ -102,6 +102,11 @@ class HrpsysJointTrajectoryBridge : public RTC::DataFlowComponentBase
     std::string groupname;
     std::vector<std::string> 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
@@ -218,22 +218,33 @@ 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;
 }
 
 #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,13 +501,25 @@ 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;
       joint_trajectory_server.setSucceeded(result);
       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
@@ -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;