diff --git a/ur_extra_msgs/CMakeLists.txt b/ur_extra_msgs/CMakeLists.txt
new file mode 100644
index 000000000..6a7ea6504
--- /dev/null
+++ b/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}
+)
diff --git a/ur_extra_msgs/msg/JointTemperatures.msg b/ur_extra_msgs/msg/JointTemperatures.msg
new file mode 100644
index 000000000..455fb61ec
--- /dev/null
+++ b/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
diff --git a/ur_extra_msgs/package.xml b/ur_extra_msgs/package.xml
new file mode 100644
index 000000000..341c14994
--- /dev/null
+++ b/ur_extra_msgs/package.xml
@@ -0,0 +1,20 @@
+
+
+ ur_extra_msgs
+ 0.0.0
+ Supplemental messages to ur_msgs, maintained by Chef
+
+ Chef Robotics
+ proprietary
+ https://github.com/chef-robotics/Universal_Robots_ROS_Driver
+
+ catkin
+
+
+ message_generation
+ message_runtime
+
+
+ std_msgs
+
+
diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt
index 48f69295f..5ecaa595f 100644
--- a/ur_robot_driver/CMakeLists.txt
+++ b/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
diff --git a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h
index 1b317b074..8dd27633c 100644
--- a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h
+++ b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h
@@ -45,6 +45,8 @@
#include
#include "ur_msgs/SetSpeedSliderFraction.h"
+#include
+
#include
#include
@@ -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> tool_data_pub_;
std::unique_ptr> robot_mode_pub_;
std::unique_ptr> safety_mode_pub_;
+ std::unique_ptr> joint_temperatures_pub_;
ros::ServiceServer set_speed_slider_srv_;
ros::ServiceServer set_io_srv_;
diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml
index 431b72c71..82d9df8b0 100644
--- a/ur_robot_driver/package.xml
+++ b/ur_robot_driver/package.xml
@@ -38,6 +38,7 @@
ur_controllers
ur_dashboard_msgs
ur_msgs
+ ur_extra_msgs
std_srvs
controller_stopper
diff --git a/ur_robot_driver/resources/rtde_output_recipe.txt b/ur_robot_driver/resources/rtde_output_recipe.txt
index 8d10be849..bd6e7e408 100644
--- a/ur_robot_driver/resources/rtde_output_recipe.txt
+++ b/ur_robot_driver/resources/rtde_output_recipe.txt
@@ -25,3 +25,4 @@ safety_mode
robot_status_bits
safety_status_bits
actual_current
+joint_temperatures
diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp
index bf5aa47a7..873c21c68 100644
--- a/ur_robot_driver/src/ros/hardware_interface.cpp
+++ b/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(robot_hw_nh, "robot_mode", 1, true));
safety_mode_pub_.reset(
new realtime_tools::RealtimePublisher(robot_hw_nh, "safety_mode", 1, true));
+ joint_temperatures_pub_.reset(
+ new realtime_tools::RealtimePublisher(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(data_pkg, "actual_digital_output_bits", actual_dig_out_bits_);
readBitsetData(data_pkg, "analog_io_types", analog_io_types_);
readBitsetData(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(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;