-
Notifications
You must be signed in to change notification settings - Fork 12
/
planner.py
128 lines (97 loc) · 4.19 KB
/
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
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
import os
from collections import deque
import numpy as np
import math
EARTH_RADIUS_EQUA = 6378137.0
DEBUG = int(os.environ.get('HAS_DISPLAY', 0))
class Plotter(object):
def __init__(self, size):
self.size = size
self.clear()
self.title = str(self.size)
def clear(self):
from PIL import Image, ImageDraw
self.img = Image.fromarray(np.zeros((self.size, self.size, 3), dtype=np.uint8))
self.draw = ImageDraw.Draw(self.img)
def dot(self, pos, node, color=(255, 255, 255), r=2):
x, y = 5.5 * (pos - node)
x += self.size / 2
y += self.size / 2
self.draw.ellipse((x-r, y-r, x+r, y+r), color)
def show(self):
if not DEBUG:
return
import cv2
cv2.imshow(self.title, cv2.cvtColor(np.array(self.img), cv2.COLOR_BGR2RGB))
cv2.waitKey(1)
class RoutePlanner(object):
def __init__(self, min_distance, max_distance, debug_size=256, lat_ref=42.0, lon_ref=2.0):
self.route = deque()
self.min_distance = min_distance
self.max_distance = max_distance
# self.mean = np.array([49.0, 8.0]) # for carla 9.9
# self.scale = np.array([111324.60662786, 73032.1570362]) # for carla 9.9
self.mean = np.array([0.0, 0.0]) # for carla 9.10
self.scale = np.array([111324.60662786, 111319.490945]) # for carla 9.10
self.debug = Plotter(debug_size)
# self.lat_ref, self.lon_ref = self._get_latlon_ref()
self.lat_ref = lat_ref
self.lon_ref = lon_ref
def set_route(self, global_plan, gps=False, global_plan_world = None):
self.route.clear()
if global_plan_world:
for (pos, cmd), (pos_word, _ )in zip(global_plan, global_plan_world):
if gps:
pos = self.gps_to_location(np.array([pos['lat'], pos['lon']]))
# pos -= self.mean
# pos *= self.scale
else:
pos = np.array([pos.location.x, pos.location.y])
# pos -= self.mean
self.route.append((pos, cmd, pos_word))
else:
for pos, cmd in global_plan:
if gps:
pos = self.gps_to_location(np.array([pos['lat'], pos['lon']]))
# pos -= self.mean
# pos *= self.scale
else:
pos = np.array([pos.location.x, pos.location.y])
# pos -= self.mean
self.route.append((pos, cmd))
def run_step(self, gps):
self.debug.clear()
if len(self.route) == 1:
return self.route[0]
to_pop = 0
farthest_in_range = -np.inf
cumulative_distance = 0.0
for i in range(1, len(self.route)):
if cumulative_distance > self.max_distance:
break
cumulative_distance += np.linalg.norm(self.route[i][0] - self.route[i-1][0])
distance = np.linalg.norm(self.route[i][0] - gps)
if distance <= self.min_distance and distance > farthest_in_range:
farthest_in_range = distance
to_pop = i
r = 255 * int(distance > self.min_distance)
g = 255 * int(self.route[i][1].value == 4)
b = 255
self.debug.dot(gps, self.route[i][0], (r, g, b))
for _ in range(to_pop):
if len(self.route) > 2:
self.route.popleft()
self.debug.dot(gps, self.route[0][0], (0, 255, 0))
self.debug.dot(gps, self.route[1][0], (255, 0, 0))
self.debug.dot(gps, gps, (0, 0, 255))
self.debug.show()
return self.route[1]
def gps_to_location(self, gps):
# gps content: numpy array: [lat, lon, alt]
lat, lon = gps
scale = math.cos(self.lat_ref * math.pi / 180.0)
my = math.log(math.tan((lat+90) * math.pi / 360.0)) * (EARTH_RADIUS_EQUA * scale)
mx = (lon * (math.pi * EARTH_RADIUS_EQUA * scale)) / 180.0
y = scale * EARTH_RADIUS_EQUA * math.log(math.tan((90.0 + self.lat_ref) * math.pi / 360.0)) - my
x = mx - scale * self.lon_ref * math.pi * EARTH_RADIUS_EQUA / 180.0
return np.array([x, y])