Added documentation and started writing tests

Also fixed some little bugs

Author: URJala

ur_controllers/doc/index.rst

@@ -11,6 +11,8 @@ 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.
+* A **trajectory_until_node**. This is not a controller in itself, but allows for executing a trajectory with any of the motion controllers while having tool contact enabled.
 
 About this package
 ------------------
@@ -378,3 +380,63 @@ 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 that the tool has made contact with something, 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.
+This is not a complete interface of the URScript function ``tool_contact(direction)``, 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 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.
+
+* ``~/enable_tool_contact [ur_msgs/action/ToolContact]``
+
+The goal section of ``ur_msgs/action/ToolContact`` has no fields, as a call to the action implicitly means that tool contact should be enabled.
+The result section has one field ``result``, which contains the result from the tool contact in the form of an integer.
+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/enable_tool_contact ur_msgs/action/ToolContact
+
+.. _trajectory_until_node:
+
+ur_controllers/TrajectoryUntilNode
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+This is not a controller in itself, but it allows the user to execute a trajectory with tool contact enabled without having to call 2 actions at the same time.
+This means that the trajectory will execute until either the trajectory is finished or tool contact has been triggered.
+Both scenarios will result in the trajectory being reported as successful.
+
+Action interface / usage
+""""""""""""""""""""""""
+The node provides an action to execute a trajectory with tool contact enabled. For the node to accept action goals, both the motion controller and the tool contact controller need to be in ``active`` state.
+
+* ``/trajectory_until_node/execute [ur_msgs/action/TrajectoryUntil]``
+
+The action contains all the same fields as the ordinary `FollowJointTrajectory <http://docs.ros.org/en/noetic/api/control_msgs/html/action/FollowJointTrajectory.html>`_ action, but has two additional fields.
+One in the goal section called ``until_type``, which is used to choose between different conditions that can stop the trajectory. Currently only tool contact is available.
+The result section contains the other new field called ``until_condition_result``, which reports whether the chosen condition was triggered or not, and also error codes if something went wrong with the controller responsible for the until condition.
+
+Implementation details
+""""""""""""""""""""""
+Upon instantiation of the node, the internal trajectory action client will connect to an action named ``motion_controller/follow_joint_trajectory``.
+This action does not exist, but upon launch of the driver, the node is remapped to connect to the ``initial_joint_controller``, default is ``scaled_joint_trajectory_controller``.
+If you wish to use the node with another motion controller use the launch argument ``initial_joint_controller:=<your_motion_controller>`` when launching the driver.

ur_controllers/src/tool_contact_controller_parameters.yaml

@@ -1,4 +1,3 @@
----
 tool_contact_controller:
   tf_prefix: {
     type: string,

ur_controllers/src/trajectory_until_node.cpp

@@ -79,7 +79,7 @@ TrajectoryUntilNode::TrajectoryUntilNode(const rclcpp::NodeOptions& options)
 
   // Create action server to advertise the "/trajectory_until/execute"
   action_server_ = rclcpp_action::create_server<TrajectoryUntil>(
-      this, "trajectory_until/execute",
+      this, std::string(this->get_name()) + "/execute",
       std::bind(&TrajectoryUntilNode::goal_received_callback, this, std::placeholders::_1, std::placeholders::_2),
       std::bind(&TrajectoryUntilNode::goal_cancelled_callback, this, std::placeholders::_1),
       std::bind(&TrajectoryUntilNode::goal_accepted_callback, this, std::placeholders::_1),

ur_robot_driver/src/hardware_interface.cpp

@@ -1046,14 +1046,20 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
   for (const auto& key : start_interfaces) {
     for (auto i = 0u; i < info_.joints.size(); i++) {
       if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) {
-        if (!start_modes_[i].empty()) {
+        if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
+              return item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO ||
+                     item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO;
+            })) {
           RCLCPP_ERROR(get_logger(), "Attempting to start position control while there is another control mode already "
                                      "requested.");
           return hardware_interface::return_type::ERROR;
         }
         start_modes_[i] = { hardware_interface::HW_IF_POSITION };
       } else if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
-        if (!start_modes_[i].empty()) {
+        if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
+              return item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO ||
+                     item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO;
+            })) {
           RCLCPP_ERROR(get_logger(), "Attempting to start velocity control while there is another control mode already "
                                      "requested.");
           return hardware_interface::return_type::ERROR;

ur_robot_driver/test/integration_test_tool_contact.py

@@ -39,8 +39,9 @@
 from rclpy.node import Node
 
 from controller_manager_msgs.srv import SwitchController
-
-from ur_msgs.action import ToolContact
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from builtin_interfaces.msg import Duration
+from ur_msgs.action import ToolContact, TrajectoryUntil
 
 sys.path.append(os.path.dirname(__file__))
 from test_common import (  # noqa: E402
@@ -49,18 +50,21 @@
     IoStatusInterface,
     ActionInterface,
     generate_driver_test_description,
+    ROBOT_JOINTS,
 )
 
 TIMEOUT_EXECUTE_TRAJECTORY = 30
 
 
 @pytest.mark.launch_test
 @launch_testing.parametrize(
-    "tf_prefix",
-    [(""), ("my_ur_")],
+    "tf_prefix, initial_joint_controller",
+    [("", "scaled_joint_trajectory_controller"), ("my_ur_", "passthrough_trajectory_controller")],
 )
-def generate_test_description(tf_prefix):
-    return generate_driver_test_description(tf_prefix=tf_prefix)
+def generate_test_description(tf_prefix, initial_joint_controller):
+    return generate_driver_test_description(
+        tf_prefix=tf_prefix, initial_joint_controller=initial_joint_controller
+    )
 
 
 class RobotDriverTest(unittest.TestCase):
@@ -85,16 +89,58 @@ def init_robot(self):
         self._tool_contact_interface = ActionInterface(
             self.node, "/tool_contact_controller/enable_tool_contact", ToolContact
         )
+        self._trajectory_until_interface = ActionInterface(
+            self.node, "/trajectory_until/execute", TrajectoryUntil
+        )
 
     def setUp(self):
         self._dashboard_interface.start_robot()
         time.sleep(1)
         self.assertTrue(self._io_status_controller_interface.resend_robot_program().success)
 
+    #
+    # Tests
+    #
+
     def test_start_tool_contact_controller(self):
         self.assertTrue(
             self._controller_manager_interface.switch_controller(
                 strictness=SwitchController.Request.BEST_EFFORT,
                 activate_controllers=["tool_contact_controller"],
             ).ok
         )
+
+    def test_tool_contact(self):
+        goal_handle = self._tool_contact_interface.send_goal()
+        self.assertTrue(goal_handle.accepted)
+
+        cancel_res = self._tool_contact_interface.cancel_goal(goal_handle)
+        self.assertEqual(cancel_res.return_code, 0)
+
+    def test_trajectory_until(self):
+        L_pose_to_down = {
+            "waypts": [[-0.45, -1.5, 1.5, -1.5, -1.5, -1.5], [-0.45, -1.2, 2.1, -2.4, -1.5, -1.5]],
+            "time_vec": [Duration(sec=3, nanosec=0), Duration(sec=6, nanosec=0)],
+        }
+
+        trajectory = JointTrajectory()
+        trajectory.joint_names = ROBOT_JOINTS
+
+        trajectory.points = [
+            JointTrajectoryPoint(
+                positions=L_pose_to_down["waypts"][i], time_from_start=L_pose_to_down["time_vec"][i]
+            )
+            for i in range(len(L_pose_to_down["waypts"]))
+        ]
+
+        goal_handle = self._trajectory_until_interface.send_goal(
+            trajectory=trajectory, until_type=TrajectoryUntil.Goal.TOOL_CONTACT
+        )
+        self.assertTrue(goal_handle.accepted)
+
+        result = self._trajectory_until_interface.get_result(
+            goal_handle=goal_handle, timeout=TIMEOUT_EXECUTE_TRAJECTORY
+        )
+
+        self.assertEqual(result.error_code, TrajectoryUntil.Result.SUCCESSFUL)
+        self.assertGreaterEqual(result.until_condition_result, TrajectoryUntil.Result.NOT_TRIGGERED)

ur_robot_driver/test/robot_driver.py

@@ -270,13 +270,6 @@ def test_trajectory_scaled(self, tf_prefix):
         )
         self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
 
-    def test_tool_contact(self, tf_prefix):
-        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

@@ -191,6 +191,18 @@ def get_result(self, goal_handle, timeout):
                 f"Exception while calling action '{self.__action_name}': {future_res.exception()}"
             )
 
+    def cancel_goal(self, goal_handle, timeout=2):
+        future_res = goal_handle.cancel_goal_async()
+        logging.info("Canceling goal from '%s' with timeout %fs", self.__action_name, timeout)
+        rclpy.spin_until_future_complete(self.__node, future_res, timeout_sec=timeout)
+        if future_res.result() is not None:
+            logging.info("  Received result: %s", future_res.result())
+            return future_res.result()
+        else:
+            raise Exception(
+                f"Exception while calling action '{self.__action_name}': {future_res.exception()}"
+            )
+
 
 class DashboardInterface(
     _ServiceInterface,