%% Create Egocentric Occupancy Maps using Range Sensors
% Occupancy Maps offer a simple yet robust way of representing one's environment in robotic applications by mapping the continuous world-space to a discrete data structure. Individual 'grid cells' can contain binary information, e.g. 0 is free space, 1 is occupied, or a probability, where values can range continously from 0 (free) to 1 (occupied), and we can use this information to facilitate more advanced workflows, such as collision detection and path planning.
% With increased autonomy in today's mobile platforms, and a push for realtime planning and decision-making, it is important for systems to be able to integrate up-to-date sensor information into their representation of the environment while keeping the data structure small so that operations remain efficient.
% This example shows how to create an "egocentric" occupancy map, which can be efficiently moved through the environment and only keeps track of the robot's immediate surroundings. We also demonstrate how this map can be continuously updated using sensor information from a simulated lidar and ground-truth map.
%% Load a Prebuilt occupancyMap to Use as "Ground-Truth"
% First we will create a non-egocentric map from a previously generated data file, which we will consider to be the ground-truth for our simulated lidar. We choose to represent this using a binaryOccupancyMap so that there are clear demarcations between obstacles and non-obstacles:

% Load saved map information
load mapData_rayTracingTrajectory

% Convert the probabilistic map matrix to a binaryOccupancyMap with a
% matching resolution
worldMap = binaryOccupancyMap(mapData.Data > .5, mapData.Resolution);

% Set the bottom-left corner of the map, relative to the world origin
worldMap.LocalOriginInWorld = mapData.GridLocationInWorld;

% Plot the ground truth
set(gcf,'Visible','on')
worldAx = subplot(1,2,1)
worldHandle = show(worldMap,'Parent',worldAx);
hold all

%% Create a Simulated Lidar
% We now create a rangeSensor object, which can be used to model a typical Lidar sensor. You can modify various properties on the rangeSensor to model a particular model of lidar - here, we will just set the range and remove any noise, leaving the rest as default. After creating the object, we can retrieve and plot a reading from our sensor by providing an SE2 pose relative to our previously created map's world frame:

% Create rangeSensor
lidar = rangeSensor;
lidar.Range = [0 5];
lidar.RangeNoise = 0;
pos = [0 0 0];

% Show lidar readings in world map
[ranges, angles] = lidar(pos, worldMap);
hSensorData = exampleHelperPlotLidar(worldAx, pos, ranges, angles);

% Show robot in world map
hRobot = exampleHelperPlotRobot(worldAx, pos);

%
%% Initialize occupancyMap as egoMap-Centric Map
% One of the major differences between the robotics.OccupancyGrid/BinaryOccupancyGrid's from R2019a and R2019b's updated occupancyMap/binaryOccupancyMap is the inclusion of a "local frame". Now, in addition to the 'GridOriginInWorld' property (i.e. the translation from world [0 0] to the bottom-left corner of the map-window) there is also 'LocalOriginInWorld' (translation from world [0 0] to the local frame), and 'GridOriginInLocal' (translation from the local frame to the bottom-left corner of the map window).
% By default, 'LocalOriginInWorld' and 'GridOriginInWorld' are collocated (and thus 'GridOriginInLocal' is by default [0 0]), but by offsetting the grid-origin relative to the local-origin, one can configure their map as an egoMap-centric map:

% By default, GridOriginInLocal = [0 0]
egoMap = occupancyMap(10,10,worldMap.Resolution);

% Offset the GridOriginInLocal such that the "local frame" is located in the
% center of the "map-window"
egoMap.GridOriginInLocal = -[diff(egoMap.XWorldLimits) diff(egoMap.YWorldLimits)]/2;

% Lastly, we will plot the local map in its new location

% Update local plot
localAx = subplot(1,2,2)
localImg = show(egoMap,'Parent',localAx);
hold all
dir_LM = plot(localAx,egoMap.LocalOriginInWorld+[0 1], egoMap.LocalOriginInWorld+[0 0],'r-','LineWidth',3);

%% Plan Path Between Start/End Points
% We can now use our ground-truth map to plan a path between two free points. There are several planners to choose from in the Navigation Toolbox, including A*, hybrid-A*, RRT, and RRT*. In this example, we choose to represent our robot as a car-like robot, which has non-holonomic constraints. We need a path planner that generates motions that respect to these constraints, so we can choose between plannerHybridAStar or plannerRRT/plannerRRTStar with an appropriate state-space, either stateSpaceDubins or stateSpaceReedsShepp.
% For this example, we will use RRT* as our planning algorithm, pair it with a Dubins statespace (for non-holonomic motion generation), and use a state-validator that leverages our ground-truth map to handle the validation of any states and/or motions generated by the planner:

% Pick start and goal locations
startPt = [-6   -5    0];
goalPt  = [ 8    7 pi/2];

