// 來源：roswrapper/ros/src/avoid_mpc/include/AvoidanceStateMachine.h
enum ProcessState {
INIT = 0u,
WAIT = 1u,
TAKEOFF = 2u,
TASK = 3u,
LAND = 4u
};

// 來源：roswrapper/ros/src/avoid_mpc/src/AvoidanceStateMachine.cpp（簡化版）
void AvoidanceStateMachine::Step(const ros::TimerEvent &event) {
double curTime = ros::Time::now().toSec(); // 獲取當前時間
switch (mStateProcess) { // 檢查當前狀態
case INIT: {
// INIT狀態代碼：檢查是否收到里程計數據
if (mbIsReceiveOdom) {
ROS_INFO("收到里程計數據，等待觸發。");
mStateProcess = WAIT; // 切換到WAIT狀態
}
break;
}
case WAIT: {
// WAIT狀態代碼：檢查起飛指令
if (mStatueQuad == quadrotor_msgs::BfctrlStatue::BFCTRL_STATUS_WAITINGCMD) {
ROS_INFO("觸發，起飛。");
mStateProcess = TAKEOFF; // 切換到TAKEOFF狀態
}
break;
}
case TAKEOFF: {
// TAKEOFF狀態代碼：發送起飛指令，檢查高度
if (mPos.z() < 0.6 * mHeight) {
// 仍在起飛，發佈起飛消息
quadrotor_msgs::TakeoffLand takeoffMsg;
takeoffMsg.takeoff_land_cmd = quadrotor_msgs::TakeoffLand::TAKEOFF;
mPubTakeoffLand.publish(takeoffMsg);
} else {
ROS_INFO("達到目標高度，開始任務。");
mStateProcess = TASK; // 切換到TASK狀態
}
break;
}
case TASK: {
// TASK狀態代碼：處理傳感器數據，規劃路徑，發送指令
// ...（MPC規劃和避障的複雜邏輯）...
PubCmd(u); // 發佈控制指令
break;
}
case LAND: {
// LAND狀態代碼：發送降落指令
quadrotor_msgs::TakeoffLand msg;
msg.takeoff_land_cmd = quadrotor_msgs::TakeoffLand::LAND;
mPubTakeoffLand.publish(msg);
break;
}
}
// 可視化代碼（如果啓用，在所有狀態下運行）
PtCloudVisualization();
ObstacleVisualization(mVecObstacles);
}

// 來源：roswrapper/ros/src/avoid_mpc/src/AvoidanceStateMachine.cpp（簡化版）
void AvoidanceStateMachine::OdomCallback(const nav_msgs::OdometryPtr &msg) {
mbIsReceiveOdom = true; // 收到里程計數據後設置標誌為true
// 存儲無人機的位置（mPos）、方向（mQuat）和速度（mVel）
mPos = Eigen::Vector3d(msg->pose.pose.position.x, /* ... */);
mQuat = Eigen::Quaterniond(msg->pose.pose.orientation.w, /* ... */);
mVel = Eigen::Vector3d(msg->twist.twist.linear.x, /* ... */);
}
void AvoidanceStateMachine::IMUCallback(const sensor_msgs::ImuConstPtr &msg) {
// 存儲無人機的加速度（mAcc）並可能優化方向
// ... 濾波和計算 ...
mAcc = mQuat.toRotationMatrix() * accbFiltered - Eigen::Vector3d(0, 0, 9.81);
}
void AvoidanceStateMachine::DepthCallback(const sensor_msgs::ImageConstPtr &msg) {
// 處理深度圖像以檢測障礙物並將其添加到地圖中
// ... 轉換並使用mKeyFrameMap更新地圖 ...
mKeyFrameMap.AddVertex(Twb, msg); // 將深度數據添加到障礙物地圖
}

// 來源：roswrapper/ros/src/avoid_mpc/src/AvoidanceStateMachine.cpp（簡化版）
void AvoidanceStateMachine::PubCmd(std::vector<double> &u) {
  quadrotor_msgs::Command cmdMsg;
  cmdMsg.header.stamp = ros::Time::now();
  cmdMsg.mode = quadrotor_msgs::Command::ACCELERATION_MODE;
  cmdMsg.acceleration.x = u[0]; // 發送X方向的期望加速度
  cmdMsg.acceleration.y = u[1]; // 發送Y方向的期望加速度
  cmdMsg.acceleration.z = u[2]; // 發送Z方向的期望加速度
  cmdMsg.yaw = 0; // 暫時保持偏航（繞垂直軸旋轉）固定
  mPubCmd.publish(cmdMsg); // 發佈指令
  }
  void AvoidanceStateMachine::PubSlowDownCmd() {
  // 根據當前速度和加速度計算減速加速度
  Eigen::Vector3d accSlow = -mVel * mParamSlowDownKp - mAcc * mParamSlowDownKd;
  // ... 對加速度施加限制 ...
  quadrotor_msgs::Command cmdMsg;
  // ... 填充減速加速度的cmdMsg ...
  mPubCmd.publish(cmdMsg); // 發佈減速指令
  }