[MAPLE-663] Spot Gripper Angle Service (#488)

## Change Overview

This adds a service `<spot_name>/set_gripper_angle` that allows us to do more than just open and close the gripper and instead set a desired joint angle for the `f1x` joint.

As a side while I was here, I also moved the previously added `/arm_joint_commands` subscription to only appear when the robot has an arm

## Testing Done

Tested on robot running maple's Action Stack

spot_driver/spot_driver/spot_ros2.py

@@ -124,6 +124,7 @@
     OverrideGraspOrCarry,
     PlaySound,
     RetrieveLogpoint,
+    SetGripperAngle,
     SetGripperCameraParameters,
     SetLEDBrightness,
     SetLocomotion,
@@ -458,9 +459,6 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw
 
         self.create_subscription(Twist, "cmd_vel", self.cmd_velocity_callback, 1, callback_group=self.group)
         self.create_subscription(Pose, "body_pose", self.body_pose_callback, 1, callback_group=self.group)
-        self.create_subscription(
-            JointState, "arm_joint_commands", self.arm_joint_cmd_callback, 100, callback_group=self.group
-        )
         self.create_service(
             Trigger,
             "claim",
@@ -568,6 +566,9 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw
                 lambda request, response: self.service_wrapper("arm_carry", self.handle_arm_carry, request, response),
                 callback_group=self.group,
             )
+            self.create_subscription(
+                JointState, "arm_joint_commands", self.arm_joint_cmd_callback, 100, callback_group=self.group
+            )
 
             if not self.gripperless:
                 self.create_service(
@@ -586,6 +587,14 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw
                     ),
                     callback_group=self.group,
                 )
+                self.create_service(
+                    SetGripperAngle,
+                    "set_gripper_angle",
+                    lambda request, response: self.service_wrapper(
+                        "set_gripper_angle", self.handle_gripper_angle, request, response
+                    ),
+                    callback_group=self.group,
+                )
 
         self.create_service(
             SetBool,
@@ -1291,6 +1300,19 @@ def handle_close_gripper(self, request: Trigger.Request, response: Trigger.Respo
         response.success, response.message = self.spot_wrapper.spot_arm.gripper_close()
         return response
 
+    def handle_gripper_angle(
+        self, request: SetGripperAngle.Request, response: SetGripperAngle.Response
+    ) -> SetGripperAngle.Response:
+        """ROS service to set the gripper angle between 0-90 degrees"""
+        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_angle_open(
+            gripper_ang=request.gripper_angle, ensure_power_on_and_stand=False
+        )
+        return response
+
     def handle_clear_behavior_fault(
         self, request: ClearBehaviorFault.Request, response: ClearBehaviorFault.Response
     ) -> ClearBehaviorFault.Response:
@@ -2527,7 +2549,7 @@ def body_pose_callback(self, data: Pose) -> None:
 
     def arm_joint_cmd_callback(self, data: JointState) -> None:
         if not self.spot_wrapper:
-            self.get_logger().info(f"Mock mode, received arm joint commdn {data}")
+            self.get_logger().info(f"Mock mode, received arm joint command {data}")
             return
         arm_joint_map = {"sh0": None, "sh1": None, "el0": None, "el1": None, "wr0": None, "wr1": None}
         # Check we have the right number of joints for the arm

spot_msgs/CMakeLists.txt

@@ -86,6 +86,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/ListLogpoints.srv"
   "srv/RetrieveLogpoint.srv"
   "srv/RobotCommand.srv"
+  "srv/SetGripperAngle.srv"
   "srv/StoreLogpoint.srv"
   "srv/TagLogpoint.srv"
   "srv/GetLEDBrightness.srv"

spot_msgs/srv/SetGripperAngle.srv

@@ -0,0 +1,4 @@
+float32 gripper_angle # In range [0, 90]
+---
+bool success
+string message