feat: Maintain backwards compatibility for spot_name and tf_prefix la…

…unch file arguments. * implemented a parameter substitution launch helper util, which passes the given launch arguments into the used parameter configuration * substitution values are type-constrained explicitly to LaunchSubstitution * all type handling is offloaded to the internal ros substitution mechanisms - is usable for any ros parameter type * launch parameters will always take precedence over config file parameters * updated rviz launch to use the prefix argument correctly * updated relevant README files * minor fix - redefined lambda in parameter interface test as a static expression
bdaiinstitute · Nov 20, 2024 · 919e247 · 919e247
1 parent e421c4a
commit 919e247
Show file tree

Hide file tree

Showing 8 changed files with 115 additions and 26 deletions.
diff --git a/README.md b/README.md
@@ -77,7 +77,7 @@ The image does not have the `proto2ros_tests` package built. You'll need to buil
 
 The Spot driver contains all of the necessary topics, services, and actions for controlling Spot over ROS 2. To launch the driver, run:
 ```
-ros2 launch spot_driver spot_driver.launch.py [config_file:=<path/to/config.yaml>] [spot_name:=<Robot Name>] [publish_point_clouds:=<True|False>] [launch_rviz:=<True|False>] [uncompress_images:=<True|False>] [publish_compressed_images:=<True|False>]
+ros2 launch spot_driver spot_driver.launch.py [config_file:=<path/to/config.yaml>] [spot_name:=<Robot Name>] [publish_point_clouds:=<True|False>] [launch_rviz:=<True|False>] [uncompress_images:=<True|False>] [publish_compressed_images:=<True|False>] [tf_prefix:=<TF Frame Prefix>]
 ```
 
 ## Configuration

diff --git a/spot_driver/README.md b/spot_driver/README.md
@@ -10,6 +10,7 @@ The primary launchfile in this package is `spot_driver.launch.py`. Once you've b
 * To launch the process within a namespace, add the launch argument `spot_name:={name}`
 * To visualize Spot in RViz, add the launch argument `launch_rviz:=True`. This will automatically generate the appropriate RViz config file for your robot's name using `rviz.launch.py`.
 * To publish point clouds, add the launch argument `publish_point_clouds:=True`. This is disabled by default.
+* To use a custom prefix for all frames in the TF tree, add the launch argument `tf_prefix:={prefix}`. Otherwise, the spot_name/namespace will be used to construct the prefix automatically.
 
 The Spot driver contains both Python and C++ nodes. Spot's Python SDK is used for many operations. For example, `spot_ros2` is the primary node that connects with Spot and creates the ROS 2 action servers and services. Spot's C++ SDK is used in nodes like `spot_image_publisher_node` to retrieve images from Spot's RGB and depth cameras at close to their native refresh rate of 15 Hz -- something that is not possible using the Python SDK. 
 

diff --git a/spot_driver/config/spot_ros_example.yaml b/spot_driver/config/spot_ros_example.yaml
@@ -22,8 +22,10 @@
     start_estop: False
 
     preferred_odom_frame: "odom" # pass either odom/vision. This frame will become the parent of body in tf2 tree and will be used in odometry topic. https://dev.bostondynamics.com/docs/concepts/geometry_and_frames.html?highlight=frame#frames-in-the-spot-robot-world for more info.
+
+    # These parameters may be overridden by the 'spot_name' and 'tf_prefix' launch arguments.
     spot_name: ""
-    frame_prefix: ""
+    # frame_prefix: "" # Set an explicit prefix for all frames in the tf2 tree (an empty string means no prefix will be used).
 
     cmd_duration: 0.25 # The duration of cmd_vel commands. Increase this if spot stutters when publishing cmd_vel.
     rgb_cameras: True  # Set to False if your robot has greyscale cameras -- otherwise you won't receive data.

diff --git a/spot_driver/launch/rviz.launch.py b/spot_driver/launch/rviz.launch.py
@@ -17,7 +17,7 @@
 THIS_PACKAGE = "spot_driver"
 
 
-def create_rviz_config(robot_name: str) -> None:
+def create_rviz_config(robot_name: str, tf_prefix: str) -> None:
     """Writes a configuration file for rviz to visualize a single spot robot"""
 
     RVIZ_TEMPLATE_FILENAME = os.path.join(get_package_share_directory(THIS_PACKAGE), "rviz", "spot_template.yaml")
