[SW-1394] add option for odom/fiducial/camera publishing from ros2 co…

…ntrol launchfile (#485)

## Change Overview

Some nodes we have written in spot_driver are compatible with the ros2 control interface -- specifically, the state publisher node, the object sync node, and the camera publishers. This by default will launch these nodes when the ros2 control stack is run on robot. There is the option to turn off image publishing in case of bandwidth issues / if the user does not need it.

Since I was copying and pasting the launch arguments for the image publishers a lot, this PR also moves these into `spot_launch_helpers.py` so they can simply be imported into all the launchfiles that use it.

Finally, this changes the topic name that the low level joint states are published on from `/<robot>/joint_states` to `/<robot>/low_level/joint_states` and updates the nodes that rely on this accordingly. This is useful here because the state publisher node publishes to `/<robot>/joint_states` and we want to have a clear distinction between these two, as they are being published from two different sources via different BD API calls (unclear if the values the high / low level query produces are the same). 

## Testing Done

- [x] Tested wiggle arm example on robot to verify new joint states topic is working properly
- [x] Verified odom transforms and images are available on robot
- [x] TF tree is connected properly :

![frames_ros2control.png](https://graphite-user-uploaded-assets-prod.s3.amazonaws.com/Xynj6CBpA3NqqBFfE8Q9/83fb0aef-6d03-4ab4-b156-e59cde5a5cff.png)

spot_driver/launch/spot_driver.launch.py

@@ -11,7 +11,7 @@
 from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution
 from launch_ros.substitutions import FindPackageShare
 
-from spot_driver.launch.spot_launch_helpers import spot_has_arm
+from spot_driver.launch.spot_launch_helpers import IMAGE_PUBLISHER_ARGS, declare_image_publisher_args, spot_has_arm
 
 THIS_PACKAGE = "spot_driver"
 
@@ -137,16 +137,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
     spot_image_publishers = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([FindPackageShare(THIS_PACKAGE), "/launch", "/spot_image_publishers.launch.py"]),
         launch_arguments={
-            key: LaunchConfiguration(key)
-            for key in [
-                "config_file",
-                "depth_registered_mode",
-                "publish_point_clouds",
-                "uncompress_images",
-                "publish_compressed_images",
-                "stitch_front_images",
-                "spot_name",
-            ]
+            key: LaunchConfiguration(key) for key in ["config_file", "spot_name"] + IMAGE_PUBLISHER_ARGS
         }.items(),
         condition=IfCondition(LaunchConfiguration("launch_image_publishers")),
     )
@@ -193,57 +184,7 @@ def generate_launch_description() -> launch.LaunchDescription:
             description="Choose whether to launch the image publishing nodes from Spot.",
         )
     )
-    launch_args.append(
-        DeclareLaunchArgument(
-            "depth_registered_mode",
-            default_value="from_nodelets",
-            choices=["disable", "from_spot", "from_nodelets"],
-            description=(
-                "If `disable` is set, do not publish registered depth images."
-                " If `from_spot` is set, request registered depth images from Spot through its SDK."
-                " If `from_nodelets` is set, use depth_image_proc::RegisterNode component nodes running on the host"
-                " computer to create registered depth images (this reduces the computational load on Spot's internal"
-                " systems)."
-            ),
-        )
-    )
-    launch_args.append(
-        DeclareLaunchArgument(
-            "publish_point_clouds",
-            default_value="False",
-            choices=["True", "true", "False", "false"],
-            description=(
-                "If true, create and publish point clouds for each depth registered and RGB camera pair. Requires that"
-                " the depth_register_mode launch argument is set to a value that is not `disable`."
-            ),
-        )
-    )
-    launch_args.append(
-        DeclareLaunchArgument(
-            "uncompress_images",
-            default_value="True",
-            choices=["True", "true", "False", "false"],
-            description="Choose whether to publish uncompressed images from Spot.",
-        )
-    )
-    launch_args.append(
-        DeclareLaunchArgument(
-            "publish_compressed_images",
-            default_value="False",
-            choices=["True", "true", "False", "false"],
-            description="Choose whether to publish compressed images from Spot.",
-        )
-    )
-    launch_args.append(
-        DeclareLaunchArgument(
-            "stitch_front_images",
-            default_value="False",
-            choices=["True", "true", "False", "false"],
-            description=(
-                "Choose whether to publish a stitched image constructed from Spot's front left and right cameras."
-            ),
-        )
-    )
+    launch_args += declare_image_publisher_args()
     launch_args.append(DeclareLaunchArgument("spot_name", default_value="", description="Name of Spot"))
 
     ld = launch.LaunchDescription(launch_args)

spot_driver/launch/spot_image_publishers.launch.py

@@ -1,7 +1,6 @@
 # Copyright (c) 2023-2024 Boston Dynamics AI Institute LLC. All rights reserved.
 
 import os
