From 1b15252caeea94de4078518c6f9059b7b6df5d1f Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Thu, 11 Apr 2024 02:49:00 +0900 Subject: [PATCH 1/4] [Navigation][Land] avoid the ascending motion in land phase --- aerial_robot_control/src/flight_navigation.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index 82c074528..d56bca77c 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -1054,7 +1054,12 @@ void BaseNavigator::rosParamInit() ros::NodeHandle nh(nh_, "navigation"); getParam(nh, "xy_control_mode", xy_control_mode_, 0); getParam(nh, "takeoff_height", takeoff_height_, 0.0); + getParam(nh, "land_descend_vel",land_descend_vel_, -0.3); + if (land_descend_vel_ >= 0) { + land_descend_vel_ == -0.3; + } + getParam(nh, "hover_convergent_duration", hover_convergent_duration_, 1.0); getParam(nh, "land_check_duration", land_check_duration_, 0.5); getParam(nh, "trajectory_reset_duration", trajectory_reset_duration_, 0.5); From 389e558d01c280fb3685b38252fe1f22c66c5913 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Thu, 11 Apr 2024 02:56:24 +0900 Subject: [PATCH 2/4] [Navigation][Land] gaurantee a minimum check duration for landing detection --- aerial_robot_control/src/flight_navigation.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index d56bca77c..33c39ffea 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -1062,6 +1062,10 @@ void BaseNavigator::rosParamInit() getParam(nh, "hover_convergent_duration", hover_convergent_duration_, 1.0); getParam(nh, "land_check_duration", land_check_duration_, 0.5); + if (land_check_duration_ < 0.5) { + land_check_duration_ = 0.5; + } + getParam(nh, "trajectory_reset_duration", trajectory_reset_duration_, 0.5); getParam(nh, "teleop_reset_duration", teleop_reset_duration_, 0.5); getParam(nh, "z_convergent_thresh", z_convergent_thresh_, 0.05); From b615e38a9dfca8c5e0895a735d5dc4aa12292305 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 28 Jan 2024 20:40:39 +0900 Subject: [PATCH 3/4] [Navigation][Land] disable to update the target xy and yaw for landing process --- .../include/aerial_robot_control/flight_navigation.h | 3 --- aerial_robot_control/src/flight_navigation.cpp | 4 ---- 2 files changed, 7 deletions(-) diff --git a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h index b6ed1b343..9e9aca157 100644 --- a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h +++ b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h @@ -463,9 +463,6 @@ namespace aerial_robot_navigation if(!teleop_flag_) return; setNaviState(LAND_STATE); - - setTargetXyFromCurrentState(); - setTargetYawFromCurrentState(); ROS_INFO("Land state"); } diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index 33c39ffea..2554732ff 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -447,8 +447,6 @@ void BaseNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg) setNaviState(LAND_STATE); //update - setTargetXyFromCurrentState(); - setTargetYawFromCurrentState(); ROS_INFO("Joy Control: Land state"); return; @@ -682,8 +680,6 @@ void BaseNavigator::update() if(normal_land && !force_att_control_flag_) { setNaviState(LAND_STATE); - setTargetXyFromCurrentState(); - setTargetYawFromCurrentState(); } } From d0a9fcc5b1ea1e7933402da526749357c27d8853 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sat, 13 Apr 2024 06:16:44 +0900 Subject: [PATCH 4/4] [Navigation][Land] add warning message for wrong value assignment --- aerial_robot_control/src/flight_navigation.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index 2554732ff..c6cc14f1f 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -1053,12 +1053,14 @@ void BaseNavigator::rosParamInit() getParam(nh, "land_descend_vel",land_descend_vel_, -0.3); if (land_descend_vel_ >= 0) { + ROS_WARN("land_descend_vel_ (current value: %f) should be negative", land_descend_vel_); land_descend_vel_ == -0.3; } getParam(nh, "hover_convergent_duration", hover_convergent_duration_, 1.0); getParam(nh, "land_check_duration", land_check_duration_, 0.5); if (land_check_duration_ < 0.5) { + ROS_WARN("land_check_duration_ (current value: %f) should be not smaller than 0.5", land_check_duration_); land_check_duration_ = 0.5; }