Add ROS param to control reverse interface robot receive timeout

Useful when the connection to the robot does not support real-time control

Author: cambel

ur_robot_driver/include/ur_robot_driver/hardware_interface.h

@@ -64,6 +64,7 @@
 #include <scaled_joint_trajectory_controller/scaled_joint_command_interface.h>
 
 #include <ur_client_library/ur/ur_driver.h>
+#include <ur_client_library/ur/robot_receive_timeout.h>
 #include <ur_robot_driver/dashboard_client_ros.h>
 
 #include <ur_dashboard_msgs/RobotMode.h>
@@ -352,6 +353,7 @@ class HardwareInterface : public hardware_interface::RobotHW
 
   std::string robot_ip_;
   std::string tf_prefix_;
+  urcl::RobotReceiveTimeout robot_receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20);
 };
 
 }  // namespace ur_driver

ur_robot_driver/launch/ur_common.launch

@@ -25,6 +25,7 @@
   <arg name="headless_mode" default="false" doc="Automatically send URScript to robot to execute. On e-Series this does require the robot to be in 'remote-control' mode. With this, the URCap is not needed on the robot."/>
   <arg name="ur_hardware_interface_node_required" default="true" doc="Shut down ros environment if ur_hardware_interface-node dies."/>
   <arg name="use_spline_interpolation" default="true" doc="True if splines should be used as interpolation on the robot controller when forwarding trajectory, if false movej or movel commands are used"/>
+  <arg name="robot_receive_timeout" default="0.02" doc="Timeout for robot receive operations in seconds"/>
 
   <!-- robot model -->
   <include file="$(arg robot_description_file)">
@@ -59,5 +60,6 @@
     <arg name="tool_tcp_port" value="$(arg tool_tcp_port)"/>
     <arg name="ur_hardware_interface_node_required" value="$(arg ur_hardware_interface_node_required)"/>
     <arg name="use_spline_interpolation" value="$(arg use_spline_interpolation)"/>
+    <arg name="robot_receive_timeout" value="$(arg robot_receive_timeout)"/>
   </include>
 </launch>

ur_robot_driver/launch/ur_control.launch

@@ -34,6 +34,7 @@
   <arg name="servoj_lookahead_time" default="0.03" doc="Specify lookahead time for servoing to position in joint space. A longer lookahead time can smooth the trajectory."/>
   <arg name="ur_hardware_interface_node_required" default="true" doc="Shut down ros environment if ur_hardware_interface-node dies."/>
   <arg name="use_spline_interpolation" default="true" doc="True if splines should be used as interpolation on the robot controller when forwarding trajectory, if false movej or movel commands are used"/>
+  <arg name="robot_receive_timeout" default="0.02" doc="Timeout for robot receive operations in seconds"/>
 
   <!-- Load hardware interface -->
   <node name="ur_hardware_interface" pkg="ur_robot_driver" type="ur_robot_driver_node" output="screen" launch-prefix="$(arg launch_prefix)" required="$(arg ur_hardware_interface_node_required)">
@@ -60,6 +61,7 @@
     <param name="servoj_gain" value="$(arg servoj_gain)"/>
     <param name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)"/>
     <param name="use_spline_interpolation" value="$(arg use_spline_interpolation)"/>
+    <param name="robot_receive_timeout" value="$(arg robot_receive_timeout)"/>
   </node>
 
   <!-- Starts socat to bridge the robot's tool communication interface to a local tty device -->

ur_robot_driver/src/hardware_interface.cpp

@@ -158,6 +158,11 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   // not used with combined_robot_hw can suppress important errors and affect real-time performance.
   robot_hw_nh.param("non_blocking_read", non_blocking_read_, false);
 
+  // Timeout for robot receive operations in seconds
+  double timeout_seconds;
+  robot_hw_nh.param("robot_receive_timeout", timeout_seconds, 0.02);
+  robot_receive_timeout_ = urcl::RobotReceiveTimeout::sec(timeout_seconds);  
+
   // Specify gain for servoing to position in joint space.
   // A higher gain can sharpen the trajectory.
   int servoj_gain = robot_hw_nh.param("servoj_gain", 2000);
@@ -693,11 +698,11 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period
   {
     if (position_controller_running_)
     {
-      ur_driver_->writeJointCommand(joint_position_command_, urcl::comm::ControlMode::MODE_SERVOJ);
+      ur_driver_->writeJointCommand(joint_position_command_, urcl::comm::ControlMode::MODE_SERVOJ, robot_receive_timeout_);
     }
     else if (velocity_controller_running_)
     {
-      ur_driver_->writeJointCommand(joint_velocity_command_, urcl::comm::ControlMode::MODE_SPEEDJ);
+      ur_driver_->writeJointCommand(joint_velocity_command_, urcl::comm::ControlMode::MODE_SPEEDJ, robot_receive_timeout_);
     }
     else if (joint_forward_controller_running_)
     {
@@ -715,7 +720,7 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period
       cartesian_velocity_command_[3] = twist_command_.angular.x;
       cartesian_velocity_command_[4] = twist_command_.angular.y;
       cartesian_velocity_command_[5] = twist_command_.angular.z;
-      ur_driver_->writeJointCommand(cartesian_velocity_command_, urcl::comm::ControlMode::MODE_SPEEDL);
+      ur_driver_->writeJointCommand(cartesian_velocity_command_, urcl::comm::ControlMode::MODE_SPEEDL, robot_receive_timeout_);
     }
     else if (pose_controller_running_)
     {
@@ -729,11 +734,11 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period
       cartesian_pose_command_[4] = rot.GetRot().y();
       cartesian_pose_command_[5] = rot.GetRot().z();
 
-      ur_driver_->writeJointCommand(cartesian_pose_command_, urcl::comm::ControlMode::MODE_POSE);
+      ur_driver_->writeJointCommand(cartesian_pose_command_, urcl::comm::ControlMode::MODE_POSE, robot_receive_timeout_);
     }
     else
     {
-      ur_driver_->writeKeepalive();
+      ur_driver_->writeKeepalive(robot_receive_timeout_);
     }
     packet_read_ = false;
   }
@@ -1292,7 +1297,7 @@ void HardwareInterface::startJointInterpolation(const hardware_interface::JointT
 {
   size_t point_number = trajectory.trajectory.points.size();
   ROS_DEBUG("Starting joint-based trajectory forward");
-  ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, point_number);
+  ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, point_number, robot_receive_timeout_);
   double last_time = 0.0;
   for (size_t i = 0; i < point_number; i++)
   {
@@ -1354,7 +1359,7 @@ void HardwareInterface::startCartesianInterpolation(const hardware_interface::Ca
 {
   size_t point_number = trajectory.trajectory.points.size();
   ROS_DEBUG("Starting cartesian trajectory forward");
-  ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, point_number);
+  ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, point_number, robot_receive_timeout_);
   double last_time = 0.0;
   for (size_t i = 0; i < point_number; i++)
   {