[control][navigation] Skip following target_pose trajectory during la…

…nding (#633)
jsk-ros-pkg · Nov 27, 2024 · 8b27d4f · 8b27d4f
1 parent 71d746f
commit 8b27d4f
Showing 1 changed file with 46 additions and 43 deletions.
diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp
@@ -686,57 +686,60 @@ void BaseNavigator::update()
   /* update the target pos and velocity */
   if (trajectory_mode_)
     {
-      if (traj_generator_ptr_.get() == nullptr)
+      if(getNaviState() == HOVER_STATE)
         {
-          if (ros::Time::now().toSec() > trajectory_reset_time_)
+          if (traj_generator_ptr_.get() == nullptr)
             {
-              setTargetZeroVel();
-              setTargetZeroAcc();
+              if (ros::Time::now().toSec() > trajectory_reset_time_)
+                {
+                  setTargetZeroVel();
+                  setTargetZeroAcc();
 
-              setTargetZeroOmega();
-              setTargetZeroAngAcc();
+                  setTargetZeroOmega();
+                  setTargetZeroAngAcc();
 
-              trajectory_mode_ = false;
+                  trajectory_mode_ = false;
 
-              ROS_INFO("[Flight nav] stop trajectory mode in POS-VEL mode");
-            }
-        }
-      else
-        {
-          // trajectory following mode
-          double t = ros::Time::now().toSec();
-          double end_t = traj_generator_ptr_->getEndSetpoint().state.t;
-          if (t > end_t)
-            {
-              ROS_INFO("[Nav] reach the end of trajectory");
-
-              setTargetZeroVel();
-              setTargetZeroAcc();
-
-              setTargetZeroOmega();
-              setTargetZeroAngAcc();
-
-              trajectory_mode_ = false;
-
-              traj_generator_ptr_.reset();
+                  ROS_INFO("[Flight nav] stop trajectory mode in POS-VEL mode");
+                }
             }
           else
             {
-              agi::QuadState target_state = traj_generator_ptr_->getState(t);
-              setTargetPos(tf::Vector3(target_state.p(0), target_state.p(1), target_state.p(2)));
-              setTargetVel(tf::Vector3(target_state.v(0), target_state.v(1), target_state.v(2)));
-              setTargetAcc(tf::Vector3(target_state.a(0), target_state.a(1), target_state.a(2)));
-
-              double target_yaw = target_state.getYaw();
-              double target_omega_z = target_state.w(2);
-              double target_ang_acc_z = target_state.tau(2);
-              setTargetYaw(target_yaw);
-              setTargetOmegaZ(target_omega_z);
-              setTargetAngAccZ(target_ang_acc_z);
-
-              tf::Vector3 curr_pos = estimator_->getPos(Frame::COG, estimate_mode_);
-              double yaw_angle = estimator_->getState(State::YAW_COG, estimate_mode_)[0];
-              ROS_INFO_THROTTLE(0.5, "[Nav] trajectory mode, target pos&yaw: [%f, %f, %f, %f], curr pos&yaw: [%f, %f, %f, %f]", target_state.p(0), target_state.p(1), target_state.p(2), target_yaw, curr_pos.x(), curr_pos.y(), curr_pos.z(), yaw_angle);
+              // trajectory following mode
+              double t = ros::Time::now().toSec();
+              double end_t = traj_generator_ptr_->getEndSetpoint().state.t;
+              if (t > end_t)
+                {
+                  ROS_INFO("[Nav] reach the end of trajectory");
+
+                  setTargetZeroVel();
+                  setTargetZeroAcc();
+
+                  setTargetZeroOmega();
+                  setTargetZeroAngAcc();
+
+                   trajectory_mode_ = false;
+
+                  traj_generator_ptr_.reset();
+                }
+              else
+                {
+                  agi::QuadState target_state = traj_generator_ptr_->getState(t);
+                  setTargetPos(tf::Vector3(target_state.p(0), target_state.p(1), target_state.p(2)));
+                  setTargetVel(tf::Vector3(target_state.v(0), target_state.v(1), target_state.v(2)));
+                  setTargetAcc(tf::Vector3(target_state.a(0), target_state.a(1), target_state.a(2)));
+
+                  double target_yaw = target_state.getYaw();
+                  double target_omega_z = target_state.w(2);
+                  double target_ang_acc_z = target_state.tau(2);
+                  setTargetYaw(target_yaw);
+                  setTargetOmegaZ(target_omega_z);
+                  setTargetAngAccZ(target_ang_acc_z);
+
+                  tf::Vector3 curr_pos = estimator_->getPos(Frame::COG, estimate_mode_);
+                  double yaw_angle = estimator_->getState(State::YAW_COG, estimate_mode_)[0];
+                  ROS_INFO_THROTTLE(0.5, "[Nav] trajectory mode, target pos&yaw: [%f, %f, %f, %f], curr pos&yaw: [%f, %f, %f, %f]", target_state.p(0), target_state.p(1), target_state.p(2), target_yaw, curr_pos.x(), curr_pos.y(), curr_pos.z(), yaw_angle);
+                }
             }
         }
     }