From f347672610e54124d96501f6dd23cf6948c9754a Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Wed, 11 Jul 2018 13:58:13 +0900 Subject: [PATCH] prev_traj_tm -> traj_start_tm --- hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp | 6 +++--- hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h | 2 +- hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp | 8 ++++---- hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h | 2 +- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp index fe766207e..b94129315 100644 --- 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 index 605c8c3c4..875745991 100644 --- 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 joint_list; bool interpolationp; - ros::Time prev_traj_tm; + ros::Time traj_start_tm; public: typedef boost::shared_ptr Ptr; diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index 16a2289b6..03c8be9fa 100644 --- 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 index 00478cde1..3e80a2891 100644 --- 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;