Lane-Level Path Planning with RoadRunner Scenario

Introduction

RoadRunner Scenario is an interactive editor that enables you to design scenarios for simulating and testing automated driving systems. You can place vehicles, define their paths and interactions in the scenario, and then simulate the scenario in the editor. RoadRunner Scenario supports in-editor playback for scenario visualization and connecting to other simulators, such as MATLAB and Simulink®, for cosimulation.

This example shows how to construct a graphical representation of the road network in MATLAB, plan the vehicle path from the specified start and goal points, and follow the path in RoadRunner using a MATLAB System object™.

This figure shows an overview of the information exchanged between RoadRunner Scenario and the lane-level path planner. The path planner reads the map and vehicle runtime information from RoadRunner Scenario to construct the graph using a navGraph (Navigation Toolbox) object. The planner also plans the vehicle trajectory using a plannerAStar (Navigation Toolbox) object.

In this example, you:

Set Up Environment

This section shows how to set up the environment to cosimulate MATLAB with RoadRunner Scenario.

Start RoadRunner application interactively by using the roadrunnerSetup function. When the function opens a dialog box, specify the RoadRunner Project Folder and RoadRunner Installation Folder locations.

rrApp = roadrunnerSetup;

This example uses two files that you must add to the RoadRunner project.

Copy these files to the RoadRunner project. To learn more about the RoadRunner environment, see RoadRunner Project and Scene System (RoadRunner).

copyfile("scenario_LLPP_01_CustomEndPoint.rrscenario",fullfile(rrApp.status.Project.Filename,"Scenarios"))
copyfile("LaneLevelPathPlanner.rrbehavior.rrmeta",fullfile(rrApp.status.Project.Filename,"Assets/Behaviors/"))

Explore RoadRunner Scenario

Open scenario_LLPP_01_CustomEndPoint scenario.

openScenario(rrApp,"scenario_LLPP_01_CustomEndPoint.rrscenario")

The scenario contains three vehicles. The blue lead car follows the lane-following built-in behavior, and the white ego car follows the custom path-planner behavior. To specify the goal position of the path, this example uses a stationary vehicle named Goal.

To simulate path-planning behavior for the ego vehicle, specify custom behavior for it using the LaneLevelPathPlanner.rrbehavior.rrmeta file, which points to the LaneLevelPathPlanner MATLAB System object. You can configure the path-planning as well as path-following behavior using these properties of the System object:

You can tune the path-planning behavior by changing values of custom behavior parameters using the Change Behavior Parameter action. Execution of this action during simulation updates properties of the MATLAB System object. For more information on tuning the behavior using parameters, see Tune Actor Parameters.

This scenario contains variables that enable you to update behavior parameters, the start position, and the goal position.

Connect to the RoadRunner Scenario server for cosimulation using the createSimulation function.

rrSim = createSimulation(rrApp);

Design Path Planner

This example uses LaneLevelPathPlanner MATLAB System object to plan the path between the start and goal points of the ego vehicle.

The LaneLevelPathPlanner MATLAB System object calls the setupImpl function during initialization. The function constructs a graphical representation of road network to plan the path. In this graphical representation, nodes or states represent the midpoint of lane centers, and edges or links represent connections between lanes.

To construct a graphical representation of the road network, get the nodes and edges of the scene in tabular format using the helperGetNodesEdges helper function.

[nodeTable,edgeTable] = helperGetNodesEdges;

To design a path planner, first create a graph data structure using a navGraph (Navigation Toolbox) object.

graph = navGraph(nodeTable,edgeTable)

graph = 
  navGraph with properties:

           States: [423×2 table]
            Links: [696×2 table]
    LinkWeightFcn: @nav.algs.distanceEuclidean

Get the starting position of the ego vehicle by reading the EgoInitialPosition variable from RoadRunner Scenario using the getScenarioVariable function.

egoStartPosition = getScenarioVariable(rrApp,"EgoInitialPosition");
egoStartPosition = str2double(split(egoStartPosition,","))';

Get the position of the goal vehicle by reading the GoalVehiclePosition variable from RoadRunner Scenario. The position of the stationary vehicle Goal is the goal position of the ego vehicle.

egoGoalPosition = getScenarioVariable(rrApp,"GoalVehiclePosition");
egoGoalPosition = str2double(split(egoGoalPosition,","))';

Create an A* planner from the navGraph object using a plannerAStar (Navigation Toolbox) object.

planner = plannerAStar(graph);

Use the planPath method of the HelperPlanPath helper class to plan the path between the start position and the goal position of the ego vehicle using the plan function. planPath takes the plannerAStar object, navGraph object, ego vehicle start position, and goal position as inputs, as well as these optional name-value arguments:

refPath = HelperPlanPath.planPath(planner,graph,egoStartPosition,egoGoalPosition);

Plot the planned path.

pRefPath = plot(refPath(:,1),refPath(:,2),Color="g",LineWidth=2);
% Plot markers for start and goal positions
pStart = plot(egoStartPosition(1),egoStartPosition(2),"o",MarkerFaceColor="b",MarkerSize=4);
pEnd = plot(egoGoalPosition(1),egoGoalPosition(2),"o",MarkerFaceColor="r",MarkerSize=4);
% Add legends
legend([pRefPath pStart pEnd],{"Reference path","Start position","Goal position"},Location="northwest")

To enable the ego vehicle to follow the planned path, the LaneLevelPathPlanner MATLAB System object calls the stepImpl function at each simulation step. The function calls the HelperPolylineEvaluator object, which computes the next ego position using the current ego speed and current ego position.

If you enable the Replan flag, the stepImpl function replans the ego trajectory using the replanPath function of the LaneLevelPathPlanner System object. The replanPath function has this primary interface:

replanPath(obj,pose,obstacleDistance)

replanPath method updates cost of the obstructing lane to infinite and replans the path with updated weight function using planPath method of HelperPlanPath helper class.

Simulate Scenario

Assign the EgoGoalPosition variable using the setScenarioVariable function.

setScenarioVariable(rrApp,EgoGoalPosition=['[',num2str(egoGoalPosition),']'])

Simulate the scenario.

set(rrSim,SimulationCommand="Start")
while strcmp(get(rrSim,"SimulationStatus"),"Running")
    pause(0.01)
end

Notice that the ego vehicle travels on the planned path.

Simulate Path Replanning

This example provides a scenario that is configured to perform path replanning for the ego vehicle when it comes within 30 meters of a stationary lead vehicle along the planned path.

Update the lead vehicle speed to 0 m/s using the LeadCarSpeed variable.

setScenarioVariable(rrApp,LeadCarSpeed="0")

Set the ReplanTriggerDistance and ObstacleDistance variables to 30.

obstacleDistance = 30;
setScenarioVariable(rrApp,ReplanTriggerDistance=num2str(obstacleDistance));
setScenarioVariable(rrApp,ObstacleDistance=num2str(obstacleDistance));

Run the simulation and notice that the ego vehicle changes lane once it is within 30 meters of the lead vehicle, and starts following the replanned path.

set(rrSim,SimulationCommand="Start")
while strcmp(get(rrSim,"SimulationStatus"),"Running")
    pause(0.01)
end