我們還是先來瀏覽一下比賽規則：
1、規劃問題初步方案
1.1、數量配置
初步確定數量配置為：旋翼飛機數量為1架，固定翼飛機為9架。旋翼飛機的作用為搜尋目標、毀傷評估、引導固定翼飛機及補充打擊，固定翼飛機的作用為搜尋靶標位置及打擊目標。
1.2、邏輯流程
旋翼飛機在引導固定翼飛機實行打擊時，首先需要解算出目標相對於世界座標系的位置，然後將位置資訊和起飛訊號傳遞給固定翼無人機，固定翼無人機按照給定航跡飛行並擊打目標，隨後返回起飛區。
2、系統框架
（PS：非常感謝師弟堅持要摒棄if-else的低階狀態機，所以我們才能開發出這套邏輯更加嚴密，使用更加方便的高階狀態機）
參考文獻：
【1】狀態機程式設計
【2】將ros變數寫入類中
簡單介紹一下程式碼中類名及主要成員作用：
MainFlight--飛機類，即被控制的物件
資料成員：
全部ros 變數（包括DJIDrone*、NodeHandle、Publisher及Subscriber等），定義在類內，但在建構函式中進行初始化。
六個抽象基類指標，分別對應六個狀態派生類（重點：定義資料成員時要定義為基類指標，但在建構函式中初始化為派生類指標，以此與六個狀態派生類繫結），目的是使一個飛機類的物件同時擁有六種狀態，並從中切換。
recent_state——飛機當前狀態，recent_state指標所對應的狀態派生類即為飛機當前指標，之後透過給recent_state賦值實現狀態切換。
成員函式（方法）：
最重要的是setState函式，接受一個新狀態的指標，並賦值給recent_state，然後執行一次派生類狀態對應的excute 函式。除此之外，還有一些介面函式，用於能使外界訪問飛機類中的資料成員（主要是ros變數和大疆DJIDrone）
此外如果以後還用這種框架的話，MainFlight 的建構函式和解構函式很重要，建構函式中要將六個基類指標初始化賦值為六個派生狀態類指標，以及將六個指標都和飛機類的物件繫結，因為切換狀態是在excute函式中進行，此函式是狀態類的成員函式，因此我們在此函式中無法直接訪問到飛機類物件的recent_state成員，因此需要在狀態類中儲存一個指向飛機類的指標my_mainflight，此時就可以在狀態類的函式中透過my_mainflight呼叫飛機類的setState方法來修改飛機類物件recent_state的值達到切換狀態的目的，這也是這個狀態機框架最重要的一點。
StateBase--狀態基類（抽象基類，不可例項化為物件）
資料成員：
只有一個數據成員：my_mainflight，是一個指向飛機類物件的指標，用法上文中已經詳細說明。
成員函式：
set_MainFlight，用於在飛機類的建構函式中將每個狀態類指標與飛機物件繫結。
純虛擬函式：excute，執行函式，將在每個派生類中進行覆蓋以實現不同狀態的執行不同的動作。
StateBase 的派生類，共6個，分別為TakeoffState（起飛）、SearchState（搜尋）、watchTFState（識別TF）、sendState（發指令給僚機）、GoHomeState（回家）、LandingState（降落）
資料成員直接從基類中繼承，沒有多加其他資料成員。
成員函式：excute，各個狀態不同的執行函式，也就是原來狀態機中每個case 中的內容。
如需修改狀態：修改每個狀態類中的excute 函式，狀態切換邏輯不受影響；同樣，修改狀態切換邏輯，執行動作不受影響。
如需新增或刪除狀態直接照著已有派生類的樣式寫類的定義即可，不需做什麼修改，只需重寫此類的excute 函式即可。
3、程式碼
(PS：為了簡單說明，我將程式碼中很多變數的定義都刪除了，大家只要能看懂整個框架就好～)
3.1、main.cpp
#include
#include
“。。/include/Drone。h”
#include
using
namespace
std
；
int
main
（
int
argc
，
char
**
argv
）
{
ros
：：
init
（
argc
，
argv
，
“StateMachineNode”
）；
double
KP
=
atof
（
argv
［
1
］）；
double
height
=
atof
（
argv
［
2
］）；
double
leader_x_avoid
=
atof
（
argv
［
3
］）；
double
leader_y_avoid
=
atof
（
argv
［
4
］）；
double
leader_x_avoid2
=
atof
（
argv
［
5
］）；
double
leader_y_avoid2
=
atof
（
argv
［
6
］）；
auto
M100
=
new
Drone
（
KP
，
height
，
leader_x_avoid
，
leader_y_avoid
，
leader_x_avoid2
，
leader_y_avoid2
）；
（
M100
->
getDrone
（））
->
request_sdk_permission_control
（）；
cout
<<
“Ctrl State = ”
<<
M100
->
getDrone
（）
->
request_sdk_permission_control
（）
<<
endl
；
cout
<<
“Mission Start”
<<
endl
；
M100
->
mission
（）；
cout
<<
“Mission Success！，release control。。。”
<<
endl
；
（
M100
->
getDrone
（））
->
release_sdk_permission_control
（）；
return
0
；
}
3.2、Drone.h
#ifndef DRONE_H
#define DRONE_H
#include
“。。/include/State。h”
#include
“。。/include/Function。h”
#include
#include
using
namespace
std
；
class
StateBase
；
class
Drone
{
public
：
Drone
（
double
K
，
double
H
，
double
Leader_x_Avoid
，
double
Leader_y_Avoid
，
double
Leader_x_Avoid2
，
double
Leader_y_Avoid2
）；
~
Drone
（）；
//switch state and do acions
void
setState
（
StateBase
*
new_state
）；
//Flight logic for mission
void
mission
（）；
//all states that a drone has
StateBase
*
takeoff_state
；
StateBase
*
search_state
；
StateBase
*
watchTF_state
；
StateBase
*
send_state
；
StateBase
*
gohome_state
；
StateBase
*
landing_state
；
//access to private member variable
DJIDrone
*
getDrone
（）；
//flight parameters
double
K_track
；
double
height
；
double
leader_x_Avoid
；
double
leader_y_Avoid
；
double
leader_x_Avoid2
；
double
leader_y_Avoid2
；
//ROS variable
ros
：：
NodeHandle
n
；
ros
：：
Publisher
tkpub
；
ros
：：
Publisher
goonpub
；
ros
：：
Publisher
UAV1_position_pub
；
private
：
//save recent state
StateBase
*
recent_state
=
nullptr
；
//DJIDrone variable
DJIDrone
*
drone
；
ros
：：
Subscriber
message_subscriber
；
}；
#endif
// DRONE_H
3.3、State.h
#ifndef STATES_H
#define STATES_H
#include
“。。/include/Drone。h”
#include
“。。/include/Function。h”
#include
#include
“。。/。。/Guidance-SDK-ROS/include/DJI_guidance。h”
class
Drone
；
class
StateBase
{
public
：
virtual
~
StateBase
（）
{}
virtual
void
excute
（
Drone
*
）
=
0
；
void
setDrone
（
Drone
*
new_drone
）；
protected
：
Drone
*
recent_drone
；
}；
class
TakeoffState
：
public
StateBase
{
public
：
TakeoffState
（）；
~
TakeoffState
（）
=
default
；
void
excute
（
Drone
*
）
override
；
private
：
time_t
start_time
；
}；
class
SearchState
：
public
StateBase
{
public
：
SearchState
（）；
~
SearchState
（）
=
default
；
void
excute
（
Drone
*
）
override
；
}；
class
watchTFState
：
public
StateBase
{
public
：
watchTFState
（）；
~
watchTFState
（）
=
default
；
void
excute
（
Drone
*
）
override
；
private
：
time_t
start_time_watchTF
；
time_t
end_time_watchTF
；
time_t
time_watchTF
；
}；
class
sendState
：
public
StateBase
{
public
：
sendState
（）；
~
sendState
（）
=
default
；
void
excute
（
Drone
*
）
override
；
}；
class
GoHomeState
：
public
StateBase
{
public
：
GoHomeState
（）；
~
GoHomeState
（）
=
default
；
void
excute
（
Drone
*
）
override
；
}；
class
LandingState
：
public
StateBase
{
public
：
LandingState
（）；
~
LandingState
（）
=
default
；
void
excute
（
Drone
*
）
override
；
}；
#endif
// STATES_H
3.4、functions.cpp
#include
#include
#include
#include
“math。h”
#include
#include
#include
“。。/include/Drone。h”
#include
“。。/include/Function。h”
using
namespace
std
；
using
namespace
DJI
：：
onboardSDK
；
using
namespace
Eigen
；
double
x_guidance
；
double
y_guidance
；
double
z_guidance
；
void
ZigZag_path_planning
（
double
a
，
double
b
，
double
distance
）
{
//more。。。
}
//Defination of member functions in class “StateBase”
void
StateBase
：：
setDrone
（
Drone
*
new_drone
）
{
recent_drone
=
new_drone
；
}
TakeoffState
：：
TakeoffState
（）
{
TakeoffState
：：
recent_drone
=
nullptr
；
}
void
TakeoffState
：：
excute
（
Drone
*
mydrone
）
{
//more。。。
}
SearchState
：：
SearchState
（）
{
SearchState
：：
recent_drone
=
nullptr
；
}
void
SearchState
：：
excute
（
Drone
*
mydrone
）
{
//more。。。
}
watchTFState
：：
watchTFState
（）
{
watchTFState
：：
recent_drone
=
nullptr
；
}
void
watchTFState
：：
excute
（
Drone
*
mydrone
）
{
//more。。。
}
sendState
：：
sendState
（）
{
sendState
：：
recent_drone
=
nullptr
；
}
void
sendState
：：
excute
（
Drone
*
mydrone
）
{
//more。。。
}
GoHomeState
：：
GoHomeState
（）
{
GoHomeState
：：
recent_drone
=
nullptr
；
}
void
GoHomeState
：：
excute
（
Drone
*
mydrone
）
{
//more。。。
}
LandingState
：：
LandingState
（）
{
LandingState
：：
recent_drone
=
nullptr
；
}
void
LandingState
：：
excute
（
Drone
*
mydrone
）
{
//more。。。
}
Drone
：：
Drone
（
double
K
，
double
H
，
double
Leader_x_Avoid
，
double
Leader_y_Avoid
，
double
Leader_x_Avoid2
，
double
Leader_y_Avoid2
）
{
//initialize parameters
K_track
=
K
；
height
=
H
；
leader_x_Avoid
=
Leader_x_Avoid
；
leader_y_Avoid
=
Leader_y_Avoid
；
leader_x_Avoid2
=
Leader_x_Avoid2
；
leader_y_Avoid2
=
Leader_y_Avoid2
；
//create ros node and a new DJIDrone
ros
：：
NodeHandle
n
；
drone
=
new
DJIDrone
（
n
）；
//initialize ROS：：subscriber
message_subscriber
=
n
。
subscribe
（
“monodata”
，
1000
，
&
messageCallback
）；
global_position_sub
=
n
。
subscribe
（
“guidance/followN1_position”
，
1000
，
&
position_callback
）；
ultrasonic_sub
=
n
。
subscribe
（
“guidance/followultrasonic”
，
1000
，
&
ultrasonic_callback
）；
uav2takeback
=
n
。
subscribe
（
“/uavtakeoffback”
，
1
，
&
uav2back_callback
）；
uav2landback
=
n
。
subscribe
（
“/uavlandback”
，
1
，
&
uav2landback_callback
）；
//initialize ROS：：Publisher
tkpub
=
n
。
advertise
<
statemachine
：：
Takeoff
>
（
“/uavtakeoff”
，
1
）；
//initialize states of drone
takeoff_state
=
new
TakeoffState
（）；
search_state
=
new
SearchState
（）；
watchTF_state
=
new
watchTFState
（）；
send_state
=
new
sendState
（）；
gohome_state
=
new
GoHomeState
（）；
landing_state
=
new
LandingState
（）；
//Bind state to drone
takeoff_state
->
setDrone
（
this
）；
search_state
->
setDrone
（
this
）；
watchTF_state
->
setDrone
（
this
）；
send_state
->
setDrone
（
this
）；
gohome_state
->
setDrone
（
this
）；
landing_state
->
setDrone
（
this
）；
}
Drone
：：~
Drone
（）
{
//delete dynamic pointer
delete
drone
；
delete
takeoff_state
；
delete
search_state
；
delete
watchTF_state
；
delete
gohome_state
；
e
；
delete
gohome_state
；
delete
landing_state
；
}
void
Drone
：：
setState
（
StateBase
*
new_state
）
{
recent_state
=
new_state
；
new_state
->
excute
（
this
）；
}
//Flight logic for mission
void
Drone
：：
mission
（）
{
ZigZag_path_planning
（
a_plane
，
b_plane
，
distance_raw
）；
this
->
setState
（
takeoff_state
）；
this
->
setState
（
search_state
）；
this
->
setState
（
watchTF_state
）；
if
（
TARGET
==
true
）
{
this
->
setState
（
send_state
）；
}
this
->
setState
（
search_state
）；
this
->
setState
（
gohome_state
）；
this
->
setState
（
landing_state
）；
}
//access to member variable in Drone
DJIDrone
*
Drone
：：
getDrone
（）
{
return
this
->
drone
；
}
請大家批評指正～
