Highway Lane Change Planner 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.

The lane change planner is a fundamental component of a highway lane change system that enables an ego vehicle to move from one lane to another. The lane change planner is expected to handle different driving behaviors to safely navigate the ego vehicle from one point to another point. This example shows how to use RoadRunner Scenario to simulate the highway lane change planner reference model used in the Generate Code for Highway Lane Change Planner example.

This figure shows an overview of the information exchanged between RoadRunner Scenario and the highway lane change planner. The lane change planner reads path action, map data, and all actor runtime from RoadRunner Scenario. The planner uses this information to sample trajectories, and then finds an optimal collision-free trajectory to navigate the ego vehicle in the scenario.

In this example, you:

Set Up Environment

This section shows how to set up the environment to cosimulate the highway lane change planner 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;

The rrApp RoadRunner object enables you to interact with RoadRunner from the MATLAB workspace. You can open the scenario and update scenario variables using this object. For more information on variables, see Generate Scenario Variations Using gRPC API (RoadRunner Scenario).

To explore the test bench model, load the highway lane change planner project.

openProject("HLCPlannerWithRRScenario");

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

HLCWithRRScenarioProject = currentProject;
projectPath = convertStringsToChars(HLCWithRRScenarioProject.RootFolder);
projectRootFolder = projectPath(1:find(projectPath=='\',1,'last')-1);

copyfile(fullfile(projectRootFolder,"HLCTestScenarios/RoadRunner/Scenes"),fullfile(rrApp.status.Project.Filename,"Scenes"),'f')
copyfile(fullfile(projectRootFolder,"HLCTestScenarios/RoadRunner/Scenarios"),fullfile(rrApp.status.Project.Filename,"Scenarios"),'f')
copyfile(fullfile(projectRootFolder,"HLCPlannerWithRRScenario/TestBench/HLCPlanner.rrbehavior.rrmeta"),fullfile(rrApp.status.Project.Filename,"Assets","Behaviors"),'f')

Explore RoadRunner scenario

This example reuses the highway lane change planner from the Generate Code for Highway Lane Change Planner example. This planner supports specific road types and needs a reference path for planning its trajectories. As it requires time input for re-planning, you must synchronize the step size of the lane change planner model with RoadRunner scenario.

Road Design

Highway lane change planner supports roads with elevation and non-uniform lane width. This example uses FreewayGauntlet.rrscene to simulate the highway lane change planner. The scene contains a one-way highway road which forks up to a bridge. Open the scene.

openScene(rrApp,"FreewayGauntlet.rrscene")

Notice that the road in the scene has elevations and variable lane width.

Ego Reference Path

To navigate the ego vehicle, the lane change planner requires a predefined global reference path. You can specify the path for the vehicle using RoadRunner Scenario editor. For more information on how to specify a path, see Path Editing (RoadRunner Scenario). If you do not specify path for the vehicle, this example generates a path using lane centers.

This example uses the scenario_HLCRR_09_Freeway_Exit.rrscenario scenario. Open the scenario.

openScenario(rrApp,"scenario_HLCRR_09_Freeway_Exit.rrscenario");

The scenario consists of four vehicles. The blue sedan is named Ego. An explicit driving path is not specified for Ego. The planner uses the global reference path generated using lane centers. The other three vehicles are configured to use the built-in behavior of RoadRunner Scenario. In this example, these vehicles are the target actors. To simulate lane change behavior for the ego vehicle Ego, specify custom behavior for it using the HLCPlanner.rrbehavior.rrmeta file. For more information on custom behavior, see Overview of Simulating RoadRunner Scenarios with MATLAB and Simulink.

Connect to the RoadRunner Scenario server for cosimulation using the createSimulation function, and enable data logging.

rrSim = rrApp.createSimulation;
rrSim.set('Logging','on');

Connection status: 1
Connected to RoadRunner Scenario server on localhost:54322, with client id {2241c3cc-feb6-4f65-92a9-c2b093b9a1f9}

rrSim is the ScenarioSimulation object. Use this object to set variables and to read scenario and map-related information.

Simulation Step Size

The lane change planner is designed to run at a step size of 0.1 seconds. Set the simulation step size of RoadRunner Scenario to 0.1 seconds.

Ts = 0.1;
rrSim.set('StepSize',Ts);

Explore Highway Lane Change Planner Test Bench

In this example, you use a system level test bench model to simulate and test the behavior of a lane change planner with RoadRunner Scenario. Open the test bench model.

open_system("HLCPlannerRRTestBench");

The test bench model contains RoadRunner Scenario blocks, which configure, read from, and write to RoadRunner Scenario, as well as these modules:

The Highway Lane Change Planner model and Planner Configuration Parameters, Metrics Assessment, and Pack Ego Pose subsystems are based on the subsystems used in the Generate Code for Highway Lane Change Planner example. This example focuses on the RoadRunner Scenario blocks and the Reference Path Lane Info, Pack Ego Pose, and Visualization subsystems.

RoadRunner Scenario Blocks

The RoadRunner Scenario blocks consist of:

Reference Path Lane Info

The Reference Path Lane Info System object is implemented as HelperReferencePathLaneInfo.m. The System object reads the specified reference path of the ego vehicle from the RoadRunner scenario to calculate the lane-related information RefPathLaneInfo, required by the highway lane change planner. If you do not specify the reference path for the ego vehicle, the System object generates the reference path using lane centers of the ego lane. The System object reads the ActorRuntime and Follow Path Action messages from RoadRunner Scenario, then constructs lane information for the reference path. The primary interface of the subsystem is:

RefPathLaneInfo = stepImpl(obj,ActorRuntime,PathAction)

RefPathLaneInfo contains this information:

HelperReferencePathLaneInfo uses the helperGetLaneInfo function to read lane and lane boundary information from RoadRunner.

[lanes,laneBoundaries] = helperGetLaneInfo(rrSim);

The lanes output argument is a structure that contains these fields:

The laneBoundaries output argument is a structure with these fields:

Pack Ego Pose

The Pack Ego Pose subsystem packs the planner output into the actor runtime message and actor pose. The Self Actor Runtime Writer blocks writes the actor runtime message to RoadRunner Scenario. The actor pose is used as input for the Metrics Assessment subsystem.

open_system("HLCPlannerRRTestBench/Pack Ego Pose");

The Map Matching System object calculates roll and pitch information of the ego vehicle using map information. The System object outputs the actor pose with elevation and banking information.

Visualization

Visualization System object creates a MATLAB plot of the capsule list of all vehicles and candidate trajectories of the ego vehicle. The HelperPlotHLCPlannerWithRRScenario script implements the visualization using this primary interface:

stepImpl(obj,VisualizationInfo,TargetActorsWorld,EgoActor)

Simulate and Visualize Lane Change Scenario

This example uses the helperSLHLCPlannerRRSetup function to initialize the ego and target actor profiles based on the selected scenario.

helperSLHLCPlannerRRSetup(rrApp,rrSim,scenarioFileName="scenario_HLCRR_09_Freeway_Exit");

This model may take a couple of minutes to update the diagram when you are compiling for the first time. Update the model before running the simulation.

set_param("HLCPlannerRRTestBench",SimulationCommand="update");

Simulate the scenario and observe the lane changes made by the ego vehicle to avoid collisions with the target actors.

rrSim.set('SimulationCommand','Start');
while strcmp(rrSim.get("SimulationStatus"),"Running")
  pause(1);
end
hLCPlot = findobj(Type="Figure",Name="Lane Change Status Plot");
if ~isempty(hLCPlot)
    close(hLCPlot);
end

Notice that the ego vehicle changes lanes twice to avoid collision. Visualize the top view of the scenario using a MATLAB figure that plots the ego vehicle, sampled trajectories, capsule list, and other vehicles in the scenario. The visualization also shows the optimal, colliding, infeasible, and non-evaluated trajectories of the ego vehicle, in the MATLAB figure.

During the simulation, the model logs signals to base workspace as logsout. You can analyze the simulation results and debug any failures in the system behavior using the HelperAnalyzeLCRRSimulationResults object. The visualizeSimulationData function of the object creates a MATLAB figure and plots the scenario. The slider in the figure enables you to select a desired simulation step to analyze different parameters shown in these panes:

Run the script and explore the plot.

visualizatonObj = HelperAnalyzeLCRRSimulationResults(logsout);
visualizatonObj.visualizeSimulationData

Explore Other Scenarios

In this example, you have explored the system behavior for the scenario_HLCRR_09_Freeway_Exit scenario, which used FreewayGauntlet.rrscene. You can use the same test bench model to explore other scenarios. This example provides these additional scenarios, which are compatible with the HLCPlannerRRTestBench model.

scenario_HLCRR_04_DenseTraffic scenario use CurvedRoad.rrscene scene, which consists of a curved highway scene with four lanes and other scenarios use WindingRoad.rrscene scene, which consists of a winding highway scene with eight lanes. You can configure the HLCPlannerRRTestBench model to simulate these scenarios using the helperSLHLCPlannerRRSetup function. For example, to configure, the test bench to simulate the scenario_HLCRR_01_SlowMoving scenario, enter this command.

helperSLHLCPlannerRRSetup(rrApp,rrSim,scenarioFileName="scenario_HLCRR_01_SlowMoving")