-from enum import Enum
 from typing import List
 
 import launch
@@ -11,16 +10,12 @@
 from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 
-from spot_driver.launch.spot_launch_helpers import get_camera_sources, spot_has_arm
-
-
-class DepthRegisteredMode(Enum):
-    DISABLE = "disable"
-    FROM_SPOT = "from_spot"
-    FROM_NODELETS = "from_nodelets"
-
-    def __repr__(self) -> str:
-        return self.value
+from spot_driver.launch.spot_launch_helpers import (
+    DepthRegisteredMode,
+    declare_image_publisher_args,
+    get_camera_sources,
+    spot_has_arm,
+)
 
 
 def create_depth_registration_nodelets(
@@ -199,58 +194,8 @@ def generate_launch_description() -> launch.LaunchDescription:
             description="Path to configuration file for the driver.",
         )
     )
-    launch_args.append(
-        DeclareLaunchArgument(
-            "depth_registered_mode",
-            default_value="from_nodelets",
-            choices=["disable", "from_spot", "from_nodelets"],
-            description=(
-                "If `disable` is set, do not publish registered depth images."
-                " If `from_spot` is set, request registered depth images from Spot through its SDK."
-                " If `from_nodelets` is set, use depth_image_proc::RegisterNode component nodes running on the host"
-                " computer to create registered depth images (this reduces the computational load on Spot's internal"
-                " systems)."
-            ),
-        )
-    )
-    launch_args.append(
-        DeclareLaunchArgument(
-            "publish_point_clouds",
-            default_value="False",
-            choices=["True", "true", "False", "false"],
-            description=(
-                "If true, create and publish point clouds for each depth registered and RGB camera pair. Requires that"
-                " the depth_register_mode launch argument is set to a value that is not `disable`."
-            ),
-        )
-    )
-    launch_args.append(
-        DeclareLaunchArgument(
-            "uncompress_images",
-            default_value="True",
-            choices=["True", "true", "False", "false"],
-            description="Choose whether to publish uncompressed images from Spot.",
-        )
-    )
-    launch_args.append(
-        DeclareLaunchArgument(
-            "publish_compressed_images",
-            default_value="False",
-            choices=["True", "true", "False", "false"],
-            description="Choose whether to publish compressed images from Spot.",
-        )
-    )
-    launch_args.append(
-        DeclareLaunchArgument(
-            "stitch_front_images",
-            default_value="False",
-            choices=["True", "true", "False", "false"],
-            description=(
-                "Choose whether to publish a stitched image constructed from Spot's front left and right cameras."
-            ),
-        )
-    )
     launch_args.append(DeclareLaunchArgument("spot_name", default_value="", description="Name of Spot"))
+    launch_args += declare_image_publisher_args()
 
     ld = launch.LaunchDescription(launch_args)
 

spot_driver/spot_driver/launch/spot_launch_helpers.py

@@ -2,15 +2,99 @@
 
 import logging
 import os
+from enum import Enum
 from typing import Any, Dict, List, Optional, Tuple
 
 import yaml
+from launch.actions import DeclareLaunchArgument
 
 from spot_wrapper.wrapper import SpotWrapper
 
 COLOR_END = "\33[0m"
 COLOR_YELLOW = "\33[33m"
 
