added option to disarm using vision-based threshold

ctu-mrs · Nov 27, 2024 · ea5ccc4 · ea5ccc4
1 parent cf57c6f
commit ea5ccc4
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 2 deletions.
diff --git a/ros_packages/mrs_precise_landing/config/precise_landing.yaml b/ros_packages/mrs_precise_landing/config/precise_landing.yaml
@@ -44,7 +44,13 @@ stages:
     speed: 0.5          # [m/s]
     height: -1.0        # [m], this is relative to the tracker_cmdk
     repeat_threshold: 2 # [-]
-    mass_factor: 0.75   # [-] mass factor for landing detection
+
+    disarming:
+      mass_factor: 0.75   # [-] mass factor for landing detection
+
+      vision:
+        enabled: false
+        distance: 0.1 # [m]
 
   repeating:
     height: 1.5 # [m]

diff --git a/ros_packages/mrs_precise_landing/src/precise_landing.cpp b/ros_packages/mrs_precise_landing/src/precise_landing.cpp
@@ -163,6 +163,8 @@ class PreciseLanding : public nodelet::Nodelet {
   double _landing_height_;
   double _landing_mass_factor_;
   int    _landing_repeat_threshold_;
+  bool   _landing_disarming_vision_enabled_;
+  double _landing_disarming_vision_distance_;
 
   // repeating params
   double _repeating_speed_;
@@ -263,8 +265,10 @@ void PreciseLanding::onInit() {
   // landing params
   param_loader.loadParam("stages/landing/speed", _landing_speed_);
   param_loader.loadParam("stages/landing/height", _landing_height_);
-  param_loader.loadParam("stages/landing/mass_factor", _landing_mass_factor_);
+  param_loader.loadParam("stages/landing/disarming/mass_factor", _landing_mass_factor_);
   param_loader.loadParam("stages/landing/repeat_threshold", _landing_repeat_threshold_);
+  param_loader.loadParam("stages/landing/disarming/vision/enabled", _landing_disarming_vision_enabled_);
+  param_loader.loadParam("stages/landing/disarming/vision/distance", _landing_disarming_vision_distance_);
 
   // repeating params
   param_loader.loadParam("stages/repeating/speed", _repeating_speed_);
@@ -1440,6 +1444,21 @@ void PreciseLanding::stateMachineTimer([[maybe_unused]] const ros::TimerEvent &e
 
       if (estimated_mass->data < (_landing_mass_factor_ * inital_mass_estimate_)) {
 
+        ROS_INFO("[PreciseLanding]: disarming using mass estimator threshold");
+
+        ROS_INFO("[PreciseLanding]: landing finished");
+
+        disarm();
+
+        changeState(IDLE_STATE);
+
+        return;
+      }
+
+      if (_landing_disarming_vision_enabled_ && alignmentCheck(0.0, 0.1, _landing_disarming_vision_distance_, 1.0)) {
+
+        ROS_INFO("[PreciseLanding]: disarming using vision-based threshold");
+
         ROS_INFO("[PreciseLanding]: landing finished");
 
         disarm();