@@ -26,9 +26,10 @@ def create_rviz_config(robot_name: str) -> None:
     with open(RVIZ_TEMPLATE_FILENAME, "r") as template_file:
         config = yaml.safe_load(template_file)
 
-        if robot_name:
+        if tf_prefix:
             # replace fixed frame with robot body frame
-            config["Visualization Manager"]["Global Options"]["Fixed Frame"] = f"{robot_name}/vision"
+            config["Visualization Manager"]["Global Options"]["Fixed Frame"] = f"{tf_prefix}vision"
+        if robot_name:
             # Add robot models for each robot
             for display in config["Visualization Manager"]["Displays"]:
                 if "RobotModel" in display["Class"]:
@@ -45,10 +46,11 @@ def create_rviz_config(robot_name: str) -> None:
 def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
     rviz_config_file = LaunchConfiguration("rviz_config_file").perform(context)
     spot_name = LaunchConfiguration("spot_name").perform(context)
+    tf_prefix = LaunchConfiguration("tf_prefix").perform(context)
 
     # It looks like passing an optional of value "None" gets converted to a string of value "None"
     if not rviz_config_file or rviz_config_file == "None":
-        create_rviz_config(spot_name)
+        create_rviz_config(spot_name, tf_prefix)
         rviz_config_file = PathJoinSubstitution([FindPackageShare(THIS_PACKAGE), "rviz", "spot.rviz"]).perform(context)
 
     rviz = launch_ros.actions.Node(
@@ -72,6 +74,13 @@ def generate_launch_description() -> launch.LaunchDescription:
             description="RViz config file",
         )
     )
+    launch_args.append(
+        DeclareLaunchArgument(
+            "tf_prefix",
+            default_value="",
+            description="apply namespace prefix to robot links and joints",
+        )
+    )
     launch_args.append(DeclareLaunchArgument("spot_name", default_value="", description="Name of Spot"))
 
     ld = launch.LaunchDescription(launch_args)

diff --git a/spot_driver/launch/spot_driver.launch.py b/spot_driver/launch/spot_driver.launch.py
@@ -1,6 +1,7 @@
 # Copyright (c) 2023-2024 Boston Dynamics AI Institute LLC. All rights reserved.
 
 import os
+from typing import Optional, Union
 
 import launch
 import launch_ros
@@ -14,8 +15,8 @@
 from spot_driver.launch.spot_launch_helpers import (
     IMAGE_PUBLISHER_ARGS,
     declare_image_publisher_args,
-    get_ros_param_dict,
     spot_has_arm,
+    substitute_launch_parameters,
 )
 
 THIS_PACKAGE = "spot_driver"
@@ -24,6 +25,8 @@
 def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
     config_file = LaunchConfiguration("config_file")
     launch_rviz = LaunchConfiguration("launch_rviz")
+    spot_name_arg = LaunchConfiguration("spot_name")
+    tf_prefix_arg = LaunchConfiguration("tf_prefix")
     rviz_config_file = LaunchConfiguration("rviz_config_file").perform(context)
     mock_enable = IfCondition(LaunchConfiguration("mock_enable", default="False")).evaluate(context)
 
@@ -32,9 +35,23 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
     if (config_file_path != "") and (not os.path.isfile(config_file_path)):
         raise FileNotFoundError("Configuration file '{}' does not exist!".format(config_file_path))
 
-    ros_params = get_ros_param_dict(config_file_path)
-    spot_name: str = ros_params["spot_name"] if "spot_name" in ros_params else ""
-    tf_prefix: str = ros_params["frame_prefix"] if "frame_prefix" in ros_params else ""
+    substitutions = {
+        "spot_name": spot_name_arg,
+        "frame_prefix": tf_prefix_arg,
+    }
+    configured_params = substitute_launch_parameters(config_file_path, substitutions, context)
+    spot_name: Optional[Union[str, LaunchConfiguration]] = (
+        configured_params["spot_name"] if "spot_name" in configured_params else None
+    )
+    tf_prefix: Optional[Union[str, LaunchConfiguration]] = (
+        configured_params["frame_prefix"] if "frame_prefix" in configured_params else None
+    )
+    if tf_prefix is None and spot_name is not None:
+        tf_prefix = (spot_name if isinstance(spot_name, str) else spot_name.perform(context)) + "/"
+    if tf_prefix is None:
+        tf_prefix = ""
+    if spot_name is None:
+        spot_name = ""
 
     if mock_enable:
         mock_has_arm = IfCondition(LaunchConfiguration("mock_has_arm")).evaluate(context)
