-
Notifications
You must be signed in to change notification settings - Fork 0
/
planner.cxx
113 lines (84 loc) · 3.92 KB
/
planner.cxx
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
#include <imav_planner/planner.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "planner");
ros::NodeHandle ph("~");
ph.getParam("verbose", verbose);
ph.getParam("error/local", loc_error);
ph.getParam("height/hover", hover_height);
ph.getParam("height/drop", drop_height);
ph.getParam("height/land", land_height);
ph.getParam("height/step", descent_step);
ph.getParam("servo/open", open_angle);
ph.getParam("servo/close", close_angle);
ph.getParam("servo/static", eq_angle);
ph.getParam("delay/hover", hover_time);
ph.getParam("delay/transition", transition_time);
ph.getParam("delay/pitStop", wait_time);
ph.getParam("delay/exit", exit_time);
ph.getParam("delay/landHover", land_wait);
ros::Rate transitRate(1.0/transition_time);
state_machine::fsm_ machine;
machine.start();
auto state = std::async(std::launch::async, state_machine::statePublish, ph, &machine);
machine.process_event(state_machine::CmdTakeOff());
if(verbose) state_machine::echo_state(machine);
// /*
// Execution loop
while(state_machine::ContMission)
{
transitRate.sleep();
machine.process_event(state_machine::CmdExploring());
if(verbose) state_machine::echo_state(machine);
transitRate.sleep();
machine.process_event(state_machine::CmdHover());
if(verbose) state_machine::echo_state(machine);
if(state_machine::PkgAttached || state_machine::HoverMode)
{
transitRate.sleep();
machine.process_event(state_machine::CmdGotoDrop());
if(verbose) state_machine::echo_state(machine);
transitRate.sleep();
machine.process_event(state_machine::CmdHover());
if(verbose) state_machine::echo_state(machine);
transitRate.sleep();
machine.process_event(state_machine::CmdDescent());
if(verbose) state_machine::echo_state(machine);
transitRate.sleep();
machine.process_event(state_machine::CmdDrop());
if(verbose) state_machine::echo_state(machine);
transitRate.sleep();
machine.process_event(state_machine::CmdDropOver());
if(verbose) state_machine::echo_state(machine);
transitRate.sleep();
machine.process_event(state_machine::CmdAscent());
if(verbose) state_machine::echo_state(machine);
}
}
transitRate.sleep();
machine.process_event(state_machine::CmdGotoLZ());
if(verbose) state_machine::echo_state(machine);
transitRate.sleep();
machine.process_event(state_machine::CmdHover());
if(verbose) state_machine::echo_state(machine);
/* transitRate.sleep();
machine.process_event(state_machine::CmdDescent());
if(verbose) state_machine::echo_state(machine);
*/
transitRate.sleep();
machine.process_event(state_machine::CmdLand());
if(verbose) state_machine::echo_state(machine);
// */
/*
// Landing test
transitRate.sleep();
machine.process_event(state_machine::CmdDescent());
if(verbose) state_machine::echo_state(machine);
state_machine::ContMission = false;
transitRate.sleep();
machine.process_event(state_machine::CmdLand());
if(verbose) state_machine::echo_state(machine);
*/
machine.stop();
return 0;
}