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
sugikazu75 · Jan 17, 2025 · b9d8a43 · b9d8a43
1 parent b40a1d3
commit b9d8a43
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 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
@@ -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
@@ -1066,6 +1066,7 @@ void BaseNavigator::rosParamInit()
     land_descend_vel_ == -0.3;
   }
 
+  getParam<double>(nh, "takeoff_xy_pos_tolerance", takeoff_xy_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) {