[MAPLE-743] Add Cartesian arm pose commands (#501)

spot_driver/spot_driver/spot_ros2.py

@@ -569,6 +569,9 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw
             self.create_subscription(
                 JointState, "arm_joint_commands", self.arm_joint_cmd_callback, 100, callback_group=self.group
             )
+            self.create_subscription(
+                PoseStamped, "arm_pose_commands", self.arm_pose_cmd_callback, 100, callback_group=self.group
+            )
 
             if not self.gripperless:
                 self.create_service(
@@ -2530,7 +2533,7 @@ def cmd_velocity_callback(self, data: Twist) -> None:
     def body_pose_callback(self, data: Pose) -> None:
         """Callback for cmd_vel command"""
         if self.spot_wrapper is None:
-            self.get_logger().info("Mock mode, received command vel " + str(data))
+            self.get_logger().info("Mock mode, received command pose " + str(data))
             return
         q = Quaternion()
         q.x = data.orientation.x
@@ -2574,6 +2577,27 @@ def arm_joint_cmd_callback(self, data: JointState) -> None:
 
         self.spot_wrapper.arm_joint_cmd(**arm_joint_map)
 
+    def arm_pose_cmd_callback(self, data: PoseStamped) -> None:
+        if not self.spot_wrapper:
+            self.get_logger().info(f"Mock mode, received arm pose command {data}")
+            return
+        result = self.spot_wrapper.spot_arm.hand_pose(
+            x=data.pose.position.x,
+            y=data.pose.position.y,
+            z=data.pose.position.z,
+            qx=data.pose.orientation.x,
+            qy=data.pose.orientation.y,
+            qz=data.pose.orientation.z,
+            qw=data.pose.orientation.w,
+            ref_frame=data.header.frame_id,
+            duration=self.cmd_duration,
+            ensure_power_on_and_stand=False,
+        )
+        if not result[0]:
+            self.get_logger().warning(f"Failed to go to arm pose: {result[1]}")
+        else:
+            self.get_logger().info("Successfully went to arm pose")
+
     def handle_graph_nav_get_localization_pose(
         self,
         request: GraphNavGetLocalizationPose.Response,