From 1a9399dd0f57e97ce46eb934da179103389e1414 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Wed, 29 May 2024 09:28:21 +0200 Subject: [PATCH] Replace keepalive count (#1002) * Use robot_receive_timeout instead of keepalive_count * Add migration note containing the keepalive_count to robot_receive_timeout migration --- ur_robot_driver/doc/migration/jazzy.rst | 10 ++++++++++ .../include/ur_robot_driver/hardware_interface.hpp | 3 +++ ur_robot_driver/src/hardware_interface.cpp | 11 +++++------ ur_robot_driver/urdf/ur.ros2_control.xacro | 4 ++-- 4 files changed, 20 insertions(+), 8 deletions(-) create mode 100644 ur_robot_driver/doc/migration/jazzy.rst diff --git a/ur_robot_driver/doc/migration/jazzy.rst b/ur_robot_driver/doc/migration/jazzy.rst new file mode 100644 index 000000000..2cb85913a --- /dev/null +++ b/ur_robot_driver/doc/migration/jazzy.rst @@ -0,0 +1,10 @@ +ur_robot_driver +^^^^^^^^^^^^^^^ + +keep_alive_count -> robot_receive_timeout +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Doing real-time control with a robot requires the control pc to conform to certain timing +constraints. For this ``keep_alive_count`` was used to estimate the tolerance that was given to the +ROS controller in terms of multiples of 20 ms. Now the timeout is directly configured using the +``robot_receive_timeout`` parameter of the hardware interface. diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index fe1ede6f7..67e27a222 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -53,6 +53,7 @@ // UR stuff #include "ur_client_library/ur/ur_driver.h" +#include "ur_client_library/ur/robot_receive_timeout.h" #include "ur_robot_driver/dashboard_client_ros.hpp" #include "ur_dashboard_msgs/msg/robot_mode.hpp" @@ -225,6 +226,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface std::shared_ptr async_thread_; std::atomic_bool rtde_comm_has_been_started_ = false; + + urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20); }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 8ff61080e..5826cf595 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -414,9 +414,6 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou tool_comm_setup->setTxIdleChars(tx_idle_chars); } - // Amount of allowed timed out reads before the reverse interface will be dropped - const int keep_alive_count = std::stoi(info_.hardware_parameters["keep_alive_count"]); - // Obtain the tf_prefix which is needed for the logging handler so that log messages from different arms are // distiguishable in the log const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix"); @@ -429,7 +426,6 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou std::bind(&URPositionHardwareInterface::handleRobotProgramState, this, std::placeholders::_1), headless_mode, std::move(tool_comm_setup), (uint32_t)reverse_port, (uint32_t)script_sender_port, servoj_gain, servoj_lookahead_time, non_blocking_read_, reverse_ip, trajectory_port, script_command_port); - ur_driver_->setKeepaliveCount(keep_alive_count); } catch (urcl::ToolCommNotAvailable& e) { RCLCPP_FATAL_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), "See parameter use_tool_communication"); @@ -438,6 +434,9 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou RCLCPP_FATAL_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), e.what()); return hardware_interface::CallbackReturn::ERROR; } + // Timeout before the reverse interface will be dropped by the robot + receive_timeout_ = urcl::RobotReceiveTimeout::sec(std::stof(info_.hardware_parameters["robot_receive_timeout"])); + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Calibration checksum: '%s'.", calibration_checksum.c_str()); // check calibration @@ -627,10 +626,10 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: runtime_state_ == static_cast(rtde::RUNTIME_STATE::PAUSING)) && robot_program_running_ && (!non_blocking_read_ || packet_read_)) { if (position_controller_running_) { - ur_driver_->writeJointCommand(urcl_position_commands_, urcl::comm::ControlMode::MODE_SERVOJ); + ur_driver_->writeJointCommand(urcl_position_commands_, urcl::comm::ControlMode::MODE_SERVOJ, receive_timeout_); } else if (velocity_controller_running_) { - ur_driver_->writeJointCommand(urcl_velocity_commands_, urcl::comm::ControlMode::MODE_SPEEDJ); + ur_driver_->writeJointCommand(urcl_velocity_commands_, urcl::comm::ControlMode::MODE_SPEEDJ, receive_timeout_); } else { ur_driver_->writeKeepalive(); diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 308071cc1..77e0c22cd 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -25,7 +25,7 @@ script_command_port:=50004 trajectory_port:=50003 non_blocking_read:=true - keep_alive_count:=2 + robot_receive_timeout:=0.04 "> @@ -69,7 +69,7 @@ ${tool_tx_idle_chars} ${tool_device_name} ${tool_tcp_port} - ${keep_alive_count} + ${robot_receive_timeout}