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,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:

spot_driver/config/spot_ros_example.yaml

@@ -21,8 +21,12 @@
     estop_timeout: 9.0
     start_estop: False
 
-    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.
+    preferred_odom_frame: "odom" # Allowed values are: [odom, vision], without a prefix. 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" # Allowed values are: [odom, vision, body], without a prefix. This will set 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.

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

@@ -41,6 +41,26 @@ inline const std::map<const std::string, const std::string> kFriendlyJointNames
     {"arm0.wr1", "arm_wr1"},        {"arm0.f1x", "arm_f1x"},
 };
 
+/**
+ * @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 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,
@@ -61,8 +63,10 @@ class ParameterInterfaceBase {
   static constexpr bool kDefaultPublishCompressedImages{false};
   static constexpr bool kDefaultPublishDepthImages{true};
   static constexpr bool kDefaultPublishDepthRegisteredImages{true};
-  static constexpr auto kDefaultPreferredOdomFrame = "odom";
-  static constexpr auto kDefaultTFRoot = "odom";
+  static constexpr std::array<const char* const, 2> kValidOdomFrameNames{"odom", "vision"};
+  static constexpr std::array<const char* const, 3> kValidTFRootFrameNames{"odom", "vision", "body"};
+  static constexpr auto kDefaultPreferredOdomFrame = kValidOdomFrameNames[0];
+  static constexpr auto kDefaultTFRoot = kValidTFRootFrameNames[0];
   static constexpr bool kDefaultGripperless{false};
   static constexpr auto kCamerasWithoutHand = {"frontleft", "frontright", "left", "right", "back"};
   static constexpr auto kCamerasWithHand = {"frontleft", "frontright", "left", "right", "back", "hand"};

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)