@@ -60,7 +77,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
         executable="spot_ros2",
         name="spot_ros2",
         output="screen",
-        parameters=[config_file, spot_driver_params],
+        parameters=[configured_params, spot_driver_params],
         namespace=spot_name,
     )
     ld.add_action(spot_driver_node)
@@ -69,7 +86,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
         package="spot_driver",
         executable="spot_inverse_kinematics_node",
         output="screen",
-        parameters=[config_file],
+        parameters=[configured_params],
         namespace=spot_name,
     )
     ld.add_action(kinematic_node)
@@ -78,14 +95,11 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
         package="spot_driver",
         executable="object_synchronizer_node",
         output="screen",
-        parameters=[config_file],
+        parameters=[configured_params],
         namespace=spot_name,
     )
     ld.add_action(object_sync_node)
 
-    if not tf_prefix and spot_name:
-        tf_prefix = spot_name + "/"
-
     robot_description = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
@@ -114,7 +128,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
         package="spot_driver",
         executable="state_publisher_node",
         output="screen",
-        parameters=[config_file],
+        parameters=[configured_params],
         namespace=spot_name,
     )
     ld.add_action(spot_robot_state_publisher)
@@ -133,14 +147,17 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
         launch_arguments={
             "spot_name": spot_name,
             "rviz_config_file": rviz_config_file,
+            "tf_prefix": tf_prefix,
         }.items(),
         condition=IfCondition(launch_rviz),
     )
     ld.add_action(rviz)
 
     spot_image_publishers = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([FindPackageShare(THIS_PACKAGE), "/launch", "/spot_image_publishers.launch.py"]),
-        launch_arguments={key: LaunchConfiguration(key) for key in ["config_file"] + IMAGE_PUBLISHER_ARGS}.items(),
+        launch_arguments={
+            key: LaunchConfiguration(key) for key in ["config_file", "tf_prefix", "spot_name"] + IMAGE_PUBLISHER_ARGS
+        }.items(),
         condition=IfCondition(LaunchConfiguration("launch_image_publishers")),
     )
     ld.add_action(spot_image_publishers)
@@ -156,6 +173,13 @@ def generate_launch_description() -> launch.LaunchDescription:
             description="Path to configuration file for the driver.",
         )
     )
+    launch_args.append(
+        DeclareLaunchArgument(
+            "tf_prefix",
+            default_value="",
+            description="apply namespace prefix to robot links and joints",
+        )
+    )
     launch_args.append(
         DeclareLaunchArgument(
             "launch_rviz",
@@ -180,6 +204,7 @@ def generate_launch_description() -> launch.LaunchDescription:
         )
     )
     launch_args += declare_image_publisher_args()
+    launch_args.append(DeclareLaunchArgument("spot_name", default_value="", description="Name of Spot"))
 
     ld = launch.LaunchDescription(launch_args)
 

diff --git a/spot_driver/launch/spot_image_publishers.launch.py b/spot_driver/launch/spot_image_publishers.launch.py
@@ -14,8 +14,8 @@
     DepthRegisteredMode,
     declare_image_publisher_args,
     get_camera_sources,
-    get_ros_param_dict,
     spot_has_arm,
+    substitute_launch_parameters,
 )
 
 
@@ -93,6 +93,8 @@ def create_point_cloud_nodelets(
 
 def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
     config_file = LaunchConfiguration("config_file")
+    spot_name_arg = LaunchConfiguration("spot_name")
+    tf_prefix_arg = LaunchConfiguration("tf_prefix")
     depth_registered_mode_config = LaunchConfiguration("depth_registered_mode")
     publish_point_clouds_config = LaunchConfiguration("publish_point_clouds")
     mock_enable = IfCondition(LaunchConfiguration("mock_enable", default="False")).evaluate(context)
@@ -102,8 +104,20 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
     if (config_file_path != "") and (not os.path.isfile(config_file_path)):
         raise FileNotFoundError("Configuration file '{}' does not exist!".format(config_file_path))
 
-    ros_params = get_ros_param_dict(config_file_path)
-    spot_name: str = ros_params["spot_name"] if "spot_name" in ros_params else ""
+    substitutions = {
+        "spot_name": spot_name_arg,
+        "frame_prefix": tf_prefix_arg,
+    }
+    configured_params = substitute_launch_parameters(config_file, substitutions, context)
+    spot_name: str = (
+        (
+            configured_params["spot_name"]
+            if isinstance(configured_params["spot_name"], str)
+            else configured_params["spot_name"].perform(context)
+        )
+        if "spot_name" in configured_params
+        else ""
+    )
 
     if mock_enable:
         mock_has_arm = IfCondition(LaunchConfiguration("mock_has_arm")).evaluate(context)
@@ -137,7 +151,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
         package="spot_driver",
         executable="spot_image_publisher_node",
         output="screen",
-        parameters=[config_file, spot_image_publisher_params],
+        parameters=[configured_params, spot_image_publisher_params],
         namespace=spot_name,
     )
     ld.add_action(spot_image_publisher_node)
