From 4fd5796e792ccf72fc2e3be26e776b8dfac48460 Mon Sep 17 00:00:00 2001 From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> Date: Tue, 29 Oct 2024 10:39:13 +0100 Subject: [PATCH] feat: Maintain backwards compatibility for spot_name and tf_prefix launch 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 --- README.md | 2 +- spot_driver/README.md | 1 + spot_driver/launch/rviz.launch.py | 14 ++++-- spot_driver/launch/spot_driver.launch.py | 49 ++++++++++++++----- .../launch/spot_image_publishers.launch.py | 32 ++++++++++-- .../spot_driver/launch/spot_launch_helpers.py | 32 +++++++++++- .../test/src/test_parameter_interface.cpp | 4 +- 7 files changed, 110 insertions(+), 24 deletions(-) diff --git a/README.md b/README.md index a16733703..d314f27c1 100644 --- 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:=] [spot_name:=] [publish_point_clouds:=] [launch_rviz:=] [uncompress_images:=] [publish_compressed_images:=] +ros2 launch spot_driver spot_driver.launch.py [config_file:=] [spot_name:=] [publish_point_clouds:=] [launch_rviz:=] [uncompress_images:=] [publish_compressed_images:=] [tf_prefix:=] ``` ## Configuration diff --git a/spot_driver/README.md b/spot_driver/README.md index 8d0cb5690..bd5d261f9 100644 --- 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/launch/rviz.launch.py b/spot_driver/launch/rviz.launch.py index 8d2b92a94..983b9a1dc 100644 --- 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") @@ -28,7 +28,7 @@ def create_rviz_config(robot_name: str) -> None: if robot_name: # 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" # Add robot models for each robot for display in config["Visualization Manager"]["Displays"]: if "RobotModel" in display["Class"]: @@ -45,10 +45,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 +73,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 index 9badab344..f025e6f79 100644 --- 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,6 +147,7 @@ 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), ) @@ -140,7 +155,9 @@ 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"] + 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 index 1631bdec8..01417c95c 100644 --- 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 index 144f3b90f..2f0292d52 100644 --- 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 @@ -243,3 +245,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 index 44943d8da..6b08adc73 100644 --- a/spot_driver/test/src/test_parameter_interface.cpp +++ b/spot_driver/test/src/test_parameter_interface.cpp @@ -427,8 +427,8 @@ TEST_F(RclcppParameterInterfaceTest, GetFramePrefixFromSpotNameFallback) { } TEST_F(RclcppParameterInterfaceTest, GetFramePrefixFromExplicitParameter) { - const auto verifyExpectedFramePrefix = [](std::shared_ptr node, - std::shared_ptr namespaced_node) -> void { + static constexpr auto verifyExpectedFramePrefix = [](std::shared_ptr node, + std::shared_ptr namespaced_node) -> void { // GIVEN we create a RclcppParameterInterface using these nodes RclcppParameterInterface parameter_interface_a{node}; RclcppParameterInterface parameter_interface_b{namespaced_node};