# 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 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(map,startPose,goalPose) % Copyright 2021 The MathWorks, Inc. %#codegen % Create a state space object stateSpace = stateSpaceSE2; % Create a state validator object validator = validatorOccupancyMap(stateSpace); % Create a binary occupancy map and assign the map to the state % validator object. validator.Map = binaryOccupancyMap(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.

Load a map into the workspace.

`map = load("exampleMaps.mat").simpleMap;`

Create a state space object.

`stateSpace = stateSpaceSE2;`

Create a state validator object.

`stateValidator = validatorOccupancyMap(stateSpace);`

Create a binary occupancy map and assign the map to the state validator object.

`stateValidator.Map = binaryOccupancyMap(map);`

Set the validation distance for the validator.

`stateValidator.ValidationDistance = 0.01;`

Initialize the `plannerHybridAStar` object with the state validator object.

`planner = plannerHybridAStar(stateValidator);`

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 = [5 5 pi/2]; goalPose = [22 4 0];```

Plan a path from the start pose to the goal pose.

`plan(planner,startPose,goalPose);`

Visualize the path using the `show` function, and hide the expansion tree.

```show(planner,Tree="off") hold on```

### 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 codegenPathPlanner -args {map,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.

`path = codegenPathPlanner_mex(map,startPose,goalPose);`

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

```scatter(path(:,1),path(:,2),... Marker="o",... MarkerFaceColor="b",... MarkerEdgeColor="b") legend("MATLAB Generated Path","Start Pose","Goal Pose","MEX Generated Path") 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(map,startPose,goalPose))`
```time = 0.1317215226 ```
`mexTime = timeit(@() codegenPathPlanner_mex(map,startPose,goalPose))`
```mexTime = 0.0208781226 ```
`time/mexTime`
```ans = 6.30906931258273 ```

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.

```mapNew = load("exampleMaze.mat").simpleMaze; startPoseNew = [10 8 pi]; goalPoseNew = [5 22 0]; pathNew = codegenPathPlanner_mex(mapNew,startPoseNew,goalPoseNew);```

Visualize the new path computed by the MEX function.

```show(binaryOccupancyMap(mapNew)) hold on scatter(pathNew(:,1),pathNew(:,2),... Marker="o",... MarkerFaceColor="b",... MarkerEdgeColor="b")```