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

README.md

@@ -82,7 +82,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_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,9 +13,12 @@ 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). 

spot_driver/config/spot_ros_example.yaml

@@ -24,6 +24,10 @@
     preferred_odom_frame: "odom" # pass either odom/vision. This will be used in the odometry topic. https://dev.bostondynamics.com/docs/concepts/geometry_and_frames.html?highlight=frame#frames-in-the-spot-robot-world for more info.
     tf_root: "odom" # Use "odom" (default), "vision", or "body" for the root frame in your TF tree.
 
+    # These parameters may be overridden by the 'spot_name' and 'tf_prefix' launch arguments.
+    spot_name: ""
+    # frame_prefix: "" # Set an explicit prefix for all frames in your TF tree (an empty string is a valid prefix option).
+
     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.
     initialize_spot_cam: False # Set to True if you are connecting to a SpotCam payload module.

spot_driver/include/spot_driver/api/default_image_client.hpp

@@ -18,7 +18,7 @@ namespace spot_ros2 {
 class DefaultImageClient : public ImageClientInterface {
  public:
   DefaultImageClient(::bosdyn::client::ImageClient* image_client, std::shared_ptr<TimeSyncApi> time_sync_api,
-                     const std::string& robot_name);
+                     const std::string& frame_prefix);
 
   [[nodiscard]] tl::expected<GetImagesResult, std::string> getImages(::bosdyn::api::GetImageRequest request,
                                                                      bool uncompress_images,
@@ -27,6 +27,6 @@ class DefaultImageClient : public ImageClientInterface {
  private:
   ::bosdyn::client::ImageClient* image_client_;
   std::shared_ptr<TimeSyncApi> time_sync_api_;
-  std::string robot_name_;
+  std::string frame_prefix_;
 };
 }  // namespace spot_ros2

spot_driver/include/spot_driver/api/default_spot_api.hpp

@@ -23,9 +23,9 @@ class DefaultSpotApi : public SpotApi {
   explicit DefaultSpotApi(const std::string& sdk_client_name, const std::chrono::seconds timesync_timeout,
                           const std::optional<std::string>& certificate = std::nullopt);
 
-  [[nodiscard]] tl::expected<void, std::string> createRobot(const std::string& robot_name,
-                                                            const std::string& ip_address,
-                                                            const std::optional<int>& port = std::nullopt) override;
+  [[nodiscard]] tl::expected<void, std::string> createRobot(const std::string& ip_address,
+                                                            const std::optional<int>& port = std::nullopt,
+                                                            const std::string& frame_prefix = "") override;
   [[nodiscard]] tl::expected<void, std::string> authenticate(const std::string& username,
                                                              const std::string& password) override;
   [[nodiscard]] tl::expected<bool, std::string> hasArm() const override;
@@ -43,7 +43,7 @@ class DefaultSpotApi : public SpotApi {
   std::shared_ptr<StateClientInterface> state_client_interface_;
   std::shared_ptr<TimeSyncApi> time_sync_api_;
   std::shared_ptr<WorldObjectClientInterface> world_object_client_interface_;
-  std::string robot_name_;
+  std::string frame_prefix_;
   const std::chrono::seconds timesync_timeout_;
 };
 }  // namespace spot_ros2

spot_driver/include/spot_driver/api/spot_api.hpp

@@ -26,8 +26,9 @@ class SpotApi {
 
   virtual ~SpotApi() = default;
 
-  virtual tl::expected<void, std::string> createRobot(const std::string& robot_name, const std::string& ip_address,
-                                                      const std::optional<int>& port = std::nullopt) = 0;
+  virtual tl::expected<void, std::string> createRobot(const std::string& ip_address,
+                                                      const std::optional<int>& port = std::nullopt,
+                                                      const std::string& frame_prefix = "") = 0;
   virtual tl::expected<void, std::string> authenticate(const std::string& username, const std::string& password) = 0;
   virtual tl::expected<bool, std::string> hasArm() const = 0;
   /**

spot_driver/include/spot_driver/conversions/decompress_images.hpp

@@ -14,10 +14,10 @@
 namespace spot_ros2 {
 
 tl::expected<int, std::string> getCvPixelFormat(const bosdyn::api::Image_PixelFormat& format);
-std_msgs::msg::Header createImageHeader(const bosdyn::api::ImageCapture& image_capture, const std::string& robot_name,
+std_msgs::msg::Header createImageHeader(const bosdyn::api::ImageCapture& image_capture, const std::string& frame_prefix,
                                         const google::protobuf::Duration& clock_skew);
 tl::expected<sensor_msgs::msg::Image, std::string> getDecompressImageMsg(const bosdyn::api::ImageCapture& image_capture,
-                                                                         const std::string& robot_name,
+                                                                         const std::string& frame_prefix,
                                                                          const google::protobuf::Duration& clock_skew);
 
 }  // namespace spot_ros2

spot_driver/include/spot_driver/conversions/robot_state.hpp

@@ -23,9 +23,20 @@
 #include <spot_msgs/msg/wi_fi_state.hpp>
 #include <string>
 #include <tf2_msgs/msg/tf_message.hpp>
+#include <type_traits>
 
 namespace spot_ros2 {
 
+namespace type_traits {
+template <typename, typename = void>
+static constexpr bool is_iterable{};
+template <class T>
+inline static constexpr bool
+    is_iterable<T, std::void_t<decltype(std::declval<T>().begin()), decltype(std::declval<T>().end())>> =
+        std::is_same_v<decltype(std::declval<T>().begin()), typename T::iterator>&&
+            std::is_same_v<decltype(std::declval<T>().end()), typename T::iterator>;
+}  // namespace type_traits
+
 /**
  * @brief Mapping from the joint names used within the Spot API to the joint names used in the Spot driver and URDF.
  * @details As an example, "fl.hx" is the name of the joint in the Spot API, and "front_left_hip_x" is the name used for
@@ -41,6 +52,59 @@ inline const std::map<const std::string, const std::string> kFriendlyJointNames
     {"arm0.wr1", "arm_wr1"},        {"arm0.f1x", "arm_f1x"},
 };
 
+static constexpr std::array<const char* const, 2> kValidOdomFrameNames{"odom", "vision"};
+
+static constexpr std::array<const char* const, 3> kValidTFRootFrameNames{"odom", "vision", "body"};
+
+/**
+ * @brief Given an input string and a prefix string which is a substring starting at the beginning of the input string,
+ * return a new string which is the difference between the input string and the prefix string.
+ * @param input
+ * @param prefix
+ * @return A new string which is the difference between the input string and the prefix string.
+ */
+std::string stripPrefix(const std::string& input, const std::string& prefix);
+
+/**
+ * @brief Given an input string and a prefix string, return a new string which is the addition of the prefix string and
+ * the input string. If the input string already contains the prefix substring at the beginning, the output will be the
+ * same as input.
+ * @param input
+ * @param prefix
+ * @return A new string which is the addition of the prefix string and the input string. If the input string already
+ * contains the prefix substring at the beginning, the output will be the same as input.
+ */
+std::string prependPrefix(const std::string& input, const std::string& prefix);
+
+/**
+ * @brief Given an input frame string, a prefix string, and a set of valid base names, return a new optional string
+ * which is guaranteed to be a valid frame option. If a valid option cannot be created, std::nullopt is returned
+ * instead.
+ * @param frame
+ * @param prefix
+ * @param base_names
+ * @return A new optional string which is guaranteed to be a valid frame option w.r.t. base_names. If a valid option
+ * cannot be created, std::nullopt is returned instead.
+ */
+template <typename T>
+static constexpr std::optional<std::string> validateFrameWithPrefix(const std::string& frame, const std::string& prefix,
+                                                                    const T& base_names) {
+  static_assert(type_traits::is_iterable<T>, "Trait bound not satisfied for argument 'base_names', type not iterable.");
+  static_assert(std::is_convertible_v<typename T::value_type, std::string>,
+                "Trait bound not satisfied for argument 'base_names', iterator values not convertible to string.");
+
+  // Make sure we already have the frame and prefix combined.
+  const std::string frame_with_prefix = prependPrefix(frame, prefix);
+
+  // Compare the given prefixed frame with all valid prefixed options and set false if no match is found.
+  const bool is_valid =
+      std::find_if(base_names.begin(), base_names.end(), [&prefix, &frame_with_prefix](const auto& option) -> bool {
+        return frame_with_prefix == (prefix + option);
+      }) != base_names.end();
+
+  return is_valid ? std::make_optional(frame_with_prefix) : std::nullopt;
+}
+
 /**
  * @brief Create a BatteryStateArray ROS message by parsing a RobotState message and applying a clock skew to it.
  *

spot_driver/include/spot_driver/image_stitcher/image_stitcher.hpp

@@ -24,6 +24,7 @@
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <spot_driver/interfaces/logger_interface_base.hpp>
+#include <spot_driver/interfaces/parameter_interface_base.hpp>
 #include <spot_driver/interfaces/rclcpp_tf_broadcaster_interface.hpp>
 #include <spot_driver/interfaces/tf_listener_interface_base.hpp>
 #include <string>
@@ -101,7 +102,7 @@ class CameraHandleBase {
 
 class RclcppCameraHandle : public CameraHandleBase {
  public:
-  explicit RclcppCameraHandle(const std::shared_ptr<rclcpp::Node>& node);
+  explicit RclcppCameraHandle(const std::shared_ptr<rclcpp::Node>& node, const std::string& frame_prefix);
 
   void publish(const Image& image, const CameraInfo& info) const override;
   void broadcast(const Transform& tf, const Time& stamp) override;
@@ -165,7 +166,8 @@ class ImageStitcher {
  public:
   ImageStitcher(std::unique_ptr<CameraSynchronizerBase> synchronizer,
                 std::unique_ptr<TfListenerInterfaceBase> tf_listener, std::unique_ptr<CameraHandleBase> camera_handle,
-                std::unique_ptr<LoggerInterfaceBase> logger);
+                std::unique_ptr<LoggerInterfaceBase> logger,
+                std::unique_ptr<ParameterInterfaceBase> parameter_interface);
 
  private:
   void callback(const std::shared_ptr<const Image>&, const std::shared_ptr<const CameraInfo>&,
@@ -175,6 +177,7 @@ class ImageStitcher {
   std::unique_ptr<TfListenerInterfaceBase> tf_listener_;
   std::unique_ptr<CameraHandleBase> camera_handle_;
   std::unique_ptr<LoggerInterfaceBase> logger_;
+  std::unique_ptr<ParameterInterfaceBase> parameter_interface_;
 
   std::optional<MiddleCamera> camera_;
 };

spot_driver/include/spot_driver/image_stitcher/image_stitcher_node.hpp

@@ -15,7 +15,11 @@ class ImageStitcherNode {
   std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> get_node_base_interface();
 
  private:
+  void initialize(std::unique_ptr<CameraSynchronizerBase> synchronizer,
+                  std::unique_ptr<TfListenerInterfaceBase> tf_listener, std::unique_ptr<LoggerInterfaceBase> logger,
+                  std::unique_ptr<ParameterInterfaceBase> parameter_interface);
+
   std::shared_ptr<rclcpp::Node> node_;
-  ImageStitcher stitcher_;
+  std::unique_ptr<ImageStitcher> stitcher_;
 };
 }  // namespace spot_ros2

spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp

@@ -42,7 +42,9 @@ class ParameterInterfaceBase {
   virtual bool getPublishDepthRegisteredImages() const = 0;
   virtual std::string getPreferredOdomFrame() const = 0;
   virtual std::string getTFRoot() const = 0;
-  virtual std::string getSpotName() const = 0;
+  virtual std::optional<std::string> getFramePrefix() const = 0;
+  virtual std::string getSpotNameWithFallbackToNamespace() const = 0;
+  virtual std::string getFramePrefixWithDefaultFallback() const = 0;
   virtual bool getGripperless() const = 0;
   virtual std::set<spot_ros2::SpotCamera> getDefaultCamerasUsed(bool has_arm, bool gripperless) const = 0;
   virtual tl::expected<std::set<spot_ros2::SpotCamera>, std::string> getCamerasUsed(bool has_arm,

spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp

@@ -37,7 +37,9 @@ class RclcppParameterInterface : public ParameterInterfaceBase {
   [[nodiscard]] bool getPublishDepthRegisteredImages() const override;
   [[nodiscard]] std::string getPreferredOdomFrame() const override;
   [[nodiscard]] std::string getTFRoot() const override;
-  [[nodiscard]] std::string getSpotName() const override;
+  [[nodiscard]] std::optional<std::string> getFramePrefix() const override;
+  [[nodiscard]] std::string getSpotNameWithFallbackToNamespace() const override;
+  [[nodiscard]] std::string getFramePrefixWithDefaultFallback() const override;
   [[nodiscard]] bool getGripperless() const override;
   [[nodiscard]] std::set<spot_ros2::SpotCamera> getDefaultCamerasUsed(const bool has_arm,
                                                                       const bool gripperless) const override;

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)