+IMAGE_PUBLISHER_ARGS = [
+    "depth_registered_mode",
+    "publish_point_clouds",
+    "uncompress_images",
+    "publish_compressed_images",
+    "stitch_front_images",
+]
+
+
+class DepthRegisteredMode(Enum):
+    """Options for obtaining depth registered images images from Spot. We can request them from Spot's SDK, generate
+    them using `depth_image_proc`'s nodelets, or just not publish them at all."""
+
+    DISABLE = "disable"
+    FROM_SPOT = "from_spot"
+    FROM_NODELETS = "from_nodelets"
+
+    def __repr__(self) -> str:
+        return self.value
+
+
+def declare_image_publisher_args() -> List[DeclareLaunchArgument]:
+    """Generates launch arguments for each element in IMAGE_PUBLISHER_ARGS. This is useful to avoid copying and pasting
+    the same launch arguments multiple times launchfiles that call the spot_image_publishers launchfile.
+
+    Returns:
+        List[DeclareLaunchArgument]: List of DeclareLaunchArguments useful for image publishing.
+    """
+    launch_args = []
+    launch_args.append(
+        DeclareLaunchArgument(
+            "depth_registered_mode",
+            default_value=DepthRegisteredMode.FROM_NODELETS.value,
+            choices=[e.value for e in DepthRegisteredMode],
+            description=(
+                f"If `{DepthRegisteredMode.DISABLE.value}` is set, do not publish registered depth images."
+                f" If `{DepthRegisteredMode.FROM_SPOT.value}` is set, request registered depth images from Spot through"
+                f" its SDK. If `{DepthRegisteredMode.FROM_NODELETS.value}` is set, use depth_image_proc::RegisterNode"
+                " component nodes running on the host computer to create registered depth images (this reduces the"
+                " computational load on Spot's internal systems)."
+            ),
+        )
+    )
+    launch_args.append(
+        DeclareLaunchArgument(
+            "publish_point_clouds",
+            default_value="False",
+            choices=["True", "true", "False", "false"],
+            description=(
+                "If true, create and publish point clouds for each depth registered and RGB camera pair. Requires that"
+                " the depth_register_mode launch argument is set to a value that is not `disable`."
+            ),
+        )
+    )
+    launch_args.append(
+        DeclareLaunchArgument(
+            "uncompress_images",
+            default_value="True",
+            choices=["True", "true", "False", "false"],
+            description="Choose whether to publish uncompressed images from Spot.",
+        )
+    )
+    launch_args.append(
+        DeclareLaunchArgument(
+            "publish_compressed_images",
+            default_value="False",
+            choices=["True", "true", "False", "false"],
+            description="Choose whether to publish compressed images from Spot.",
+        )
+    )
+    launch_args.append(
+        DeclareLaunchArgument(
+            "stitch_front_images",
+            default_value="False",
+            choices=["True", "true", "False", "false"],
+            description=(
+                "Choose whether to publish a stitched image constructed from Spot's front left and right cameras."
+            ),
+        )
+    )
+    return launch_args
+
 
 def get_ros_param_dict(config_file_path: str) -> Dict[str, Any]:
     """Get a dictionary of parameter_name: parameter_value from a ROS config yaml file.

spot_ros2_control/README.md

@@ -1,6 +1,6 @@
 # spot_ros2_control
 
-This is a ROS 2 package designed to communicate with Spot's low level API through ROS 2 control. It sets up a hardware interface that can be either `mock` or connected to the robot using the [Spot C++ SDK](https://github.com/boston-dynamics/spot-cpp-sdk) (currently in progress). By default, it loads a standard joint state broadcaster and forward position controller provided from `ros2_control`.
+This is a ROS 2 package designed to communicate with Spot's low level API through ROS 2 control. It sets up a hardware interface that can be either `mock` or connected to the robot using the [Spot C++ SDK](https://github.com/boston-dynamics/spot-cpp-sdk). By default, it loads a standard joint state broadcaster and forward position controller provided from `ros2_control`.
 
 ## On-robot
 
@@ -24,8 +24,17 @@ You can then run the launchfile with the following command:
 ros2 launch spot_ros2_control spot_ros2_control.launch.py hardware_interface:=robot config_file:=path/to/spot_ros.yaml
 ```
 
-This hardware interface can stream the joint angles of the robot using the low level API at ~333 Hz. 
-Both state streaming and command streaming are implemented. 
+If you wish to launch these nodes in a namespace, add the argument `spot_name:=<Robot Name>`.
+
+This hardware interface will stream the joint angles of the robot using the low level API at 333 Hz onto the topic `/<Robot Name>/low_level/joint_states`.
+
+Commands can be sent on the topic `/<Robot Name>/forward_position_controller/commands`. This will forward position commands directly to the spot sdk through the hardware interface. 
+
+> [!CAUTION]
+> When using the forward position controller, there is no safety mechanism in place to ensure smooth motion. The ordering of the command must match the ordering of the joints specified in the controller configuration file ([here for robots with an arm](config/spot_default_controllers_with_arm.yaml) or [here for robots without an arm](config/spot_default_controllers_without_arm.yaml)), and the robot can move in unpredictable and dangerous ways if this is not set correctly. Make sure to keep a safe distance from the robot when working with this controller and ensure the e-stop can easily be pressed if needed.
+
+Additionally, the state publisher node, object synchronization node, and image publisher nodes from [`spot_driver`](../spot_driver/) will get launched by default when running on the robot to provide extra information such as odometry topics and camera feeds.
+To turn off the image publishers (which can cause problems with bandwidth), add the launch argument `launch_image_publishers:=false`.
 
 ## Mock
 
@@ -35,7 +44,7 @@ To use a mock hardware interface, run:
 ros2 launch spot_ros2_control spot_ros2_control.launch.py hardware_interface:=mock
 ```
 
-By default, this will load a robot with no arm. If you want your mock robot to have an arm, add the launch argument `mock_has_arm:=True`. 
+By default, this will load a robot with no arm. If you want your mock robot to have an arm, add the launch argument `mock_arm:=True`. 
 
 ## Examples
 

spot_ros2_control/examples/src/joint_command_passthrough.cpp

@@ -20,7 +20,7 @@ class JointCommandPassthrough : public rclcpp::Node {
   JointCommandPassthrough() : Node("joint_passthrough") {
     std::string robot_namespace = declare_parameter("robot_namespace", "Spot");
     robot_namespace = robot_namespace.empty() ? "" : robot_namespace + "/";
-    std::string joint_state_topic = robot_namespace + declare_parameter("joint_state_topic", "joint_states");
+    std::string joint_state_topic = robot_namespace + declare_parameter("joint_state_topic", "low_level/joint_states");
     std::string joint_commands_topic = robot_namespace + declare_parameter("joint_commands_topic", "joint_commands");
     std::string controller_commands_topic =
         robot_namespace + declare_parameter("controller_commands_topic", "forward_position_controller/commands");

spot_ros2_control/examples/src/noarm_squat.cpp

@@ -18,8 +18,13 @@ class NoarmSquat : public rclcpp::Node {
  public:
   NoarmSquat()
       : Node("noarm_squat"), squat_state_{SquatState::INITIALIZING}, initialized_{false}, count_{0}, njoints_{12} {
-    stand_joint_angles_ = declare_parameter("stand_joint_angles", std::vector<double>{});
-    squat_joint_angles_ = declare_parameter("squat_joint_angles", std::vector<double>{});
+    // The following joint angles are in the order of FL_hip_x, FL_hip_y, FL_knee, FR..., RL..., RR...
+    stand_joint_angles_ = declare_parameter(
+        "stand_joint_angles",
+        std::vector<double>{0.12, 0.72, -1.45, -0.12, 0.72, -1.45, 0.12, 0.72, -1.45, -0.12, 0.72, -1.45});
+    squat_joint_angles_ = declare_parameter(
+        "squat_joint_angles",
+        std::vector<double>{0.15, 1.30, -2.25, -0.15, 1.30, -2.25, 0.15, 1.30, -2.25, -0.15, 1.30, -2.25});
     const auto command_rate = declare_parameter("command_rate", 50.0);  // how frequently to send commands in Hz
     const auto seconds_per_motion =
         declare_parameter("seconds_per_motion", 2.0);  // how many seconds the squat and stand motions should take
@@ -36,7 +41,7 @@ class NoarmSquat : public rclcpp::Node {
 
     command_pub_ = create_publisher<std_msgs::msg::Float64MultiArray>("forward_position_controller/commands", 10);
     joint_states_sub_ = create_subscription<sensor_msgs::msg::JointState>(
-        "joint_states", 10, std::bind(&NoarmSquat::joint_states_callback, this, std::placeholders::_1));
+        "low_level/joint_states", 10, std::bind(&NoarmSquat::joint_states_callback, this, std::placeholders::_1));
 
     const auto timer_period =
         std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::duration<double>(1. / command_rate));

spot_ros2_control/examples/src/wiggle_arm.cpp

@@ -19,9 +19,12 @@ enum class WiggleState { WIGGLE_DOWN, WIGGLE_MIDDLE, WIGGLE_UP, RESET };
 class WiggleArm : public rclcpp::Node {
  public:
   WiggleArm() : Node("wiggle_arm"), wiggle_state_{WiggleState::WIGGLE_DOWN}, initialized_{false} {
-    joints_to_wiggle_ = declare_parameter("joints_to_wiggle", std::vector<int>{});
-    wiggle_up_offsets_ = declare_parameter("wiggle_up_offsets", std::vector<double>{});
-    wiggle_down_offsets_ = declare_parameter("wiggle_down_offsets", std::vector<double>{});
+    // Indices of the joints to wiggle. This corresponds to WR0 and F1X
+    joints_to_wiggle_ = declare_parameter("joints_to_wiggle", std::vector<int>{16, 18});
+    // Offset from starting position of each joints_to_wiggle for the first wiggle (up)
+    wiggle_up_offsets_ = declare_parameter("wiggle_up_offsets", std::vector<double>{1.0, -0.5});
+    // Offset from starting position of each joints_to_wiggle for the second wiggle (down)
+    wiggle_down_offsets_ = declare_parameter("wiggle_down_offsets", std::vector<double>{-1.0, -0.5});
     const auto command_rate = declare_parameter("command_rate", 50.0);  // how frequently to send commands in Hz
     const auto seconds_per_motion =
         declare_parameter("seconds_per_motion", 2.0);  // how many seconds each wiggle should take
@@ -34,7 +37,7 @@ class WiggleArm : public rclcpp::Node {
     }
 
     joint_states_sub_ = create_subscription<sensor_msgs::msg::JointState>(
-        "joint_states", 10, std::bind(&WiggleArm::joint_states_callback, this, std::placeholders::_1));
+        "low_level/joint_states", 10, std::bind(&WiggleArm::joint_states_callback, this, std::placeholders::_1));
     command_pub_ = create_publisher<std_msgs::msg::Float64MultiArray>("forward_position_controller/commands", 10);
     const auto timer_period =
         std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::duration<double>(1. / command_rate));