Implement test of tool contact

Author: URJala

ur_robot_driver/test/robot_driver.py

@@ -249,6 +249,13 @@ 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

@@ -240,7 +240,11 @@ class IoStatusInterface(
     _ServiceInterface,
     namespace="/io_and_status_controller",
     initial_services={"set_io": SetIO},
-    services={"resend_robot_program": Trigger},
+    services={
+        "resend_robot_program": Trigger,
+        "start_tool_contact": Trigger,
+        "end_tool_contact": Trigger,
+    },
 ):
     pass