-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathd-flight.py
107 lines (86 loc) · 4.2 KB
/
d-flight.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
106
107
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import NavSatFix
# Define the area boundaries and altitude restriction
AREA_1_LATITUDE_MIN = 44.44132
AREA_1_LATITUDE_MAX = 44.44634
AREA_1_LONGITUDE_MIN = 8.83075
AREA_1_LONGITUDE_MAX = 8.84036
AREA_1_MIN_ALTITUDE = 100
AREA_2_LATITUDE_MIN = 44.40892
AREA_2_LATITUDE_MAX = 44.44132
AREA_2_LONGITUDE_MIN = 8.2319
AREA_2_LONGITUDE_MAX = 8.83075
AREA_2_MIN_ALTITUDE = 1000
AREA_3_LATITUDE_MIN = 44.45642
AREA_3_LATITUDE_MAX = 44.45621
AREA_3_LONGITUDE_MIN = 8.23456
AREA_3_LONGITUDE_MAX = 8.5423
AREA_3_MIN_ALTITUDE = 50
AREA_4_LATITUDE_MIN = 44.45623
AREA_4_LATITUDE_MAX = 44.21354
AREA_4_LONGITUDE_MIN = 8.26865
AREA_4_LONGITUDE_MAX = 8.3695
AREA_4_MIN_ALTITUDE = 150
AREA_5_LATITUDE_MIN = 44.6562
AREA_5_LATITUDE_MAX = 44.6821
AREA_5_LONGITUDE_MIN = 8.2356
AREA_5_LONGITUDE_MAX = 8.3265
AREA_5_MIN_ALTITUDE = 120
AREA_6_LATITUDE_MIN = 44.68953
AREA_6_LATITUDE_MAX = 44.69532
AREA_6_LONGITUDE_MIN = 8.3659
AREA_6_LONGITUDE_MAX = 8.3895
AREA_6_MIN_ALTITUDE = 140
def gps_callback(msg):
latitude = msg.latitude
longitude = msg.longitude
altitude = msg.altitude
# Check if the drone is within the specified area
if (AREA_1_LATITUDE_MIN <= latitude <= AREA_1_LATITUDE_MAX and
AREA_1_LONGITUDE_MIN <= longitude <= AREA_1_LONGITUDE_MAX):
# Check if the altitude is below the restricted minimum
if altitude < AREA_1_MIN_ALTITUDE:
rospy.logwarn("Please increase the altitude of the vehicle, restricted minimum altitude in this area is 100 meters")
else : rospy.logwarn(" restricted minimum altitude in this area is 100 meters")
# Check if the drone is within the specified area
if (AREA_2_LATITUDE_MIN <= latitude <= AREA_2_LATITUDE_MAX and
AREA_2_LONGITUDE_MIN <= longitude <= AREA_2_LONGITUDE_MAX):
rospy.logwarn("Please leave the area immediately")
# Check if the drone is within the specified area
if (AREA_3_LATITUDE_MIN <= latitude <= AREA_3_LATITUDE_MAX and
AREA_3_LONGITUDE_MIN <= longitude <= AREA_3_LONGITUDE_MAX):
# Check if the altitude is below the restricted minimum
if altitude < AREA_3_MIN_ALTITUDE:
rospy.logwarn("Please increase the altitude of the vehicle, restricted minimum altitude in this area is 50 meters")
else : rospy.logwarn(" Safe to fly, restricted minimum altitude in this area is 50 meters")
# Check if the drone is within the specified area
if (AREA_4_LATITUDE_MIN <= latitude <= AREA_4_LATITUDE_MAX and
AREA_4_LONGITUDE_MIN <= longitude <= AREA_4_LONGITUDE_MAX):
# Check if the altitude is below the restricted minimum
if altitude < AREA_4_MIN_ALTITUDE:
rospy.logwarn("Please increase the altitude of the vehicle, restricted minimum altitude in this area is 150 meters")
else : rospy.logwarn("Safe to fly, restricted minimum altitude in this area is 150 meters")
# Check if the drone is within the specified area
if (AREA_5_LATITUDE_MIN <= latitude <= AREA_5_LATITUDE_MAX and
AREA_5_LONGITUDE_MIN <= longitude <= AREA_5_LONGITUDE_MAX):
# Check if the altitude is below the restricted minimum
if altitude < AREA_5_MIN_ALTITUDE:
rospy.logwarn("Please increase the altitude of the vehicle, restricted minimum altitude in this area is 120 meters")
else : rospy.logwarn("Safe to fly, restricted minimum altitude in this area is 120 meters")
# Check if the drone is within the specified area
if (AREA_6_LATITUDE_MIN <= latitude <= AREA_6_LATITUDE_MAX and
AREA_6_LONGITUDE_MIN <= longitude <= AREA_6_LONGITUDE_MAX):
# Check if the altitude is below the restricted minimum
if altitude < AREA_6_MIN_ALTITUDE:
rospy.logwarn("Please increase the altitude of the vehicle, restricted minimum altitude in this area is 140 meters")
else : rospy.logwarn("Safe to fly, restricted minimum altitude in this area is 140 meters")
def airsim_drone_monitor():
rospy.init_node('airsim_drone_monitor', anonymous=True)
rospy.Subscriber('/airsim_node/drone/gps', NavSatFix, gps_callback)
rospy.spin()
if __name__ == '__main__':
try:
airsim_drone_monitor()
except rospy.ROSInterruptException:
pass