| 37 | |
| 38 | |
| 39 | class RoutePlanner(object): |
| 40 | def __init__(self, min_distance, max_distance, debug_size=256): |
| 41 | self.route = deque() |
| 42 | self.min_distance = min_distance |
| 43 | self.max_distance = max_distance |
| 44 | |
| 45 | # self.mean = np.array([49.0, 8.0]) # for carla 9.9 |
| 46 | # self.scale = np.array([111324.60662786, 73032.1570362]) # for carla 9.9 |
| 47 | self.mean = np.array([0.0, 0.0]) # for carla 9.10 |
| 48 | self.scale = np.array([111324.60662786, 111319.490945]) # for carla 9.10 |
| 49 | |
| 50 | self.debug = Plotter(debug_size) |
| 51 | |
| 52 | def set_route(self, global_plan, gps=False, global_plan_world = None): |
| 53 | self.route.clear() |
| 54 | |
| 55 | if global_plan_world: |
| 56 | for (pos, cmd), (pos_word, _ )in zip(global_plan, global_plan_world): |
| 57 | if gps: |
| 58 | pos = np.array([pos['lat'], pos['lon']]) |
| 59 | pos -= self.mean |
| 60 | pos *= self.scale |
| 61 | else: |
| 62 | pos = np.array([pos.location.x, pos.location.y]) |
| 63 | pos -= self.mean |
| 64 | |
| 65 | self.route.append((pos, cmd, pos_word)) |
| 66 | else: |
| 67 | for pos, cmd in global_plan: |
| 68 | if gps: |
| 69 | pos = np.array([pos['lat'], pos['lon']]) |
| 70 | pos -= self.mean |
| 71 | pos *= self.scale |
| 72 | else: |
| 73 | pos = np.array([pos.location.x, pos.location.y]) |
| 74 | pos -= self.mean |
| 75 | |
| 76 | self.route.append((pos, cmd)) |
| 77 | |
| 78 | def run_step(self, gps): |
| 79 | self.debug.clear() |
| 80 | |
| 81 | if len(self.route) == 1: |
| 82 | return self.route[0] |
| 83 | |
| 84 | to_pop = 0 |
| 85 | farthest_in_range = -np.inf |
| 86 | cumulative_distance = 0.0 |
| 87 | |
| 88 | for i in range(1, len(self.route)): |
| 89 | if cumulative_distance > self.max_distance: |
| 90 | break |
| 91 | |
| 92 | cumulative_distance += np.linalg.norm(self.route[i][0] - self.route[i-1][0]) |
| 93 | distance = np.linalg.norm(self.route[i][0] - gps) |
| 94 | |
| 95 | if distance <= self.min_distance and distance > farthest_in_range: |
| 96 | farthest_in_range = distance |