Added controller to enable and disable tool contact (backport #940)

ur_controllers/CMakeLists.txt

@@ -51,6 +51,11 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
 
 include_directories(include)
 
+generate_parameter_library(
+  tool_contact_controller_parameters
+  src/tool_contact_controller_parameters.yaml
+)
+
 generate_parameter_library(
   force_mode_controller_parameters
   src/force_mode_controller_parameters.yaml
@@ -87,6 +92,7 @@ generate_parameter_library(
 )
 
 add_library(${PROJECT_NAME} SHARED
+  src/tool_contact_controller.cpp
   src/force_mode_controller.cpp
   src/scaled_joint_trajectory_controller.cpp
   src/speed_scaling_state_broadcaster.cpp
@@ -99,6 +105,7 @@ target_include_directories(${PROJECT_NAME} PRIVATE
   include
 )
 target_link_libraries(${PROJECT_NAME}
+  tool_contact_controller_parameters
   force_mode_controller_parameters
   gpio_controller_parameters
   speed_scaling_state_broadcaster_parameters
@@ -107,6 +114,7 @@ target_link_libraries(${PROJECT_NAME}
   passthrough_trajectory_controller_parameters
   ur_configuration_controller_parameters
 )
+
 ament_target_dependencies(${PROJECT_NAME}
   ${THIS_PACKAGE_INCLUDE_DEPENDS}
 )

ur_controllers/controller_plugins.xml

@@ -34,4 +34,9 @@
       Controller used to get and change the configuration of the robot
     </description>
   </class>
+  <class name="ur_controllers/ToolContactController" type="ur_controllers::ToolContactController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      Controller to use the tool contact functionality of the robot.
+    </description>
+  </class>
 </library>

ur_controllers/doc/index.rst

@@ -11,6 +11,7 @@ robot family. Currently this contains:
   but it uses the speed scaling reported to align progress of the trajectory between the robot and controller.
 * A **io_and_status_controller** that allows setting I/O ports, controlling some UR-specific
   functionality and publishes status information about the robot.
+* A **tool_contact_controller** that exposes an action to enable the tool contact function on the robot.
 
 About this package
 ------------------
@@ -378,3 +379,46 @@ The controller provides the ``~/enable_freedrive_mode`` topic of type ``[std_msg
 * to deactivate freedrive mode is enough to publish a ``False`` msg on the indicated topic or
   to deactivate the controller or to stop publishing ``True`` on the enable topic and wait for the
   controller timeout.
+
+.. _tool_contact_controller:
+
+ur_controllers/ToolContactController
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+This controller can enable tool contact on the robot. When tool contact is enabled, and the robot
+senses whether the tool has made contact with something. When that happens, it will stop all
+motion, and retract to where it first sensed the contact.
+
+This controller can be used with any of the motion controllers.
+
+The controller is not a direct representation of the URScript function `tool_contact(direction)
+<https://www.universal-robots.com/manuals/EN/HTML/SW5_21/Content/prod-scriptmanual/all_scripts/tool_contact%28direction%29.htm?Highlight=tool_contact>`_,
+as it does not allow for choosing the direction. The direction of tool contact will always be the
+current TCP direction of movement.
+
+Parameters
+""""""""""
+
++-------------------------+--------+---------------+---------------------------------------------------------------------------------------+
+| Parameter name          | Type   | Default value | Description                                                                           |
+|                         |        |               |                                                                                       |
++-------------------------+--------+---------------+---------------------------------------------------------------------------------------+
+| ``tf_prefix``           | string | <empty>       | Urdf prefix of the corresponding arm                                                  |
++-------------------------+--------+---------------+---------------------------------------------------------------------------------------+
+| ``action_monitor_rate`` | double | 20.0          | The rate at which the action should be monitored in Hz.                               |
++-------------------------+--------+---------------+---------------------------------------------------------------------------------------+
+
+Action interface / usage
+""""""""""""""""""""""""
+The controller provides one action for enabling tool contact. For the controller to accept action goals it needs to be in ``active`` state.
+
+* ``~/detect_tool_contact [ur_msgs/action/ToolContact]``
+
+  The action definition of ``ur_msgs/action/ToolContact`` has no fields, as a call to the action implicitly means that tool contact should be enabled.
+  The result of the action is available through the status of the action itself. If the action succeeds it means that tool contact was detected, otherwise tool contact will remain active until it is either cancelled by the user, or aborted by the hardware.
+  The action provides no feedback.
+
+  The action can be called from the command line using the following command, when the controller is active:
+
+  .. code-block::
+
+     ros2 action send_goal /tool_contact_controller/detect_tool_contact ur_msgs/action/ToolContact

ur_controllers/include/ur_controllers/tool_contact_controller.hpp

@@ -0,0 +1,135 @@
+// Copyright 2025, Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Jacob Larsen [email protected]
+ * \date    2025-01-07
+ *
+ *
+ *
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_
+#define UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_
+
+#include <chrono>
+#include <vector>
+#include <memory>
+
+#include <controller_interface/controller_interface.hpp>
+#include "std_msgs/msg/bool.hpp"
+#include <rclcpp_action/server.hpp>
+#include <rclcpp_action/create_server.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/server_goal_handle.hpp>
+#include <rclcpp/duration.hpp>
+
+#include <realtime_tools/realtime_buffer.hpp>
+#include <realtime_tools/realtime_server_goal_handle.hpp>
+
+#include <ur_msgs/action/tool_contact.hpp>
+#include "ur_controllers/tool_contact_controller_parameters.hpp"
+
+namespace ur_controllers
+{
+class ToolContactController : public controller_interface::ControllerInterface
+{
+public:
+  ToolContactController() = default;
+  ~ToolContactController() override = default;
+
+  controller_interface::CallbackReturn on_init() override;
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
+  controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+  controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+  controller_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state) override;
+
+  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+private:
+  using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<ur_msgs::action::ToolContact>;
+  using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
+  using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;
+
+  RealtimeGoalHandleBuffer rt_active_goal_;                     ///< Currently active action goal, if any.
+  ur_msgs::action::ToolContact::Feedback::SharedPtr feedback_;  ///< preallocated feedback
+  rclcpp::TimerBase::SharedPtr goal_handle_timer_;              ///< Timer to frequently check on the running goal
+
+  // non-rt function that will be called with action_monitor_period to monitor the rt action
+  rclcpp::Duration action_monitor_period_ = rclcpp::Duration::from_seconds(0.05);
+  void action_handler();
+
+  rclcpp_action::GoalResponse goal_received_callback(const rclcpp_action::GoalUUID& /*uuid*/,
+                                                     std::shared_ptr<const ur_msgs::action::ToolContact::Goal> goal);
+
+  void
+  goal_accepted_callback(std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::ToolContact>> goal_handle);
+
+  rclcpp_action::CancelResponse goal_canceled_callback(
+      const std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::ToolContact>> goal_handle);
+
+  std::atomic<bool> tool_contact_enable_ = false;
+  std::atomic<bool> tool_contact_active_ = false;
+  std::atomic<bool> tool_contact_abort_ = false;
+  std::atomic<bool> change_requested_ = false;
+  std::atomic<bool> logged_once_ = false;
+  std::atomic<bool> should_reset_goal = false;
+
+  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> tool_contact_result_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> major_version_state_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> tool_contact_state_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> tool_contact_set_state_interface_;
+
+  rclcpp_action::Server<ur_msgs::action::ToolContact>::SharedPtr tool_contact_action_server_;
+
+  std::shared_ptr<tool_contact_controller::ParamListener> tool_contact_param_listener_;
+  tool_contact_controller::Params tool_contact_params_;
+
+  static constexpr double TOOL_CONTACT_STANDBY = 1.0;
+  static constexpr double TOOL_CONTACT_WAITING_BEGIN = 2.0;
+  static constexpr double TOOL_CONTACT_EXECUTING = 3.0;
+  static constexpr double TOOL_CONTACT_FAILURE_BEGIN = 4.0;
+  static constexpr double TOOL_CONTACT_WAITING_END = 5.0;
+  static constexpr double TOOL_CONTACT_SUCCESS_END = 6.0;
+  static constexpr double TOOL_CONTACT_FAILURE_END = 7.0;
+};
+}  // namespace ur_controllers
+
+#endif  // UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_