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

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:=<path/to/config.yaml>] [spot_name:=<Robot Name>] [publish_point_clouds:=<True|False>] [launch_rviz:=<True|False>] [uncompress_images:=<True|False>] [publish_compressed_images:=<True|False>]
+ros2 launch spot_driver spot_driver.launch.py [config_file:=<path/to/config.yaml>] [spot_name:=<Robot Name>] [publish_point_clouds:=<True|False>] [launch_rviz:=<True|False>] [uncompress_images:=<True|False>] [publish_compressed_images:=<True|False>] [tf_prefix:=<TF Frame Prefix>]
 ```
 
 ## Configuration

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. 
 

spot_driver/config/spot_ros_example.yaml

@@ -23,6 +23,10 @@
 
     preferred_odom_frame: "odom" # pass either odom/vision. This frame will become the parent of body in tf2 tree and will be used in odometry topic. https://dev.bostondynamics.com/docs/concepts/geometry_and_frames.html?highlight=frame#frames-in-the-spot-robot-world for more info.
 
+    # 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 the tf2 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

@@ -22,9 +22,9 @@ class DefaultSpotApi : public SpotApi {
   explicit DefaultSpotApi(const std::string& sdk_client_name,
                           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;
@@ -42,6 +42,6 @@ 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_;
 };
 }  // 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,38 @@ 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> kValidOdomFrameOptions{"odom", "vision"};
+
+/**
+ * @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 and a prefix string, return a new optional string which is guaranteed to be a
+ * valid odom frame option. If a valid option cannot be created, std::nullopt is returned instead.
+ * @param frame
+ * @param prefix
+ * @return A new optional string which is guaranteed to be a valid odom frame option. If a valid option cannot be
+ * created, std::nullopt is returned instead.
+ */
+std::optional<std::string> validatePreferredOdomFrame(const std::string& frame, 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

@@ -40,11 +40,13 @@ class ParameterInterfaceBase {
   virtual bool getPublishDepthImages() const = 0;
   virtual bool getPublishDepthRegisteredImages() const = 0;
   virtual std::string getPreferredOdomFrame() const = 0;
-  virtual std::string getSpotName() const = 0;
+  virtual std::optional<std::string> getFramePrefix() const = 0;
+  virtual std::string getSpotNameWithFallbackToNamespace() 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,
                                                                                     bool gripperless) const = 0;
+  virtual std::string getFramePrefixWithDefaultFallback() const = 0;
 
  protected:
   // These are the definitions of the default values for optional parameters.

spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp

@@ -35,12 +35,14 @@ class RclcppParameterInterface : public ParameterInterfaceBase {
   [[nodiscard]] bool getPublishDepthImages() const override;
   [[nodiscard]] bool getPublishDepthRegisteredImages() const override;
   [[nodiscard]] std::string getPreferredOdomFrame() const override;
-  [[nodiscard]] std::string getSpotName() const override;
+  [[nodiscard]] std::optional<std::string> getFramePrefix() const override;
+  [[nodiscard]] std::string getSpotNameWithFallbackToNamespace() const override;
   [[nodiscard]] bool getGripperless() const override;
   [[nodiscard]] std::set<spot_ros2::SpotCamera> getDefaultCamerasUsed(const bool has_arm,
                                                                       const bool gripperless) const override;
   [[nodiscard]] tl::expected<std::set<spot_ros2::SpotCamera>, std::string> getCamerasUsed(
       const bool has_arm, const bool gripperless) const override;
+  [[nodiscard]] std::string getFramePrefixWithDefaultFallback() const override;
 
  private:
   std::shared_ptr<rclcpp::Node> node_;

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)