Gaddison/spot sdk examples conversion (#564)

Co-authored-by: Katie Hughes <[email protected]>

spot_examples/README.md

@@ -21,11 +21,15 @@ Once the driver has been started, different examples can be run with the followi
 ros2 run spot_examples <example_node>
 ```
 Follow the links on each of the node names for more detailed documentation about how each example works, possible command line arguments, and other safety considerations you may need to take into account. 
-
 * [`walk_forward`](docs/walk_forward.md): A simple example that shows how to use ROS 2 to send `RobotCommand` goals to the Spot driver. If you are new to Spot and ROS 2, we recommend starting here.
 * [`arm_simple`](docs/arm_simple.md): An example of converting the [BD Simple Arm Motion](https://dev.bostondynamics.com/python/examples/arm_simple/readme) example to use ROS 2. 
 * [`send_inverse_kinematic_requests`](docs/send_inverse_kinematics_requests.md): An example that shows how to send inverse kinematics requests to the Spot Arm using ROS 2. 
-* [`batch_trajectory`](docs/batch_trajectory.md): An example that shows how to send very long trajectories to Spot using ROS2. 
+* [`batch_trajectory`](docs/batch_trajectory.md): An example that shows how to send very long trajectories to Spot using ROS 2. 
+* [`hello_spot`](docs/hello_spot.md): An example of converting the [BD Hello Spot](
+https://dev.bostondynamics.com/python/examples/hello_spot/readme
+) example to use ROS 2, demonstrating basic movement and image streaming. 
+* [`arm_with_body_follow`](docs/arm_with_body_follow.md): An example that demonstrates simultaneous locomotion and manipulation using ROS 2.
+* [`wasd`](docs/wasd.md): An example that offers basic teleoperation of Spot's locomotion and manipulation capabilities using ROS 2.
 
 ## Adding new examples
 To add examples that demonstrate other features of the Spot driver, create a new node `new_example.py` in [spot_examples](spot_examples) and add it to `setup.py`. Make sure to also write a documentation file (`new_example.md`) in [docs](docs) that explains how to run the example and how the code works, and link to it in this central README. 

spot_examples/docs/arm_with_body_follow.md

@@ -0,0 +1,69 @@
+# arm_with_body_follow
+An example that demonstrates simultaneous locomotion and manipulation using ROS 2.
+
+## Running the Example
+For this example, make sure to position the robot with 2m of clear space in front of the robot, either sitting or standing, as it will stand, move its arm forward, then walk forward. After the Spot driver is running, you can start the example with:
+```bash
+ros2 run spot_examples arm_with_body_follow
+```
+If you launched the driver with a namespace, use the following command instead:
+```bash
+ros2 run spot_examples arm_with_body_follow --robot <spot_name>
+```
+The robot will stand up, place its arm in front of itself, then slowly walk forward.
+
+## Understanding the Code
+
+Now let's go through [the code](../spot_examples/arm_with_body_follow.py) and see what's happening.
+
+Similar to other examples, Hello Spot is designed as a class composed with a [ROS Node](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Nodes/Understanding-ROS2-Nodes.html) that allows communication over [ROS topics](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Topics/Understanding-ROS2-Topics.html).
+
+```python
+class ArmWithBodyFollow:
+    def __init__(self, robot_name: Optional[str] = None, node: Optional[Node] = None) -> None:
+        self.node = ros_scope.node()
+```
+We first run `initialize_robot()`, which will claim then power on the robot:
+```python
+        result = self.robot.command("claim")
+        ...
+        result = self.robot.command("power_on")
+```
+
+Then we run `move()`, which will compose a synchronized command that allows simultaneous execution of mobility and manipulation commands.
+
+To do this, we first craft a position for the arm to move to in front of the robot, using BD's `bodsyn.client.math_helpers` functions to assist in consecutive transformation composition: 
+```python
+        odom_T_body = self.tf_listener.lookup_a_tform_b(self.odom_frame_name, self.grav_aligned_body_frame_name)
+        ...
+        odom_T_hand = odom_T_body_se3 * math_helpers.SE3Pose.from_proto(body_T_hand)
+```
+then creating the arm_command:
+```python
+        arm_command = RobotCommandBuilder.arm_pose_command(
+            odom_T_hand.x,
+            odom_T_hand.y,
+            odom_T_hand.z,
+            odom_T_hand.rot.w,
+            odom_T_hand.rot.x,
+            odom_T_hand.rot.y,
+            odom_T_hand.rot.z,
+            ODOM_FRAME_NAME,
+            seconds,
+        )
+```
+
+as well as the `follow_arm_command()` that tells the robot's body to follow the arm. 
+```python
+        follow_arm_command = RobotCommandBuilder.follow_arm_command()
+```
+We build the synchro command, convert, and send the goal.
+```python
+        cmd = RobotCommandBuilder.build_synchro_command(follow_arm_command, arm_command)
+        ...
+        action_goal = RobotCommand.Goal()
+        convert(cmd, action_goal.command)
+        ...
+        self.robot_command_client.send_goal_and_wait("arm_with_body_follow", action_goal)
+```
+This functionality is exposed to spot_ros2 thanks to our `convert()` function, which allows us to essentially send bosdyn protobuf commands using ROS 2.

spot_examples/docs/hello_spot.md

@@ -0,0 +1,133 @@
+# hello_spot
+An example of converting the [BD Hello Spot](
+https://dev.bostondynamics.com/python/examples/hello_spot/readme) example to use ROS 2, demonstrating basic movement and image streaming.
+
+## Running the Example
+For this example, make sure to position the robot with 0.5m of clear space on all sides, either sitting or standing, as it will twist and move in place. After the Spot driver is running, you can start the example with:
+```bash
+ros2 run spot_examples hello_spot
+```
+If you launched the driver with a namespace, use the following command instead:
+```bash
+ros2 run spot_examples hello_spot --robot <spot_name>
+```
+The robot should stand (if not already), then twist its body along the yaw axis, then simultaneously bow and take a photo from its front left camera, which will be displayed in a pop up and saved locally on the PC from which spot_driver is run.
+
+## Understanding the Code
+
+Now let's go through [the code](../spot_examples/hello_spot.py) and see what's happening.
+
+
+Similar to other examples, Hello Spot is designed as a class composed with a [ROS Node](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Nodes/Understanding-ROS2-Nodes.html) that allows communication over [ROS topics](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Topics/Understanding-ROS2-Topics.html).
+```python
+class HelloSpot:
+    def __init__(self, robot_name: Optional[str] = None, node: Optional[Node] = None) -> None:
+        self.node = ros_scope.node()
+```
+In this example, we will create a ROS 2 subscriber to listen to the images Spot takes from its front left camera, which it publishes to the `/<robot_name>/camera/frontleft/image` topic.
+```python
+        self.image_sub = self.node.create_subscription(
+            Image, namespace_with(robot_name, "camera/frontleft/image"), self.image_callback, 10
+        )
+```
+
+
+We then instantiate both SimpleSpotCommander() and ActionClientWrapper() as ways to send both simple and more complex commands to Spot, respectively.
+```python
+        self.robot = SimpleSpotCommander(self._robot_name, node)
+        self.robot_command_client = ActionClientWrapper(RobotCommand, 'robot_command', node)
+```
+The wrapper we use here automatically waits for the action server to become available during construction.  It also offers the `send_goal_and_wait` function that we’ll use later.
+
+
+In `initialize_robot()` we first claim the lease of the robot using SimpleSpotCommander,
+```python
+        result = self.robot.command("claim")
+```
+
+then power it on
+```python
+        result = self.robot.command("power_on")
+```
+
+Our first commanded movement, `stand_default()`, is a simple stand: 
+```python
+        result = self.robot.command("stand")
+```
+Our second movement, `stand_twisted()`, will request a stand with a 0.4 radian twist in yaw. For this more complicated movement, we will use the ActionClientWrapper we instantiated, as it will allow us to send more complex RobotCommand goals. We start by creating the SO(3) rotation matrix detailing this twist:
+```python
+        footprint_R_body = bosdyn.geometry.EulerZXY(yaw=0.4, roll=0.0, pitch=0.0)
+```
+then feed this rotation as an argument to our RobotCommandGoal
+```python
+        cmd = RobotCommandBuilder.synchro_stand_command(footprint_R_body=footprint_R_body)
+
+```
+which is converted into a ROS 2 action goal, and synchronously sent to the robot: 
+```python
+        action_goal = RobotCommand.Goal()
+        convert(cmd, action_goal.command)
+        self.logger.info("Twisting robot")
+        self.robot_command_client.send_goal_and_wait("twisting_robot", action_goal)
+```
+
+Our final movement, `stand_3_pt_traj()` will have the robot follow a standing trajectory through thre points, resembling a bow. 
+
+We first obtain the robot's current "world" frame to robot body frame transformation, which we will use as a starting point relative to which the next body positions will be based.
+```python
+        odom_T_flat_body = self.tf_listener.lookup_a_tform_b(self.odom_frame_name, self.grav_aligned_body_frame_name)
+```
+Note: The "odom" frame is the most basic "world frame" proxy provided by BD's spot-sdk, and the "grav aligned body frame" represents the robot's body's current 7 DOF pose (orientation + position), but with the Z axis fixed vertically.
+
+Next, we create the transformations for the remaining 3 body pose "keyframes"
+```python
+        flat_body_T_pose1 = math_helpers.SE3Pose(x=0.075, y=0, z=0, rot=math_helpers.Quat())
+        flat_body_T_pose2 = math_helpers.SE3Pose(x=0.0, y=0, z=0, rot=math_helpers.Quat(w=0.9848, x=0, y=0.1736, z=0))
+        flat_body_T_pose3 = math_helpers.SE3Pose(x=0.0, y=0, z=0, rot=math_helpers.Quat())
+```
+
+convert each of these into protobuf form
+```python
+        traj_point1 = trajectory_pb2.SE3TrajectoryPoint(
+            pose=(odom_T_flat_body_se3 * flat_body_T_pose1).to_proto(), time_since_reference=seconds_to_duration(t1)
+        )
+        traj_point2 = trajectory_pb2.SE3TrajectoryPoint(
+            pose=(odom_T_flat_body_se3 * flat_body_T_pose2).to_proto(), time_since_reference=seconds_to_duration(t2)
+        )
+        traj_point3 = trajectory_pb2.SE3TrajectoryPoint(
+            pose=(odom_T_flat_body_se3 * flat_body_T_pose3).to_proto(), time_since_reference=seconds_to_duration(t3)
+        )
+```
+assemble into a single protobuf that is converted to a ROS 2 goal and synchronously sent to the robot
+```python
+        traj = trajectory_pb2.SE3Trajectory(points=[traj_point1, traj_point2, traj_point3])
+
+        body_control = spot_command_pb2.BodyControlParams(
+            body_pose=spot_command_pb2.BodyControlParams.BodyPose(
+                root_frame_name=ODOM_FRAME_NAME, base_offset_rt_root=traj
+            )
+        )
+
+        mobility_params = spot_command_pb2.MobilityParams(body_control=body_control)
+
+        stand_command = RobotCommandBuilder.synchro_stand_command(params=mobility_params)
+
+        stand_command_goal = RobotCommand.Goal()
+        convert(stand_command, stand_command_goal.command)
+        self.logger.info("Beginning absolute body control while standing.")
+        self.robot_command_client.send_goal_and_wait(action_name="hello_spot", goal=stand_command_goal, timeout_sec=10)
+```
+
+
+Lastly, we call `_maybe_display_image()` and `_maybe_save_image()`, which display and save the latest image message stored in `self.latest_image_raw`. 
+```python
+        self.logger.info("Displaying image.")
+        self._maybe_display_image()
+        self._maybe_save_image()
+```
+`self.latest_image_raw` is updated every time the spot_driver publishes a new image to the `/<robot_name>/camera/frontleft/image` topic, thanks to the `image_sub` and `image_callback()`
+```python
+        self.image_sub = self.node.create_subscription(
+            Image, namespace_with(robot_name, "camera/frontleft/image"), self.image_callback, 10
+        )
+```

spot_examples/docs/wasd.md

@@ -0,0 +1,125 @@
+# wasd
+An example that offers basic teleoperation of Spot's locomotion and manipulation capabilities using ROS 2.
+
+## Running the Example
+For this example, make sure to position the robot with 2m of clear space on all sides, as you will be able to command walking in translation and rotation and arm stowing and unstowing.
+```bash
+ros2 run spot_examples wasd
+```
+If you launched the driver with a namespace, use the following command instead:
+```bash
+ros2 run spot_examples wasd --robot <spot_name>
+```
+The robot will stand up, and will execute any commands you send through the curses (text-based) interface in your terminal.
+
+
+*Keybinds*
+
+Following is a list of keyboard keys and the robot functions to which they map.
+
+    esc : make the robot stop executing all running commands
+    tab : sit and quit example
+      p : toggle power
+      f : stand
+      v : sit
+      w : move forward
+      s : move backward
+      a : move left
+      d : move right
+      q : rotate (yaw) left
+      e : rotate (yaw) right
+      u : unstow arm
+      j : stow arm
+      b : battery change pose
+      r : self-right (after battery change)
+
+
+
+## Understanding the Code
+
+Now let's go through [the code](../spot_examples/arm_with_body_follow.py) and see what's happening.
+
+Similar to other examples, Hello Spot is designed as a class composed with a [ROS Node](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Nodes/Understanding-ROS2-Nodes.html) that allows communication over [ROS topics](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Topics/Understanding-ROS2-Topics.html).
+
+```python
+class WasdInterface:
+    ...
+    def __init__(self, robot_name: Optional[str] = None) -> None:
+        self.node = ros_scope.node()
+```
+In the `__init__()` function of the `WasdInterface` class, we create a `_command_dictionary` member, which maps keypresses to member functions that command functions of the robot. For example, a key press on key "v" will call the `_sit()` member function:
+```python
+        self._command_dictionary = {
+            ...
+            ord("v"): self._sit,
+            ...
+        }
+```
+
+The function `_sit()`, in turn, calls the robot's `/sit` service, by sending an asynchronous call of ROS 2 message type `Trigger` through the sit service client `self.cli_sit`.
+```python
+    def _sit(self) -> None:
+        self.cli_sit.call_async(Trigger.Request())
+```
+
+The sit service client, `self.cli_sit`, was instantiated in the constructor of WasdInterface along with several other service clients for services such as stand, power on, and stow arm 
+```python
+        self.cli_sit = self.node.create_client(Trigger, namespace_with(robot_name, "sit"))
+        self.cli_stand = self.node.create_client(Trigger, namespace_with(robot_name, "stand"))
+        ...
+        self.cli_power_on = self.node.create_client(Trigger, namespace_with(robot_name, "power_on"))
+        ...
+        self.cli_stow = self.node.create_client(Trigger, namespace_with(robot_name, "arm_stow"))
+```
+each, of which, had assigned a corresponding keymapping in `_command_dictionary` for its member function
+```python
+            ord("f"): self._stand,
+            ...
+            ord("p"): self._toggle_power,
+            ...
+            ord("j"): self._stow,
+
+```
+
+To command a forward movement, the "w" key is mapped to the `_move_forward()` member function,
+```python 
+            ord("w"): self._move_forward,
+```
+which, in turn, calls the helper function `_velocity_cmd_helper()` with details about the direction and magnitude of the velocity requested
+```python
+    def _move_forward(self) -> None:
+        self._velocity_cmd_helper("move_forward", v_x=VELOCITY_BASE_SPEED)
+```
+
+The `_velocity_cmd_helper()` function takes 2D holonomic velocity requests (x, y translation and z rotation), crafts a ROS 2 `geometry_msgs/msg/Twist` message, and publishes to the robot's `/cmd_vel` topic.
+```python
+    def _velocity_cmd_helper(self, desc: str = "", v_x: float = 0.0, v_y: float = 0.0, v_rot: float = 0.0) -> None:
+        twist = Twist()
+        twist.linear.x = v_x
+        twist.linear.y = v_y
+        twist.angular.z = v_rot
+        start_time = time.time()
+        while time.time() - start_time < VELOCITY_CMD_DURATION:
+            self.pub_cmd_vel.publish(twist)
+            time.sleep(0.01)
+        self.pub_cmd_vel.publish(Twist())
+```
+As shown above, we don't send this cmd_vel topic once; rather, we continuously send it unti VELOCITY_CMD_DURATION, at which point we send a default `Twist` message, where all velocity fields default to 0, stopping the robot.
+
+Lasty, we use ROS 2 subscribers to obtain and display information about the robot's state. The current battery state, for example, which includes battery state-of-charge percentage and charge/discharge state, is stored in `WasdInterface`'s `latest_battery_status` member, 
+```python
+        self.latest_battery_status: Optional[BatteryStateArray] = None
+```
+which is updated every time the battery state ROS 2 subscriber sees a message published by the robot on the `/<robot_name>/status/battery_states` topic
+```python
+        self.sub_battery_state = self.node.create_subscription(
+            BatteryStateArray, namespace_with(robot_name, "status/battery_states"), self._status_battery_callback, 1
+        )
+```
+and calls `_status_battery_callback()`
+```python
+    def _status_battery_callback(self, msg: BatteryState) -> None:
+        self.latest_battery_status = msg
+```
+
+At each 'tick' of the curses UI refresh, we use `_battery_str()` to return a formatted string represntation of `self.latest_battery_status` to be printed on the screen.

spot_examples/setup.py

@@ -23,6 +23,9 @@
             "arm_simple = spot_examples.arm_simple:main",
             "send_inverse_kinematics_requests = spot_examples.send_inverse_kinematics_requests:main",
             "batch_trajectory = spot_examples.batch_trajectory:main",
+            "hello_spot = spot_examples.hello_spot:main",
+            "arm_with_body_follow = spot_examples.arm_with_body_follow:main",
+            "wasd = spot_examples.wasd:main",
         ],
     },
 )