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();