fixed a bug which could cause state estimator getting stuck in starte…

…d state when subestimators got into error state
ctu-mrs · Oct 3, 2023 · 00f50d4 · 00f50d4
1 parent 176b40b
commit 00f50d4
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/src/estimators/state/state_generic.cpp b/src/estimators/state/state_generic.cpp
@@ -333,6 +333,10 @@ void StateGeneric::timerCheckHealth(const ros::TimerEvent &event) {
 
       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
 
+      if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+        changeState(ERROR_STATE);
+      }
+
       if (est_lat_->isRunning() && est_alt_->isRunning() && est_hdg_->isRunning()) {
         ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
         changeState(RUNNING_STATE);