From c19d364feb20fab81253a21cd4564954d26e3785 Mon Sep 17 00:00:00 2001 From: Ryohei Ueda Date: Wed, 14 Aug 2019 21:23:12 +0900 Subject: [PATCH] Publish target joint states --- include/ur_modern_driver/ros/rt_publisher.h | 3 +++ src/ros/rt_publisher.cpp | 18 +++++++++++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/include/ur_modern_driver/ros/rt_publisher.h b/include/ur_modern_driver/ros/rt_publisher.h index 2bb2df79..c89c1e2a 100644 --- a/include/ur_modern_driver/ros/rt_publisher.h +++ b/include/ur_modern_driver/ros/rt_publisher.h @@ -45,6 +45,7 @@ class RTPublisher : public URRTPacketConsumer private: NodeHandle nh_; Publisher joint_pub_; + Publisher target_joint_pub_; Publisher wrench_pub_; Publisher tool_vel_pub_; Publisher joint_temperature_pub_; @@ -56,6 +57,7 @@ class RTPublisher : public URRTPacketConsumer bool temp_only_; bool publishJoints(RTShared& packet, Time& t); + bool publishTargetJoints(RTShared& packet, Time& t); bool publishWrench(RTShared& packet, Time& t); bool publishTool(RTShared& packet, Time& t); bool publishTransform(RTShared& packet, Time& t); @@ -66,6 +68,7 @@ class RTPublisher : public URRTPacketConsumer public: RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame, bool temp_only = false) : joint_pub_(nh_.advertise("joint_states", 1)) + , target_joint_pub_(nh_.advertise("target_joint_states", 1)) , wrench_pub_(nh_.advertise("wrench", 1)) , tool_vel_pub_(nh_.advertise("tool_velocity", 1)) , joint_temperature_pub_(nh_.advertise("joint_temperature", 1)) diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp index f1f9daa5..28f7c050 100644 --- a/src/ros/rt_publisher.cpp +++ b/src/ros/rt_publisher.cpp @@ -33,6 +33,21 @@ bool RTPublisher::publishJoints(RTShared& packet, Time& t) return true; } +bool RTPublisher::publishTargetJoints(RTShared& packet, Time& t) +{ + sensor_msgs::JointState joint_msg; + joint_msg.header.stamp = t; + + joint_msg.name.assign(joint_names_.begin(), joint_names_.end()); + joint_msg.position.assign(packet.q_target.begin(), packet.q_target.end()); + joint_msg.velocity.assign(packet.qd_target.begin(), packet.qd_target.end()); + joint_msg.effort.assign(packet.i_target.begin(), packet.i_target.end()); + + target_joint_pub_.publish(joint_msg); + + return true; +} + bool RTPublisher::publishWrench(RTShared& packet, Time& t) { geometry_msgs::WrenchStamped wrench_msg; @@ -121,7 +136,8 @@ bool RTPublisher::publish(RTShared& packet) res = publishJoints(packet, time) && publishWrench(packet, time); } - return res && publishTool(packet, time) && publishTransform(packet, time) && publishTemperature(packet, time); + return res && publishTargetJoints(packet, time) && publishTool(packet, time) && publishTransform(packet, time) && + publishTemperature(packet, time); } bool RTPublisher::consume(RTState_V1_6__7& state)