Waypoint Updater

Implementation

Waypoint updater publishes a queue of n waypoints ahead of the vehicle position, each with a target velocity. For the simulator, n=100 should be sufficient. For the site (the real-world test track), we'll need n=20. We dequeue traversed waypoints and enqueue new points, preserving and reusing those in the middle. When a light-state changes, the entire queue may need updating. The vehicle should stop at the final base waypoint in the simulator (the default provided by the waypoint loader). This is overridden for the test-site, enabling the vehicle to loop around the track repeatedly.

Initialization of queue:

• Create a persisted queue
• Locate closest waypoint (in the base waypoint list) ahead of current position of vehicle and enqueue n waypoints.
• Persist the index of the last retrieved point in the base list.

Operation cycle at a frequency of 2 Hz:

• Manage waypoint queue. Dequeue consumed waypoints with coordinates "behind" the current vehicle position. Enqueue waypoints from base list starting from last until queue is restored to n in length. (Unless final is encountered).
• Update velocity. If red/yellow light NOT within range (4 m for site, 62 m for simulator), set waypoint velocity to defaults (10 km/h for site, 40 km/h for simulator) given in base list. If red/yellow within range, update each waypoint velocity so that the vehicle comes to a halt at the stop-line waypoint. Decrease velocity at a constant rate of -1.0 m/s². Velocity beyond the stop line should be set to zero.

---

Optimization

As the vehicle moves along a path, the waypoint updater is responsible for making changes to the planned path. Presently, there are no dynamic obstacles other than the traffic lights. In theory, we can make a plan and then simply follow that plan, add to the end of that plan, and make adjustments when a traffic light changes state.

Currently, the code creates a completely new list of waypoints on each cycle. (This is inefficient...as an initial draft.) This list contains the next 50 waypoints from our present position. The re-planning event occurs at a rate of once per pose callback (10 times per second). (In my own testing, I extended the list to 150 waypoints and moved the planning out of the pose callback to a timed loop, at a rate of once per second, or even once every three seconds. See the current dev branch. -James)

Ideally this planning can occur far less frequently. For example only when a change in traffic light is detected or as we near the end of our current waypoint list. Alternatively we might add a few new waypoints to the end of our current list (and avoid re-creating the entire list).

An optimized updater is important to the Drive-By-Wire node because it will reduce unnecessary accelerations due to plan changes. For instance, when the vehicle is approaching a red traffic light, a ramp down in velocity is planned. If the one plan is followed, the acceleration will be smoothly executed. On the other hand, presently the updater replans too frequently, and because the vehicle is in a new location, a new velocity ramp is created. This leads to interruptions in the deceleration plan to the stop line.

Reminder: We should stop at the final waypoint in the global list. (Do not keep going around the track.)

https://carnd.slack.com/archives/C6NVDVAQ3/p1503519259000268

---

Waypoint Follower

There appears to be an implementation or usage issue in pure_pursuit_core.cpp. If the same plan is repeatedly presented to the waypoint follower, once the vehicle exceeds the lookahead distance (determined by velocity) from the beginning of the list, then rather than finding the next correct waypoint, the first point in the list is returned. In other words, if the same set of waypoints is given, then the node stops looking ahead and hangs on the zeroth item. It does not handle a plan with points too far in the past. As a result, we will need to trim the planned path of items already traversed and present a set of waypoints from the current vehicle position forward.