After #70 is merged, switch target to master and merge this.
Changes
Using Route Progress Tracking
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()
Implementation of Route Progress Tracking
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
Goal Condition Includes Route Progress
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
New Scenario
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
Testing
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
Depends on
After #70 is merged, switch target to
master
and merge this.Changes
Using Route Progress Tracking
SDVRoute
classroute_nodes
to theSDVRoute
constructorupdate_route_progress
parameter inupdate_reference_path()
toTrue
route_complete()
Implementation of Route Progress Tracking
SDVRoute
stores theseroute_nodes
in a list at the start_update_route_progress()
is used to update the vehicle's progress along its routeroute_nodes
and removing the nodes that have been visited__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_update_route_progress()
is only called inupdate_reference_path()
and only removes nodes fromroute_nodes
that are on the vehicle's reference pathroute_complete()
returnsTrue
(i.e., the route is complete) whenroute_nodes
is emptyGoal Condition Includes Route Progress
SVPlanner
uses route progress tracking with itsSDVRoute
objectget_planner_state()
, androute_complete()
is also checked therePlannerState
has a new member calledroute_complete
which gets its value fromroute_complete()
PlannerState.route_complete
is used as part of the goal condition (inhas_reached_goal_frenet()
, inManeuverUtils.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 goalNew Scenario
scenarios/test_scenarios/gs_bathurst_double_loop.osm
has been added to test thisTesting
scenarios/test_scenarios
:gs_straight_reverse.osm
where the vehicle just drives in reverse for 10 seconds