From eefcc96b7a83503689b46e5af1d0f2d46ad16fe8 Mon Sep 17 00:00:00 2001 From: sugikazu75 Date: Fri, 17 Jan 2025 16:08:38 +0900 Subject: [PATCH 1/5] [navigation] show target height and xy position when arming --- .../include/aerial_robot_control/flight_navigation.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 9e9aca157..1375da5aa 100644 --- a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h +++ b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h @@ -407,7 +407,8 @@ namespace aerial_robot_navigation setInitHeight(estimator_->getPos(Frame::COG, estimate_mode_).z()); setTargetYawFromCurrentState(); - ROS_INFO_STREAM("init height for takeoff: " << init_height_); + ROS_INFO_STREAM("init height for takeoff: " << init_height_ << ", target height: " << getTargetPos().z()); + ROS_INFO_STREAM("target xy pos: " << "[" << getTargetPos().x() << ", " << getTargetPos().y() << "]"); ROS_INFO("Start state"); } From 17f2e230d5dfc3731a2b3b9874166513be282fed Mon Sep 17 00:00:00 2001 From: sugikazu75 Date: Fri, 17 Jan 2025 16:09:36 +0900 Subject: [PATCH 2/5] [navigation] disable takeoff is initial xy pos error is larger than threshold --- .../include/aerial_robot_control/flight_navigation.h | 7 +++++++ aerial_robot_control/src/flight_navigation.cpp | 1 + 2 files changed, 8 insertions(+) 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 1375da5aa..aac1b103c 100644 --- a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h +++ b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h @@ -270,6 +270,7 @@ namespace aerial_robot_navigation bool lock_teleop_; ros::Time force_landing_start_time_; + double takeoff_xy_pos_tolerance_; double hover_convergent_start_time_; double hover_convergent_duration_; double land_check_start_time_; @@ -365,6 +366,12 @@ namespace aerial_robot_navigation { if(getNaviState() == TAKEOFF_STATE) return; + if(fabs(estimator_->getPos(Frame::COG, estimate_mode_).x() - getTargetPos().x()) > takeoff_xy_pos_tolerance_ || fabs(estimator_->getPos(Frame::COG, estimate_mode_).y() - getTargetPos().y()) > takeoff_xy_pos_tolerance_) + { + ROS_ERROR_STREAM("initial xy pos error: [" << estimator_->getPos(Frame::COG, estimate_mode_).x() - getTargetPos().x() << ", " << estimator_->getPos(Frame::COG, estimate_mode_).y() - getTargetPos().y() << "] is larger than threshold " << takeoff_xy_pos_tolerance_ << ". switch back to ARM_OFF_STATE"); + setNaviState(STOP_STATE); + } + if(getNaviState() == ARM_ON_STATE) { setNaviState(TAKEOFF_STATE); diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index 3ef37bd67..9e94bc68f 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -1065,6 +1065,7 @@ void BaseNavigator::rosParamInit() land_descend_vel_ == -0.3; } + getParam(nh, "takeoff_xy_pos_tolerance", takeoff_xy_pos_tolerance_, 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) { From aafb9744056eb7669fa0df529823c8852dd49de9 Mon Sep 17 00:00:00 2001 From: sugihara Date: Sun, 19 Jan 2025 15:56:37 +0900 Subject: [PATCH 3/5] [navigation] use euclidean distance to check initial position error --- .../include/aerial_robot_control/flight_navigation.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 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 aac1b103c..20bdb6f5c 100644 --- a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h +++ b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h @@ -366,9 +366,12 @@ namespace aerial_robot_navigation { if(getNaviState() == TAKEOFF_STATE) return; - if(fabs(estimator_->getPos(Frame::COG, estimate_mode_).x() - getTargetPos().x()) > takeoff_xy_pos_tolerance_ || fabs(estimator_->getPos(Frame::COG, estimate_mode_).y() - getTargetPos().y()) > takeoff_xy_pos_tolerance_) + double pos_x_error = estimator_->getPos(Frame::COG, estimate_mode_).x() - getTargetPos().x(); + double pos_y_error = estimator_->getPos(Frame::COG, estimate_mode_).y() - getTargetPos().y(); + double pos_xy_error_dist = std::sqrt(pos_x_error * pos_x_error + pos_y_error * pos_y_error); + if(pos_xy_error_dist > takeoff_xy_pos_tolerance_) { - ROS_ERROR_STREAM("initial xy pos error: [" << estimator_->getPos(Frame::COG, estimate_mode_).x() - getTargetPos().x() << ", " << estimator_->getPos(Frame::COG, estimate_mode_).y() - getTargetPos().y() << "] is larger than threshold " << takeoff_xy_pos_tolerance_ << ". switch back to ARM_OFF_STATE"); + ROS_ERROR_STREAM("initial xy error distance: " << pos_xy_error_dist << " is larger than threshold " << takeoff_xy_pos_tolerance_ << ". switch back to ARM_OFF_STATE"); setNaviState(STOP_STATE); } From e867fdc8beecd8468cb5898305ca126884327d7c Mon Sep 17 00:00:00 2001 From: sugihara Date: Sun, 19 Jan 2025 16:02:18 +0900 Subject: [PATCH 4/5] [navgation] fix definition of position error for initial check. use target - current --- .../include/aerial_robot_control/flight_navigation.h | 4 ++-- 1 file changed, 2 insertions(+), 2 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 20bdb6f5c..a3ac941d4 100644 --- a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h +++ b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h @@ -366,8 +366,8 @@ namespace aerial_robot_navigation { if(getNaviState() == TAKEOFF_STATE) return; - double pos_x_error = estimator_->getPos(Frame::COG, estimate_mode_).x() - getTargetPos().x(); - double pos_y_error = estimator_->getPos(Frame::COG, estimate_mode_).y() - getTargetPos().y(); + double pos_x_error = getTargetPos().x() - estimator_->getPos(Frame::COG, estimate_mode_).x(); + double pos_y_error = getTargetPos().y() - estimator_->getPos(Frame::COG, estimate_mode_).y(); double pos_xy_error_dist = std::sqrt(pos_x_error * pos_x_error + pos_y_error * pos_y_error); if(pos_xy_error_dist > takeoff_xy_pos_tolerance_) { From f4e551b7d4dbac78db4b54d9be9fa6c17a748afd Mon Sep 17 00:00:00 2001 From: sugikazu75 Date: Mon, 20 Jan 2025 16:07:43 +0900 Subject: [PATCH 5/5] [navigation] check the difference in height between arming and takeoff to takeoff --- .../include/aerial_robot_control/flight_navigation.h | 9 +++++++++ aerial_robot_control/src/flight_navigation.cpp | 1 + 2 files changed, 10 insertions(+) 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 a3ac941d4..e030e69f8 100644 --- a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h +++ b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h @@ -271,6 +271,7 @@ namespace aerial_robot_navigation ros::Time force_landing_start_time_; double takeoff_xy_pos_tolerance_; + double takeoff_z_pos_tolerance_; double hover_convergent_start_time_; double hover_convergent_duration_; double land_check_start_time_; @@ -366,6 +367,7 @@ namespace aerial_robot_navigation { if(getNaviState() == TAKEOFF_STATE) return; + /* check xy position error in initial state */ double pos_x_error = getTargetPos().x() - estimator_->getPos(Frame::COG, estimate_mode_).x(); double pos_y_error = getTargetPos().y() - estimator_->getPos(Frame::COG, estimate_mode_).y(); double pos_xy_error_dist = std::sqrt(pos_x_error * pos_x_error + pos_y_error * pos_y_error); @@ -375,6 +377,13 @@ namespace aerial_robot_navigation setNaviState(STOP_STATE); } + /* check difference in height between arming and takeoff */ + if(fabs(init_height_ - estimator_->getPos(Frame::COG, estimate_mode_).z()) > takeoff_z_pos_tolerance_) + { + ROS_ERROR_STREAM("difference between init height and current height: " << fabs(init_height_ - estimator_->getPos(Frame::COG, estimate_mode_).z()) << " is larger than threshold " << takeoff_z_pos_tolerance_ << ". switch back to ARM_OFF_STATE"); + setNaviState(STOP_STATE); + } + if(getNaviState() == ARM_ON_STATE) { setNaviState(TAKEOFF_STATE); diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index 9e94bc68f..f39d6aa71 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -1066,6 +1066,7 @@ void BaseNavigator::rosParamInit() } getParam(nh, "takeoff_xy_pos_tolerance", takeoff_xy_pos_tolerance_, 0.3); + getParam(nh, "takeoff_z_pos_tolerance", takeoff_z_pos_tolerance_, 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) {