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

[Imaniac230]

Change Overview

Hi,

I have updated the parameter interface with a new frame_prefix parameter that would allow to explicitly define the prefix for all frames. I have integrated the spot_name parameter into the interface as well.

I believe this would be useful especially in these cases:

• when users want to integrate the nodes into their launches, but have them organized in different namespaces (ex.: /spot/chassis/ and /spot/sensors/), which is not possible with the current default behavior
• when users want to have more fine-grained control or specific requirements for the frame names

The spot_driver.launch.py launch file does expose a tf_prefix parameter, however, it only effects the static transforms from the model URDF file. All dynamic transforms from the actual spot nodes were still broadcasting the default prefix, which was somewhat confusing when the driver was launched with a specific desired value in the tf_prefix launch param and was not behaving as expected.

The proposed new behavior would:

1. Always use the explicitly given frame prefix from parameters, if given.
2. If not given, then use the default spot_name + '/' prefix with a fallback to the node namespace if robot name is also not given (this is essentially identical to the original behavior).

The actual prefix construction logic in cpp nodes was also exported directly into a method in the parameter interface. This way all nodes can use the single definition instead of creating it independently at various different places.

Summary

• added a getFramePrefix() method to parameter interface - returns an optional value from the frame_prefix argument
• added a getFramePrefixWithDefaultFallback() to the parameter interface - returns a valid prefix based on the frame_prefix and/or robot_name arguments or an empty string - this is the single place where all nodes should source the prefix from
• refactored DefaultImageClient, DefaultSpotApi, and the decompress image utils to use the more explicit frame_prefix argument instead of robot_name
• exported the local stripPrefix() function from object synchronizer to robot state conversions for reusability
• SpotImagePublisher, Kinematic, ObjectSynchronizer, StatePublisher, and ImageStitcher nodes now all use the single getFramePrefixWithDefaultFallback() method to get the prefix instead of each creating it independently

Currently, I removed the tf_prefix and spot_name launch arguments from spot_image_publishers.launch.py and spot_driver.launch.py, but I'm guessing that they should probably be preserved for cases when a config file is not used at all?

I haven't updated any tests yet, nor added any meaningful new ones, but I would be happy to receive any feedback on whether this whole proposal is even something that would actually be desired.

Testing Done

I have verified the behaviors with different scenarios experimentally with a robot:

1. specifying the config parameter values
2. launching the driver: ros2 launch spot_driver spot_driver.launch.py config_file:=spot_ros2/spot_driver/config/spot_ros_example.yaml stitch_front_images:=True
3. observing that all frames in the tf tree are valid and published correctly, and that all expected topics publish data

This will also have to go in hand with a small addition to the Wrapper constructor here: Imaniac230/spot_wrapper@eb1dc6d

TODO

• Verify that the all changes work physically with a robot as expected
• Verify that all tests for the pre-commit behavior still pass as expected

  All spot_driver tests except the python test_robot_state.test_joint_states are passing OK. I am currently not able to get that one to execute correctly even from the main branch, before any changes.

Add tests for additional scenarios:

• Default behavior without any robot name and frame prefix
• Default behavior if only robot name is given without a frame prefix
• Explicit frame prefix being correctly used if given

Maintain consistent compatibility with the original launch file arguments:

• Implement a launch helper util to handle passing the launch arguments to node parameters

Finally:

• Update the READMEs where relevant

[Imaniac230]

Looks like there are some problems with access rights. Probably again related to this being an external PR?

Build and push docker image fails with:

ERROR: failed to solve: failed to push ghcr.io/bdaiinstitute/spot_ros2_jammy_humble:pr-506: unexpected status from POST request to https://ghcr.io/v2/bdaiinstitute/spot_ros2_jammy_humble/blobs/uploads/: 403 Forbidden
Error: buildx failed with: ERROR: failed to solve: failed to push ghcr.io/bdaiinstitute/spot_ros2_jammy_humble:pr-506: unexpected status from POST request to https://ghcr.io/v2/bdaiinstitute/spot_ros2_jammy_humble/blobs/uploads/: 403 Forbidden

[Imaniac230]

Regarding the spot_name and tf_prefix parameters in launch files, I think that a simplified version of something like this in launch helpers should work.

It would not have to do any of the fancy key manipulation, as the current design always just expects a wild-carded config file. It would simply check if the launch parameters are set and if so, substitute their values into the ones from the given config file. If no config file was used, it would output an empty configuration with only those corresponding parameters set. Otherwise, we either have an empty config or pass the provided config file as is.

[Imaniac230]

@tcappellari-bdai I have implemented a small launch utility to work with the launch arguments. The spot_name usage is maintained exactly as it was originally, and the tf_prefix is now fully usable across the whole launch sequence as well. The launch parameters, if specified, will always take priority over parameters from a given config file.

I've updated the interface tests and rebased onto the latest main, so all standing todo items that I could think of should be complete, once you have some time to take a look at this.

[tcappellari-bdai]

