Kabirk/add simple subscriber (#580)

Signed-off-by: kabirkedia <[email protected]> Co-authored-by: Tiffany Cappellari <[email protected]> Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: gaddison-bdai <[email protected]> Co-authored-by: Katie Hughes <[email protected]>
bdaiinstitute · Feb 20, 2025 · 702c2fa · 702c2fa
1 parent 1a50f41
commit 702c2fa
Show file tree

Hide file tree

Showing 14 changed files with 256 additions and 14 deletions.
diff --git a/spot_examples/docs/arm_simple.md b/spot_examples/docs/arm_simple.md
@@ -42,9 +42,9 @@ If you want to ensure you only communicate with the robot via the ROS 2 driver (
 ```
 This gives us four components, which we'll use in many ROS 2 programs:
 * A node: [ROS 2 nodes](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Nodes/Understanding-ROS2-Nodes.html) are the objects that interact with [ROS 2 topics](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Topics/Understanding-ROS2-Topics.html) and almost all ROS programs require them. Here we fetch the one in scope, already instantiated and serviced in the background. Had we instantiated a node of our own, we would have had to spin it ourselves.
-* A TF listener: This handles computing transforms.  As we'll see later, it can be used in place of Spot API `RobotStateClient` to get information about where frames on the robot are.  For more information about ROS 2 TF see [here](https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Tf2-Main.html).  For more information about our [TF wrapper](https://github.com/bdaiinstitute/ros_utilities/blob/main/bdai_ros2_wrappers/bdai_ros2_wrappers/tf_listener_wrapper.py) and how we use it in these examples, see the [simple_walk_forward example](../simple_walk_forward/).
+* A TF listener: This handles computing transforms.  As we'll see later, it can be used in place of Spot API `RobotStateClient` to get information about where frames on the robot are.  For more information about ROS 2 TF see [here](https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Tf2-Main.html).  For more information about our [TF wrapper](https://github.com/bdaiinstitute/ros_utilities/blob/main/synchros2/synchros2/tf_listener_wrapper.py) and how we use it in these examples, see the [simple_walk_forward example](../simple_walk_forward/).
 * A spot commander: This is a [wrapper](../utilities/utilities/simple_spot_commander.py) around service clients that call the spot driver to do simple things like get the lease and stand.  This is used in place of calls like `blocking_stand`.
-* A robot command action client: This is the ROS 2 action client that sends goals to the ROS 2 action server (for more information about ROS 2 actions see [here](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Actions/Understanding-ROS2-Actions.html)).  This is used in place of the Spot API `RobotCommandClient`.  We use a [wrapper](https://github.com/bdaiinstitute/ros_utilities/blob/main/bdai_ros2_wrappers/bdai_ros2_wrappers/action_client.py) around the built in ROS 2 action client that allows us to wait for the goal to return without risk of deadlock.
+* A robot command action client: This is the ROS 2 action client that sends goals to the ROS 2 action server (for more information about ROS 2 actions see [here](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Actions/Understanding-ROS2-Actions.html)).  This is used in place of the Spot API `RobotCommandClient`.  We use a [wrapper](https://github.com/bdaiinstitute/ros_utilities/blob/main/synchros2/synchros2/action_client.py) around the built in ROS 2 action client that allows us to wait for the goal to return without risk of deadlock.
 
 The original code uses direct calls to the API to power on and stand the robot:
 ```python

diff --git a/spot_examples/docs/simple_sub.md b/spot_examples/docs/simple_sub.md
@@ -0,0 +1,70 @@
+# Spot Robot Interface Documentation
+
+## Overview
+As a developer using spot_ros2, it took me a lot of time to figure out how to use subscription from my local planner to send pose to the Spot robot. Therefore, I am providing an example of how to use subscriptions and actions together.
+
+This implementation uses helper files `robot_commander` and `simple_spot_commander` to handle robot commands. The script listens to a ROS topic and converts received subscription messages into actions for the robot accordingly.
+
+## Main Functionality
+
+### Initialization
+1. The `SpotRobotInterface` class provides an interface to control the Spot. It listens for pose commands and directs the robot accordingly. The interface ensures safe command execution by buffering messages when the robot is busy.
+2. The script retrieves the robot name from the cli argument.
+3. An instance of `RobotCommander` is created with the robot name.
+4. The script logs the robot's name and initializes the robot.
+5. If initialization fails, the script logs an error and exits.
+
+### Topic Subscription and Action Execution
+1. The script subscribes to the `/pose_commands` topic and listens for messages of type `Pose` using the `listen_to_pose_commands` of the `SpotRobotInterface`.
+2. Subscription messages received on the topic are buffered and converted into actions for the robot using `process_message`. It sends the command to the spot and if any other message comes in, it buffers it.
+3. The `process_message` uses `execute_command` to execute the action using the `robot_commander` instance.
+
+### Message Processing
+1. The script maintains a buffer (`latest_message`) for incoming messages and a flag (`is_busy`) to track execution status.
+2. It listens to the `/pose_commands` topic using `Subscription(Pose, "/pose_commands")`.
+3. If the robot is busy executing an action, incoming messages are buffered.
+4. If the robot is not busy:
+   - The robot executes the action based on the received `Pose` message using `robot_commander.walk_forward_with_vision_frame_goal(message)`.
+   - Once execution is complete, `is_busy` is reset.
+   - If a buffered message exists, it is processed next.
+   - The script ensures sequential execution by waiting for each action to complete before sending the next `Pose` command.
+
+## Dependencies
+- ROS2
+- synchros2
+- geometry_msgs
+- Boston Dynamics SDK
+
+## Error Handling
+- If the robot fails to initialize, a log message is generated, and the script exits.
+- If the robot is busy, new messages are buffered instead of being lost.
+
+## Execution
+To run the script, execute:
+```sh
+ros2 run spot_examples simple_sub --robot <robot_name>
+```
+
+## Implementation Details
+
+### SpotRobotInterface Class
+- Initializes a `RobotCommander` instance.
+- Subscribes to `/pose_commands` to receive pose messages.
+- Buffers messages when busy and ensures sequential execution.
+
+### RobotCommander Class
+- Handles robot initialization and command execution.
+- Converts received `Pose` messages into actions using Boston Dynamics' `SE3Pose` and `RobotCommandBuilder`.
+- Uses `TFListenerWrapper` to transform poses into the robot's frame.
+
+### SimpleSpotCommander Class
+- Provides a ROS service interface for fundamental robot commands such as `claim`, `stand`, and `power_on`.
+- Uses `synchros2` for ROS 2 service communication.
+
+## Notes
+- This script uses `synchros2` for ROS communication.
+- It assumes that `RobotCommander` has a method `walk_forward_with_vision_frame_goal` to handle movement actions.
+- The script ensures that actions are executed sequentially and prevents command loss by buffering messages when the robot is busy.
+
+## Author
+- Kabir Kedia <[email protected]>
diff --git a/spot_examples/docs/walk_forward.md b/spot_examples/docs/walk_forward.md
@@ -23,21 +23,21 @@ class WalkForward:
         super().__init__('walk_forward')
         node = node or ros_scope.node()
 ```
-Note here we default to the [current scope](https://github.com/bdaiinstitute/ros_utilities/blob/main/bdai_ros2_wrappers/bdai_ros2_wrappers/scope.py) node if none is provided.
+Note here we default to the [current scope](https://github.com/bdaiinstitute/ros_utilities/blob/main/synchros2/synchros2/scope.py) node if none is provided.
 
 Then we set up ROS's [TF](https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Tf2-Main.html), which helps us transform between robot frames:
 ```python
          self._tf_listener = TFListenerWrapper(node)
          self._tf_listener.wait_for_a_tform_b(BODY_FRAME_NAME, VISION_FRAME_NAME)
 ```
-We use a [wrapper](https://github.com/bdaiinstitute/ros_utilities/blob/main/bdai_ros2_wrappers/bdai_ros2_wrappers/tf_listener_wrapper.py) that supports synchronous operation around ROS 2’s asynchronous [TF implementation](https://github.com/ros2/rclpy/tree/humble).  Passing it the body and vision frame names causes the wrapper to wait until it sees those frames.  This lets us make sure the robot is started and TF is working before proceeeding.
+We use a [wrapper](https://github.com/bdaiinstitute/ros_utilities/blob/main/synchros2/synchros2/tf_listener_wrapper.py) that supports synchronous operation around ROS 2’s asynchronous [TF implementation](https://github.com/ros2/rclpy/tree/humble).  Passing it the body and vision frame names causes the wrapper to wait until it sees those frames.  This lets us make sure the robot is started and TF is working before proceeeding.
 
 In order to perform small actions with the robot we use the [SimpleSpotCommander class](../../utilities/utilities/simple_spot_commander.py).  This wraps some service clients that talk to services offered by the spot driver.
 ```python
         self._robot = SimpleSpotCommander(self._robot_name, node)
 ```
 
-Finally we want to be able to command Spot to do things.  We do this via a [wrapper](https://github.com/bdaiinstitute/ros_utilities/blob/main/bdai_ros2_wrappers/bdai_ros2_wrappers/action_client.py) around the action client that talks to an action server running in the Spot driver (for more information about ROS 2 actions see [here](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Actions/Understanding-ROS2-Actions.html)):
+Finally we want to be able to command Spot to do things.  We do this via a [wrapper](https://github.com/bdaiinstitute/ros_utilities/blob/main/synchros2/synchros2/action_client.py) around the action client that talks to an action server running in the Spot driver (for more information about ROS 2 actions see [here](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Actions/Understanding-ROS2-Actions.html)):
 ```python
         self._robot_command_client = ActionClientWrapper(RobotCommand, 'robot_command', node)
 ```
@@ -92,4 +92,4 @@ Finally we send the goal to the Spot driver:
 ```python
         self._robot_command_client.send_goal_and_wait(action_goal)
 ```
-Note that `send_goal_and_wait` is a function of our [ActionClientWrapper](https://github.com/bdaiinstitute/ros_utilities/blob/main/bdai_ros2_wrappers/bdai_ros2_wrappers/action_client.py) and not a built in ROS 2 function.
+Note that `send_goal_and_wait` is a function of our [ActionClientWrapper](https://github.com/bdaiinstitute/ros_utilities/blob/main/synchros2/synchros2/action_client.py) and not a built in ROS 2 function.
diff --git a/spot_examples/package.xml b/spot_examples/package.xml
@@ -7,7 +7,7 @@
   <maintainer email="[email protected]">AI Institute</maintainer>
   <license>MIT</license>
 
-  <exec_depend>bdai_ros2_wrappers</exec_depend>
+  <exec_depend>synchros2</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_pep257</test_depend>

diff --git a/spot_examples/setup.py b/spot_examples/setup.py
@@ -26,6 +26,7 @@
             "hello_spot = spot_examples.hello_spot:main",
             "arm_with_body_follow = spot_examples.arm_with_body_follow:main",
             "wasd = spot_examples.wasd:main",
+            "simple_sub = spot_examples.simple_sub:main",
         ],
     },
 )
diff --git a/spot_examples/spot_examples/arm_simple.py b/spot_examples/spot_examples/arm_simple.py
@@ -21,7 +21,7 @@ def hello_arm(robot_name: Optional[str] = None) -> bool:
     # Set up basic ROS2 utilities for communicating with the driver
     node = ros_scope.node()
     if node is None:
-        raise ValueError("no ROS 2 node available (did you use bdai_ros2_wrapper.process.main?)")
+        raise ValueError("no ROS 2 node available (did you use synchros2.process.main?)")
     logger = node.get_logger()
 
     odom_frame_name = namespace_with(robot_name, ODOM_FRAME_NAME)

diff --git a/spot_examples/spot_examples/batch_trajectory.py b/spot_examples/spot_examples/batch_trajectory.py
@@ -679,7 +679,7 @@ def main(args: argparse.Namespace) -> None:
     # Set up basic ROS2 utilities for communicating with the driver.
     node = ros_scope.node()
     if node is None:
-        raise ValueError("No ROS 2 node available (did you use bdai_ros2_wrapper.process.main?)")
+        raise ValueError("No ROS 2 node available (did you use synchros2.process.main?)")
 
     spot_runner = SpotRunner(node, args)
     spot_runner.test_run()

diff --git a/spot_examples/spot_examples/hello_spot.py b/spot_examples/spot_examples/hello_spot.py
@@ -30,7 +30,7 @@ class HelloSpot:
     def __init__(self, robot_name: Optional[str] = None, node: Optional[Node] = None) -> None:
         self.node = ros_scope.node()
         if self.node is None:
-            raise ValueError("no ROS 2 node available (did you use bdai_ros2_wrapper.process.main?)")
+            raise ValueError("no ROS 2 node available (did you use synchros2.process.main?)")
         self.logger = self.node.get_logger()
 
         self.image_sub = self.node.create_subscription(

diff --git a/spot_examples/spot_examples/robot_commander.py b/spot_examples/spot_examples/robot_commander.py
@@ -0,0 +1,111 @@
+import logging
+from typing import Optional
+
+import synchros2.scope as ros_scope
+from bosdyn.client.frame_helpers import BODY_FRAME_NAME, VISION_FRAME_NAME
+from bosdyn.client.math_helpers import Quat, SE3Pose
+from bosdyn.client.robot_command import RobotCommandBuilder
+from bosdyn_msgs.conversions import convert
+from geometry_msgs.msg import Pose
+from rclpy.node import Node
+from synchros2.action_client import ActionClientWrapper
+from synchros2.tf_listener_wrapper import TFListenerWrapper
+from synchros2.utilities import fqn, namespace_with
+
+from spot_msgs.action import RobotCommand  # type: ignore
+
+from .simple_spot_commander import SimpleSpotCommander
+
+# Where we want the robot to walk to relative to itself
+
+
+class RobotCommander:
+    def __init__(self, robot_name: Optional[str] = None, node: Optional[Node] = None) -> None:
+        self._logger = logging.getLogger(fqn(self.__class__))
+        self._node = node or ros_scope.node()
+        if self._node is None:
+            raise ValueError("no ROS 2 node available (did you use synchros2.process.main?)")
+        self._robot_name = robot_name
+        self._logger.info(f"{self._robot_name}")
+
+        self._body_frame_name = namespace_with(self._robot_name, BODY_FRAME_NAME)
+        self._vision_frame_name = namespace_with(self._robot_name, VISION_FRAME_NAME)
+        self._tf_listener = TFListenerWrapper(self._node)
+        self._tf_listener.wait_for_a_tform_b(self._body_frame_name, self._vision_frame_name)
+        self._robot = SimpleSpotCommander(robot_name=self._robot_name, node=self._node)
+        self._robot_command_client = ActionClientWrapper(
+            RobotCommand, namespace_with(self._robot_name, "robot_command"), self._node
+        )
+
+    def initialize_robot(self) -> bool:
+        self._logger.info(f"Robot name: {self._robot_name}")
+        self._logger.info("Claiming robot")
+        result = self._robot.command("claim")
+        if not result.success:
+            self._logger.error("Unable to claim robot message was " + result.message)
+            return False
+        self._logger.info("Claimed robot")
+
+        # Stand the robot up.
+        self._logger.info("Powering robot on")
+        result = self._robot.command("power_on")
+        if not result.success:
+            self._logger.error("Unable to power on robot message was " + result.message)
+            return False
+        self._logger.info("Standing robot up")
+        result = self._robot.command("stand")
+        if not result.success:
+            self._logger.error("Robot did not stand message was " + result.message)
+            return False
+        self._logger.info("Successfully stood up.")
+        return True
+
+    def walk_forward_with_vision_frame_goal(self, waypoint: Pose) -> bool:
+        """
+        Walk forward to a goal in the world frame.
+        Arguments:
+            waypoint: The waypoint to walk to with respect to the body frame.
+                      Type is geometry_msgs.msg.Pose.
+                      This is using the vision frame of the spot
+        """
+        self._logger.info("Walking")
+        world_t_robot = self._tf_listener.lookup_a_tform_b(self._vision_frame_name, self._body_frame_name)
+        world_t_robot_se2 = SE3Pose(
+            world_t_robot.transform.translation.x,
+            world_t_robot.transform.translation.y,
+            world_t_robot.transform.translation.z,
+            Quat(
+                world_t_robot.transform.rotation.w,
+                world_t_robot.transform.rotation.x,
+                world_t_robot.transform.rotation.y,
+                world_t_robot.transform.rotation.z,
+            ),
+        ).get_closest_se2_transform()
+
+        goal = SE3Pose(
+            waypoint.position.x,
+            waypoint.position.y,
+            waypoint.position.z,
+            Quat(
+                waypoint.orientation.w,
+                waypoint.orientation.x,
+                waypoint.orientation.y,
+                waypoint.orientation.z,
+            ),
+        ).get_closest_se2_transform()
+        world_t_goal = world_t_robot_se2 * goal
+        proto_goal = RobotCommandBuilder.synchro_se2_trajectory_point_command(
+            goal_x=world_t_goal.x,
+            goal_y=world_t_goal.y,
+            goal_heading=world_t_goal.angle,
+            frame_name=VISION_FRAME_NAME,  # use Boston Dynamics' frame conventions
+        )
+        action_goal = RobotCommand.Goal()
+        convert(proto_goal, action_goal.command)
+        result = self._robot_command_client.send_goal_and_wait("walk_forward", action_goal, timeout_sec=5)
+        if result is not None:
+            self._logger.info(f"Result: {result.message}")
+            self._logger.info(f"Navigated to waypoint: {waypoint}")
+        else:
+            self._logger.info(f"Failed to navigate to waypoint: {waypoint}")
+        return result
diff --git a/spot_examples/spot_examples/send_inverse_kinematics_requests.py b/spot_examples/spot_examples/send_inverse_kinematics_requests.py
@@ -329,7 +329,7 @@ def main(args: argparse.Namespace) -> None:
     # Set up basic ROS2 utilities for communicating with the driver.
     node = ros_scope.node()
     if node is None:
-        raise ValueError("No ROS 2 node available (did you use bdai_ros2_wrapper.process.main?)")
+        raise ValueError("No ROS 2 node available (did you use synchros2.process.main?)")
 
     spot_runner = SpotRunner(node, args)
     spot_runner.test_run()

diff --git a/spot_examples/spot_examples/simple_spot_commander.py b/spot_examples/spot_examples/simple_spot_commander.py
@@ -32,7 +32,7 @@ def __init__(self, robot_name: Optional[str] = None, node: Optional[Node] = None
         self._logger = logging.getLogger(fqn(self.__class__))
         node = node or ros_scope.node()
         if node is None:
-            raise ValueError("no ROS 2 node available (did you use bdai_ros2_wrapper.process.main?)")
+            raise ValueError("no ROS 2 node available (did you use synchros2.process.main?)")
         self._command_map: Dict[str, Client] = {}
         for service_basename in TRIGGER_SERVICES:
             service_name = namespace_with(robot_name, service_basename)