Get frame_id prefix from paramters when using tf publishing is on

luxonis · Dec 6, 2024 · fd2c36d · fd2c36d
1 parent 5aed7fa
commit fd2c36d
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 0 deletions.
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
@@ -5,6 +5,7 @@
 
 #include "depthai-shared/common/CameraBoardSocket.hpp"
 #include "depthai/pipeline/Node.hpp"
+#include "depthai_ros_driver/param_handlers/camera_param_handler.hpp"
 #include "depthai_ros_driver/utils.hpp"
 #include "rclcpp/logger.hpp"
 
@@ -99,6 +100,8 @@ class BaseNode {
     std::string baseDAINodeName;
     bool intraProcessEnabled;
     rclcpp::Logger logger;
+
+    std::unique_ptr<param_handlers::CameraParamHandler> ph;
 };
 }  // namespace dai_nodes
 }  // namespace depthai_ros_driver
diff --git a/depthai_ros_driver/src/dai_nodes/base_node.cpp b/depthai_ros_driver/src/dai_nodes/base_node.cpp
@@ -15,6 +15,7 @@ namespace dai_nodes {
 BaseNode::BaseNode(const std::string& daiNodeName, std::shared_ptr<rclcpp::Node> node, std::shared_ptr<dai::Pipeline> /*pipeline*/)
     : baseNode(node), baseDAINodeName(daiNodeName), logger(node->get_logger()) {
     intraProcessEnabled = node->get_node_options().use_intra_process_comms();
+    ph = std::make_unique<param_handlers::CameraParamHandler>(baseNode, "camera");
 };
 BaseNode::~BaseNode() = default;
 void BaseNode::setNodeName(const std::string& daiNodeName) {
@@ -46,6 +47,10 @@ std::string BaseNode::getSocketName(dai::CameraBoardSocket socket) {
 }
 
 std::string BaseNode::getTFPrefix(const std::string& frameName) {
+    if(ph->getParam<bool>("i_publish_tf_from_calibration")) {
+        return ph->getParam<std::string>("i_tf_base_frame") + "_" + frameName;
+    }
+
     return std::string(getROSNode()->get_name()) + "_" + frameName;
 }