added handling of range measurements outside of (min_range,max_range)

and estimator recovery after getting back to valid height
ctu-mrs · Aug 1, 2024 · 5720fdc · 5720fdc
1 parent 660847b
commit 5720fdc
Show file tree

Hide file tree

Showing 2 changed files with 56 additions and 15 deletions.
diff --git a/include/mrs_uav_state_estimators/estimators/correction.h b/include/mrs_uav_state_estimators/estimators/correction.h
@@ -1010,6 +1010,12 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m
     return {};
   }
 
+  const double eps = 1e-3;
+  if (msg->range <= msg->min_range + eps ||  msg->range >= msg->max_range - eps) {
+    ROS_WARN_THROTTLE(1.0, "[%s]: range measurement %.2f outside of its valid range (%.2f, %.2f)", ros::this_node::getName().c_str(), msg->range, msg->min_range, msg->max_range);
+    return {};
+  }
+
   geometry_msgs::PoseStamped range_point;
 
   range_point.header           = msg->header;

diff --git a/src/estimators/agl/garmin_agl.cpp b/src/estimators/agl/garmin_agl.cpp
@@ -179,26 +179,61 @@ void GarminAgl::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event)
     return;
   }
 
-  if (isInState(INITIALIZED_STATE)) {
+  switch (getCurrentSmState()) {
 
-    if (est_agl_garmin_->isReady()) {
-      changeState(READY_STATE);
-      ROS_INFO("[%s]: Estimator is ready to start", getPrintName().c_str());
-    } else {
-      ROS_INFO("[%s]: %s subestimators to be ready", getPrintName().c_str(), Support::waiting_for_string.c_str());
-      return;
+    case UNINITIALIZED_STATE: {
+      break;
     }
-  }
 
+    case INITIALIZED_STATE: {
 
-  if (isInState(STARTED_STATE)) {
-    ROS_INFO("[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+      if (est_agl_garmin_->isInitialized()) {
+        changeState(READY_STATE);
+        ROS_INFO("[%s]: Estimator is ready to start", getPrintName().c_str());
+      } else {
+        ROS_INFO("[%s]: %s subestimators to be ready", getPrintName().c_str(), Support::waiting_for_string.c_str());
+        return;
+      }
+      break;
+    }
 
-    if (est_agl_garmin_->isRunning()) {
-      ROS_INFO("[%s]: Subestimators converged", getPrintName().c_str());
-      changeState(RUNNING_STATE);
-    } else {
-      return;
+    case READY_STATE: {
+      break;
+    }
+
+    case STARTED_STATE: {
+
+      ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+
+      if (est_agl_garmin_->isError()) {
+        changeState(ERROR_STATE);
+      }
+
+      if (est_agl_garmin_->isRunning()) {
+        ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
+        changeState(RUNNING_STATE);
+      } else {
+        return;
+      }
+      break;
+    }
+
+    case RUNNING_STATE: {
+      if (est_agl_garmin_->isError()) {
+        changeState(ERROR_STATE);
+      }
+      break;
+    }
+
+    case STOPPED_STATE: {
+      break;
+    }
+
+    case ERROR_STATE: {
+      if (est_agl_garmin_->isReady()) {
+        changeState(READY_STATE);
+      }
+      break;
     }
   }
 }