reset estimator service

ctu-mrs · Jun 25, 2024 · 86b90b0 · 86b90b0
1 parent ba32d50
commit 86b90b0
Show file tree

Hide file tree

Showing 2 changed files with 73 additions and 0 deletions.
diff --git a/launch/estimation_manager.launch b/launch/estimation_manager.launch
@@ -70,6 +70,7 @@
       <remap from="~height_agl_out" to="~height_agl" />
 
       <remap from="~change_estimator_in" to="~change_estimator" />
+      <remap from="~reset_estimator_in" to="~reset_estimator" />
       <remap from="~toggle_service_callbacks_in" to="~toggle_service_callbacks" />
 
       <remap from="~failsafe_out" to="control_manager/failsafe" />

diff --git a/src/estimation_manager/estimation_manager.cpp b/src/estimation_manager/estimation_manager.cpp
@@ -345,6 +345,9 @@ class EstimationManager : public nodelet::Nodelet {
   bool               callbackChangeEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
   int                estimator_switch_count_ = 0;
 
+  ros::ServiceServer srvs_reset_estimator_;
+  bool               callbackResetEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+
 
   ros::ServiceServer srvs_toggle_callbacks_;
   bool               callbackToggleServiceCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
@@ -1042,6 +1045,7 @@ void EstimationManager::timerInitialization([[maybe_unused]] const ros::TimerEve
 
   /*//{ initialize service servers */
   srvs_change_estimator_ = nh_.advertiseService("change_estimator_in", &EstimationManager::callbackChangeEstimator, this);
+  srvs_reset_estimator_ = nh_.advertiseService("reset_estimator_in", &EstimationManager::callbackResetEstimator, this);
   srvs_toggle_callbacks_ = nh_.advertiseService("toggle_service_callbacks_in", &EstimationManager::callbackToggleServiceCallbacks, this);
   /*//}*/
 
@@ -1131,6 +1135,74 @@ bool EstimationManager::callbackChangeEstimator(mrs_msgs::String::Request& req,
 }
 /*//}*/
 
+/*//{ callbackResetEstimator() */
+bool EstimationManager::callbackResetEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+
+  if (!sm_->isInitialized()) {
+    return false;
+  }
+
+  if (!callbacks_enabled_) {
+    res.success = false;
+    res.message = ("Service callbacks are disabled");
+    ROS_WARN("[%s]: Ignoring service call. Callbacks are disabled.", getName().c_str());
+    return true;
+  }
+
+
+  bool                                                target_estimator_found = false;
+  boost::shared_ptr<mrs_uav_managers::StateEstimator> target_estimator;
+  for (auto estimator : estimator_list_) {
+    if (estimator->getName() == req.value) {
+      target_estimator       = estimator;
+      target_estimator_found = true;
+      break;
+    }
+  }
+
+  if (target_estimator_found) {
+
+
+    if (target_estimator->getName() == active_estimator_->getName()) {
+      res.success = false;
+      res.message = ("Cannot reset active estimator");
+      ROS_WARN("[%s]: Ignoring service call. Cannot reset active estimator.", getName().c_str());
+      return true;
+    }
+
+      target_estimator->reset();
+      ROS_INFO("[EstimationManager]: Estimator %s reset", target_estimator->getName().c_str());
+
+      double t_wait_left = 5;
+      while (t_wait_left > 0) {
+        ROS_INFO("[EstimationManager]: Attempting starting %s estimator", target_estimator->getName().c_str());
+        target_estimator->start();
+
+        if (target_estimator->isRunning()) {
+          ROS_INFO("[EstimationManager]: Reset of %s estimator successful", target_estimator->getName().c_str());
+          break;
+        } 
+
+        const double start_period = 1.0;
+        ros::Duration(start_period).sleep();
+        t_wait_left -= start_period;
+
+      }
+
+  } else {
+    ROS_WARN("[%s]: Reset of invalid estimator %s requested", getName().c_str(), req.value.c_str());
+    res.success = false;
+    res.message = ("Not a valid estimator type");
+    return true;
+  }
+
+  res.success = true;
+  res.message = "Estimator reset successful";
+
+  return true;
+}
+/*//}*/
+
 /* //{ callbackToggleServiceCallbacks() */
 bool EstimationManager::callbackToggleServiceCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {