Initial commit

Author: roquarrel

.gitattributes

@@ -0,0 +1,28 @@
+* text=auto
+
+*.fig binary
+*.mat binary
+*.mdl binary diff merge=mlAutoMerge
+*.mdlp binary
+*.mex* binary
+*.mlapp binary
+*.mldatx binary
+*.mlproj binary
+*.mlx binary
+*.p binary
+*.sfx binary
+*.sldd binary
+*.slreqx binary merge=mlAutoMerge
+*.slmx binary merge=mlAutoMerge
+*.sltx binary
+*.slxc binary
+*.slx binary merge=mlAutoMerge
+*.slxp binary
+
+## Other common binary file types
+*.docx binary
+*.exe binary
+*.jpg binary
+*.pdf binary
+*.png binary
+*.xlsx binary

.gitignore

@@ -0,0 +1,33 @@
+# Autosave files
+*.asv
+*.m~
+*.autosave
+*.slx.r*
+*.mdl.r*
+
+# Derived content-obscured files
+*.p
+
+# Compiled MEX files
+*.mex*
+
+# Packaged app and toolbox files
+*.mlappinstall
+*.mltbx
+
+# Deployable archives
+*.ctf
+
+# Generated helpsearch folders
+helpsearch*/
+
+# Code generation folders
+slprj/
+sccprj/
+codegen/
+
+# Cache files
+*.slxc
+
+# Cloud based storage dotfile
+.MATLABDriveTag

AutonomousHaulTruck.prj

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<MATLABProject xmlns="http://www.mathworks.com/MATLABProjectFile" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" version="1.0"/>

CONTRIBUTING.md

