| 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 |
| 97 | to_pop = i |
| 98 | |
| 99 | r = 255 * int(distance > self.min_distance) |
| 100 | g = 255 * int(self.route[i][1].value == 4) |
| 101 | b = 255 |
| 102 | self.debug.dot(gps, self.route[i][0], (r, g, b)) |
| 103 | |
| 104 | for _ in range(to_pop): |
| 105 | if len(self.route) > 2: |
| 106 | self.route.popleft() |
| 107 | |
| 108 | self.debug.dot(gps, self.route[0][0], (0, 255, 0)) |
| 109 | self.debug.dot(gps, self.route[1][0], (255, 0, 0)) |
| 110 | self.debug.dot(gps, gps, (0, 0, 255)) |
| 111 | self.debug.show() |
| 112 | |
| 113 | return self.route[1] |