diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h index aff9e09d7..3ae6d25a7 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -177,6 +178,7 @@ class HardwareInterface : public hardware_interface::RobotHW */ void publishPose(); + void publishJointTemperatures(const ros::Time& timestamp); void publishIOData(); void publishToolData(); void publishRobotAndSafetyMode(); @@ -230,6 +232,7 @@ class HardwareInterface : public hardware_interface::RobotHW urcl::vector6d_t joint_positions_; urcl::vector6d_t joint_velocities_; urcl::vector6d_t joint_efforts_; + urcl::vector6d_t joint_temperatures_; urcl::vector6d_t fts_measurements_; urcl::vector6d_t tcp_pose_; std::bitset<18> actual_dig_out_bits_; @@ -256,6 +259,8 @@ class HardwareInterface : public hardware_interface::RobotHW std::bitset<11> safety_status_bits_; std::unique_ptr> tcp_pose_pub_; + typedef std::shared_ptr> JTPublisherPtr; + std::vector joint_temperature_pubs_; std::unique_ptr> io_pub_; std::unique_ptr> tool_data_pub_; std::unique_ptr> robot_mode_pub_; diff --git a/ur_robot_driver/resources/rtde_output_recipe.txt b/ur_robot_driver/resources/rtde_output_recipe.txt index 8d10be849..a8e6fbe2f 100644 --- a/ur_robot_driver/resources/rtde_output_recipe.txt +++ b/ur_robot_driver/resources/rtde_output_recipe.txt @@ -1,6 +1,7 @@ timestamp actual_q actual_qd +joint_temperatures speed_scaling target_speed_fraction runtime_state diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index bb838387e..e05f87d75 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -55,6 +55,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) @@ -354,6 +355,12 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw { io_pub_->msg_.analog_out_states[i].pin = i; } + for (size_t i = 0; i < joint_names_.size(); i++) + { + JTPublisherPtr joint_temperature_pub(new realtime_tools::RealtimePublisher(root_nh, "joint_temperatures/"+joint_names_[i], 1)); + joint_temperature_pubs_.push_back(joint_temperature_pub); + } + tool_data_pub_.reset(new realtime_tools::RealtimePublisher(robot_hw_nh, "tool_data", 1)); robot_mode_pub_.reset( @@ -449,6 +456,7 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period) readData(data_pkg, "runtime_state", runtime_state_); readData(data_pkg, "actual_TCP_force", fts_measurements_); readData(data_pkg, "actual_TCP_pose", tcp_pose_); + readData(data_pkg, "joint_temperatures", joint_temperatures_); readData(data_pkg, "standard_analog_input0", standard_analog_input_[0]); readData(data_pkg, "standard_analog_input1", standard_analog_input_[1]); readData(data_pkg, "standard_analog_output0", standard_analog_output_[0]); @@ -478,6 +486,7 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period) extractToolPose(time); transformForceTorque(); publishPose(); + publishJointTemperatures(time); publishRobotAndSafetyMode(); // pausing state follows runtime state when pausing @@ -719,6 +728,24 @@ void HardwareInterface::publishPose() } } +void HardwareInterface::publishJointTemperatures(const ros::Time& timestamp) +{ + for (size_t i = 0; i < joint_names_.size(); i++) + { + if (joint_temperature_pubs_[i]) + { + if (joint_temperature_pubs_[i]->trylock()) + { + joint_temperature_pubs_[i]->msg_.header.stamp = timestamp; + joint_temperature_pubs_[i]->msg_.header.frame_id = joint_names_[i]; + joint_temperature_pubs_[i]->msg_.temperature = joint_temperatures_[i]; + + joint_temperature_pubs_[i]->unlockAndPublish(); + } + } + } +} + void HardwareInterface::extractRobotStatus() { robot_status_resource_.mode =