allow service callbacks when in vins_kickoff estimator

ctu-mrs · Aug 22, 2024 · fa1c01b · fa1c01b
1 parent 4422e6a
commit fa1c01b
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 5 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/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.";