From 0585800f6d57ed6639dbea6547ef8f4c5e082c12 Mon Sep 17 00:00:00 2001
From: URJala <jala@universal-robots.com>
Date: Thu, 11 Apr 2024 08:32:43 +0000
Subject: [PATCH] Implemented test of get_version service and integrated it
 with the test of tool contact

---
 ur_robot_driver/test/robot_driver.py | 14 ++++++++++++++
 ur_robot_driver/test/test_common.py  |  9 +++++++--
 2 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py
index 96c859947..f797c8db7 100755
--- a/ur_robot_driver/test/robot_driver.py
+++ b/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
diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py
index 435a20c37..91a6b7951 100644
--- a/ur_robot_driver/test/test_common.py
+++ b/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