Adds full nonblocking readout support (Multiarm part 4) - v2 (#673) (#703)

Adds full nonblocking readout support

---------

Co-authored-by: Lennart Nachtigall <[email protected]>
(cherry picked from commit 87363e4)

Co-authored-by: Lennart Nachtigall <[email protected]>

Author: mergify[bot]

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -221,6 +221,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
 
   std::unique_ptr<urcl::UrDriver> ur_driver_;
   std::shared_ptr<std::thread> async_thread_;
+
+  bool rtde_comm_has_been_started_ = false;
 };
 }  // namespace ur_robot_driver
 

ur_robot_driver/src/hardware_interface.cpp

@@ -413,6 +413,7 @@ URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous
   RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Initializing driver...");
   registerUrclLogHandler();
   try {
+    rtde_comm_has_been_started_ = false;
     ur_driver_ = std::make_unique<urcl::UrDriver>(
         robot_ip, script_filename, output_recipe_filename, input_recipe_filename,
         std::bind(&URPositionHardwareInterface::handleRobotProgramState, this, std::placeholders::_1), headless_mode,
@@ -444,8 +445,6 @@ URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous
                         "README.md] for details.");
   }
 
-  ur_driver_->startRTDECommunication();
-
   async_thread_ = std::make_shared<std::thread>(&URPositionHardwareInterface::asyncThread, this);
 
   RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "System successfully started!");
@@ -507,6 +506,12 @@ void URPositionHardwareInterface::asyncThread()
 hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::Time& time,
                                                                   const rclcpp::Duration& period)
 {
+  // We want to start the rtde comm the latest point possible due to the delay times arising from setting up the
+  // communication with multiple arms
+  if (!rtde_comm_has_been_started_) {
+    ur_driver_->startRTDECommunication();
+    rtde_comm_has_been_started_ = true;
+  }
   std::unique_ptr<rtde::DataPackage> data_pkg = ur_driver_->getDataPackage();
 
   if (data_pkg) {
@@ -587,8 +592,8 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
 
     return hardware_interface::return_type::OK;
   }
-
-  RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Unable to read from hardware...");
+  if (!non_blocking_read_)
+    RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Unable to read from hardware...");
   // TODO(anyone): could not read from the driver --> return ERROR --> on error will be called
   return hardware_interface::return_type::OK;
 }
@@ -641,6 +646,9 @@ void URPositionHardwareInterface::initAsyncIO()
 
 void URPositionHardwareInterface::checkAsyncIO()
 {
+  if (!rtde_comm_has_been_started_) {
+    return;
+  }
   for (size_t i = 0; i < 18; ++i) {
     if (!std::isnan(standard_dig_out_bits_cmd_[i]) && ur_driver_ != nullptr) {
       if (i <= 7) {