Implemented test of get_version service and integrated it with the te…

…st of tool contact

ur_robot_driver/test/robot_driver.py

@@ -249,6 +249,20 @@ 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
+from ur_msgs.srv import SetIO, GetVersion
 
 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,7 +240,12 @@ 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,
+        "get_version": GetVersion,
+    },
 ):
     pass