Publish target joint states

ros-industrial-attic · Aug 14, 2019 · 0f626c9 · 0f626c9
1 parent fd2e38a
commit 0f626c9
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 1 deletion.
diff --git 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<sensor_msgs::JointState>("joint_states", 1))
+    , target_joint_pub_(nh_.advertise<sensor_msgs::JointState>("target_joint_states", 1))
     , wrench_pub_(nh_.advertise<geometry_msgs::WrenchStamped>("wrench", 1))
     , tool_vel_pub_(nh_.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1))
     , joint_temperature_pub_(nh_.advertise<sensor_msgs::Temperature>("joint_temperature", 1))

diff --git 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,7 @@ 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)