Revert "Added service to enable and disable tool contact"

This reverts commit f6bf8c6.

Author: URJala

ur_controllers/include/ur_controllers/gpio_controller.hpp

@@ -57,7 +57,6 @@
 #include "rclcpp/duration.hpp"
 #include "std_msgs/msg/bool.hpp"
 #include "gpio_controller_parameters.hpp"
-#include "ur_msgs/srv/get_version.hpp"
 
 namespace ur_controllers
 {
@@ -80,16 +79,6 @@ enum CommandInterfaces
   ZERO_FTSENSOR_ASYNC_SUCCESS = 32,
   HAND_BACK_CONTROL_CMD = 33,
   HAND_BACK_CONTROL_ASYNC_SUCCESS = 34,
-  START_TOOL_CONTACT_CMD = 35,
-  START_TOOL_CONTACT_ASYNC_SUCCESS = 36,
-  END_TOOL_CONTACT_CMD = 37,
-  END_TOOL_CONTACT_ASYNC_SUCCESS = 38,
-  GET_VERSION_CMD = 39,
-  GET_VERSION_ASYNC_SUCCESS = 40,
-  GET_VERSION_MAJOR = 41,
-  GET_VERSION_MINOR = 42,
-  GET_VERSION_BUGFIX = 43,
-  GET_VERSION_BUILD = 44
 };
 
 enum StateInterfaces
@@ -147,13 +136,6 @@ class GPIOController : public controller_interface::ControllerInterface
 
   bool zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp);
 
-  bool startToolContact(std_srvs::srv::Trigger::Request::SharedPtr req,
-                        std_srvs::srv::Trigger::Response::SharedPtr resp);
-
-  bool endToolContact(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp);
-
-  bool getVersion(ur_msgs::srv::GetVersion::Request::SharedPtr req, ur_msgs::srv::GetVersion::Response::SharedPtr resp);
-
   void publishIO();
 
   void publishToolData();
@@ -181,9 +163,6 @@ class GPIOController : public controller_interface::ControllerInterface
   rclcpp::Service<ur_msgs::srv::SetIO>::SharedPtr set_io_srv_;
   rclcpp::Service<ur_msgs::srv::SetPayload>::SharedPtr set_payload_srv_;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr tare_sensor_srv_;
-  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr start_tool_contact_srv_;
-  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr end_tool_contact_srv_;
-  rclcpp::Service<ur_msgs::srv::GetVersion>::SharedPtr get_version_srv_;
 
   std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::IOStates>> io_pub_;
   std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::ToolDataMsg>> tool_data_pub_;

ur_controllers/src/gpio_controller.cpp

@@ -97,20 +97,6 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c
   config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_cmd");
   config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_async_success");
 
-  // Start or stop tool contact functionality
-  config.names.emplace_back(tf_prefix + "start_tool_contact/start_tool_contact_cmd");
-  config.names.emplace_back(tf_prefix + "start_tool_contact/start_tool_contact_async_success");
-  config.names.emplace_back(tf_prefix + "end_tool_contact/end_tool_contact_cmd");
-  config.names.emplace_back(tf_prefix + "end_tool_contact/end_tool_contact_async_success");
-
-  // Get version service
-  config.names.emplace_back(tf_prefix + "get_version/get_version_cmd");
-  config.names.emplace_back(tf_prefix + "get_version/get_version_async_success");
-  config.names.emplace_back(tf_prefix + "get_version/get_version_major");
-  config.names.emplace_back(tf_prefix + "get_version/get_version_minor");
-  config.names.emplace_back(tf_prefix + "get_version/get_version_bugfix");
-  config.names.emplace_back(tf_prefix + "get_version/get_version_build");
-
   return config;
 }
 
@@ -326,17 +312,6 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre
     tare_sensor_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
         "~/zero_ftsensor",
         std::bind(&GPIOController::zeroFTSensor, this, std::placeholders::_1, std::placeholders::_2));
-
-    start_tool_contact_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
-        "~/start_tool_contact",
-        std::bind(&GPIOController::startToolContact, this, std::placeholders::_1, std::placeholders::_2));
-
-    end_tool_contact_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
-        "~/end_tool_contact",
-        std::bind(&GPIOController::endToolContact, this, std::placeholders::_1, std::placeholders::_2));
-
-    get_version_srv_ = get_node()->create_service<ur_msgs::srv::GetVersion>(
-        "~/get_version", std::bind(&GPIOController::getVersion, this, std::placeholders::_1, std::placeholders::_2));
   } catch (...) {
     return LifecycleNodeInterface::CallbackReturn::ERROR;
   }
@@ -545,81 +520,6 @@ bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*r
   return true;
 }
 
