Adding maximum retry counter in gpio controller (Multiarm part 3) - v2 (#672) (#696)

* Add waitForAsyncCommand method in gpio controller which introduces a timeout while waiting for the async command success

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

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

Author: mergify[bot]

ur_controllers/include/ur_controllers/gpio_controller.hpp

@@ -184,6 +184,12 @@ class GPIOController : public controller_interface::ControllerInterface
   // TODO(anyone) publishers to add: tcp_pose_pub_
   // TODO(anyone) subscribers to add: script_command_sub_
   // TODO(anyone) service servers to add: resend_robot_program_srv_, deactivate_srv_, set_payload_srv_, tare_sensor_srv_
+
+  /**
+   * @brief wait until a command interface isn't in state ASYNC_WAITING anymore or until the parameter maximum_retries
+   * have been reached
+   */
+  bool waitForAsyncCommand(std::function<double(void)> get_value);
 };
 }  // namespace ur_controllers
 

ur_controllers/src/gpio_controller.cpp

@@ -347,9 +347,9 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs:
 
     RCLCPP_INFO(get_node()->get_logger(), "Setting digital output '%d' to state: '%1.0f'.", req->pin, req->state);
 
-    while (command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) {
-      // Asynchronous wait until the hardware interface has set the io value
-      std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) {
+      RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the "
+                                            "mocked interface)");
     }
 
     resp->success = static_cast<bool>(command_interfaces_[IO_ASYNC_SUCCESS].get_value());
@@ -361,9 +361,9 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs:
 
     RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%1.0f'.", req->pin, req->state);
 
-    while (command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) {
-      // Asynchronous wait until the hardware interface has set the io value
-      std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) {
+      RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the "
+                                            "mocked interface)");
     }
 
     resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value());
@@ -374,9 +374,9 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs:
 
     RCLCPP_INFO(get_node()->get_logger(), "Setting tool voltage to: '%1.0f'.", req->state);
 
-    while (command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) {
-      // Asynchronous wait until the hardware interface has set the io value
-      std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) {
+      RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the "
+                                            "mocked interface)");
     }
 
     resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value());
@@ -398,9 +398,11 @@ bool GPIOController::setSpeedSlider(ur_msgs::srv::SetSpeedSliderFraction::Reques
     command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_CMD].set_value(
         static_cast<double>(req->speed_slider_fraction));
 
-    while (command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) {
-      // Asynchronouse wait until the hardware interface has set the slider value
-      std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    if (!waitForAsyncCommand([&]() {
+          return command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_ASYNC_SUCCESS].get_value();
+        })) {
+      RCLCPP_WARN(get_node()->get_logger(), "Could not verify that target speed fraction was set. (This might happen "
+                                            "when using the mocked interface)");
     }
     resp->success =
         static_cast<bool>(command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_ASYNC_SUCCESS].get_value());
@@ -421,9 +423,10 @@ bool GPIOController::resendRobotProgram(std_srvs::srv::Trigger::Request::SharedP
   // call the service in the hardware
   command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_CMD].set_value(1.0);
 
-  while (command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) {
-    // Asynchronous wait until the hardware interface has set the slider value
-    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  if (!waitForAsyncCommand(
+          [&]() { return command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].get_value(); })) {
+    RCLCPP_WARN(get_node()->get_logger(), "Could not verify that program was sent. (This might happen when using the "
+                                          "mocked interface)");
   }
   resp->success =
       static_cast<bool>(command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].get_value());
@@ -446,9 +449,10 @@ bool GPIOController::handBackControl(std_srvs::srv::Trigger::Request::SharedPtr
   // call the service in the hardware
   command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_CMD].set_value(1.0);
 
-  while (command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) {
-    // Asynchronous wait until the command has been executed
-    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  if (!waitForAsyncCommand(
+          [&]() { return command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].get_value(); })) {
+    RCLCPP_WARN(get_node()->get_logger(), "Could not verify that hand_back_control was correctly triggered. (This "
+                                          "might happen when using the mocked interface)");
   }
   resp->success =
       static_cast<bool>(command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].get_value());
@@ -474,9 +478,10 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP
   command_interfaces_[CommandInterfaces::PAYLOAD_COG_Y].set_value(req->center_of_gravity.y);
   command_interfaces_[CommandInterfaces::PAYLOAD_COG_Z].set_value(req->center_of_gravity.z);
 
-  while (command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) {
-    // Asynchronous wait until the hardware interface has set the payload
-    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  if (!waitForAsyncCommand(
+          [&]() { return command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_value(); })) {
+    RCLCPP_WARN(get_node()->get_logger(), "Could not verify that payload was set. (This might happen when using the "
+                                          "mocked interface)");
   }
 
   resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_value());
@@ -499,9 +504,10 @@ bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*r
   // call the service in the hardware
   command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_CMD].set_value(1.0);
 
-  while (command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) {
-    // Asynchronous wait until the hardware interface has set the slider value
-    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  if (!waitForAsyncCommand(
+          [&]() { return command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_value(); })) {
+    RCLCPP_WARN(get_node()->get_logger(), "Could not verify that FTS was zeroed. (This might happen when using the "
+                                          "mocked interface)");
   }
 
   resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_value());
@@ -524,6 +530,20 @@ void GPIOController::initMsgs()
   io_msg_.analog_out_states.resize(2);
 }
 
+bool GPIOController::waitForAsyncCommand(std::function<double(void)> get_value)
+{
+  const auto maximum_retries = params_.check_io_successfull_retries;
+  int retries = 0;
+  while (get_value() == ASYNC_WAITING) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    retries++;
+
+    if (retries > maximum_retries)
+      return false;
+  }
+  return true;
+}
+
 }  // namespace ur_controllers
 
 #include "pluginlib/class_list_macros.hpp"

ur_controllers/src/gpio_controller_parameters.yaml

@@ -4,3 +4,8 @@ gpio_controller:
     default_value: "",
     description: "Urdf prefix of the corresponding arm"
    }
+   check_io_successfull_retries: {
+      type: int,
+      default_value: 10,
+      description: "Amount of retries for checking if the selected gpio was set successfully"
+   }