diff --git a/depthai_bridge/include/depthai_bridge/TFPublisher.hpp b/depthai_bridge/include/depthai_bridge/TFPublisher.hpp index 4ed97ac4..10923937 100644 --- a/depthai_bridge/include/depthai_bridge/TFPublisher.hpp +++ b/depthai_bridge/include/depthai_bridge/TFPublisher.hpp @@ -70,7 +70,6 @@ class TFPublisher { bool modelNameAvailable(); std::unique_ptr paramClient; std::shared_ptr tfPub; - std::string nodeName; std::string camName; std::string camModel; std::string baseFrame; diff --git a/depthai_bridge/src/TFPublisher.cpp b/depthai_bridge/src/TFPublisher.cpp index 1cf31744..5ec5cdee 100644 --- a/depthai_bridge/src/TFPublisher.cpp +++ b/depthai_bridge/src/TFPublisher.cpp @@ -37,7 +37,6 @@ TFPublisher::TFPublisher(std::shared_ptr 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(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(extrinsics["toCameraSocket"].get()), camModel, rsCompatibilityMode) + "_camera_frame"); } else { ts.header.frame_id = baseFrame; @@ -110,7 +109,7 @@ void TFPublisher::publishCamTransforms(nlohmann::json camData, std::shared_ptrget_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(imuExtr["toCameraSocket"].get()), camModel, rsCompatibilityMode) + "_camera_frame"); auto extrMat = calHandler.getImuToCameraExtrinsics(static_cast(imuExtr["toCameraSocket"].get())); // pass parts of 4x4 matrix to transfFromExtr diff --git a/depthai_descriptions/launch/urdf_launch.py b/depthai_descriptions/launch/urdf_launch.py index ec7012ee..336087d3 100644 --- a/depthai_descriptions/launch/urdf_launch.py +++ b/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", diff --git a/depthai_ros_driver/CMakeLists.txt b/depthai_ros_driver/CMakeLists.txt index ab35f9c0..59e05d96 100644 --- a/depthai_ros_driver/CMakeLists.txt +++ b/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) diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp index cd7b16d6..23a83d13 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/base_node.hpp +++ b/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 baseNode; std::shared_ptr pipeline; diff --git a/depthai_ros_driver/launch/driver.launch.py b/depthai_ros_driver/launch/driver.launch.py index 7fd02432..be35e0ba 100644 --- a/depthai_ros_driver/launch/driver.launch.py +++ b/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"), diff --git a/depthai_ros_driver/package.xml b/depthai_ros_driver/package.xml index 7fb5d130..c6cbdf7a 100644 --- a/depthai_ros_driver/package.xml +++ b/depthai_ros_driver/package.xml @@ -35,6 +35,7 @@ diagnostic_updater diagnostic_msgs ffmpeg_image_transport_msgs + ament_cmake_gtest ament_cmake diff --git a/depthai_ros_driver/src/dai_nodes/base_node.cpp b/depthai_ros_driver/src/dai_nodes/base_node.cpp index 3cc89a53..2c88aa99 100644 --- a/depthai_ros_driver/src/dai_nodes/base_node.cpp +++ b/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")); diff --git a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp index 7a7652ea..8552b270 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/imu.cpp @@ -36,7 +36,7 @@ void Imu::setInOut(std::shared_ptr /* pipeline */) {} void Imu::setupQueues(std::shared_ptr /* device */) { imuQ = imuNode->out.createOutputQueue(ph->getParam("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(); diff --git a/depthai_ros_driver/src/param_handlers/driver_param_handler.cpp b/depthai_ros_driver/src/param_handlers/driver_param_handler.cpp index e533c164..14eaea32 100644 --- a/depthai_ros_driver/src/param_handlers/driver_param_handler.cpp +++ b/depthai_ros_driver/src/param_handlers/driver_param_handler.cpp @@ -1,5 +1,7 @@ #include "depthai_ros_driver/param_handlers/driver_param_handler.hpp" +#include + #include "depthai/common/UsbSpeed.hpp" #include "depthai_ros_driver/utils.hpp" #include "rclcpp/logger.hpp" @@ -7,8 +9,6 @@ #include "rclcpp/parameter.hpp" #include "rclcpp/parameter_value.hpp" -#include - namespace depthai_ros_driver { namespace param_handlers { DriverParamHandler::DriverParamHandler(std::shared_ptr node, const std::string& name, const std::string& deviceName, bool rsCompat) @@ -59,7 +59,7 @@ void DriverParamHandler::declareParams() { declareAndLogParam("i_publish_tf_from_calibration", true); declareAndLogParam("i_tf_device_name", getROSNode()->get_name()); declareAndLogParam("i_tf_device_model", ""); - declareAndLogParam("i_tf_base_frame", "oak"); + declareAndLogParam("i_tf_base_frame", getROSNode()->get_name()); declareAndLogParam("i_tf_parent_frame", "oak_parent_frame"); declareAndLogParam("i_tf_cam_pos_x", "0.0"); declareAndLogParam("i_tf_cam_pos_y", "0.0"); diff --git a/depthai_ros_driver/test/tf_prefix_test.cpp b/depthai_ros_driver/test/tf_prefix_test.cpp new file mode 100644 index 00000000..4b56a187 --- /dev/null +++ b/depthai_ros_driver/test/tf_prefix_test.cpp @@ -0,0 +1,54 @@ +#include + +#include +#include + +#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& node) : BaseNode("imu", node, nullptr, "OAK-D", false) {} + + void setupQueues(std::shared_ptr) override {} + void setNames() override {} + void setInOut(std::shared_ptr) 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("oak"); + node->declare_parameter("driver.i_publish_tf_from_calibration", true); + node->declare_parameter("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("oak"); + node->declare_parameter("driver.i_publish_tf_from_calibration", false); + node->declare_parameter("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