Use robot_receive_timeout instead of keepalive_count

UniversalRobots · May 29, 2024 · 5ab3f4f · 5ab3f4f
1 parent 4255d83
commit 5ab3f4f
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 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
@@ -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
 

diff --git 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<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();