Introduce hand back control service (#528) (#670)

Add hand back control service to enable return of urcap and mixed ROS/Teach Pendant programming

Co-authored-by: Felix Exner (fexner) <[email protected]>
(cherry picked from commit e27cb90)

Co-authored-by: livanov93 <[email protected]>

Author: mergify[bot]

ur_controllers/include/ur_controllers/gpio_controller.hpp

@@ -76,6 +76,8 @@ enum CommandInterfaces
   PAYLOAD_ASYNC_SUCCESS = 30,
   ZERO_FTSENSOR_CMD = 31,
   ZERO_FTSENSOR_ASYNC_SUCCESS = 32,
+  HAND_BACK_CONTROL_CMD = 33,
+  HAND_BACK_CONTROL_ASYNC_SUCCESS = 34,
 };
 
 enum StateInterfaces
@@ -125,6 +127,9 @@ class GPIOController : public controller_interface::ControllerInterface
   bool resendRobotProgram(std_srvs::srv::Trigger::Request::SharedPtr req,
                           std_srvs::srv::Trigger::Response::SharedPtr resp);
 
+  bool handBackControl(std_srvs::srv::Trigger::Request::SharedPtr req,
+                       std_srvs::srv::Trigger::Response::SharedPtr resp);
+
   bool setPayload(const ur_msgs::srv::SetPayload::Request::SharedPtr req,
                   ur_msgs::srv::SetPayload::Response::SharedPtr resp);
 
@@ -152,6 +157,7 @@ class GPIOController : public controller_interface::ControllerInterface
 
   // services
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr resend_robot_program_srv_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr hand_back_control_srv_;
   rclcpp::Service<ur_msgs::srv::SetSpeedSliderFraction>::SharedPtr set_speed_slider_srv_;
   rclcpp::Service<ur_msgs::srv::SetIO>::SharedPtr set_io_srv_;
   rclcpp::Service<ur_msgs::srv::SetPayload>::SharedPtr set_payload_srv_;

ur_controllers/src/gpio_controller.cpp

@@ -84,6 +84,10 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c
   config.names.emplace_back("zero_ftsensor/zero_ftsensor_cmd");
   config.names.emplace_back("zero_ftsensor/zero_ftsensor_async_success");
 
+  // hand back control --> make UR-program return
+  config.names.emplace_back("hand_back_control/hand_back_control_cmd");
+  config.names.emplace_back("hand_back_control/hand_back_control_async_success");
+
   return config;
 }
 
@@ -278,6 +282,10 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre
         "~/resend_robot_program",
         std::bind(&GPIOController::resendRobotProgram, this, std::placeholders::_1, std::placeholders::_2));
 
+    hand_back_control_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
+        "~/hand_back_control",
+        std::bind(&GPIOController::handBackControl, this, std::placeholders::_1, std::placeholders::_2));
+
     set_payload_srv_ = get_node()->create_service<ur_msgs::srv::SetPayload>(
         "~/set_payload", std::bind(&GPIOController::setPayload, this, std::placeholders::_1, std::placeholders::_2));
 
@@ -408,6 +416,31 @@ bool GPIOController::resendRobotProgram(std_srvs::srv::Trigger::Request::SharedP
   return true;
 }
 
+bool GPIOController::handBackControl(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
+                                     std_srvs::srv::Trigger::Response::SharedPtr resp)
+{
+  // reset success flag
+  command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+  // 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));
+  }
+  resp->success =
+      static_cast<bool>(command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].get_value());
+
+  if (resp->success) {
+    RCLCPP_INFO(get_node()->get_logger(), "Deactivated control");
+  } else {
+    RCLCPP_ERROR(get_node()->get_logger(), "Could not deactivate control");
+    return false;
+  }
+
+  return true;
+}
+
 bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedPtr req,
                                 ur_msgs::srv::SetPayload::Response::SharedPtr resp)
 {

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -182,6 +182,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   double resend_robot_program_async_success_;
   double zero_ftsensor_cmd_;
   double zero_ftsensor_async_success_;
+  double hand_back_control_cmd_;
+  double hand_back_control_async_success_;
   bool first_pass_;
   bool initialized_;
   double system_interface_initialized_;

ur_robot_driver/src/hardware_interface.cpp

@@ -244,6 +244,12 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
   command_interfaces.emplace_back(hardware_interface::CommandInterface(
       "resend_robot_program", "resend_robot_program_async_success", &resend_robot_program_async_success_));
 
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("hand_back_control", "hand_back_control_cmd", &hand_back_control_cmd_));
+
+  command_interfaces.emplace_back(hardware_interface::CommandInterface(
+      "hand_back_control", "hand_back_control_async_success", &hand_back_control_async_success_));
+
   command_interfaces.emplace_back(hardware_interface::CommandInterface("payload", "mass", &payload_mass_));
   command_interfaces.emplace_back(
       hardware_interface::CommandInterface("payload", "cog.x", &payload_center_of_gravity_[0]));
@@ -562,6 +568,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
       target_speed_fraction_cmd_ = NO_NEW_CMD_;
       resend_robot_program_cmd_ = NO_NEW_CMD_;
       zero_ftsensor_cmd_ = NO_NEW_CMD_;
+      hand_back_control_cmd_ = NO_NEW_CMD_;
       initialized_ = true;
     }
 
@@ -665,6 +672,12 @@ void URPositionHardwareInterface::checkAsyncIO()
     resend_robot_program_cmd_ = NO_NEW_CMD_;
   }
 
+  if (!std::isnan(hand_back_control_cmd_) && ur_driver_ != nullptr) {
+    robot_program_running_ = false;
+    hand_back_control_async_success_ = true;
+    hand_back_control_cmd_ = NO_NEW_CMD_;
+  }
+
   if (!std::isnan(payload_mass_) && !std::isnan(payload_center_of_gravity_[0]) &&
       !std::isnan(payload_center_of_gravity_[1]) && !std::isnan(payload_center_of_gravity_[2]) &&
       ur_driver_ != nullptr) {