[navigation] check the difference in height between arming and takeof…

…f to takeoff
sugikazu75 · Jan 20, 2025 · f4e551b · f4e551b
1 parent e867fdc
commit f4e551b
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 0 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
@@ -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
@@ -1066,6 +1066,7 @@ void BaseNavigator::rosParamInit()
   }
 
   getParam<double>(nh, "takeoff_xy_pos_tolerance", takeoff_xy_pos_tolerance_, 0.3);
+  getParam<double>(nh, "takeoff_z_pos_tolerance", takeoff_z_pos_tolerance_, 0.3);
   getParam<double>(nh, "hover_convergent_duration", hover_convergent_duration_, 1.0);
   getParam<double>(nh, "land_check_duration", land_check_duration_, 0.5);
   if (land_check_duration_ < 0.5) {