[SW-951] Add gripper open and close services to spot_ros2 (#344)

spot_driver/spot_driver/spot_ros2.py

@@ -592,6 +592,23 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw
                 callback_group=self.group,
             )
 
+            self.create_service(
+                Trigger,
+                "open_gripper",
+                lambda request, response: self.service_wrapper(
+                    "open_gripper", self.handle_open_gripper, request, response
+                ),
+                callback_group=self.group,
+            )
+            self.create_service(
+                Trigger,
+                "close_gripper",
+                lambda request, response: self.service_wrapper(
+                    "close_gripper", self.handle_close_gripper, request, response
+                ),
+                callback_group=self.group,
+            )
+
         self.create_service(
             SetBool,
             "stair_mode",
@@ -1309,6 +1326,24 @@ def handle_carry(self, request: Trigger.Request, response: Trigger.Response) ->
         response.success, response.message = self.spot_wrapper.spot_arm.arm_carry()
         return response
 
+    def handle_open_gripper(self, request: Trigger.Request, response: Trigger.Response) -> Trigger.Response:
+        """ROS service handler to open the gripper."""
+        if self.spot_wrapper is None:
+            response.success = False
+            response.message = "Spot wrapper is undefined"
+            return response
+        response.success, response.message = self.spot_wrapper.spot_arm.gripper_open()
+        return response
+
+    def handle_close_gripper(self, request: Trigger.Request, response: Trigger.Response) -> Trigger.Response:
+        """ROS service handler to close the gripper."""
+        if self.spot_wrapper is None:
+            response.success = False
+            response.message = "Spot wrapper is undefined"
+            return response
+        response.success, response.message = self.spot_wrapper.spot_arm.gripper_close()
+        return response
+
     def handle_clear_behavior_fault(
         self, request: ClearBehaviorFault.Request, response: ClearBehaviorFault.Response
     ) -> ClearBehaviorFault.Response: