allow overriding of the tf child frame ids so multiple instances of a…

…ruco_detect won't conflict
UbiquityRobotics · May 30, 2024 · 8c4b4ff · 8c4b4ff
1 parent 501b839
commit 8c4b4ff
Showing 1 changed file with 6 additions and 2 deletions.
diff --git a/aruco_detect/src/aruco_detect.cpp b/aruco_detect/src/aruco_detect.cpp
@@ -95,6 +95,8 @@ class FiducialsNode {
 
     double fiducial_len;
 
+    std::string tf_prefix;
+
     bool doPoseEstimation;
     bool haveCamInfo;
     bool publishFiducialTf;
@@ -510,15 +512,15 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg)
                         ts.transform.rotation.z = q.z();
                         ts.header.frame_id = frameId;
                         ts.header.stamp = msg->header.stamp;
-                        ts.child_frame_id = "fiducial_" + std::to_string(ids[i]);
+                        ts.child_frame_id = tf_prefix + std::to_string(ids[i]);
                         broadcaster.sendTransform(ts);
                     }
                     else {
                         geometry_msgs::TransformStamped ts;
                         ts.transform = ft.transform;
                         ts.header.frame_id = frameId;
                         ts.header.stamp = msg->header.stamp;
-                        ts.child_frame_id = "fiducial_" + std::to_string(ft.fiducial_id);
+                        ts.child_frame_id = tf_prefix + std::to_string(ft.fiducial_id);
                         broadcaster.sendTransform(ts);
                     }
                 }
@@ -614,6 +616,8 @@ FiducialsNode::FiducialsNode() : nh(), pnh("~"), it(nh)
     pnh.param<bool>("vis_msgs", vis_msgs, false);
     pnh.param<bool>("verbose", verbose, false);
 
+    pnh.param<std::string>("tf_prefix", tf_prefix, "fiducial_");
+
     std::string str;
     std::vector<std::string> strs;