prev_traj_tm -> traj_start_tm

start-jsk · Jul 11, 2018 · f347672 · f347672
1 parent c54fe61
commit f347672
Show file tree

Hide file tree

Showing 4 changed files with 9 additions and 9 deletions.
diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
@@ -330,7 +330,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc()
 
   // 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 - prev_traj_tm;
+  commanded_joint_trajectory_point.time_from_start = tm_on_execute - traj_start_tm;
   commanded_joint_trajectory_point.positions.resize(joint_list.size());
   commanded_joint_trajectory_point.velocities.resize(joint_list.size());
   commanded_joint_trajectory_point.accelerations.resize(joint_list.size());
@@ -342,7 +342,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc()
       commanded_joint_trajectory_point.accelerations[j] = parent->body->link(joint_list[j])->ddq;
       commanded_joint_trajectory_point.effort[j]        = parent->body->link(joint_list[j])->u;
     }
-  error_joint_trajectory_point.time_from_start = tm_on_execute - prev_traj_tm;
+  error_joint_trajectory_point.time_from_start = tm_on_execute - traj_start_tm;
   error_joint_trajectory_point.positions.resize(joint_list.size());
   error_joint_trajectory_point.velocities.resize(joint_list.size());
   error_joint_trajectory_point.accelerations.resize(joint_list.size());
@@ -504,7 +504,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
       parent->m_service0->playPattern(angles, rpy, zmp, duration);
     }
   }
-  prev_traj_tm = ros::Time::now();
+  traj_start_tm = ros::Time::now();
 
   interpolationp = true;
 }

diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h
@@ -102,7 +102,7 @@ class HrpsysJointTrajectoryBridge : public RTC::DataFlowComponentBase
     std::string groupname;
     std::vector<std::string> joint_list;
     bool interpolationp;
-    ros::Time prev_traj_tm;
+    ros::Time traj_start_tm;
 
   public:
     typedef boost::shared_ptr<jointTrajectoryActionObj> Ptr;

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -218,7 +218,7 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory
     OpenHRP::dSequenceSequence rpy, zmp;
     m_service0->playPattern(angles, rpy, zmp, duration);
   }
-  prev_traj_tm = ros::Time::now();
+  traj_start_tm = ros::Time::now();
 
   interpolationp = true;
 }
@@ -443,9 +443,9 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
         //joint_state.velocity
         //joint_state.effort
         follow_joint_trajectory_feedback.joint_names.push_back(j->name);
-        follow_joint_trajectory_feedback.desired.time_from_start = tm_on_execute - prev_traj_tm;
-        follow_joint_trajectory_feedback.actual.time_from_start = tm_on_execute - prev_traj_tm;
-        follow_joint_trajectory_feedback.error.time_from_start = tm_on_execute - prev_traj_tm;
+        follow_joint_trajectory_feedback.desired.time_from_start = tm_on_execute - traj_start_tm;
+        follow_joint_trajectory_feedback.actual.time_from_start = tm_on_execute - traj_start_tm;
+        follow_joint_trajectory_feedback.error.time_from_start = tm_on_execute - traj_start_tm;
         follow_joint_trajectory_feedback.desired.positions.push_back(j->q);
         follow_joint_trajectory_feedback.actual.positions.push_back(j->q);
         follow_joint_trajectory_feedback.error.positions.push_back(0);

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
@@ -87,7 +87,7 @@ class HrpsysSeqStateROSBridge  : public HrpsysSeqStateROSBridgeImpl
   void clock_cb(const rosgraph_msgs::ClockPtr& str) {};
 
   bool follow_action_initialized;
-  ros::Time prev_traj_tm;
+  ros::Time traj_start_tm;
 
   boost::mutex tf_mutex;
   double tf_rate;