Waypoint Updater

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 deceleration plan to the stop line.