# Generate Code for Path Planning Using Hybrid A Star

This example shows how to perform code generation to plan a collision-free path for a vehicle through a map using the Hybrid A* algorithm. After you verify the algorithm in MATLAB®, use the `codegen` (MATLAB Coder) function 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, `codegenPathPlanner`, that uses a `plannerHybridAStar` object to plan a path from the start pose to the goal pose in the map.

```function path = codegenPathPlanner(mapData,startPose,goalPose) %#codegen % Create a binary occupancy map map = binaryOccupancyMap(mapData); % Create a state space object stateSpace = stateSpaceSE2; % Update state space bounds to be the same as map limits. stateSpace.StateBounds = [map.XWorldLimits;map.YWorldLimits;[-pi pi]]; % Construct a state validator object using the statespace and map object validator = validatorOccupancyMap(stateSpace,Map=map); % Set the validation distance for the validator validator.ValidationDistance = 0.01; % Assign the state validator object to the plannerHybridAStar object planner = plannerHybridAStar(validator); % Compute a path for the given start and goal poses pathObj = plan(planner,startPose,goalPose); % Extract the path poses from the path object path = pathObj.States; end ```

This function acts as a wrapper for a standard Hybrid A* 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 `codegenPathPlanner` function in your current folder.

### Verify Path Planning Algorithm in MATLAB

Verify the path planning algorithm in MATLAB before generating code.

Generate a random 2-D maze map.

```map = mapMaze(5,MapSize=[25 25],MapResolution=1); mapData = occupancyMatrix(map);```

Define start and goal poses as [x y theta] vectors. x and y specify the position in meters, and theta specifies the orientation angle in radians.

```startPose = [3 3 pi/2]; goalPose = [22 22 pi/2];```

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

`path = codegenPathPlanner(mapData,startPose,goalPose);`

Visualize the computed path.

```show(binaryOccupancyMap(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") % Path plot(path(:,1),path(:,2),"r-",LineWidth=2) legend("Start Pose","Goal Pose","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 calling `codegen` at the MATLAB command line. Specify sample input arguments for each input to the function using the `-args` option and `func_inputs` input argument.

Call the `codegen` function and specify the input arguments in a cell array. This function creates a separate `codegenPathPlanner_mex` function to use. You can also produce C code by using the `options` input argument. This step can take some time.

`codegen -config:mex codegenPathPlanner -args {mapData,startPose,goalPose}`
```Code generation successful. ```

### 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 = codegenPathPlanner_mex(mapData,startPose,goalPose);`

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

```scatter(mexPath(:,1),mexPath(:,2),... Marker="o",... MarkerFaceColor="b",... MarkerEdgeColor="b") legend("Start Pose","Goal Pose","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(@() codegenPathPlanner(mapData,startPose,goalPose))`
```time = 0.3735 ```
`mexTime = timeit(@() codegenPathPlanner_mex(mapData,startPose,goalPose))`
```mexTime = 0.0474 ```
`time/mexTime`
```ans = 7.8872 ```

In this example, the MEX function runs more than five 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 pi/2];```

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

`pathNew = codegenPathPlanner_mex(mapDataNew,startPoseNew,goalPoseNew);`

Visualize the new path computed by the MEX function.

```show(binaryOccupancyMap(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") % Path plot(pathNew(:,1),pathNew(:,2),"r-",LineWidth=2) legend("Start Pose","Goal Pose","MEX Generated Path") legend(Location='northeast')``` 