fixes from the uav tests

ctu-mrs · Apr 23, 2024 · 680d4ac · 680d4ac
1 parent 797d905
commit 680d4ac
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 6 deletions.
diff --git a/ros_packages/mrs_landing_pad_estimation/config/landing_pad_estimation.yaml b/ros_packages/mrs_landing_pad_estimation/config/landing_pad_estimation.yaml
@@ -38,10 +38,10 @@ lkf:
   B: []
 
   # measurement noise
-  R: [1.0, 0, 0, 0,
-      0, 1.0, 0, 0,
-      0, 0, 1.0, 0,
-      0, 0, 0, 1.0]
+  R: [10.0, 0, 0, 0,
+      0, 10.0, 0, 0,
+      0, 0, 10.0, 0,
+      0, 0, 0, 10.0]
 
   # process covariance
   Q: [1.0, 0, 0, 0,

diff --git a/ros_packages/mrs_precise_landing/config/precise_landing.yaml b/ros_packages/mrs_precise_landing/config/precise_landing.yaml
@@ -41,7 +41,7 @@ stages:
     in_alignment_duration: 0.5 # [s]
 
   landing:
-    speed: 0.25          # [m/s]
+    speed: 0.5          # [m/s]
     height: -1.0         # [m], this is relative to the tracker_cmdk
     repeat_threshold: 2  # [-]
 

diff --git a/ros_packages/mrs_precise_landing/src/precise_landing.cpp b/ros_packages/mrs_precise_landing/src/precise_landing.cpp
@@ -389,7 +389,7 @@ bool PreciseLanding::callbackAbort([[maybe_unused]] std_srvs::Trigger::Request &
 
 //}
 
-/* callbackStart() //{ */
+/* callbackLand() //{ */
 
 bool PreciseLanding::callbackLand(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
 
@@ -443,6 +443,14 @@ bool PreciseLanding::callbackLand(std_srvs::Trigger::Request &req, std_srvs::Tri
       res.success = false;
       return true;
     }
+
+    if (!see_landing_pad_) {
+      ss << "landing pad not detected";
+      ROS_ERROR_STREAM_THROTTLE(1.0, "[PreciseLanding]: " << ss.str());
+      res.message = ss.str();
+      res.success = false;
+      return true;
+    }
   }
 
   {