[Dragon][Naviagation] refactor the landing process by introducing a n…

…ew state: pre_land_state (#652) Co-authored-by: Moju Zhao <[email protected]>
jsk-ros-pkg · Jan 12, 2025 · 5c0eb27 · 5c0eb27
1 parent 469996c
commit 5c0eb27
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 22 deletions.
diff --git a/robots/dragon/include/dragon/dragon_navigation.h b/robots/dragon/include/dragon/dragon_navigation.h
@@ -54,8 +54,6 @@ namespace aerial_robot_navigation
 
     void update() override;
 
-    inline const bool getLandingFlag() const { return landing_flag_; }
-
   private:
     ros::Publisher curr_target_baselink_rot_pub_;
     ros::Publisher joint_control_pub_;
@@ -81,13 +79,15 @@ namespace aerial_robot_navigation
 
     /* landing process */
     bool level_flag_;
-    bool landing_flag_;
     bool servo_torque_;
 
     /* rosparam */
     double height_thresh_;
     string joints_torque_control_srv_name_, gimbals_torque_control_srv_name_;
     double baselink_rot_change_thresh_;
     double baselink_rot_pub_interval_;
+
+    // addtional state 
+    static constexpr uint8_t PRE_LAND_STATE = 0x20;
   };
 };
diff --git a/robots/dragon/src/control/lqi_gimbal_control.cpp b/robots/dragon/src/control/lqi_gimbal_control.cpp
@@ -225,8 +225,8 @@ void DragonLQIGimbalController::gimbalControl()
       std::cout << "gimbal force for horizontal control:"  << std::endl << f_xy << std::endl;
     }
 
-  /* external wrench compensation */
-  if(boost::dynamic_pointer_cast<aerial_robot_navigation::DragonNavigator>(navigator_)->getLandingFlag())
+  /* clear external wrench compensation */
+  if(navigator_->getNaviState() != aerial_robot_navigation::HOVER_STATE)
     {
       dragon_robot_model_->resetExternalStaticWrench(); // clear the external wrench
       extra_vectoring_force_.setZero(); // clear the extra vectoring force
@@ -321,7 +321,7 @@ bool DragonLQIGimbalController::clearExternalWrenchCallback(gazebo_msgs::BodyReq
 /* extra vectoring force  */
 void DragonLQIGimbalController::extraVectoringForceCallback(const std_msgs::Float32MultiArrayConstPtr& msg)
 {
-  if(navigator_->getNaviState() != aerial_robot_navigation::HOVER_STATE || navigator_->getForceLandingFlag() || boost::dynamic_pointer_cast<aerial_robot_navigation::DragonNavigator>(navigator_)->getLandingFlag()) return;
+  if(navigator_->getNaviState() != aerial_robot_navigation::HOVER_STATE || navigator_->getForceLandingFlag()) return;
 
   if(extra_vectoring_force_.size() != msg->data.size())
     {

diff --git a/robots/dragon/src/dragon_navigation.cpp b/robots/dragon/src/dragon_navigation.cpp
@@ -7,7 +7,6 @@ DragonNavigator::DragonNavigator():
   BaseNavigator(),
   servo_torque_(false),
   level_flag_(false),
-  landing_flag_(false),
   curr_target_baselink_rot_(0, 0, 0),
   final_target_baselink_rot_(0, 0, 0)
 {
@@ -86,7 +85,6 @@ void DragonNavigator::landingProcess()
                 }
             }
 
-
           joint_control_pub_.publish(joint_control_msg);
           final_target_baselink_rot_.setValue(0, 0, 0);
 
@@ -98,23 +96,23 @@ void DragonNavigator::landingProcess()
 
           /* force set the current deisre tilt to current estimated tilt */
           curr_target_baselink_rot_.setValue(curr_roll, curr_pitch, 0);
-        }
 
-      level_flag_ = true;
+          if(getNaviState() == LAND_STATE)
+            {
+              setNaviState(PRE_LAND_STATE);
+              setTeleopFlag(false);
+              setTargetPosZ(estimator_->getState(State::Z_COG, estimate_mode_)[0]);
+              setTargetVelZ(0);
+              land_height_ = 0; // reset the land height, since it is updated in the first land_state which is forced to change to hover state to level the orientation. Thus, it is possible to have the same land height just after switching back to land state and thus stop in midair
+              ROS_INFO("[Navigation] shift to pre_land state to make the robot level");
+            }
 
-      if(getNaviState() == LAND_STATE && !landing_flag_)
-        {
-          landing_flag_ = true;
-          setTeleopFlag(false);
-          setTargetPosZ(estimator_->getState(State::Z_COG, estimate_mode_)[0]);
-          setTargetVelZ(0);
-          land_height_ = 0; // reset the land height, since it is updated in the first land_state which is forced to change to hover state to level the orientation. Thus, it is possible to have the same land height just after switching back to land state and thus stop in midair
-          setNaviState(HOVER_STATE);
+          level_flag_ = true;
         }
     }
 
   /* back to landing process */
-  if(landing_flag_)
+  if(getNaviState() == PRE_LAND_STATE)
     {
       const auto joint_state = robot_model_->kdlJointToMsg(robot_model_->getJointPositions());
       bool already_level = true;
@@ -129,9 +127,9 @@ void DragonNavigator::landingProcess()
 
       if(curr_target_baselink_rot_.length()) already_level = false;
 
-      if(already_level && getNaviState() == HOVER_STATE)
+      if(already_level)
         {
-          ROS_WARN("gimbal control: back to land state");
+          ROS_INFO("[Navigation] back to land state");
           setNaviState(LAND_STATE);
           setTeleopFlag(true);
         }
@@ -188,7 +186,6 @@ void DragonNavigator::reset()
   BaseNavigator::reset();
 
   level_flag_ = false;
-  landing_flag_ = false;
 }