diff --git a/include/ur_modern_driver/ros/rt_publisher.h b/include/ur_modern_driver/ros/rt_publisher.h index 8e606200..2bb2df79 100644 --- 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 joint_names_; + std::vector 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 index af46c011..a753b743 100644 --- a/src/ros/rt_publisher.cpp +++ b/src/ros/rt_publisher.cpp @@ -98,7 +98,10 @@ bool RTPublisher::publishTemperature(RTShared& packet, Time& t) { sensor_msgs::Temperature msg; msg.header.stamp = t; - msg.header.frame_id = joint_names_[i]; + // assumption: origins of the link frames are coincident with the origins + // of the joints. As the temperature sensors are assumed to be located in + // the joints, using the names of the link frames here should be acceptable. + msg.header.frame_id = link_names_[i]; msg.temperature = packet.motor_temperatures[i]; joint_temperature_pub_.publish(msg);