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;