Support configurable TF frame prefixes

depthai_bridge/include/depthai_bridge/TFPublisher.hpp

@@ -70,7 +70,6 @@ class TFPublisher {
     bool modelNameAvailable();
     std::unique_ptr<rclcpp::AsyncParametersClient> paramClient;
     std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tfPub;
-    std::string nodeName;
     std::string camName;
     std::string camModel;
     std::string baseFrame;

depthai_bridge/src/TFPublisher.cpp

@@ -37,7 +37,6 @@ TFPublisher::TFPublisher(std::shared_ptr<rclcpp::Node> node,
                          const std::string& customXacroArgs,
                          const bool rsCompatibilityMode)
     : camName(camName),
-      nodeName(node->get_name()),
       camModel(camModel),
       baseFrame(baseFrame),
       parentFrame(parentFrame),
@@ -96,11 +95,11 @@ void TFPublisher::publishCamTransforms(nlohmann::json camData, std::shared_ptr<r
         }
 
         std::string name = getSocketName(static_cast<dai::CameraBoardSocket>(cam[0]), camModel, rsCompatibilityMode);
-        ts.child_frame_id = nodeName + std::string("_") + name + std::string("_camera_frame");
+        ts.child_frame_id = getFrameName(baseFrame, name + std::string("_camera_frame"));
         // check if the camera is at the end of the chain
         if(extrinsics["toCameraSocket"] != -1) {
             ts.header.frame_id = getFrameName(
-                nodeName,
+                baseFrame,
                 getSocketName(static_cast<dai::CameraBoardSocket>(extrinsics["toCameraSocket"].get<int>()), camModel, rsCompatibilityMode) + "_camera_frame");
         } else {
             ts.header.frame_id = baseFrame;
@@ -110,7 +109,7 @@ void TFPublisher::publishCamTransforms(nlohmann::json camData, std::shared_ptr<r
             ts.transform.rotation.z = 0.0;
         }
         // rotate optical fransform
-        opticalTS.child_frame_id = getOpticalFrameName(nodeName, name, rsCompatibilityMode);
+        opticalTS.child_frame_id = getOpticalFrameName(baseFrame, name, rsCompatibilityMode);
         opticalTS.header.frame_id = ts.child_frame_id;
         opticalTS.transform.rotation.w = 0.5;
         opticalTS.transform.rotation.x = -0.5;
@@ -124,10 +123,10 @@ void TFPublisher::publishImuTransform(nlohmann::json json, std::shared_ptr<rclcp
     geometry_msgs::msg::TransformStamped ts;
     ts.header.stamp = node->get_clock()->now();
     auto imuExtr = json["imuExtrinsics"];
-    ts.child_frame_id = nodeName + std::string("_imu_frame");
+    ts.child_frame_id = getFrameName(baseFrame, "imu_frame");
     if(imuExtr["toCameraSocket"] != -1) {
         ts.header.frame_id = getFrameName(
-            nodeName,
+            baseFrame,
             getSocketName(static_cast<dai::CameraBoardSocket>(imuExtr["toCameraSocket"].get<int>()), camModel, rsCompatibilityMode) + "_camera_frame");
         auto extrMat = calHandler.getImuToCameraExtrinsics(static_cast<dai::CameraBoardSocket>(imuExtr["toCameraSocket"].get<int>()));
         // pass parts of 4x4 matrix to transfFromExtr

depthai_descriptions/launch/urdf_launch.py

@@ -32,7 +32,11 @@ def launch_setup(context, *args, **kwargs):
     rs_compat = LaunchConfiguration("rs_compat", default="false")
     use_composition = LaunchConfiguration("use_composition", default="false")
 
-    name = LaunchConfiguration("tf_prefix").perform(context)
+    name = LaunchConfiguration("name", default="").perform(context)
+    if not name:
+        name = (
+            LaunchConfiguration("tf_prefix").perform(context).strip("/").split("/")[-1]
+        )
     robot_description = {
         "robot_description": Command(
             [
@@ -107,6 +111,11 @@ def generate_launch_description():
             default_value="",
             description='Specifies the namespace of the robot state publisher node. Default value will be ""',
         ),
+        DeclareLaunchArgument(
+            "name",
+            default_value="",
+            description="Name of the robot_state_publisher node. Defaults to the last segment of tf_prefix.",
+        ),
         DeclareLaunchArgument(
             "camera_model",
             default_value="OAK-D",

depthai_ros_driver/CMakeLists.txt

@@ -152,6 +152,13 @@ ament_python_install_package(${PROJECT_NAME})
 ament_export_include_directories(include)
 
 install(DIRECTORY include/ DESTINATION include)
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(${PROJECT_NAME}_tf_prefix_test test/tf_prefix_test.cpp)
+  target_link_libraries(${PROJECT_NAME}_tf_prefix_test
+    ${COMMON_LIB_NAME}
+    rclcpp::rclcpp)
+endif()
 if(BUILD_TESTING AND TEST_DEPTHAI_ROS_DRIVER)
   # Integration tests
   find_package(ament_cmake_ros REQUIRED)

depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp

@@ -81,13 +81,13 @@ class BaseNode {
      */
     std::string getName();
     /**
-     * @brief    Append ROS node name to the frameName given.
+     * @brief    Append the configured TF prefix to the frameName given.
      *
      * @param[in]  frameName  The frame name
      */
     std::string getFrameName(const std::string& frameName = "");
     /**
-     * @brief    Append ROS node name to the frameName given and append optical frame suffix to it.
+     * @brief    Append the configured TF prefix to the frameName given and append optical frame suffix to it.
      *
      * @param[in]  frameName  The frame name
      */
@@ -99,6 +99,7 @@ class BaseNode {
     rclcpp::Logger getLogger();
 
    private:
+    std::string getTFPrefix();
     std::shared_ptr<rclcpp::Node> baseNode;
 
     std::shared_ptr<dai::Pipeline> pipeline;

depthai_ros_driver/launch/driver.launch.py

@@ -73,6 +73,9 @@ def launch_setup(context, *args, **kwargs):
     pointcloud_enable = LaunchConfiguration("pointcloud.enable", default="false")
     namespace = LaunchConfiguration("namespace", default="").perform(context)
     name = LaunchConfiguration("name").perform(context)
+    tf_prefix = LaunchConfiguration("tf_prefix", default="").perform(context).strip("/")
+    if not tf_prefix:
+        tf_prefix = name
 
     # If RealSense compatibility is enabled, we need to override some parameters, topics and node names
     parameter_overrides = {}
@@ -153,9 +156,8 @@ def launch_setup(context, *args, **kwargs):
         params = {
             "driver": {
                 "i_publish_tf_from_calibration": True,
-                "i_tf_tf_prefix": name,
                 "i_tf_camera_model": cam_model,
-                "i_tf_base_frame": name,
+                "i_tf_base_frame": tf_prefix,
                 "i_tf_parent_frame": parent_frame,
                 "i_tf_cam_pos_x": cam_pos_x.perform(context),
                 "i_tf_cam_pos_y": cam_pos_y.perform(context),
@@ -188,9 +190,10 @@ def launch_setup(context, *args, **kwargs):
             ),
             launch_arguments={
                 "namespace": namespace,
-                "tf_prefix": name,
+                "name": name,
+                "tf_prefix": tf_prefix,
                 "camera_model": camera_model,
-                "base_frame": name,
+                "base_frame": tf_prefix,
                 "parent_frame": parent_frame,
                 "cam_pos_x": cam_pos_x,
                 "cam_pos_y": cam_pos_y,
@@ -235,6 +238,11 @@ def generate_launch_description():
     declared_arguments = [
         DeclareLaunchArgument("name", default_value="oak"),
         DeclareLaunchArgument("namespace", default_value=""),
+        DeclareLaunchArgument(
+            "tf_prefix",
+            default_value="",
+            description="Prefix for image, IMU, and calibration TF frame IDs. Defaults to the node name.",
+        ),
         DeclareLaunchArgument("parent_frame", default_value="oak_parent_frame"),
         DeclareLaunchArgument("camera_model", default_value="OAK-D-PRO"),
         DeclareLaunchArgument("cam_pos_x", default_value="0.0"),

depthai_ros_driver/package.xml

@@ -35,6 +35,7 @@
   <depend>diagnostic_updater</depend>
   <depend>diagnostic_msgs</depend>
   <depend>ffmpeg_image_transport_msgs</depend>
+  <test_depend>ament_cmake_gtest</test_depend>
   <export>
     <build_type>ament_cmake</build_type>
   </export>

depthai_ros_driver/src/dai_nodes/base_node.cpp

@@ -48,12 +48,24 @@ bool BaseNode::rsCompatibilityMode() {
     return rsCompat;
 }
 
+std::string BaseNode::getTFPrefix() {
+    auto node = getROSNode();
+    bool publishTF = false;
+    if(node->get_parameter("driver.i_publish_tf_from_calibration", publishTF) && publishTF) {
+        std::string baseFrame;
+        if(node->get_parameter("driver.i_tf_base_frame", baseFrame) && !baseFrame.empty()) {
+            return baseFrame;
+        }
+    }
+    return node->get_name();
+}
+
 std::string BaseNode::getFrameName(const std::string& frameName) {
-    return depthai_bridge::getFrameName(getROSNode()->get_name(), frameName);
+    return depthai_bridge::getFrameName(getTFPrefix(), frameName);
 }
 
 std::string BaseNode::getOpticalFrameName(const std::string& frameName) {
-    return depthai_bridge::getOpticalFrameName(getROSNode()->get_name(), frameName, rsCompat);
+    return depthai_bridge::getOpticalFrameName(getTFPrefix(), frameName, rsCompat);
 }
 dai::Node::Input& BaseNode::getInput(int /*linkType = 0*/) {
     throw(std::runtime_error("getInput() not implemented"));

depthai_ros_driver/src/dai_nodes/sensors/imu.cpp

@@ -36,7 +36,7 @@ void Imu::setInOut(std::shared_ptr<dai::Pipeline> /* pipeline */) {}
 
 void Imu::setupQueues(std::shared_ptr<dai::Device> /* device */) {
     imuQ = imuNode->out.createOutputQueue(ph->getParam<int>("i_max_q_size"), false);
-    auto tfPrefix = std::string(getROSNode()->get_name()) + "_" + getName();
+    auto tfPrefix = getFrameName(getName());
     auto imuMode = ph->getSyncMethod();
     rclcpp::PublisherOptions options;
     options.qos_overriding_options = rclcpp::QosOverridingOptions();

depthai_ros_driver/src/param_handlers/driver_param_handler.cpp

@@ -1,14 +1,14 @@
 #include "depthai_ros_driver/param_handlers/driver_param_handler.hpp"
 
+#include <stdexcept>
+
 #include "depthai/common/UsbSpeed.hpp"
 #include "depthai_ros_driver/utils.hpp"
 #include "rclcpp/logger.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/parameter_value.hpp"
 
-#include <stdexcept>
-
 namespace depthai_ros_driver {
 namespace param_handlers {
 DriverParamHandler::DriverParamHandler(std::shared_ptr<rclcpp::Node> node, const std::string& name, const std::string& deviceName, bool rsCompat)
@@ -59,7 +59,7 @@ void DriverParamHandler::declareParams() {
     declareAndLogParam<bool>("i_publish_tf_from_calibration", true);
     declareAndLogParam<std::string>("i_tf_device_name", getROSNode()->get_name());
     declareAndLogParam<std::string>("i_tf_device_model", "");
-    declareAndLogParam<std::string>("i_tf_base_frame", "oak");
+    declareAndLogParam<std::string>("i_tf_base_frame", getROSNode()->get_name());
     declareAndLogParam<std::string>("i_tf_parent_frame", "oak_parent_frame");
     declareAndLogParam<std::string>("i_tf_cam_pos_x", "0.0");
     declareAndLogParam<std::string>("i_tf_cam_pos_y", "0.0");

depthai_ros_driver/test/tf_prefix_test.cpp

@@ -0,0 +1,54 @@
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+
+#include "depthai_ros_driver/dai_nodes/base_node.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace depthai_ros_driver {
+namespace dai_nodes {
+namespace {
+class TestNode : public BaseNode {
+   public:
+    explicit TestNode(const std::shared_ptr<rclcpp::Node>& node) : BaseNode("imu", node, nullptr, "OAK-D", false) {}
+
+    void setupQueues(std::shared_ptr<dai::Device>) override {}
+    void setNames() override {}
+    void setInOut(std::shared_ptr<dai::Pipeline>) override {}
+    void closeQueues() override {}
+};
+
+class TFPrefixTest : public ::testing::Test {
+   protected:
+    static void SetUpTestSuite() {
+        rclcpp::init(0, nullptr);
+    }
+
+    static void TearDownTestSuite() {
+        rclcpp::shutdown();
+    }
+};
+
+TEST_F(TFPrefixTest, UsesConfiguredBaseFrameWhenPublishingCalibrationTF) {
+    auto node = std::make_shared<rclcpp::Node>("oak");
+    node->declare_parameter("driver.i_publish_tf_from_calibration", true);
+    node->declare_parameter<std::string>("driver.i_tf_base_frame", "robot1/oakd");
+    TestNode testNode(node);
+
+    EXPECT_EQ(testNode.getFrameName("imu"), "robot1/oakd_imu");
+    EXPECT_EQ(testNode.getOpticalFrameName("rgb"), "robot1/oakd_rgb_camera_optical_frame");
+}
+
+TEST_F(TFPrefixTest, FallsBackToNodeNameWhenCalibrationTFIsDisabled) {
+    auto node = std::make_shared<rclcpp::Node>("oak");
+    node->declare_parameter("driver.i_publish_tf_from_calibration", false);
+    node->declare_parameter<std::string>("driver.i_tf_base_frame", "robot1/oakd");
+    TestNode testNode(node);
+
+    EXPECT_EQ(testNode.getFrameName("imu"), "oak_imu");
+    EXPECT_EQ(testNode.getOpticalFrameName("rgb"), "oak_rgb_camera_optical_frame");
+}
+}  // namespace
+}  // namespace dai_nodes
+}  // namespace depthai_ros_driver