Use the link names for temperatures instead of the joint names

This resolves ros-industrial-attic#315
fmauch · Jul 2, 2019 · cc5fd74 · cc5fd74
1 parent c663703
commit cc5fd74
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 1 deletion.
diff --git a/include/ur_modern_driver/ros/rt_publisher.h b/include/ur_modern_driver/ros/rt_publisher.h
@@ -36,6 +36,9 @@ using namespace tf;
 
 const std::string JOINTS[] = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
                                "wrist_1_joint",      "wrist_2_joint",       "wrist_3_joint" };
+// links with origin in the joints configured above.
+const std::string LINKS[] = { "shoulder_link", "upper_arm_link", "forearm_link",
+                              "wrist_1_link",  "wrist_2_link",   "wrist_3_link" };
 
 class RTPublisher : public URRTPacketConsumer
 {
@@ -47,6 +50,7 @@ class RTPublisher : public URRTPacketConsumer
   Publisher joint_temperature_pub_;
   TransformBroadcaster transform_broadcaster_;
   std::vector<std::string> joint_names_;
+  std::vector<std::string> link_names_;
   std::string base_frame_;
   std::string tool_frame_;
   bool temp_only_;
@@ -73,6 +77,10 @@ class RTPublisher : public URRTPacketConsumer
     {
       joint_names_.push_back(joint_prefix + j);
     }
+    for (auto const& link : LINKS)
+    {
+      link_names_.push_back(joint_prefix + link);
+    }
   }
 
   virtual bool consume(RTState_V1_6__7& state);

diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp
@@ -98,7 +98,7 @@ bool RTPublisher::publishTemperature(RTShared& packet, Time& t)
   {
     sensor_msgs::Temperature msg;
     msg.header.stamp = t;
-    msg.header.frame_id = joint_names_[i];
+    msg.header.frame_id = link_names_[i];
     msg.temperature = packet.motor_temperatures[i];
 
     joint_temperature_pub_.publish(msg);