From ea3bbc6a323e0fd559106ee66e6cf5694ecb9978 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Sat, 1 Jun 2024 11:37:17 +0200 Subject: [PATCH] Use robot_receive_timeout instead of keepalive_count (#1009) (cherry picked from commit 5ab3f4f1876c6ea0738add4e26d624a2f532bead) Co-authored-by: Felix Exner --- .../include/ur_robot_driver/hardware_interface.hpp | 3 +++ ur_robot_driver/src/hardware_interface.cpp | 11 +++++------ 2 files changed, 8 insertions(+), 6 deletions(-) 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..6404e69fc 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::millisec(std::stoi(info_.hardware_parameters["keep_alive_count"]) * 20); + 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();