Replace keepalive count (#1002)

* Use robot_receive_timeout instead of keepalive_count

* Add migration note containing the keepalive_count to robot_receive_timeout migration

Author: Felix Exner (fexner)

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.

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<std::thread> async_thread_;
 
   std::atomic_bool rtde_comm_has_been_started_ = false;
+
+  urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20);
 };
 }  // namespace ur_robot_driver
 

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<uint32_t>(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();

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
     ">
 
     <xacro:property name="config_kinematics_parameters" value="${xacro.load_yaml(kinematics_parameters_file)}"/>
@@ -69,7 +69,7 @@
           <param name="tool_tx_idle_chars">${tool_tx_idle_chars}</param>
           <param name="tool_device_name">${tool_device_name}</param>
           <param name="tool_tcp_port">${tool_tcp_port}</param>
-          <param name="keep_alive_count">${keep_alive_count}</param>
+          <param name="robot_receive_timeout">${robot_receive_timeout}</param>
         </xacro:unless>
       </hardware>