📈 🎉 Pub joint temps (#1)

* Add `ur_rtde_msgs` package; include msg for joint temps

* First pass at publishing joint temps

* Fix bad vector assignment when creating joint temp msg

* Add newline to end of Joint Temps msg

* Rename `ur_rtde_msgs`->`ur_extra_msgs`

Author: cgeering

ur_extra_msgs/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ur_extra_msgs)
+
+set(MESSAGE_DEPENDENCIES
+  std_msgs
+)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED
+  COMPONENTS
+    message_generation
+    ${MESSAGE_DEPENDENCIES}
+)
+
+add_message_files(
+  DIRECTORY msg/
+)
+
+generate_messages(
+  DEPENDENCIES
+    ${MESSAGE_DEPENDENCIES}
+)
+
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+catkin_package(
+  CATKIN_DEPENDS
+    message_runtime
+    ${MESSAGE_DEPENDENCIES}
+)

ur_extra_msgs/msg/JointTemperatures.msg

@@ -0,0 +1,5 @@
+# This message holds data to describe the temperatures of all robot joints.
+Header header
+
+string[] joint_names
+float64[] temperatures

ur_extra_msgs/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ur_extra_msgs</name>
+  <version>0.0.0</version>
+  <description>Supplemental messages to ur_msgs, maintained by Chef</description>
+
+  <maintainer email="[email protected]">Chef Robotics</maintainer>
+  <license>proprietary</license>
+  <url type="website">https://github.com/chef-robotics/Universal_Robots_ROS_Driver</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Core dependencies -->
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+  <!-- Message dependencies should be declared with `depend` -->
+  <depend>std_msgs</depend>
+
+</package>

ur_robot_driver/CMakeLists.txt

@@ -27,6 +27,7 @@ find_package(catkin REQUIRED
     ur_controllers
     ur_dashboard_msgs
     ur_msgs
+    ur_extra_msgs
 )
 find_package(Boost REQUIRED)
 
@@ -52,6 +53,7 @@ catkin_package(
     ur_controllers
     ur_dashboard_msgs
     ur_msgs
+    ur_extra_msgs
     std_srvs
   DEPENDS
     Boost

ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h

@@ -45,6 +45,8 @@
 #include <ur_msgs/SetIO.h>
 #include "ur_msgs/SetSpeedSliderFraction.h"
 
+#include <ur_extra_msgs/JointTemperatures.h>
+
 #include <ur_controllers/speed_scaling_interface.h>
 #include <ur_controllers/scaled_joint_command_interface.h>
 
@@ -180,6 +182,7 @@ class HardwareInterface : public hardware_interface::RobotHW
   void publishIOData();
   void publishToolData();
   void publishRobotAndSafetyMode();
+  void publishJointTemperatures(const ros::Time& timestamp);
 
   /*!
    * \brief Read and evaluate data in order to set robot status properties for industrial
@@ -229,6 +232,7 @@ class HardwareInterface : public hardware_interface::RobotHW
   vector6d_t joint_positions_;
   vector6d_t joint_velocities_;
   vector6d_t joint_efforts_;
+  vector6d_t joint_temperatures_;
   vector6d_t fts_measurements_;
   vector6d_t tcp_pose_;
   std::bitset<18> actual_dig_out_bits_;
@@ -259,6 +263,7 @@ class HardwareInterface : public hardware_interface::RobotHW
   std::unique_ptr<realtime_tools::RealtimePublisher<ur_msgs::ToolDataMsg>> tool_data_pub_;
   std::unique_ptr<realtime_tools::RealtimePublisher<ur_dashboard_msgs::RobotMode>> robot_mode_pub_;
   std::unique_ptr<realtime_tools::RealtimePublisher<ur_dashboard_msgs::SafetyMode>> safety_mode_pub_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<ur_extra_msgs::JointTemperatures>> joint_temperatures_pub_;
 
   ros::ServiceServer set_speed_slider_srv_;
   ros::ServiceServer set_io_srv_;

ur_robot_driver/package.xml

@@ -38,6 +38,7 @@
   <depend>ur_controllers</depend>
   <depend>ur_dashboard_msgs</depend>
   <depend>ur_msgs</depend>
+  <depend>ur_extra_msgs</depend>
   <depend>std_srvs</depend>
 
   <exec_depend>controller_stopper</exec_depend>

ur_robot_driver/resources/rtde_output_recipe.txt

@@ -25,3 +25,4 @@ safety_mode
 robot_status_bits
 safety_status_bits
 actual_current
+joint_temperatures

ur_robot_driver/src/ros/hardware_interface.cpp

@@ -53,6 +53,7 @@ HardwareInterface::HardwareInterface()
   , joint_positions_{ { 0, 0, 0, 0, 0, 0 } }
   , joint_velocities_{ { 0, 0, 0, 0, 0, 0 } }
   , joint_efforts_{ { 0, 0, 0, 0, 0, 0 } }
+  , joint_temperatures_{ { 0, 0, 0, 0, 0, 0 } }
   , standard_analog_input_{ { 0, 0 } }
   , standard_analog_output_{ { 0, 0 } }
   , joint_names_(6)
@@ -358,6 +359,8 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
       new realtime_tools::RealtimePublisher<ur_dashboard_msgs::RobotMode>(robot_hw_nh, "robot_mode", 1, true));
   safety_mode_pub_.reset(
       new realtime_tools::RealtimePublisher<ur_dashboard_msgs::SafetyMode>(robot_hw_nh, "safety_mode", 1, true));
+  joint_temperatures_pub_.reset(
+      new realtime_tools::RealtimePublisher<ur_extra_msgs::JointTemperatures>(robot_hw_nh, "joint_temperatures", 1));
 
   // Set the speed slider fraction used by the robot's execution. Values should be between 0 and 1.
   // Only set this smaller than 1 if you are using the scaled controllers (as by default) or you know what you're
@@ -466,6 +469,7 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period)
     readBitsetData<uint64_t>(data_pkg, "actual_digital_output_bits", actual_dig_out_bits_);
     readBitsetData<uint32_t>(data_pkg, "analog_io_types", analog_io_types_);
     readBitsetData<uint32_t>(data_pkg, "tool_analog_input_types", tool_analog_input_types_);
+    readData(data_pkg, "joint_temperatures", joint_temperatures_);
 
     extractRobotStatus();
 
@@ -477,6 +481,7 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period)
     transformForceTorque();
     publishPose();
     publishRobotAndSafetyMode();
+    publishJointTemperatures(time);
 
     // pausing state follows runtime state when pausing
     if (runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSED))
@@ -713,6 +718,27 @@ void HardwareInterface::publishPose()
   }
 }
 
+void HardwareInterface::publishJointTemperatures(const ros::Time& timestamp)
+{
+  if (joint_temperatures_pub_)
+  {
+    if (joint_temperatures_pub_->trylock())
+    {
+      joint_temperatures_pub_->msg_.header.stamp = timestamp;
+      joint_temperatures_pub_->msg_.joint_names.clear();
+      joint_temperatures_pub_->msg_.temperatures.clear();
+
+      for (size_t i = 0; i < joint_names_.size(); i++)
+      {
+        joint_temperatures_pub_->msg_.joint_names.push_back(joint_names_[i]);
+        joint_temperatures_pub_->msg_.temperatures.push_back(joint_temperatures_[i]);
+      }
+
+      joint_temperatures_pub_->unlockAndPublish();
+    }
+  }
+}
+
 void HardwareInterface::extractRobotStatus()
 {
   using namespace rtde_interface;