From dbfa63834faeb8c96929a67b01a042d29012ca52 Mon Sep 17 00:00:00 2001 From: gtatiya Date: Thu, 16 Apr 2020 13:36:15 -0400 Subject: [PATCH] made changes to get effort feedback in joint_states accourding to https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/issues/146#issuecomment-610186709. Got ERROR: Could not get fresh data package from robot --- .../ur_robot_driver/ros/hardware_interface.h | 3 ++- ur_robot_driver/resources/ros_control.urscript | 16 ++++++++++++++++ ur_robot_driver/resources/rtde_output_recipe.txt | 6 ++++++ ur_robot_driver/src/ros/hardware_interface.cpp | 12 ++++++++++++ 4 files changed, 36 insertions(+), 1 deletion(-) 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 2bb0f0f5f..6bc5630ff 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 @@ -242,7 +242,8 @@ class HardwareInterface : public hardware_interface::RobotHW std::vector joint_names_; int32_t robot_mode_; int32_t safety_mode_; - + std::array output_double_register_; + std::unique_ptr> tcp_pose_pub_; std::unique_ptr> io_pub_; std::unique_ptr> tool_data_pub_; diff --git a/ur_robot_driver/resources/ros_control.urscript b/ur_robot_driver/resources/ros_control.urscript index 8b148f1a2..4557bc307 100644 --- a/ur_robot_driver/resources/ros_control.urscript +++ b/ur_robot_driver/resources/ros_control.urscript @@ -93,8 +93,23 @@ thread speedThread(): stopj(5.0) end +thread torqueThread(): + while True: + torques = get_joint_torques() + write_output_float_register(4, torques[0]) + write_output_float_register(5, torques[1]) + write_output_float_register(6, torques[2]) + write_output_float_register(7, torques[3]) + write_output_float_register(8, torques[4]) + write_output_float_register(9, torques[5]) + sync() + end +end + socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket") +torque_thread = run torqueThread() + control_mode = MODE_UNINITIALIZED thread_move = 0 global keepalive = -2 @@ -132,5 +147,6 @@ end textmsg("Stopping communication and control") control_mode = MODE_STOPPED join thread_move +kill torque_thread textmsg("All threads ended") socket_close("reverse_socket") diff --git a/ur_robot_driver/resources/rtde_output_recipe.txt b/ur_robot_driver/resources/rtde_output_recipe.txt index 7674c3a5c..1639f564b 100644 --- a/ur_robot_driver/resources/rtde_output_recipe.txt +++ b/ur_robot_driver/resources/rtde_output_recipe.txt @@ -22,3 +22,9 @@ tool_output_current tool_temperature robot_mode safety_mode +output_double_register_24 +output_double_register_25 +output_double_register_26 +output_double_register_27 +output_double_register_28 +output_double_register_29 diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index 279f5e9cc..d5ccfa068 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -413,6 +413,12 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period) readData(data_pkg, "tool_temperature", tool_temperature_); readData(data_pkg, "robot_mode", robot_mode_); readData(data_pkg, "safety_mode", safety_mode_); + readData(data_pkg, "output_double_register_24", output_double_register_[0]); + readData(data_pkg, "output_double_register_25", output_double_register_[1]); + readData(data_pkg, "output_double_register_26", output_double_register_[2]); + readData(data_pkg, "output_double_register_27", output_double_register_[3]); + readData(data_pkg, "output_double_register_28", output_double_register_[4]); + readData(data_pkg, "output_double_register_29", output_double_register_[5]); readBitsetData(data_pkg, "actual_digital_input_bits", actual_dig_in_bits_); readBitsetData(data_pkg, "actual_digital_output_bits", actual_dig_out_bits_); readBitsetData(data_pkg, "analog_io_types", analog_io_types_); @@ -705,6 +711,12 @@ void HardwareInterface::publishToolData() tool_data_pub_->msg_.tool_output_voltage = tool_output_voltage_; tool_data_pub_->msg_.tool_current = tool_output_current_; tool_data_pub_->msg_.tool_temperature = tool_temperature_; + tool_data_pub_->msg_.output_double_register_24 = output_double_register_[0]; + tool_data_pub_->msg_.output_double_register_25 = output_double_register_[1]; + tool_data_pub_->msg_.output_double_register_26 = output_double_register_[2]; + tool_data_pub_->msg_.output_double_register_27 = output_double_register_[3]; + tool_data_pub_->msg_.output_double_register_28 = output_double_register_[4]; + tool_data_pub_->msg_.output_double_register_29 = output_double_register_[5]; tool_data_pub_->unlockAndPublish(); } }