Track Route Progress - Githubissues

Depends on

After #70 is merged, switch target to master and merge this.

Changes

A vehicle's progress along its route can be tracked using the SDVRoute class
If you want a vehicle's progress along its route to be tracked, pass the vehicle's route_nodes to the SDVRoute constructor
To update a vehicle's progress along its route, set the update_route_progress parameter in update_reference_path() to True
To see if a vehicle has completed its route, call route_complete()

A vehicle's route is seen as an ordered list of cartesian points that the vehicle has to "visit"
SDVRoute stores these route_nodes in a list at the start
The list is full at the start, so the vehicle has made zero progress along its route
_update_route_progress() is used to update the vehicle's progress along its route
It does this by going through route_nodes and removing the nodes that have been visited
A visited node is one that appears anywhere on the vehicle's reference path
At the start (in __init__), when zero progress has been made, _update_route_progress() will go through the list removing nodes that are not on the reference path until it reaches a node that is on the reference path
Then, after reaching this node, it removes nodes that are on the reference path, and stops once it reaches a node that isn't on the reference path
The reason it does this is to track the vehicle's initial progress (in case the user places the vehicle in the middle or at the end of its route)
After the start, _update_route_progress() is only called in update_reference_path() and only removes nodes from route_nodes that are on the vehicle's reference path
route_complete() returns True (i.e., the route is complete) when route_nodes is empty

SVPlanner uses route progress tracking with its SDVRoute object
Route progress is update in get_planner_state(), and route_complete() is also checked there
PlannerState has a new member called route_complete which gets its value from route_complete()
PlannerState.route_complete is used as part of the goal condition (in has_reached_goal_frenet(), in ManeuverUtils.py): if the vehicle is complete its route and is either within the threshold or is passed its goal point, then it has reached its goal

Since route progress is being tracked and it is part of the goal condition, scenarios with routes that are more than one loop can be created
So scenarios/test_scenarios/gs_bathurst_double_loop.osm has been added to test this

Run all of scenarios/test_scenarios:
```
bash scripts/run_scenarios.bash -ov
```
The vehicles should reach their goals in all scenarios except for gs_straight_reverse.osm where the vehicle just drives in reverse for 10 seconds