LocalPlanner implements the basic behavior of following a trajectory of waypoints that is generated on-the-fly. The low-level motion of the vehicle is computed by using two PID controllers, one is used for the lateral control and the other for the longitudinal control (cruise sp
| 32 | |
| 33 | |
| 34 | class LocalPlanner(object): |
| 35 | """ |
| 36 | LocalPlanner implements the basic behavior of following a trajectory |
| 37 | of waypoints that is generated on-the-fly. |
| 38 | The low-level motion of the vehicle is computed by using two PID controllers, |
| 39 | one is used for the lateral control |
| 40 | and the other for the longitudinal control (cruise speed). |
| 41 | |
| 42 | When multiple paths are available (intersections) |
| 43 | this local planner makes a random choice. |
| 44 | """ |
| 45 | |
| 46 | # Minimum distance to target waypoint as a percentage |
| 47 | # (e.g. within 80% of total distance) |
| 48 | |
| 49 | # FPS used for dt |
| 50 | FPS = 20 |
| 51 | |
| 52 | def __init__(self, agent): |
| 53 | """ |
| 54 | :param agent: agent that regulates the vehicle |
| 55 | :param vehicle: actor to apply to local planner logic onto |
| 56 | """ |
| 57 | self._vehicle = agent.vehicle |
| 58 | self._map = agent.vehicle.get_world().get_map() |
| 59 | |
| 60 | self._target_speed = None |
| 61 | self.sampling_radius = None |
| 62 | self._min_distance = None |
| 63 | self._current_waypoint = None |
| 64 | self.target_road_option = None |
| 65 | self._next_waypoints = None |
| 66 | self.target_waypoint = None |
| 67 | self._vehicle_controller = None |
| 68 | self._global_plan = None |
| 69 | self._pid_controller = None |
| 70 | self.waypoints_queue = deque(maxlen=20000) # queue with tuples of (waypoint, RoadOption) |
| 71 | self._buffer_size = 5 |
| 72 | self._waypoint_buffer = deque(maxlen=self._buffer_size) |
| 73 | |
| 74 | self._init_controller() # initializing controller |
| 75 | |
| 76 | def reset_vehicle(self): |
| 77 | """Reset the ego-vehicle""" |
| 78 | self._vehicle = None |
| 79 | print("Resetting ego-vehicle!") |
| 80 | |
| 81 | def _init_controller(self): |
| 82 | """ |
| 83 | Controller initialization. |
| 84 | |
| 85 | dt -- time difference between physics control in seconds. |
| 86 | This is can be fixed from server side |
| 87 | using the arguments -benchmark -fps=F, since dt = 1/F |
| 88 | |
| 89 | target_speed -- desired cruise speed in km/h |
| 90 | |
| 91 | min_distance -- minimum distance to remove waypoint from queue |