@@ -0,0 +1,25 @@
+# Contributing
+
+>_If you believe you have discovered a security vulnerability, please **do not** open an issue or make a pull request.  Follow the instructions in the [SECURITY.MD](SECURITY.MD) file in this repository._
+
+Thank you for your interest in contributing to a MathWorks repository!  We encourage contributions large and small to this repository.  
+
+**Contributions do not have to be code!** If you see a way to explain things more clearly or a great example of how to use something, please contribute it (or a link to your content).  We welcome issues even if you don't code the solution.  We also welcome pull requests to resolve issues that we haven't gotten to yet!
+
+## How to contribute
+
+* **Open an issue:** Start by [creating an issue](https://docs.github.com/en/issues/tracking-your-work-with-issues/creating-an-issue) in the repository that you're interested in.  That will start a conversation with the maintainer.  When you are creating a bug report, please include as many details as possible.  Please remember that other people do not have your background or understanding of the issue; make sure you are clear and complete in your description.
+* **Work in your own public fork:** If you choose to make a contribution, you should [fork the repository](https://docs.github.com/en/get-started/quickstart/fork-a-repo).  This creates an editable copy on GitHub where you can write, test, and refine your changes.  We suggest that you keep your changes small and focused on the issue you submitted.
+* **Sign a Contributor License Agreement (CLA):** We require that all outside contributors sign a [CLA](https://en.wikipedia.org/wiki/Contributor_License_Agreement) before we can accept your contribution.  When you create a pull request (see below), we'll reach out to you if you do not already have one on file.  Essentially, the CLA gives us permission to publish your contribution as part of the repository.
+* **Make a pull request:** "[Pull Request](https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/proposing-changes-to-your-work-with-pull-requests/about-pull-requests)" is a confusing term, but it means exactly what it says:  You're requesting that the maintainers of the repository pull your changes in.  If you don't have a CLA on file, we'll reach out to you.  Your contribution will be reviewed, and we may ask you to revise your pull request based on our feedback.  Once everyone is satisfied, we'll merge your pull request into the repository.
+
+## Guidelines
+
+We don't have best practices for writing MATLAB® code, but we do have some recommendations:
+
+* You should not have any warnings or errors in the [code analyzer report](http://www.mathworks.com/help/matlab/matlab_prog/matlab-code-analyzer-report.html)
+* [Loren Shure's blog](https://blogs.mathworks.com/loren) has [great advice on improving your MATLAB code](https://blogs.mathworks.com/loren/category/best-practice/)
+* Examples should be written as [live scripts](https://www.mathworks.com/help/matlab/matlab_prog/what-is-a-live-script-or-function.html) and then [exported as HTML](https://www.mathworks.com/help/matlab/matlab_prog/share-live-scripts.html).
+* We adhere to the [CommonMark](https://commonmark.org/) specification where it does not conflict with GitHub rendering.  If you edit your Markdown in Visual Studio Code or a similar editor, it uses [markdownlint](https://github.com/DavidAnson/markdownlint) to highlight issues in your Markdown.
+
+**Again, thanks for contributing, and we look forward to your issues and pull requests!**

CreateLocalPlannerToNavigateGlobalPath.mlx

@@ -0,0 +1,62 @@
+function se2Path = TerrainAwarePlannerFcn(start,goal,dem,gridLoc,tuneableParams,fixedParams)
+%TerrainAwarePlannerFcn Wrapper of terrain informed plannerHybridAStar
+%
+% Copyright 2023 The MathWorks, Inc.
+
+    arguments
+        start (:,3) double
+        goal (:,3) double
+        dem (:,:) single
+        gridLoc (1,2) double
+        tuneableParams
+        fixedParams
+    end
+    persistent costMap prevDEM prevGridLoc prevMaxAngle
+
+    if isempty(costMap)
+        % Create costMaps from dem
+        costMap = exampleHelperDem2mapLayers(dem,tuneableParams.MaxAngle,fixedParams.Resolution);
+
+        % Store previous inputs
+        prevDEM = dem;
+        prevMaxAngle = tuneableParams.MaxAngle;
+        prevGridLoc = gridLoc;
+    else
+        updateMaps = ~isequal(prevDEM,dem) || ~isequal(prevGridLoc,gridLoc) || ~isequal(prevMaxAngle,tuneableParams.MaxAngle);
+
+        % Store previous inputs
+        prevDEM = dem;
+        prevMaxAngle = tuneableParams.MaxAngle;
+        prevGridLoc = gridLoc;
+
+        % Check whether costmap must be updated
+        if updateMaps
+            updatePlannerMaps(costMap,dem,prevMaxAngle,fixedParams.Resolution);
+        end
+    end
+    
+    % Initialize planner
+    planner = exampleHelperCreateTerrainPlanner(costMap,tuneableParams,fixedParams);
+    
+    % Update tuneable properties
+    names = fieldnames(tuneableParams);
+    for i = 1:numel(names)
+        name = names{i};
+        if isprop(planner,name)
+            planner.(name) = tuneableParams.(name);
+        end
+    end
+
+    % Plan a path
+    pathObj = planner.plan(start,goal);
+    pathObj.interpolate(max(1000,pathObj.NumStates));
+    se2Path = pathObj.States;
+end
+
+function updatePlannerMaps(origCostMap,dem,maxAngle,res)
+    % Create new costMap and validator map
+    tmpCostMap = exampleHelperDem2mapLayers(dem,maxAngle,res);
+
+    % Update planner's cost function
+    origCostMap.syncWith(tmpCostMap);
+end

CreateRoutePlannerUsingDigitalElevationData.mlx

@@ -0,0 +1,39 @@
+function planner = exampleHelperCreateTerrainPlanner(costMap,tuneableParams,fixedParams,transitionFcn)
+%exampleHelperCreateTerrainPlanner Creates plannerHybridAStar from incoming parameters and cost function
+%
+% Copyright 2023 The MathWorks, Inc.
+
+    if nargin == 3
+        % Create plannerHybridAStar with terrain-aware cost function
+        gWeight = 1; % Weight
+        transitionFcn = @(motionSegment)exampleHelperZHeuristic(motionSegment, costMap, gWeight);
+    end
+    % Create a stateValidator object and update the statebounds
+    map = getLayer(costMap,"terrainObstacles");
+    ss = stateSpaceSE2([map.XLocalLimits; map.YWorldLimits; [-pi pi]]);
+    vehDims = exampleHelperVehicleGeometry(coder.const(fixedParams.Length),coder.const(fixedParams.Width),"collisionChecker");
+    collisionChecker = inflationCollisionChecker(vehDims,fixedParams.NumCircles);
+    vehMap = vehicleCostmap(single(map.getOccupancy),'CollisionChecker',collisionChecker,'MapLocation',map.GridLocationInWorld);
+    validator = validatorVehicleCostmap(ss,Map=vehMap,ThetaIndex=3);
+    
+    % Initialize planner
+    planner = plannerHybridAStar(validator, TransitionCostFcn=transitionFcn);
+    
+    % Update fixed properties
+    names = fieldnames(fixedParams);
+    for i = 1:numel(names)
+        name = names{i};
+        if isprop(planner,name)
+            planner.(name) = fixedParams.(name);
+        end
+    end
+    
+    % Update tuneable properties
+    names = fieldnames(tuneableParams);
+    for i = 1:numel(names)
+        name = names{i};
+        if isprop(planner,name)
+            planner.(name) = tuneableParams.(name);
+        end
+    end
+end

CreateTerrainAwareGlobalPlanners.mlx

@@ -0,0 +1,35 @@
+function [costMap,maxSlope] = exampleHelperDem2mapLayers(dem,maxInclineAngle,res)
+%exampleHelperDem2mapLayers Converts DEM data to a multiLayerMap storing cost,
+%gradients, and obstacles based on the slope limit
+%
+% Copyright 2023 The MathWorks, Inc.
+
+    arguments
+        dem (:,:) double {mustBeNumeric}
+        maxInclineAngle (1,1) double {mustBeFinite, mustBeReal} = 15;
+        res (1,1) double {mustBePositive} = 1;
+    end
+    % Discretize and Store Environment Information in Map Layers
+
+    % Query and store the Z-height
+    zLayer = mapLayer(dem,LayerName="Z",Resolution=res);
+    [gx,gy] = gradient(dem);
+    
+    dzdxLayer = mapLayer(gx*res,LayerName="dzdx",Resolution=res);
+    dzdyLayer = mapLayer(gy*res,LayerName="dzdy",Resolution=res);
+    maxSlope = tand(maxInclineAngle); % Max preferred slope for vehicle
+    
+    slope2cost = @(x)exampleHelperGradient2Cost(maxSlope,x);
+    GX = dzdxLayer.getMapData();
+    GY = dzdyLayer.getMapData();
+    xCost = mapLayer(slope2cost(GX),LayerName="xCost",Resolution=res);
+    yCost = mapLayer(slope2cost(GY),LayerName="yCost",Resolution=res);
+    diagCost = mapLayer(slope2cost(sqrt(GX.^2+GY.^2)),LayerName="diagCost",Resolution=res);
+    
+    % Terrain Obstacles
+    mInvalidSlope = getMapData(xCost) > 1 | getMapData(yCost) > 1 | getMapData(diagCost) > 1;
+    terrainObstacles = binaryOccupancyMap(mInvalidSlope,LayerName="terrainObstacles",Resolution=res);
+    
+    % Combine Individual Map Layers
+    costMap = multiLayerMap({zLayer xCost yCost dzdxLayer dzdyLayer diagCost terrainObstacles});
+end