From 797d905c8af9221784cda2abfd070763c68a5488 Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Tue, 2 Apr 2024 20:04:14 +0200
Subject: [PATCH] estimator won't no loneger auto-prefix uav_name to camera
 frame

---
 .../config/landing_pad_estimation.yaml        |  9 ++++++++
 .../src/landing_pad_estimation.cpp            | 23 +++++++++++++++----
 2 files changed, 27 insertions(+), 5 deletions(-)

diff --git a/ros_packages/mrs_landing_pad_estimation/config/landing_pad_estimation.yaml b/ros_packages/mrs_landing_pad_estimation/config/landing_pad_estimation.yaml
index a87bc0b..ba89a5c 100644
--- a/ros_packages/mrs_landing_pad_estimation/config/landing_pad_estimation.yaml
+++ b/ros_packages/mrs_landing_pad_estimation/config/landing_pad_estimation.yaml
@@ -5,6 +5,9 @@ tag_ids: [0, 10] # [-]
 # (detections are transformed to this frame first)
 estimation_frame: "fixed_origin"
 
+# the UAV's body frame
+body_frame: "fcu"
+
 # internal loop rate of the LKF
 prediction_rate: 100.0 # [Hz]
 
@@ -15,6 +18,12 @@ correction_timeout: 2.0 # [s]
 # anything detection beyond this distance (in the fcu frame) will be thrown away
 max_relative_distance: 8.0
 
+# settings for the mrs_lib::Transformer
+transformer:
+
+  # should unprefixed frame_id be automatically prefixed with the UAV_NAME?
+  autoprefix_uav_name: false
+
 # parameters for the LKF estimator
 # don't change
 lkf:
diff --git a/ros_packages/mrs_landing_pad_estimation/src/landing_pad_estimation.cpp b/ros_packages/mrs_landing_pad_estimation/src/landing_pad_estimation.cpp
index aab7a8d..4412219 100644
--- a/ros_packages/mrs_landing_pad_estimation/src/landing_pad_estimation.cpp
+++ b/ros_packages/mrs_landing_pad_estimation/src/landing_pad_estimation.cpp
@@ -86,8 +86,12 @@ class LandingPadEstimation : public nodelet::Nodelet {
   std::string      _uav_name_;
   std::vector<int> _tag_ids_;
   std::string      _estimation_frame_;
+  std::string      _full_estimation_frame_;
+  std::string      _body_frame_;
+  std::string      _full_body_frame_;
   double           _correction_timeout_;
   double           max_relative_distance_;
+  bool             _autoprefix_uav_name_;
 
   mrs_lib::Transformer transformer_;
 
@@ -133,14 +137,19 @@ void LandingPadEstimation::onInit() {
   param_loader.loadParam("uav_name", _uav_name_);
   param_loader.loadParam("tag_ids", _tag_ids_);
   param_loader.loadParam("estimation_frame", _estimation_frame_);
+  param_loader.loadParam("body_frame", _body_frame_);
   param_loader.loadParam("correction_timeout", _correction_timeout_);
   param_loader.loadParam("max_relative_distance", max_relative_distance_);
+  param_loader.loadParam("transformer/autoprefix_uav_name", _autoprefix_uav_name_);
 
   if (!param_loader.loadedSuccessfully()) {
     ROS_ERROR("[LandingPadEstimation]: Could not load all parameters!");
     ros::shutdown();
   }
 
+  _full_estimation_frame_ = _uav_name_ + "/" + _estimation_frame_;
+  _full_body_frame_       = _uav_name_ + "/" + _body_frame_;
+
   // state matrix
   param_loader.loadMatrixStatic("lkf/A", A_);
 
@@ -182,7 +191,11 @@ void LandingPadEstimation::onInit() {
   // | ----------------------- transfomer ----------------------- |
 
   transformer_ = mrs_lib::Transformer("LandingPadEstimation");
-  transformer_.setDefaultPrefix(_uav_name_);
+
+  if (_autoprefix_uav_name_) {
+    transformer_.setDefaultPrefix(_uav_name_);
+  }
+
   transformer_.retryLookupNewest(true);
 
   // | --------------------------- lkf -------------------------- |
@@ -237,10 +250,10 @@ void LandingPadEstimation::callbackTagDetections(const apriltag_ros::AprilTagDet
   // | ------------------- check for outliers ------------------- |
 
   {
-    auto result = transformer_.transformSingle(tag_pose.value(), "fcu");
+    auto result = transformer_.transformSingle(tag_pose.value(), _full_body_frame_);
 
     if (!result) {
-      ROS_ERROR("[LandingPadEstimation]: could not transform the tag detection to 'fcu'");
+      ROS_ERROR("[LandingPadEstimation]: could not transform the tag detection to '%s'", _full_body_frame_.c_str());
       return;
     }
 
@@ -252,10 +265,10 @@ void LandingPadEstimation::callbackTagDetections(const apriltag_ros::AprilTagDet
 
   // | ------------------- transform the pose ------------------- |
 
-  auto result = transformer_.transformSingle(tag_pose.value(), _estimation_frame_);
+  auto result = transformer_.transformSingle(tag_pose.value(), _full_estimation_frame_);
 
   if (!result) {
-    ROS_ERROR("[LandingPadEstimation]: could not transform the tag detection to '%s'", _estimation_frame_.c_str());
+    ROS_ERROR("[LandingPadEstimation]: could not transform the tag detection to '%s'", _full_estimation_frame_.c_str());
     return;
   }