% Create a temporary map and inflate it so that the robot maintains 
% some distance between itself and any occupied
binOcc_path = binaryOccupancyMap(worldMap);
inflate(binOcc_path, 0.1);

% Create a StateSpace object - the planner will use this when randomly
% sampling points
stateSpace = stateSpaceDubins;

% Reduce the turning radius to better fit the size of map and obstacle
% density
stateSpace.MinTurningRadius = .5;

% Create a StateValidator object - the planner will use this to check
% the randomly sampled points against the map to determine if they lie in
% free space
validator = validatorOccupancyMap(stateSpace)
validator.Map = binOcc_path;
validator.ValidationDistance = .1;

% Create our planner using the previously created StateSpace and StateValidator objects
planner = plannerRRTStar(stateSpace, validator);
planner.MaxConnectionDistance = 1;

% Set a seed for the randomly generated path so we get reproducible results
    % Note: Remove this to generate different paths between waypoints over consecutive runs
rng(1, 'twister')

% Plan a path between start and goal points
    
path = plan(planner, startPt, goalPt);

% Note: If the planner is unable to find a route between the points you specify, there are a few things you can try:
    % 1) Check that your StateSpace object is configured properly:
        % Is the default MinTurningRadius too large on stateSpaceDubins?
            % Reduce this property to allow for finer motion generation
                % stateSpace.MinTurningRadius = x;
        % Is your map larger than the StateBounds?
            % Increase the bounds of the StateSpace and ensure that the
            % planner can adequately sample the space:
                % stateSpace.StateBounds = [map.XWorldLimits; map.YWorldLimits; thetaMin thetaMax];
    % 2) Do the planner's properties make sense given the search space?
        % Is your search space very large? 
            % Try increasing the planner's MaxConnectionDistance
                % planner.MaxConnectionDistance = y;
        % Is your space cluttered or complex to navigate?
            % Try increasing the MaxNumTreeNodes and MaxIterations properties
                % planner.MaxNumTreeNodes = planner.MaxNumTreeNodes*2;
                % planner.MaxIterations = planner.MaxIterations*2;

%% Generate a Trajectory Along the Path
% The planner has provided a set of states that transition us from startPt to goalPt while adhering to our non-holonomic constraints, but it does not say anything about the rate of travel between the points. To address this we can use the waypointTrajectory feature, which takes our set of poses, and timestamps corresponding to those poses, and interpolates points between each state to form a trajectory.
% In this example, we want the robot move with a constant linear velocity, so we calculate the time-stamps for each waypoint as the distance from start-point to the waypoint divided by the desired velocity:

% Get distance between each waypoint
pt2ptDist = distance(stateSpace, path.States(1:end-1,:), path.States(2:end,:))

% Set desired speed
linVelDes = 0.5; % m/s

% Get times for each waypoint
T = cumsum(pt2ptDist)/linVelDes;

% Create trajectory, with points generated at 10Hz
traj = waypointTrajectory(path.States, [0; T], 'SampleRate', 10);

%% Reset the Simulation to Initial State
% We now have all of the components required to simulate our robot moving through an environment. To initialize the simulation, we set our robot's local origin to the first point on our trajectory and clear the map:

% Reset trajectory
traj.reset;

% Set robot and map to start location and clear the data in the egocentric map
robotCurrentPose = path.States(1,:);
move(egoMap, robotCurrentPose(1:2));
setOccupancy(egoMap, repmat(egoMap.DefaultValue,egoMap.GridSize));

%% Run the Simulation
% The simulation loop is fairly simple, the primary operations are to:
%     1) Update the robot's pose and move the egoMap to the new location
%     2) Retrieve sensor information from the lidar
%     3) Update the local map with this new information
%     4) Refresh the visualization

while ~traj.isDone
    % Increment robot along trajectory
    [pts, orient] = traj.step;
    
    % Get orientation angle from quaternion
    orient = rotmat(orient,'point');
    orientZ = rotm2eul(orient);
    
    % Move the robot to the new location
    robotCurrentPose = [pts(1:2) orientZ(1)];
    move(egoMap, robotCurrentPose(1:2),'MoveType','Absolute');
    
    % Retrieve sensor information from the lidar and insert it into the egoMap
    [ranges, angles] = lidar(robotCurrentPose, worldMap);
    insertRay(egoMap, robotCurrentPose,ranges,angles,lidar.Range(2));

    % Update egoMap-centric plot
    show(egoMap, 'Parent', localAx, 'FastUpdate', 1);
    
    % Update orientation vector(s)
    set(dir_LM, 'XData', robotCurrentPose(1)+[0 cos(robotCurrentPose(3))], 'YData', robotCurrentPose(2)+[0 sin(robotCurrentPose(3))]);

    % Update world plot
    exampleHelperUpdateRobotAndLidar(hRobot, hSensorData, robotCurrentPose, ranges, angles);

    % Call drawnow to push updates to the figure
    drawnow limitrate
end

% Copyright 2019 The MathWorks, Inc.
%