From ea5ccc4e377ffa9620a69301215806de889f8277 Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Wed, 27 Nov 2024 10:02:48 +0100
Subject: [PATCH] added option to disarm using vision-based threshold

---
 .../config/precise_landing.yaml               |  8 ++++++-
 .../src/precise_landing.cpp                   | 21 ++++++++++++++++++-
 2 files changed, 27 insertions(+), 2 deletions(-)

diff --git a/ros_packages/mrs_precise_landing/config/precise_landing.yaml b/ros_packages/mrs_precise_landing/config/precise_landing.yaml
index f7646bb..f8db0ae 100644
--- 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
index 2e248bb..db08131 100644
--- 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();