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
|
#!/usr/bin/python
import sys
import math
import urllib
import urllib2
import time
id = '123456789012345'
server = 'http://localhost:5055'
period = 1
step = 0.001
waypoints = [
(40.722412, -74.006288),
(40.728592, -74.005258),
(40.728348, -74.002822),
(40.725437, -73.996750),
(40.721778, -73.999818),
(40.723323, -74.002994)
]
points = []
for i in range(0, len(waypoints)):
(lat1, lon1) = waypoints[i]
(lat2, lon2) = waypoints[(i + 1) % len(waypoints)]
length = math.sqrt((lat2 - lat1) ** 2 + (lon2 - lon1) ** 2)
count = int(math.ceil(length / step))
for j in range(0, count):
lat = lat1 + (lat2 - lat1) * j / count
lon = lon1 + (lon2 - lon1) * j / count
points.append((lat, lon))
#changed by Erez
def send(lat, lon, course, alarm):
params = (('id', id), ('timestamp', int(time.time())), ('lat', lat), ('lon', lon), ('bearing', course))
if alarm:
params = params + (('alarm', 'true'),)
urllib2.urlopen(server + '?' + urllib.urlencode(params)).read()
def course(lat1, lon1, lat2, lon2):
lat1 = lat1 * math.pi / 180
lon1 = lon1 * math.pi / 180
lat2 = lat2 * math.pi / 180
lon2 = lon2 * math.pi / 180
y = math.sin(lon2 - lon1) * math.cos(lat2)
x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(lon2 - lon1)
return (math.atan2(y, x) % (2 * math.pi)) * 180 / math.pi
index = 0
while True:
(lat1, lon1) = points[index % len(points)]
(lat2, lon2) = points[(index + 1) % len(points)]
alarm = ((index % 10) == 0)#added by Erez
send(lat1, lon1, course(lat1, lon1, lat2, lon2), alarm)
time.sleep(period)
index += 1
|
