Generate Code for Path Planning Using RRT Star Planner

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

1. On the MATLAB toolstrip Apps tab, under Code Generation, click the MATLAB Coder app icon. The app opens the Select Source Files page.

2. In the Select Source Files page, 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. The app creates a project with the default name codegenRRTStar.prj in the current folder.

3. Click Next to go to the Define Input Types page.

Define Input Types

1. Enter or select the test file codegenRRTStar_testbench.m.

2. Click Autodefine Input Types. 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).

3. Click Next to go to the Check for Run-Time Issues step.

Check for Run-Time Issues

1. In the Check for Run-Time Issues dialog box, specify a test file or enter code that calls the entry-point function with example inputs. For this example, use the test file codegenRRTStar_testbench that you used to define the input types.

2. Click Check for Issues. The app generates 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.

3. Click Next to go to the Generate Code step.

Generate MEX File

1. In the Generate Code dialog box, set Build 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.

2. Click Generate.

MATLAB Coder generates a MEX file codegenRRTStar_mex in your current folder. The MATLAB Coder app indicates that code generation succeeded. It displays the source MATLAB files and generated output files on the left side of the page. On the Variables tab, it displays information about the MATLAB source variables. On the Target Build Log tab, it displays the build log, including C/C++ compiler warnings and errors. By default, in the code window, the app displays the C source code file, codegenRRTStar.c. To view a different file, in the Source Code or Output Files pane, click the file name.

3. Click View 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).

4. Click Next to open the Finish Workflow page.

Review the Finish Workflow Page

The Finish Workflow page indicates that code generation succeeded. It provides a project summary and links to generated output.

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')