From fa1c01b9eff6aa4b19918a956eb126ef22859b7d Mon Sep 17 00:00:00 2001
From: Matej Petrlik <matej.petrlik@fel.cvut.cz>
Date: Thu, 22 Aug 2024 10:21:40 +0200
Subject: [PATCH 1/2] allow service callbacks when in vins_kickoff estimator

---
 .../estimation_manager/private_handlers.h              |  2 +-
 src/estimation_manager/estimation_manager.cpp          | 10 ++++++----
 2 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/include/mrs_uav_managers/estimation_manager/private_handlers.h b/include/mrs_uav_managers/estimation_manager/private_handlers.h
index 4d36f0d1..8d86c01a 100644
--- a/include/mrs_uav_managers/estimation_manager/private_handlers.h
+++ b/include/mrs_uav_managers/estimation_manager/private_handlers.h
@@ -14,7 +14,7 @@ typedef boost::function<bool(const std::string)> loadConfigFile_t;
 
 struct PrivateHandlers_t
 {
-  std::shared_ptr<mrs_lib::ParamLoader> param_loader;
+  std::unique_ptr<mrs_lib::ParamLoader> param_loader;
   loadConfigFile_t                      loadConfigFile;
 };
 
diff --git a/src/estimation_manager/estimation_manager.cpp b/src/estimation_manager/estimation_manager.cpp
index bcb6d409..3cf972dd 100644
--- a/src/estimation_manager/estimation_manager.cpp
+++ b/src/estimation_manager/estimation_manager.cpp
@@ -951,7 +951,7 @@ void EstimationManager::timerInitialization([[maybe_unused]] const ros::TimerEve
     std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> ph = std::make_shared<mrs_uav_managers::estimation_manager::PrivateHandlers_t>();
 
     ph->loadConfigFile = boost::bind(&EstimationManager::loadConfigFile, this, _1);
-    ph->param_loader   = std::make_shared<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, estimator->getName()), "EstimationManager/" + estimator->getName());
+    ph->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, estimator->getName()), "EstimationManager/" + estimator->getName());
 
     if (_custom_config_ != "") {
       ph->param_loader->addYamlFile(_custom_config_);
@@ -986,7 +986,7 @@ void EstimationManager::timerInitialization([[maybe_unused]] const ros::TimerEve
     std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> ph = std::make_shared<mrs_uav_managers::estimation_manager::PrivateHandlers_t>();
 
     ph->loadConfigFile = boost::bind(&EstimationManager::loadConfigFile, this, _1);
-    ph->param_loader   = std::make_shared<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, est_alt_agl_->getName()), "EstimationManager/" + est_alt_agl_->getName());
+    ph->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, est_alt_agl_->getName()), "EstimationManager/" + est_alt_agl_->getName());
 
     if (_custom_config_ != "") {
       ph->param_loader->addYamlFile(_custom_config_);
@@ -1076,14 +1076,16 @@ bool EstimationManager::callbackChangeEstimator(mrs_msgs::String::Request& req,
     return false;
   }
 
-  if (!callbacks_enabled_) {
+  // enable switching out from vins_kickoff estimator during takeoff
+  if (!callbacks_enabled_ && active_estimator_->getName() != "vins_kickoff") {
     res.success = false;
     res.message = ("Service callbacks are disabled");
     ROS_WARN("[%s]: Ignoring service call. Callbacks are disabled.", getName().c_str());
     return true;
   }
 
-  if (req.value == "dummy" || req.value == "ground_truth") {
+  // switching into these estimators during flight is dangerous with realhw, so we don't allow it 
+  if (req.value == "dummy" || req.value == "ground_truth" || req.value == "vins_kickoff") {
     res.success = false;
     std::stringstream ss;
     ss << "Switching to " << req.value << " estimator is not allowed.";

From 141f0c78697d45b7412e96241f3de2032cd4d228 Mon Sep 17 00:00:00 2001
From: Matej Petrlik <matej.petrlik@fel.cvut.cz>
Date: Thu, 22 Aug 2024 15:22:05 +0200
Subject: [PATCH 2/2] fixed transforms when vins_kickoff is the initial
 estimator

---
 launch/estimation_manager.launch            | 2 ++
 src/transform_manager/transform_manager.cpp | 5 ++++-
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/launch/estimation_manager.launch b/launch/estimation_manager.launch
index 08f0c5af..65271b67 100644
--- a/launch/estimation_manager.launch
+++ b/launch/estimation_manager.launch
@@ -60,6 +60,8 @@
       <remap from="~imu_in" to="hw_api/imu" />
       <remap from="~hw_api_capabilities_in" to="hw_api/capabilities" />
       <remap from="~control_manager_diagnostics_in" to="control_manager/diagnostics" />
+      <remap from="~control_reference_in" to="control_manager/control_reference" />
+      <remap from="~controller_diagnostics_in" to="control_manager/controller_diagnostics" />
 
       <remap from="~profiler" to="profiler" />
       <remap from="~odom_main_out" to="~odom_main" />
diff --git a/src/transform_manager/transform_manager.cpp b/src/transform_manager/transform_manager.cpp
index c130ba6b..87c81f2f 100644
--- a/src/transform_manager/transform_manager.cpp
+++ b/src/transform_manager/transform_manager.cpp
@@ -453,7 +453,10 @@ void TransformManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg)
     pose_first_.header             = msg->header;
     pose_fixed_diff_.orientation.w = 1;
 
-    is_first_frame_id_set_ = true;
+    // we don't want vins_kickoff to be our first estimator
+    if (msg->header.frame_id != ch_->uav_name + "/vins_kickoff_origin") {
+      is_first_frame_id_set_ = true;
+    }
   }
 
   // publish static tf from fixed_origin to local_origin based on the first message