@@ -180,7 +194,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
                 (f"{cam_prefix}/virtual_camera/image", f"{cam_prefix}/camera/{virtual_camera_frame}/image"),
                 (f"{cam_prefix}/virtual_camera/camera_info", f"{cam_prefix}/camera/{virtual_camera_frame}/camera_info"),
             ],
-            parameters=[config_file, stitcher_params],
+            parameters=[configured_params, stitcher_params],
             condition=IfCondition(LaunchConfiguration("stitch_front_images")),
         )
         ld.add_action(image_stitcher_node)
@@ -196,6 +210,14 @@ def generate_launch_description() -> launch.LaunchDescription:
             description="Path to configuration file for the driver.",
         )
     )
+    launch_args.append(
+        DeclareLaunchArgument(
+            "tf_prefix",
+            default_value="",
+            description="apply namespace prefix to robot links and joints",
+        )
+    )
+    launch_args.append(DeclareLaunchArgument("spot_name", default_value="", description="Name of Spot"))
     launch_args += declare_image_publisher_args()
 
     ld = launch.LaunchDescription(launch_args)

diff --git a/spot_driver/spot_driver/launch/spot_launch_helpers.py b/spot_driver/spot_driver/launch/spot_launch_helpers.py
@@ -3,10 +3,12 @@
 import logging
 import os
 from enum import Enum
-from typing import Any, Dict, List, Optional, Tuple
+from typing import Any, Dict, List, Optional, Tuple, Union
 
 import yaml
+from launch import LaunchContext
 from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
 
 from spot_wrapper.wrapper import SpotWrapper
 
@@ -247,3 +249,31 @@ def spot_has_arm(config_file_path: str) -> bool:
         logger=logger,
     )
     return spot_wrapper.has_arm()
+
+
+def substitute_launch_parameters(
+    config_file_path: Union[str, LaunchConfiguration],
+    substitutions: Dict[str, LaunchConfiguration],
+    context: LaunchContext,
+) -> Dict[str, Any]:
+    """Pass the given ROS launch parameter substitutions into parameters from the ROS config yaml file.
+
+    Args:
+        config_file_path (str | LaunchConfiguration): Path to the config yaml.
+        substitutions (Dict[str, LaunchConfiguration]): Dictionary of parameter_name: parameter_value containing the
+                                                        desired launch parameter substitutions.
+        context (LaunchContext): Context for acquiring the launch configuration inner values.
+
+    Returns:
+        dict[str, Any]: dictionary of the substituted parameter_name: parameter_value.
+        If there is no config file, returns a dictionary containing only the given substitutions.
+        If there is no config file and the substitutions don't have any values, returns an empty dictionary.
+    """
+    config_params: Dict[str, Any] = get_ros_param_dict(
+        config_file_path if isinstance(config_file_path, str) else config_file_path.perform(context)
+    )
+    for key, value in substitutions.items():
+        if value.perform(context):
+            config_params[key] = value
+
+    return config_params
diff --git a/spot_driver/test/src/test_parameter_interface.cpp b/spot_driver/test/src/test_parameter_interface.cpp
@@ -497,8 +497,8 @@ TEST_F(RclcppParameterInterfaceTest, GetFramePrefixFromSpotNameFallback) {
 }
 
 TEST_F(RclcppParameterInterfaceTest, GetFramePrefixFromExplicitParameter) {
-  const auto verifyExpectedFramePrefix = [](std::shared_ptr<rclcpp::Node> node,
-                                            std::shared_ptr<rclcpp::Node> namespaced_node) -> void {
+  static constexpr auto verifyExpectedFramePrefix = [](std::shared_ptr<rclcpp::Node> node,
+                                                       std::shared_ptr<rclcpp::Node> namespaced_node) -> void {
     // GIVEN we create a RclcppParameterInterface using these nodes
     RclcppParameterInterface parameter_interface_a{node};
     RclcppParameterInterface parameter_interface_b{namespaced_node};