From ab0f0df13123bb803c3fc6c813b0743313ed1edc Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Mon, 9 Jul 2018 20:49:45 +0900 Subject: [PATCH 1/3] Set time_from_start to feedback of joint trajectory action --- hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp | 3 +++ hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h | 1 + hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp | 4 ++++ hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h | 1 + 4 files changed, 9 insertions(+) diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp index 336917762..fe766207e 100644 --- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp @@ -330,6 +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.positions.resize(joint_list.size()); commanded_joint_trajectory_point.velocities.resize(joint_list.size()); commanded_joint_trajectory_point.accelerations.resize(joint_list.size()); @@ -341,6 +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.positions.resize(joint_list.size()); error_joint_trajectory_point.velocities.resize(joint_list.size()); error_joint_trajectory_point.accelerations.resize(joint_list.size()); @@ -502,6 +504,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory( parent->m_service0->playPattern(angles, rpy, zmp, duration); } } + prev_traj_tm = ros::Time::now(); interpolationp = true; } diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h index 4a4e3bd1a..605c8c3c4 100644 --- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h +++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h @@ -102,6 +102,7 @@ class HrpsysJointTrajectoryBridge : public RTC::DataFlowComponentBase std::string groupname; std::vector joint_list; bool interpolationp; + ros::Time prev_traj_tm; public: typedef boost::shared_ptr Ptr; diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index 30bd3b7c7..e27b05c90 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -218,6 +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(); interpolationp = true; } @@ -442,6 +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.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 bf615738c..00478cde1 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h @@ -87,6 +87,7 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl void clock_cb(const rosgraph_msgs::ClockPtr& str) {}; bool follow_action_initialized; + ros::Time prev_traj_tm; boost::mutex tf_mutex; double tf_rate; From c54fe612f21f23379fdbec7df5519d5f927b691f Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Tue, 10 Jul 2018 19:32:13 +0900 Subject: [PATCH 2/3] Stop feedback when trajectory is completed --- hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index e27b05c90..16a2289b6 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -578,7 +578,8 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id) } if ( !follow_joint_trajectory_feedback.joint_names.empty() && !follow_joint_trajectory_feedback.actual.positions.empty() && - follow_action_initialized ) { + follow_action_initialized && + follow_joint_trajectory_server.isActive() ) { follow_joint_trajectory_server.publishFeedback(follow_joint_trajectory_feedback); } } // end: m_mcangleIn From f347672610e54124d96501f6dd23cf6948c9754a Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Wed, 11 Jul 2018 13:58:13 +0900 Subject: [PATCH 3/3] 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;