-bool GPIOController::startToolContact(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
-                                      std_srvs::srv::Trigger::Response::SharedPtr resp)
-{
-  // reset success flag
-  command_interfaces_[CommandInterfaces::START_TOOL_CONTACT_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
-
-  // call the service in the hardware
-  command_interfaces_[CommandInterfaces::START_TOOL_CONTACT_CMD].set_value(1.0);
-
-  if (!waitForAsyncCommand(
-          [&]() { return command_interfaces_[CommandInterfaces::START_TOOL_CONTACT_ASYNC_SUCCESS].get_value(); })) {
-    RCLCPP_WARN(get_node()->get_logger(), "Could not verify that tool contact was started.");
-  }
-
-  resp->success =
-      static_cast<bool>(command_interfaces_[CommandInterfaces::START_TOOL_CONTACT_ASYNC_SUCCESS].get_value());
-
-  if (resp->success) {
-    RCLCPP_INFO(get_node()->get_logger(), "Successfully started tool contact");
-  } else {
-    RCLCPP_ERROR(get_node()->get_logger(), "Could not start tool contact");
-    return false;
-  }
-
-  return true;
-}
-
-bool GPIOController::endToolContact(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
-                                    std_srvs::srv::Trigger::Response::SharedPtr resp)
-{
-  // reset success flag
-  command_interfaces_[CommandInterfaces::END_TOOL_CONTACT_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
-
-  // call the service in the hardware
-  command_interfaces_[CommandInterfaces::END_TOOL_CONTACT_CMD].set_value(1.0);
-
-  if (!waitForAsyncCommand(
-          [&]() { return command_interfaces_[CommandInterfaces::END_TOOL_CONTACT_ASYNC_SUCCESS].get_value(); })) {
-    RCLCPP_WARN(get_node()->get_logger(), "Could not verify that tool contact was stopped.");
-  }
-
-  resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::END_TOOL_CONTACT_ASYNC_SUCCESS].get_value());
-
-  if (resp->success) {
-    RCLCPP_INFO(get_node()->get_logger(), "Successfully stopped tool contact");
-  } else {
-    RCLCPP_ERROR(get_node()->get_logger(), "Could not stop tool contact");
-    return false;
-  }
-
-  return true;
-}
-
-bool GPIOController::getVersion(ur_msgs::srv::GetVersion::Request::SharedPtr /*req*/,
-                                ur_msgs::srv::GetVersion::Response::SharedPtr resp)
-{
-  // reset success flag
-  command_interfaces_[CommandInterfaces::GET_VERSION_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
-
-  // call the service in the hardware
-  command_interfaces_[CommandInterfaces::GET_VERSION_CMD].set_value(1.0);
-
-  if (!waitForAsyncCommand(
-          [&]() { return command_interfaces_[CommandInterfaces::GET_VERSION_ASYNC_SUCCESS].get_value(); })) {
-    RCLCPP_WARN(get_node()->get_logger(), "Could not verify software version of robot.");
-  }
-
-  resp->major = static_cast<uint32_t>(command_interfaces_[CommandInterfaces::GET_VERSION_MAJOR].get_value());
-  resp->minor = static_cast<uint32_t>(command_interfaces_[CommandInterfaces::GET_VERSION_MINOR].get_value());
-  resp->bugfix = static_cast<uint32_t>(command_interfaces_[CommandInterfaces::GET_VERSION_BUGFIX].get_value());
-  resp->build = static_cast<uint32_t>(command_interfaces_[CommandInterfaces::GET_VERSION_BUILD].get_value());
-
-  return true;
-}
-
 void GPIOController::initMsgs()
 {
   io_msg_.digital_in_states.resize(standard_digital_output_cmd_.size());

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -191,16 +191,6 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   bool initialized_;
   double system_interface_initialized_;
   bool async_thread_shutdown_;
-  double start_tool_contact_cmd_;
-  double start_tool_contact_async_success_;
-  double end_tool_contact_cmd_;
-  double end_tool_contact_async_success_;
-  double get_version_async_success_;
-  double get_version_cmd_;
-  double get_version_major_;
-  double get_version_minor_;
-  double get_version_bugfix_;
-  double get_version_build_;
 
   // payload stuff
   urcl::vector3d_t payload_center_of_gravity_;

ur_robot_driver/src/hardware_interface.cpp

@@ -298,36 +298,6 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
   command_interfaces.emplace_back(hardware_interface::CommandInterface(
       tf_prefix + "zero_ftsensor", "zero_ftsensor_async_success", &zero_ftsensor_async_success_));
 
-  command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      tf_prefix + "start_tool_contact", "start_tool_contact_cmd", &start_tool_contact_cmd_));
-
-  command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      tf_prefix + "start_tool_contact", "start_tool_contact_async_success", &start_tool_contact_async_success_));
-
-  command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + "end_tool_contact",
-                                                                       "end_tool_contact_cmd", &end_tool_contact_cmd_));
-
-  command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      tf_prefix + "end_tool_contact", "end_tool_contact_async_success", &end_tool_contact_async_success_));
-
-  command_interfaces.emplace_back(
-      hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_cmd", &get_version_cmd_));
-
-  command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      tf_prefix + "get_version", "get_version_async_success", &get_version_async_success_));
-
-  command_interfaces.emplace_back(
-      hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_major", &get_version_major_));
-
-  command_interfaces.emplace_back(
-      hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_minor", &get_version_minor_));
-
-  command_interfaces.emplace_back(
-      hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_bugfix", &get_version_bugfix_));
-
-  command_interfaces.emplace_back(
-      hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_build", &get_version_build_));
-
   return command_interfaces;
 }
 
@@ -633,9 +603,6 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
       resend_robot_program_cmd_ = NO_NEW_CMD_;
       zero_ftsensor_cmd_ = NO_NEW_CMD_;
       hand_back_control_cmd_ = NO_NEW_CMD_;
-      start_tool_contact_cmd_ = NO_NEW_CMD_;
-      end_tool_contact_cmd_ = NO_NEW_CMD_;
-      get_version_cmd_ = NO_NEW_CMD_;
       initialized_ = true;
     }
 
@@ -760,26 +727,6 @@ void URPositionHardwareInterface::checkAsyncIO()
     zero_ftsensor_async_success_ = ur_driver_->zeroFTSensor();
     zero_ftsensor_cmd_ = NO_NEW_CMD_;
   }
-
-  if (!std::isnan(start_tool_contact_cmd_) && ur_driver_ != nullptr) {
-    start_tool_contact_async_success_ = ur_driver_->startToolContact();
-    start_tool_contact_cmd_ = NO_NEW_CMD_;
-  }
-
-  if (!std::isnan(end_tool_contact_cmd_) && ur_driver_ != nullptr) {
-    end_tool_contact_async_success_ = ur_driver_->endToolContact();
-    end_tool_contact_cmd_ = NO_NEW_CMD_;
-  }
-
-  if (!std::isnan(get_version_cmd_) && ur_driver_ != nullptr) {
-    urcl::VersionInformation version_info = ur_driver_->getVersion();
-    get_version_cmd_ = NO_NEW_CMD_;
-    get_version_major_ = version_info.major;
-    get_version_minor_ = version_info.minor;
-    get_version_bugfix_ = version_info.bugfix;
-    get_version_build_ = version_info.build;
-    get_version_async_success_ = 1.0;
-  }
 }
 
 void URPositionHardwareInterface::updateNonDoubleValues()

