Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. :param debug: boolean flag to activate waypoints debugging :return: control to be applied
(self, debug=False)
| 217 | self._global_plan = True |
| 218 | |
| 219 | def run_step(self, debug=False): |
| 220 | """ |
| 221 | Execute one step of local planning which involves running the longitudinal and lateral PID controllers to |
| 222 | follow the waypoints trajectory. |
| 223 | |
| 224 | :param debug: boolean flag to activate waypoints debugging |
| 225 | :return: control to be applied |
| 226 | """ |
| 227 | |
| 228 | # not enough waypoints in the horizon? => add more! |
| 229 | if not self._global_plan and len(self._waypoints_queue) < int(self._waypoints_queue.maxlen * 0.5): |
| 230 | self._compute_next_waypoints(k=100) |
| 231 | |
| 232 | if len(self._waypoints_queue) == 0 and len(self._waypoint_buffer) == 0: |
| 233 | control = carla.VehicleControl() |
| 234 | control.steer = 0.0 |
| 235 | control.throttle = 0.0 |
| 236 | control.brake = 1.0 |
| 237 | control.hand_brake = False |
| 238 | control.manual_gear_shift = False |
| 239 | |
| 240 | return control |
| 241 | |
| 242 | # Buffering the waypoints |
| 243 | if not self._waypoint_buffer: |
| 244 | for _ in range(self._buffer_size): |
| 245 | if self._waypoints_queue: |
| 246 | self._waypoint_buffer.append( |
| 247 | self._waypoints_queue.popleft()) |
| 248 | else: |
| 249 | break |
| 250 | |
| 251 | # current vehicle waypoint |
| 252 | vehicle_transform = self._vehicle.get_transform() |
| 253 | self._current_waypoint = self._map.get_waypoint(vehicle_transform.location) |
| 254 | # target waypoint |
| 255 | self.target_waypoint, self._target_road_option = self._waypoint_buffer[0] |
| 256 | # move using PID controllers |
| 257 | control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint) |
| 258 | |
| 259 | # purge the queue of obsolete waypoints |
| 260 | max_index = -1 |
| 261 | |
| 262 | for i, (waypoint, _) in enumerate(self._waypoint_buffer): |
| 263 | if waypoint.transform.location.distance(vehicle_transform.location) < self._min_distance: |
| 264 | max_index = i |
| 265 | if max_index >= 0: |
| 266 | for i in range(max_index + 1): |
| 267 | self._waypoint_buffer.popleft() |
| 268 | |
| 269 | if debug: |
| 270 | draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], self._vehicle.get_location().z + 1.0) |
| 271 | |
| 272 | return control |
| 273 | |
| 274 | def done(self): |
| 275 | """ |
nothing calls this directly
no test coverage detected