| 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() |