Add a customizable frame prefix ros param and unify the default fallback in param interface

README.md

@@ -87,7 +87,7 @@ Further documentation on how each of these packages can be used can be found in
 * [`spot_driver`](spot_driver): Core driver for operating Spot. This contains all of the necessary topics, services, and actions for controlling Spot and receiving state information over ROS 2.
   * The driver can be launched via the following command after building and sourcing your workspace. More details can be found on the [`spot_driver` README](spot_driver/README.md).
     ```
-    ros2 launch spot_driver spot_driver.launch.py [config_file:=<path/to/config.yaml>] [spot_name:=<Spot Name>] [launch_rviz:=<True|False>] [launch_image_publishers:=<True|False>] [publish_point_clouds:=<True|False>] [uncompress_images:=<True|False>] [publish_compressed_images:=<True|False>] [stitch_front_images:=<True|False>]
+    ros2 launch spot_driver spot_driver.launch.py [config_file:=<path/to/config.yaml>] [spot_name:=<Spot Name>] [tf_prefix:=<TF Frame Prefix>] [launch_rviz:=<True|False>] [launch_image_publishers:=<True|False>] [publish_point_clouds:=<True|False>] [uncompress_images:=<True|False>] [publish_compressed_images:=<True|False>] [stitch_front_images:=<True|False>]
     ```
 * [`spot_examples`](spot_examples): Examples of how to control Spot via the Spot driver.
 * [`spot_msgs`](spot_msgs): Custom messages, services, and interfaces relevant for operating Spot.

spot_common/spot_common/launch/spot_launch_helpers.py

@@ -3,10 +3,12 @@
 import logging
 import os
 from enum import Enum
-from typing import Any, Dict, List, Literal, Optional, Tuple
+from typing import Any, Dict, List, Literal, Optional, Tuple, Union
 
 import yaml
+from launch import LaunchContext, Substitution
 from launch.actions import DeclareLaunchArgument
+from launch.substitutions import PathJoinSubstitution
 from synchros2.launch.actions import DeclareBooleanLaunchArgument
 
 from spot_wrapper.wrapper import SpotWrapper
@@ -22,6 +24,8 @@
 _PORT: Literal["port"] = "port"
 _CAMERAS_USED: Literal["cameras_used"] = "cameras_used"
 _GRIPPERLESS: Literal["gripperless"] = "gripperless"
+_SPOT_NAME: Literal["spot_name"] = "spot_name"
+_FRAME_PREFIX: Literal["frame_prefix"] = "frame_prefix"
 
 
 IMAGE_PUBLISHER_ARGS = [
@@ -135,8 +139,8 @@ def get_ros_param_dict(config_file_path: str) -> Dict[str, Any]:
             raise yaml.YAMLError(f"Config file {config_file_path} couldn't be parsed: failed with '{exc}'")
 
 
-def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional[int], Optional[str]]:
-    """Obtain the username, password, hostname, port, and certificate of Spot from the environment variables or,
+def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional[int], Optional[str], Optional[str]]:
+    """Obtain the username, password, hostname, port, certificate, and name of Spot from the environment variables or,
     if they are not set, the configuration file yaml.
 
     Args:
@@ -146,7 +150,8 @@ def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional
         ValueError: If any of username, password, hostname is not set.
 
     Returns:
-        Tuple[str, str, str, Optional[int], Optional[str]]: username, password, hostname, port, certificate
+        Tuple[str, str, str, Optional[int], Optional[str], Optional[str]]: username, password, hostname, port,
+        certificate, spot_name
     """
     # Get value from environment variables
     username = os.getenv("BOSDYN_CLIENT_USERNAME")
@@ -155,6 +160,7 @@ def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional
     portnum = os.getenv("SPOT_PORT")
     port = int(portnum) if portnum else None
     certificate = os.getenv("SPOT_CERTIFICATE")
+    spot_name: Optional[str] = None
 
     ros_params = get_ros_param_dict(config_file_path)
     # only set username/password/hostname if they were not already set as environment variables.
@@ -174,7 +180,9 @@ def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional
             f"[Username: '{username}' Password: '{password}' Hostname: '{hostname}']. Ensure that your environment "
             "variables are set or update your config_file yaml."
         )
-    return username, password, hostname, port, certificate
+    if _SPOT_NAME in ros_params and ros_params[_SPOT_NAME]:
+        spot_name = ros_params[_SPOT_NAME]
+    return username, password, hostname, port, certificate, spot_name
 
 
 def default_camera_sources(has_arm: bool, gripperless: bool) -> List[str]:
@@ -236,18 +244,17 @@ def get_camera_sources(config_file_path: str, has_arm: bool) -> List[str]:
     return camera_sources
 
 
-def spot_has_arm(config_file_path: str, spot_name: str) -> bool:
+def spot_has_arm(config_file_path: str) -> bool:
     """Check if Spot has an arm querying the robot through SpotWrapper
 
     Args:
         config_file_path (str): Path to configuration yaml
