From fae86efae2f84f98a6bbe29d5989491da3b0a35c Mon Sep 17 00:00:00 2001 From: cgeering Date: Tue, 4 Oct 2022 17:52:06 -0700 Subject: [PATCH 1/5] Add `ur_rtde_msgs` package; include msg for joint temps --- ur_robot_driver/CMakeLists.txt | 2 ++ ur_robot_driver/package.xml | 1 + ur_rtde_msgs/CMakeLists.txt | 30 ++++++++++++++++++++++++++ ur_rtde_msgs/msg/JointTemperatures.msg | 5 +++++ ur_rtde_msgs/package.xml | 20 +++++++++++++++++ 5 files changed, 58 insertions(+) create mode 100644 ur_rtde_msgs/CMakeLists.txt create mode 100644 ur_rtde_msgs/msg/JointTemperatures.msg create mode 100644 ur_rtde_msgs/package.xml diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 48f69295f..43e711e12 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_rtde_msgs ) find_package(Boost REQUIRED) @@ -52,6 +53,7 @@ catkin_package( ur_controllers ur_dashboard_msgs ur_msgs + ur_rtde_msgs std_srvs DEPENDS Boost diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 431b72c71..75a46dc0a 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_rtde_msgs std_srvs controller_stopper diff --git a/ur_rtde_msgs/CMakeLists.txt b/ur_rtde_msgs/CMakeLists.txt new file mode 100644 index 000000000..e6d1b2fd0 --- /dev/null +++ b/ur_rtde_msgs/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ur_rtde_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_rtde_msgs/msg/JointTemperatures.msg b/ur_rtde_msgs/msg/JointTemperatures.msg new file mode 100644 index 000000000..e4c9f561c --- /dev/null +++ b/ur_rtde_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 \ No newline at end of file diff --git a/ur_rtde_msgs/package.xml b/ur_rtde_msgs/package.xml new file mode 100644 index 000000000..4bd1b3803 --- /dev/null +++ b/ur_rtde_msgs/package.xml @@ -0,0 +1,20 @@ + + + ur_rtde_msgs + 0.0.0 + Messages to encapsulate data UR RTDE packets + + Chef Robotics + proprietary + https://github.com/chef-robotics/Universal_Robots_ROS_Driver + + catkin + + + message_generation + message_runtime + + + std_msgs + + From d657b9bc6d71b3c434b1d2ac7cd37f81ef43a46f Mon Sep 17 00:00:00 2001 From: cgeering Date: Tue, 4 Oct 2022 19:01:37 -0700 Subject: [PATCH 2/5] First pass at publishing joint temps --- .../ur_robot_driver/ros/hardware_interface.h | 5 +++++ .../resources/rtde_output_recipe.txt | 1 + .../src/ros/hardware_interface.cpp | 22 +++++++++++++++++++ 3 files changed, 28 insertions(+) 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..b2d7fdf5b 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/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..c3f76e67a 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,23 @@ void HardwareInterface::publishPose() } } +void HardwareInterface::publishJointTemperatures(const ros::Time& timestamp) +{ + if (joint_temperatures_pub_) + { + if (joint_temperatures_pub_->trylock()) + { + for (size_t i = 0; i < joint_names_.size(); i++) + { + joint_temperatures_pub_->msg_.header.stamp = timestamp; + joint_temperatures_pub_->msg_.joint_names[i] = joint_names_[i]; + joint_temperatures_pub_->msg_.temperatures[i] = joint_temperatures_[i]; + } + joint_temperatures_pub_->unlockAndPublish(); + } + } +} + void HardwareInterface::extractRobotStatus() { using namespace rtde_interface; From 6222cf8f4b491bbc6d7383f6c16a8e9526ed8cc2 Mon Sep 17 00:00:00 2001 From: CJ Date: Tue, 4 Oct 2022 20:18:43 -0700 Subject: [PATCH 3/5] Fix bad vector assignment when creating joint temp msg --- ur_robot_driver/src/ros/hardware_interface.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index c3f76e67a..52f879237 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -724,12 +724,16 @@ void HardwareInterface::publishJointTemperatures(const ros::Time& timestamp) { 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_.header.stamp = timestamp; - joint_temperatures_pub_->msg_.joint_names[i] = joint_names_[i]; - joint_temperatures_pub_->msg_.temperatures[i] = joint_temperatures_[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(); } } From 7ff5f474d3d74e3b31783255e3a691cd2f24e07e Mon Sep 17 00:00:00 2001 From: cgeering Date: Wed, 5 Oct 2022 09:47:56 -0700 Subject: [PATCH 4/5] Add newline to end of Joint Temps msg --- ur_rtde_msgs/msg/JointTemperatures.msg | 2 +- ur_rtde_msgs/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ur_rtde_msgs/msg/JointTemperatures.msg b/ur_rtde_msgs/msg/JointTemperatures.msg index e4c9f561c..455fb61ec 100644 --- a/ur_rtde_msgs/msg/JointTemperatures.msg +++ b/ur_rtde_msgs/msg/JointTemperatures.msg @@ -2,4 +2,4 @@ Header header string[] joint_names -float64[] temperatures \ No newline at end of file +float64[] temperatures diff --git a/ur_rtde_msgs/package.xml b/ur_rtde_msgs/package.xml index 4bd1b3803..80a1bea5a 100644 --- a/ur_rtde_msgs/package.xml +++ b/ur_rtde_msgs/package.xml @@ -2,7 +2,7 @@ ur_rtde_msgs 0.0.0 - Messages to encapsulate data UR RTDE packets + Messages to encapsulate data from UR RTDE packets Chef Robotics proprietary From f0ed05591812b152ec4690575215ca034fc94e6e Mon Sep 17 00:00:00 2001 From: cgeering Date: Wed, 5 Oct 2022 16:32:20 -0700 Subject: [PATCH 5/5] Rename `ur_rtde_msgs`->`ur_extra_msgs` --- {ur_rtde_msgs => ur_extra_msgs}/CMakeLists.txt | 2 +- {ur_rtde_msgs => ur_extra_msgs}/msg/JointTemperatures.msg | 0 {ur_rtde_msgs => ur_extra_msgs}/package.xml | 4 ++-- ur_robot_driver/CMakeLists.txt | 4 ++-- .../include/ur_robot_driver/ros/hardware_interface.h | 4 ++-- ur_robot_driver/package.xml | 2 +- ur_robot_driver/src/ros/hardware_interface.cpp | 2 +- 7 files changed, 9 insertions(+), 9 deletions(-) rename {ur_rtde_msgs => ur_extra_msgs}/CMakeLists.txt (95%) rename {ur_rtde_msgs => ur_extra_msgs}/msg/JointTemperatures.msg (100%) rename {ur_rtde_msgs => ur_extra_msgs}/package.xml (82%) diff --git a/ur_rtde_msgs/CMakeLists.txt b/ur_extra_msgs/CMakeLists.txt similarity index 95% rename from ur_rtde_msgs/CMakeLists.txt rename to ur_extra_msgs/CMakeLists.txt index e6d1b2fd0..6a7ea6504 100644 --- a/ur_rtde_msgs/CMakeLists.txt +++ b/ur_extra_msgs/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(ur_rtde_msgs) +project(ur_extra_msgs) set(MESSAGE_DEPENDENCIES std_msgs diff --git a/ur_rtde_msgs/msg/JointTemperatures.msg b/ur_extra_msgs/msg/JointTemperatures.msg similarity index 100% rename from ur_rtde_msgs/msg/JointTemperatures.msg rename to ur_extra_msgs/msg/JointTemperatures.msg diff --git a/ur_rtde_msgs/package.xml b/ur_extra_msgs/package.xml similarity index 82% rename from ur_rtde_msgs/package.xml rename to ur_extra_msgs/package.xml index 80a1bea5a..341c14994 100644 --- a/ur_rtde_msgs/package.xml +++ b/ur_extra_msgs/package.xml @@ -1,8 +1,8 @@ - ur_rtde_msgs + ur_extra_msgs 0.0.0 - Messages to encapsulate data from UR RTDE packets + Supplemental messages to ur_msgs, maintained by Chef Chef Robotics proprietary diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 43e711e12..5ecaa595f 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -27,7 +27,7 @@ find_package(catkin REQUIRED ur_controllers ur_dashboard_msgs ur_msgs - ur_rtde_msgs + ur_extra_msgs ) find_package(Boost REQUIRED) @@ -53,7 +53,7 @@ catkin_package( ur_controllers ur_dashboard_msgs ur_msgs - ur_rtde_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 b2d7fdf5b..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,7 +45,7 @@ #include #include "ur_msgs/SetSpeedSliderFraction.h" -#include +#include #include #include @@ -263,7 +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_; + 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 75a46dc0a..82d9df8b0 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -38,7 +38,7 @@ ur_controllers ur_dashboard_msgs ur_msgs - ur_rtde_msgs + ur_extra_msgs std_srvs controller_stopper diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index 52f879237..873c21c68 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -360,7 +360,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw 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)); + 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