@Imaniac230 just letting you know that I haven't forgotten about this PR and I'm still working to get it to work with our current researchers' set-ups so as not to interrupt their workflow :)

[Imaniac230]

@tcappellari-bdai thanks for the update. I took a second look at it now after a while, and there are some things that are not quite right. I want to test them in more detail, so I reverted it into a draft in the meantime.

P.S.: Sorry for the force-push.

[Imaniac230]

Summary of changes made in 3f2d35a and 10ca6ae.

There were two bugs I had here:

1. I had a mistake in the launch file, where it would try to create the prefix from an empty spot name.
   • The handling of spot_name and frame_prefix params in launch files is now exported into a get_name_and_prefix() util in spot_launch_helpers.py and new unit tests are implemented for the helpers added in this PR in test_launch_helpers.py.
2. I had the frame_prefix argument in spot_ros2 defined with an empty default value, which meant that the spot_name would never actually be used, since the empty frame_prefix can still be used a valid option.
   • This is now correctly handled without a default value and a test was added to test_ros_interfaces.py to cover this.

Additional fixes:

The error message for preferred_odom_frame in spot_ros2 always displayed the two valid options with the prefix:

spot_ros2/spot_driver/spot_driver/spot_ros2.py

Lines 355 to 368 in 796bf25

	self.tf_name_raw_kinematic: str = frame_prefix + "odom"
	self.tf_name_vision_odom: Parameter = self.declare_parameter(
	"tf_name_vision_odom", self.frame_prefix + "vision"
	)
	self.tf_name_raw_vision: str = self.frame_prefix + "vision"
	preferred_odom_frame_references = [self.tf_name_raw_kinematic, self.tf_name_raw_vision]
	if self.preferred_odom_frame.value not in preferred_odom_frame_references:
	error_msg = (
	f'rosparam "preferred_odom_frame" should be one of {preferred_odom_frame_references}, got'
	f' "{self.preferred_odom_frame.value}"'
	)
	self.get_logger().error(error_msg)
	raise ValueError(error_msg)

This could be misleading, since the non-prefixed odom and vision options are always supposed to be valid. Originally, this actually failed if the preferred_odom_frame was given as just odom or vision, which was fixed in previous commits.

• I extended the message to always display the full list of valid options with or without a prefix, and also added a test to check that the final preferred_odom_frame is always set correctly. I didn't find any actual usage of this in the python nodes, however. I also removed the two parameters tf_name_vision_odom and tf_name_kinematic_odom, since they weren't used anywhere.

The cpp nodes state_publisher and object_synchronizer did not have any validation checks for the preferred_odom_frame parameter:

spot_ros2/spot_driver/src/robot_state/state_publisher.cpp

Lines 36 to 39 in 796bf25

	const auto preferred_odom_frame = parameter_interface_->getPreferredOdomFrame();
	is_using_vision_ = preferred_odom_frame == "vision";
	full_odom_frame_id_ =
	preferred_odom_frame.find('/') == std::string::npos ? frame_prefix_ + preferred_odom_frame : preferred_odom_frame;

So, even though the spot_ros2 node would fail with an error by design, these would launch without any warnings about an invalid odom frame. And, since the prefix could now be virtually any string, the parsing with the '/' character would also not be sufficient anymore.

• To implement checks and streamline the parsing process, I added validatePreferredOdomFrame() and prependPrefix() functions to robot state conversions. Both, state_publisher and object_synchronizer, will now check if the provided preferred_odom_frame is valid (given the frame_prefix), and if not, then fallback to a default odom frame setting and log a warning message. Unit tests for stripPrefix(), prependPrefix(), and validatePreferredOdomFrame() are now also implemented in test_robot_state.cpp.

In general, all three nodes (spot_ros2, state_publisher, object_synchronizer) will verify that the given preferred_odom_frame parameter is always one of these options: prefix+odom, prefix+vision, odom, and vision.

One thing I'm not sure about is this section:

spot_ros2/spot_driver/src/conversions/robot_state.cpp

Lines 149 to 152 in 796bf25

	const auto parent_frame_name = transform.parent_frame_name().find('/') == std::string::npos
	? prefix + transform.parent_frame_name()
	: transform.parent_frame_name();
	const auto frame_name = frame_id.find('/') == std::string::npos ? prefix + frame_id : frame_id;

If I understand it correctly, these frames are coming directly from the robot messages. So it should never happen that they would be pre-pended with the prefix for ROS frames?

Once I get to the physical robot I will check that I didn't break anything with these new changes and mark this as ready. Also, I think that additional integration tests for state_publisher and object_synchronizer could be added:

1. Provide an invalid preferred_odom_frame
   1. Check that the warning message is correctly logged.
   2. Check that the root parent frame in the TF tree is correctly set to the default option.
2. Provide a valid preferred_odom_frame (different from the default option)
   1. Check that no warning mesage is logged.
   2. Check that the root parent frame in the TF tree is correctly set to the given option.

And this comment is now longer that I expected it to be.