-
Notifications
You must be signed in to change notification settings - Fork 0
/
mission_planner.py
105 lines (94 loc) · 2.45 KB
/
mission_planner.py
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
#!/bin/python3
"""
mission_planner.py
"""
import rclpy
from as2_python_api.mission_interpreter.mission import Mission
from as2_python_api.mission_interpreter.mission_interpreter import MissionInterpreter
def main():
"""Set a mission plan and execute it."""
mission_json = """
{
"target": "drone0",
"verbose": "False",
"use_sim_time": "True",
"plan": [
{
"behavior": "takeoff",
"args": {
"height": 1.0,
"speed": 1.0
}
},
{
"behavior": "go_to",
"args": {
"_x": 5.0,
"_y": 5.0,
"_z": 1.0,
"speed": 0.5,
"yaw_mode": 1
}
},
{
"behavior": "go_to",
"args": {
"_x": 5.0,
"_y": -5.0,
"_z": 1.0,
"speed": 0.5,
"yaw_mode": 1
}
},
{
"behavior": "go_to",
"args": {
"_x": -5.0,
"_y": 5.0,
"_z": 1.0,
"speed": 0.5,
"yaw_mode": 1
}
},
{
"behavior": "go_to",
"args": {
"_x": -5.0,
"_y": -5.0,
"_z": 1.0,
"speed": 0.5,
"yaw_mode": 1
}
},
{
"behavior": "go_to",
"args": {
"_x": 0.0,
"_y": 0.0,
"_z": 1.0,
"speed": 0.5,
"yaw_mode": 1
}
},
{
"behavior": "land",
"args": {
"speed": 0.5
}
}
]
}
"""
mission = Mission.parse_raw(mission_json)
print(mission)
rclpy.init()
interpreter = MissionInterpreter(mission=mission)
print("Start mission!")
interpreter.drone.arm()
interpreter.drone.offboard()
interpreter.perform_mission(debug=True)
print("Mission completed!")
interpreter.shutdown()
rclpy.shutdown()
if __name__ == "__main__":
main()