From cf57c6fe8ebb4d5c7984977ef230ef19db6cb408 Mon Sep 17 00:00:00 2001
From: Tomas Baca <klaxalk@gmail.com>
Date: Wed, 23 Oct 2024 13:18:37 +0200
Subject: [PATCH] changes from the robofly

---
 .../launch/apriltag_detector.launch                 |  2 +-
 .../mrs_precise_landing/config/precise_landing.yaml | 13 +++++++------
 .../mrs_precise_landing/src/precise_landing.cpp     |  4 +++-
 3 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/ros_packages/mrs_landing_pad_estimation/launch/apriltag_detector.launch b/ros_packages/mrs_landing_pad_estimation/launch/apriltag_detector.launch
index ed70e54..ebcd737 100644
--- a/ros_packages/mrs_landing_pad_estimation/launch/apriltag_detector.launch
+++ b/ros_packages/mrs_landing_pad_estimation/launch/apriltag_detector.launch
@@ -25,7 +25,7 @@
       <remap from="tag_detections" to="~/tag_detections" />
       <remap from="tag_detections_image" to="~/tag_detections_image" />
 
-      <param name="publish_tag_detections_image" type="bool" value="true" /><!-- default: false -->
+      <param name="publish_tag_detections_image" type="bool" value="false" /><!-- default: false -->
       <param name="queue_size" type="int" value="$(arg queue_size)" />
 
     </node>
diff --git a/ros_packages/mrs_precise_landing/config/precise_landing.yaml b/ros_packages/mrs_precise_landing/config/precise_landing.yaml
index f5ae6b2..f7646bb 100644
--- a/ros_packages/mrs_precise_landing/config/precise_landing.yaml
+++ b/ros_packages/mrs_precise_landing/config/precise_landing.yaml
@@ -17,8 +17,8 @@ desired_heading:
 stages:
 
   aligning:
-    speed: 1.0    # [m/s]
-    height: 3.0   # [m]
+    speed: 0.5    # [m/s]
+    height: 1.5   # [m]
     radius: 0.2   # [m]
     timeout: 10.0 # [s]
 
@@ -42,16 +42,17 @@ stages:
 
   landing:
     speed: 0.5          # [m/s]
-    height: -1.0         # [m], this is relative to the tracker_cmdk
-    repeat_threshold: 2  # [-]
+    height: -1.0        # [m], this is relative to the tracker_cmdk
+    repeat_threshold: 2 # [-]
+    mass_factor: 0.75   # [-] mass factor for landing detection
 
   repeating:
-    height: 3.0 # [m]
+    height: 1.5 # [m]
     speed: 1.5 # [m/s]
     timeout: 10.0 # [m]
 
   aborting:
-    height: 3.0 # [m]
+    height: 1.5 # [m]
 
 # rate of the feedback loop
 rate: 10.0 # [Hz]
diff --git a/ros_packages/mrs_precise_landing/src/precise_landing.cpp b/ros_packages/mrs_precise_landing/src/precise_landing.cpp
index f74f437..2e248bb 100644
--- a/ros_packages/mrs_precise_landing/src/precise_landing.cpp
+++ b/ros_packages/mrs_precise_landing/src/precise_landing.cpp
@@ -161,6 +161,7 @@ class PreciseLanding : public nodelet::Nodelet {
 
   double _landing_speed_;
   double _landing_height_;
+  double _landing_mass_factor_;
   int    _landing_repeat_threshold_;
 
   // repeating params
@@ -262,6 +263,7 @@ 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/repeat_threshold", _landing_repeat_threshold_);
 
   // repeating params
@@ -1436,7 +1438,7 @@ void PreciseLanding::stateMachineTimer([[maybe_unused]] const ros::TimerEvent &e
 
       auto estimated_mass = sh_mass_estimate_.getMsg();
 
-      if (estimated_mass->data < (0.5 * inital_mass_estimate_)) {
+      if (estimated_mass->data < (_landing_mass_factor_ * inital_mass_estimate_)) {
 
         ROS_INFO("[PreciseLanding]: landing finished");