Service for setting a pose the robot will hold

spot_driver/spot_driver/spot_ros2.py

@@ -124,6 +124,7 @@
     LoadSound,
     OverrideGraspOrCarry,
     PlaySound,
+    PosedStand,
     RetrieveLogpoint,
     ReturnLease,
     SetGripperAngle,
@@ -858,6 +859,12 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw
             lambda request, response: self.service_wrapper("dock", self.handle_dock, request, response),
             callback_group=self.group,
         )
+        self.create_service(
+            PosedStand,
+            "posed_stand",
+            lambda request, response: self.service_wrapper("posed_stand", self.handle_posed_stand, request, response),
+            callback_group=self.group,
+        )
 
         # This doesn't use the service wrapper because it's not a trigger, and we want different mock responses
         self.create_service(ListWorldObjects, "list_world_objects", self.handle_list_world_objects)
@@ -1856,6 +1863,20 @@ def handle_dock(self, request: Dock.Request, response: Dock.Response) -> Dock.Re
         response.success, response.message = self.spot_wrapper.spot_docking.dock(request.dock_id)
         return response
 
+    def handle_posed_stand(self, request: PosedStand.Request, response: PosedStand.Response) -> PosedStand.Response:
+        """ROS service handler to stand the robot with a user-defined body pose."""
+        if self.spot_wrapper is None:
+            response.success = False
+            response.message = "Spot wrapper is undefined"
+            return response
+        response.success, response.message = self.spot_wrapper.stand(
+            body_height=request.body_height,
+            body_roll=request.body_roll,
+            body_pitch=request.body_pitch,
+            body_yaw=request.body_yaw,
+        )
+        return response
+
     def handle_max_vel(self, request: SetVelocity.Request, response: SetVelocity.Response) -> SetVelocity.Response:
         """
         Handle a max_velocity service call. This will modify the mobility params to have a limit on the maximum

spot_msgs/CMakeLists.txt

@@ -102,6 +102,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/GetGripperCameraParameters.srv"
   "srv/SetGripperCameraParameters.srv"
   "srv/OverrideGraspOrCarry.srv"
+  "srv/PosedStand.srv"
   "srv/SetStandHeight.srv"
   "action/ExecuteDance.action"
   "action/NavigateTo.action"

spot_msgs/srv/PosedStand.srv

@@ -0,0 +1,12 @@
+# See https://dev.bostondynamics.com/python/bosdyn-client/src/bosdyn/client/robot_command.html?highlight=feedback#bosdyn.client.robot_command.RobotCommandBuilder.stand_command
+
+# Offset of the body from the default stand height, in metres
+float32 body_height
+
+# RPY of the body relative to the robot's default stand pose in radians
+float32 body_yaw
+float32 body_pitch
+float32 body_roll
+---
+bool success
+string message