Make non blocking read timeout configurable

Author: URJala

ur_robot_driver/doc/hardware_interface_parameters.rst

@@ -30,6 +30,14 @@ non_blocking_read (default: "true")
 If set to false, the ROS control cycle will wait for the robot to send a status update. Tests have
 shown that better real-time performance is achievable when setting this to ``true``. Required to be
 set to ``true`` when combining with other hardware components.
+When set to ``false``, the hardware interface will be stopped if it fails to read an RTDE package from the robot.
+when set to ``true``, the hardware interface will be stopped if it fails to read an RTDE package within the configured timeout (see non_blocking_read_timeout).
+
+non_blocking_read_timeout (default: 0.04 (seconds))
+---------------------------------------------------
+Timeout value to be used when non_blocking_read is ``true``.
+If the value is 0 (or less) the timeout will be disabled.
+Note: for values <0.010 the driver might fail on startup, especially if used with URSim.
 
 output_recipe_filename (Required)
 ---------------------------------

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -204,8 +204,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   tf2::Quaternion tcp_rotation_quat_;
   Quaternion tcp_rotation_buffer;
 
-  rclcpp::Duration time_since_successful_read = rclcpp::Duration(0, 0);
-  rclcpp::Duration max_time_between_reads = rclcpp::Duration(0, 4000000);  // 4 ms
+  rclcpp::Duration time_since_successful_read_ = rclcpp::Duration(0, 0);
+  rclcpp::Duration non_blocking_read_timeout_ = rclcpp::Duration(0, 0);
 
   bool packet_read_;
 

ur_robot_driver/src/hardware_interface.cpp

@@ -630,6 +630,9 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou
   non_blocking_read_ = (info_.hardware_parameters["non_blocking_read"] == "true") ||
                        (info_.hardware_parameters["non_blocking_read"] == "True");
 
+  non_blocking_read_timeout_ = rclcpp::Duration::from_seconds(stod(info_.hardware_parameters["non_blocking_read_"
+                                                                                             "timeout"]));
+
   // Specify gain for servoing to position in joint space.
   // A higher gain can sharpen the trajectory.
   const int servoj_gain = stoi(info_.hardware_parameters["servoj_gain"]);
@@ -877,8 +880,6 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
   if (!rtde_comm_has_been_started_) {
     ur_driver_->startRTDECommunication();
     rtde_comm_has_been_started_ = true;
-    // Without this the driver will error out immediately, regardless of non_blocking_read_ (because of timeout)
-    return hardware_interface::return_type::OK;
   }
   std::unique_ptr<rtde::DataPackage> data_pkg = ur_driver_->getDataPackage();
 
@@ -968,21 +969,22 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
     hw_moprim_states_[0] = static_cast<uint8_t>(current_moprim_execution_status_.load());
     hw_moprim_states_[1] = static_cast<double>(ready_for_new_moprim_);
 
-    time_since_successful_read = rclcpp::Duration(0, 0);
+    time_since_successful_read_ = rclcpp::Duration(0, 0);
 
     return hardware_interface::return_type::OK;
   }
 
   if (non_blocking_read_) {
-    time_since_successful_read += period;
-    if (time_since_successful_read > max_time_between_reads) {
+    time_since_successful_read_ += period;
+    if (non_blocking_read_timeout_ > rclcpp::Duration(0, 0) &&
+        time_since_successful_read_ > non_blocking_read_timeout_) {
       std::stringstream ss;
-      ss << "It has been too long since the last successful reading of the hardware. \nTime since last successful "
-            "read: "
-         << time_since_successful_read.seconds() * 1000
-         << " milliseconds \nThe maximum allowed time is: " << max_time_between_reads.seconds() * 1000
-         << " milliseconds";
+      ss << "Non blocking read timeout exceeded. \n"
+         << "Time since last successful read of the hardware: " << time_since_successful_read_.seconds() * 1000
+         << " milliseconds \n"
+         << "The non blocking read time out is: " << non_blocking_read_timeout_.seconds() * 1000 << " milliseconds";
       RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), ss.str().c_str());
+      // For very small values of non_blocking_read_timeout_ (< 10 ms) this might error on startup
       return hardware_interface::return_type::ERROR;
     }
     return hardware_interface::return_type::OK;

ur_robot_driver/urdf/ur.ros2_control.xacro

@@ -25,6 +25,7 @@
     script_command_port:=50004
     trajectory_port:=50003
     non_blocking_read:=true
+    non_blocking_read_timeout:=0.04
     robot_receive_timeout:=0.04
     rw_rate:=0
     ">
@@ -60,6 +61,7 @@
           <param name="trajectory_port">${trajectory_port}</param>
           <param name="tf_prefix">${tf_prefix}</param>
           <param name="non_blocking_read">${non_blocking_read}</param>
+          <param name="non_blocking_read_timeout">${non_blocking_read_timeout}</param>
           <param name="servoj_gain">2000</param>
           <param name="servoj_lookahead_time">0.03</param>
           <param name="use_tool_communication">${use_tool_communication}</param>