Merge branch 'vins_kickoff'

ctu-mrs · Aug 22, 2024 · 0850045 · 0850045
2 parents e7c64fd + 141f0c7
commit 0850045
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 6 deletions.
diff --git 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/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/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.";

diff --git 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