ur_robot_driver/test/robot_driver.py

@@ -249,20 +249,6 @@ def test_trajectory_scaled(self, tf_prefix):
         )
         self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
 
-    def test_tool_contact(self, tf_prefix):
-        if self._io_status_controller_interface.get_version().major < 5:
-            start_result = self._io_status_controller_interface.start_tool_contact()
-            self.assertEqual(start_result.success, False)
-
-            end_result = self._io_status_controller_interface.end_tool_contact()
-            self.assertEqual(end_result.success, False)
-        else:
-            start_result = self._io_status_controller_interface.start_tool_contact()
-            self.assertEqual(start_result.success, True)
-
-            end_result = self._io_status_controller_interface.end_tool_contact()
-            self.assertEqual(end_result.success, True)
-
     def test_trajectory_scaled_aborts_on_violation(self, tf_prefix):
         """Test that the robot correctly aborts the trajectory when the constraints are violated."""
         # Construct test trajectory

ur_robot_driver/test/test_common.py

@@ -52,7 +52,7 @@
     IsProgramRunning,
     Load,
 )
-from ur_msgs.srv import SetIO, GetVersion
+from ur_msgs.srv import SetIO
 
 TIMEOUT_WAIT_SERVICE = 10
 TIMEOUT_WAIT_SERVICE_INITIAL = 120  # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable.
@@ -240,12 +240,7 @@ class IoStatusInterface(
     _ServiceInterface,
     namespace="/io_and_status_controller",
     initial_services={"set_io": SetIO},
-    services={
-        "resend_robot_program": Trigger,
-        "get_version": GetVersion,
-        "start_tool_contact": Trigger,
-        "end_tool_contact": Trigger,
-    },
+    services={"resend_robot_program": Trigger},
 ):
     pass
 

ur_robot_driver/urdf/ur.ros2_control.xacro

@@ -224,25 +224,6 @@
         <command_interface name="zero_ftsensor_async_success"/>
       </gpio>
 
-      <gpio name="${tf_prefix}start_tool_contact">
-        <command_interface name="start_tool_contact_cmd"/>
-        <command_interface name="start_tool_contact_async_success"/>
-      </gpio>
-
-      <gpio name="${tf_prefix}end_tool_contact">
-        <command_interface name="end_tool_contact_cmd"/>
-        <command_interface name="end_tool_contact_async_success"/>
-      </gpio>
-
-      <gpio name="${tf_prefix}get_version">
-        <command_interface name="get_version_cmd"/>
-        <command_interface name="get_version_async_success"/>
-        <command_interface name="get_version_major"/>
-        <command_interface name="get_version_minor"/>
-        <command_interface name="get_version_bugfix"/>
-        <command_interface name="get_version_build"/>
-      </gpio>
-
       <gpio name="${tf_prefix}system_interface">
         <state_interface name="initialized"/>
       </gpio>