Generate Code for Path Planning Using RRT Star Planner

This example uses:

This example shows how to perform code generation to plan a collision-free path for a vehicle through a map using the RRT* algorithm. After you verify the algorithm in MATLAB®, use the MATLAB Coder (MATLAB Coder) app to generate a MEX function. Use the generated MEX file in the algorithm to visualize the planned path.

Write Algorithm to Plan Path

Create a function, codegenRRTStar, that uses a plannerRRTStar object to plan a path from the start pose to the goal pose in the map.

function [path,treeData] = codegenRRTStar(mapData,startPose,goalPose)
    %#codegen
   
    % Create an occupancy map.
    omap = occupancyMap(mapData);

    % Create a state space object.
    stateSpace = stateSpaceSE2;

    % Update state space bounds to be the same as map limits.
    stateSpace.StateBounds = [omap.XWorldLimits;omap.YWorldLimits;[-pi pi]];

    % Construct a state validator object using the statespace and map
    % object.
    stateValidator = validatorOccupancyMap(stateSpace,Map=omap);
    
    % Set the validation distance for the validator.
    stateValidator.ValidationDistance = 0.01;
    
    % Create RRT* path planner and allow further optimization after goal is
    % reached. Reduce the maximum iterations and increase the maximum 
    % connection distance.
    planner = plannerRRTStar(stateSpace,stateValidator, ...
                             ContinueAfterGoalReached=true, ...
                             MaxIterations=2500, ...
                             MaxConnectionDistance=1);
    
    % Compute a path for the given start and goal poses.
    [pathObj,solnInfo] = plan(planner,startPose,goalPose);
    
    % Extract the path poses from the path object.
    path = pathObj.States;

    % Extract the tree expansion data from the solution information 
    % structure.
    treeData = solnInfo.TreeData;
end

This function acts as a wrapper for a standard RRT* path planning call. It accepts standard inputs and returns a collision-free path as an array. Because you cannot use a handle object as an input or output of a function that is supported for code generation, create the planner object inside the function. Save the codegenRRTStar function in your current folder.

Verify Path Planning Algorithm in MATLAB

Verify the path planning algorithm in MATLAB before generating code using the test file, codegenRRTStar_testbench.m.

% Generate a random 2-D maze map.
map = mapMaze(5,MapSize=[25 25],MapResolution=1);
mapData = occupancyMatrix(map);

% Define start and goal poses.
startPose = [3 3 pi/2];
goalPose = [22 22 0];

%Plan the path for the specified start pose, and goal pose, and map.
[path,treeData] = codegenRRTStar(mapData,startPose,goalPose);

%Visualize the computed path.
show(occupancyMap(mapData))
hold on
% Start state
scatter(startPose(1,1),startPose(1,2),"g","filled")
% Goal state
scatter(goalPose(1,1),goalPose(1,2),"r","filled")
% Tree expansion
plot(treeData(:,1),treeData(:,2),"c.-")
% Path
plot(path(:,1),path(:,2),"r-",LineWidth=2)
legend("Start Pose","Goal Pose","Tree expansion","MATLAB Generated Path")
legend(Location='northwest')

Generate Code for Path Planning Algorithm

You can use either the codegen (MATLAB Coder) function or the MATLAB Coder (MATLAB Coder) app to generate code. For this example, generate a MEX file by using the MATLAB Coder™ app.

Open the MATLAB Coder App and Select Source Files

On the MATLAB toolstrip Apps tab, under Code Generation, click the MATLAB Coder app icon. The app opens the Create MATLAB Coder Project dialog box.
Provide the name of the MATLAB Coder project file and the folder in which you want to place the file. The MATLAB Coder toolstrip tab and the MATLAB Coder panel opens.
Either click the Add Entry Points button in the Inputs section of the MATLAB Coder panel or the Entry Points button in the toolstrip. The Entry Points tab opens.
Enter or select the name of the entry-point function codegenRRTStar. An entry-point function is a top-level MATLAB function from which you generate code.

Define Input Types