-        spot_name (str): Name of spot
 
     Returns:
         bool: True if spot has an arm, False otherwise
     """
     logger = logging.getLogger("spot_driver_launch")
-    username, password, hostname, port, certificate = get_login_parameters(config_file_path)
+    username, password, hostname, port, certificate, spot_name = get_login_parameters(config_file_path)
     gripperless = get_gripperless(get_ros_param_dict(config_file_path))
     spot_wrapper = SpotWrapper(
         username=username,
@@ -260,3 +267,55 @@ def spot_has_arm(config_file_path: str, spot_name: str) -> bool:
         gripperless=gripperless,
     )
     return spot_wrapper.has_arm()
+
+
+def substitute_launch_parameters(
+    config_file_path: Union[str, Substitution],
+    substitutions: Dict[str, Substitution],
+    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 | Substitution): Path to the config yaml.
+        substitutions (Dict[str, Substitution]): 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
+
+
+def get_name_and_prefix(ros_params: Dict[str, Any]) -> Tuple[Union[str, Substitution], Union[str, Substitution]]:
+    """Get the Spot robot name and ROS TF frame prefix from the provided ROS parameters, which may be taken directly
+    from the yaml config or passed through from launch arguments. This will compose the frame prefix from the Spot name
+    if not given explicitly.
+
+    Args:
+        ros_params (dict[str, Any]): A dictionary of parameter_name: parameter_value.
+
+    Returns:
+        Tuple[str | Substitution, str | Substitution]: spot_name, tf_prefix.
+    """
+    spot_name: Union[str, Substitution] = ros_params[_SPOT_NAME] if _SPOT_NAME in ros_params else ""
+    tf_prefix: Optional[Union[str, Substitution]] = ros_params[_FRAME_PREFIX] if _FRAME_PREFIX in ros_params else None
+    if tf_prefix is None:
+        if isinstance(spot_name, Substitution):
+            tf_prefix = PathJoinSubstitution([spot_name, ""])
+        elif isinstance(spot_name, str) and spot_name:
+            tf_prefix = spot_name + "/"
+        else:
+            tf_prefix = ""
+
+    return spot_name, tf_prefix

spot_common/test/pytests/test_launch_helpers.py

@@ -0,0 +1,173 @@
+# Copyright (c) 2023-2024 Boston Dynamics AI Institute LLC. See LICENSE file for more info.
+
+"""
+Tests to check the ROS launch helper utilities.
+"""
+
+import tempfile
+import unittest
+
+import yaml
+from launch import LaunchContext, Substitution
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+
+from spot_common.launch.spot_launch_helpers import get_name_and_prefix, substitute_launch_parameters
+
+
+class LaunchHelpersTest(unittest.TestCase):
+    def setUp(self) -> None:
+        self.name_key: str = "spot_name"
+        self.prefix_key: str = "frame_prefix"
+        self.user_key: str = "username"
+
+        self.name_value: str = "test_spot"
+        self.prefix_value: str = "test_prefix_"
+        self.user_value: str = "test_username"
+
+        self.context = LaunchContext()
+
+    def test_substitute_launch_parameters(self) -> None:
+        """
+        Test the util substitute_launch_parameters.
+        """
+        with tempfile.NamedTemporaryFile(mode="w", suffix="config.yaml") as temp:
+            data = {
+                self.name_key: self.name_value,
+                self.prefix_key: self.prefix_value,
+                self.user_key: self.user_value,
+            }
+            yaml.dump({"/**": {"ros__parameters": data}}, temp.file)
+            temp.file.close()
+
+            # Empty substitutions should not modify the yaml file params.
+            params = substitute_launch_parameters(temp.file.name, {}, self.context)
+            self.assertEqual(params[self.name_key], self.name_value, "Substitution empty, should not change.")
+            self.assertEqual(params[self.prefix_key], self.prefix_value, "Substitution empty, should not change.")
+            self.assertEqual(params[self.user_key], self.user_value, "Substitution empty, should not change.")
+
+            # Empty launch arguments should not modify the yaml file params.
+            substitutions = {
+                self.name_key: LaunchConfiguration(self.name_key, default=""),
+                self.prefix_key: LaunchConfiguration(self.prefix_key, default=""),
+                self.user_key: LaunchConfiguration(self.user_key, default=""),
+            }
+            params = substitute_launch_parameters(temp.file.name, substitutions, self.context)
+            self.assertEqual(params[self.name_key], self.name_value, "Launch argument empty, should not change.")
+            self.assertEqual(params[self.prefix_key], self.prefix_value, "Launch argument empty, should not change.")
+            self.assertEqual(params[self.user_key], self.user_value, "Launch argument empty, should not change.")
+
+            # Substitutions should modify only their corresponding keys.
+            substitutions = {
+                self.name_key: LaunchConfiguration(self.name_key, default="spot_name_overridden"),
+                self.user_key: LaunchConfiguration(self.user_key, default="username_overridden"),
+            }
+            params = substitute_launch_parameters(temp.file.name, substitutions, self.context)
+            self.assertIsInstance(params[self.name_key], Substitution, "Launch argument set, should override.")
+            self.assertEqual(
+                params[self.name_key].perform(self.context),
+                "spot_name_overridden",
+                "Launch argument set, should override.",
+            )
+            self.assertEqual(params[self.prefix_key], self.prefix_value, "Substitution empty, should not change.")
+            self.assertIsInstance(params[self.user_key], Substitution, "Launch argument set, should override.")
+            self.assertEqual(
+                params[self.user_key].perform(self.context),
+                "username_overridden",
+                "Launch argument set, should override.",
+            )
+            substitutions = {
+                self.prefix_key: PathJoinSubstitution(["prefix_overridden", ""]),
+            }
+            params = substitute_launch_parameters(temp.file.name, substitutions, self.context)
+            self.assertEqual(params[self.name_key], self.name_value, "Substitution empty, should not change.")
+            self.assertIsInstance(params[self.prefix_key], Substitution, "Substitution set, should override.")
+            self.assertEqual(
+                params[self.prefix_key].perform(self.context),
+                "prefix_overridden/",
+                "Substitution set, should override.",
+            )
+            self.assertEqual(params[self.user_key], self.user_value, "Substitution empty, should not change.")
+
+            # Giving non-substitution types as parameter substitutions should fail.
+            substitutions = {self.name_key: "overridden"}
+            self.assertRaises(
+                AttributeError,
+                substitute_launch_parameters,
+                temp.file.name,
+                substitutions,
+                self.context,
+            )
+
+    def test_get_name_and_prefix(self) -> None:
+        """
+        Test the util get_name_and_prefix.
+        """
+        name, prefix = get_name_and_prefix({})
+        self.assertTrue(name == "" and prefix == "", "Empty parameters.")
+
+        name, prefix = get_name_and_prefix({self.name_key: ""})
+        self.assertTrue(name == "" and prefix == "", "Empty parameters.")
+
+        name, prefix = get_name_and_prefix({self.name_key: "", self.prefix_key: ""})
+        self.assertTrue(name == "" and prefix == "", "Empty parameters.")
+
+        name, prefix = get_name_and_prefix({self.name_key: self.name_value})
+        self.assertTrue(name == self.name_value and prefix == self.name_value + "/", "Prefix from name.")
+
+        name, prefix = get_name_and_prefix({self.name_key: self.name_value, self.prefix_key: ""})
+        self.assertTrue(name == self.name_value and prefix == "", "Explicit prefix.")
+
+        name, prefix = get_name_and_prefix({self.name_key: self.name_value, self.prefix_key: self.prefix_value})
+        self.assertTrue(name == self.name_value and prefix == self.prefix_value, "Explicit prefix.")
+
+        name, prefix = get_name_and_prefix({self.name_key: "", self.prefix_key: self.prefix_value})
+        self.assertTrue(name == "" and prefix == self.prefix_value, "Explicit prefix.")
+
+        # Should also work when values are Substitution types.
+        name_launch_param = LaunchConfiguration(self.name_key, default=self.name_value)
+        prefix_launch_param = LaunchConfiguration(self.prefix_key, default=self.prefix_value)
+
+        name, prefix = get_name_and_prefix({self.name_key: name_launch_param})
+        self.assertTrue(
+            isinstance(name, Substitution) and isinstance(prefix, Substitution), "Launch argument: prefix from name."
+        )
+        self.assertTrue(
+            name.perform(self.context) == self.name_value and prefix.perform(self.context) == self.name_value + "/",
+            "Launch argument: prefix from name.",
+        )
+
+        name, prefix = get_name_and_prefix({self.name_key: name_launch_param, self.prefix_key: prefix_launch_param})
+        self.assertTrue(
+            isinstance(name, Substitution) and isinstance(prefix, Substitution), "Launch argument: explicit prefix."
+        )
+        self.assertTrue(
+            name.perform(self.context) == self.name_value and prefix.perform(self.context) == self.prefix_value,
+            "Launch argument: explicit prefix.",
+        )
+
+        name_path_join_substitution = PathJoinSubstitution(self.name_value)
+        prefix_path_join_substitution = PathJoinSubstitution([self.prefix_value, ""])
+
+        name, prefix = get_name_and_prefix({self.name_key: name_path_join_substitution})
+        self.assertTrue(
+            isinstance(name, Substitution) and isinstance(prefix, Substitution), "Substitution: prefix from name."
+        )
+        self.assertTrue(
+            name.perform(self.context) == self.name_value and prefix.perform(self.context) == self.name_value + "/",
+            "Substitution: prefix from name.",
+        )
+
+        name, prefix = get_name_and_prefix(
+            {self.name_key: name_path_join_substitution, self.prefix_key: prefix_path_join_substitution}
+        )
+        self.assertTrue(
+            isinstance(name, Substitution) and isinstance(prefix, Substitution), "Substitution: explicit prefix."
+        )
+        self.assertTrue(
+            name.perform(self.context) == self.name_value and prefix.perform(self.context) == self.prefix_value + "/",
+            "Substitution: explicit prefix.",
+        )
+
+
+if __name__ == "__main__":
+    unittest.main()

spot_driver/README.md

@@ -3,7 +3,7 @@
 The Spot driver contains all of the necessary topics, services, and actions for controlling Spot over ROS 2.
 To launch the driver, run the following command, with the appropriate launch arguments and/or config file that are discussed below.
 ```
-ros2 launch spot_driver spot_driver.launch.py [config_file:=<path/to/config.yaml>] [spot_name:=<Spot Name>] [launch_rviz:=<True|False>] [launch_image_publishers:=<True|False>] [publish_point_clouds:=<True|False>] [uncompress_images:=<True|False>] [publish_compressed_images:=<True|False>] [stitch_front_images:=<True|False>]
+ros2 launch spot_driver spot_driver.launch.py [config_file:=<path/to/config.yaml>] [spot_name:=<Spot Name>] [tf_prefix:=<TF Frame Prefix>] [launch_rviz:=<True|False>] [launch_image_publishers:=<True|False>] [publish_point_clouds:=<True|False>] [uncompress_images:=<True|False>] [publish_compressed_images:=<True|False>] [stitch_front_images:=<True|False>]
 ```
 
 ## Configuration
@@ -13,16 +13,19 @@ If using environment variables, define `BOSDYN_CLIENT_USERNAME`, `BOSDYN_CLIENT_
 
 ## Namespacing
 By default, the driver is launched in the global namespace.
-To avoid this, it is recommended to launch the driver with the launch argument `spot_name:=<Spot Name>`.
+To avoid this, it is recommended to either launch the driver with the launch argument `spot_name:=<Spot Name>` or update the `spot_name` parameter in your config file (a specified launch argument will override the config file parameter).
 This will place all of the nodes, topics, services, and actions provided by the driver in the `<Spot Name>` namespace.
-Additionally, it will prefix all of the TF frames and joints of the robot with `<Spot Name>`.
+
+By default, it will also prefix all of the TF frames and joints of the robot with `<Spot Name>`.
+If you want to change this behavior and instead use a custom prefix `<TF Frame Prefix>` for all frames in the TF tree, either launch the driver with the launch argument `tf_prefix:=<TF Frame Prefix>` or update the `frame_prefix` parameter in your config file (a specified launch argument will override the config file parameter).
+If you use the config file parameter `frame_prefix`, you can disable prefixing altogether by setting it to an empty string.
 
 ## Frames
 Background information about Spot's frames from Boston Dynamics can be found [here](https://dev.bostondynamics.com/docs/concepts/geometry_and_frames). 
 By default, the Spot driver will place the "odom" frame as the root of the TF tree.
-This can be changed by setting the `tf_root` parameter in your config file to either "vision" or "body".
+This can be changed by setting the `tf_root` parameter in your config file to either "vision" or "body" (value must be given without a prefix).
 The Spot driver will also publish odometry topics with respect to the "odom" frame by default.
-If you wish to change this to "vision", update the `preferred_odom_frame` parameter in your config file.
+If you wish to change this to "vision", update the `preferred_odom_frame` parameter in your config file (value must be given without a prefix).
 
 ## Simple Robot Commands
 Many simple robot commands can be called as services from the command line once the driver is running. For example: