From b9d8a4315d8c8cd12e7ed2985b650635446eec8e Mon Sep 17 00:00:00 2001 From: Sugihara Kazuki Date: Fri, 17 Jan 2025 16:11:44 +0900 Subject: [PATCH] Pr/navigation/safety takeoff (#24) * [navigation] show target height and xy position when arming * [navigation] disable takeoff is initial xy pos error is larger than threshold --- .../include/aerial_robot_control/flight_navigation.h | 10 +++++++++- aerial_robot_control/src/flight_navigation.cpp | 1 + 2 files changed, 10 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 29b65075b..f9cfae52a 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 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_; @@ -366,6 +367,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); @@ -408,7 +415,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"); } diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index 3da515055..f8e53b8e7 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -1066,6 +1066,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) {