To specify the input data types for the entry-point function, use Automatically Define Input Types. Enter or select the test file codegenRRTStar_testbench.m.
Click Run. The test file, codegenRRTStar_testbench.m, calls the entry-point function, codegenRRTStar, with the expected input types. The app determines that the input mapData is logical(25x25), the input startPose is double(1x3), and the input goalPose is double(1x3).

Check for Run-Time Issues

Check the entry-point function for issues arising at run time by generating and running a MEX function. To do this, click Run Generated MEX in the toolstrip and provide a script that exercises your entry-point function. For this example, use the test file codegenRRTStar_testbench that you used to define the input types.
The app automatically generates and runs a MEX function. It runs the test script codegenRRTStar_testbench replacing calls to codegenRRTStar with calls to the generated MEX. If the app detects issues during the MEX function generation or execution, it provides warning and error messages. Click these messages to navigate to the problematic code and fix the issue. In this example, the app does not detect issues.

Generate MEX File

In the toolstrip, set Output Type to MEX and Language to C. Use the default values for the other project build configuration settings. Instead of generating a MEX, you can choose to generate C/C++ build types. Different project settings are available for the MEX and C/C++ build types. When you switch between MEX and C/C++ code generation, verify the settings that you choose.
Click Generate Code > Generate Code and Build. MATLAB Coder generates a MEX file codegenRRTStar_mex in your current folder. The Output section of the MATLAB Coder panel indicates that code generation succeeds and displays links to the generated output folder and the code generation report.
Click Code generation report to view the report in the Report Viewer. If the code generator detects errors or warnings during code generation, the report describes the issues and provides links to the problematic MATLAB code. For more information, see Code Generation Reports (MATLAB Coder).

For additional information about MATLAB Coder capabilities and the files it generates, see Generate C Code by Using the MATLAB Coder App (MATLAB Coder).

Verify Results Using Generated MEX Function

Plan the path by calling the MEX version of the path planning algorithm for the specified start pose, and goal pose, and map.

mexPath = codegenRRTStar_mex(mapData,startPose,goalPose);

Visualize the path computed by the MEX version of the path planning algorithm.

plot(mexPath(:,1),mexPath(:,2),"b:",LineWidth=2)
legend("Start Pose","Goal Pose","Tree expansion","MATLAB Generated Path","MEX Generated Path")
legend(Location='northwest')
hold off

Check Performance of Generated Code

Compare the execution time of the generated MEX function to the execution time of your original function by using timeit.

time = timeit(@() codegenRRTStar(mapData,startPose,goalPose))

time = 
              1.5130424783

mexTime = timeit(@() codegenRRTStar_mex(mapData,startPose,goalPose))

mexTime = 
              0.1934125783

time/mexTime

ans = 
          7.82287528349442

In this example, the MEX function runs more than seven times faster. Results may vary for your system.

Plan Path in New Map Using Generated MEX Function

Plan a path for a new start and goal poses in a new map. The size of the new map must be the same as the map used to generate the MEX function.

Generate a random 2-D maze map.

mapNew = mapMaze(5,MapSize=[25 25],MapResolution=1);
mapDataNew = occupancyMatrix(mapNew);

Specify start and goal poses.

startPoseNew = [22 3 pi/2];
goalPoseNew = [3 22 0];

Plan the path for the specified start pose, and goal pose, and map.

[pathNew,treeDataNew] = codegenRRTStar_mex(mapDataNew,startPoseNew,goalPoseNew);

Visualize the new path computed by the MEX function.

show(occupancyMap(mapDataNew))
hold on
% Start state
scatter(startPoseNew(1,1),startPoseNew(1,2),"g","filled")
% Goal state
scatter(goalPoseNew(1,1),goalPoseNew(1,2),"r","filled")
% Tree expansion
plot(treeDataNew(:,1),treeDataNew(:,2),"c.-")
% Path
plot(pathNew(:,1),pathNew(:,2),"r-",LineWidth=2)
legend("Start Pose","Goal Pose","Tree expansion","MEX Generated Path")
legend(Location='northeast')

How useful was this information?

Unrated 1 star 2 stars 3 stars 4 stars 5 stars