classdef AstarPlanner < handle
% Reference:
% Astar_tutorial.pdf
% A* zip @ http://www.mathworks.com/matlabcentral/fileexchange/26248-a---a-star--search-for-path-planning-tutorial
properties (SetAccess=public, GetAccess=public)
costmap % world cost map: obstacle = Inf
G % index of goal point
S % index of start point
openlist
openlist_maxlen
quiet = true % flag to show intermediate results
% tag state values (different from Dstar)
b % backpointer (0 means not set), point to parent
t % tag: NEW OPEN CLOSED
h % distance map, path cost to start
NEW = 0;
OPEN = 1;
CLOSED = 2;
occgrid
goal
start
spincount = 0
end
methods
%%%%%%%%%%%%%%%%%%%% constructor %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function obj = AstarPlanner(world, goal, start)
obj.goal = goal;
obj.start = start;
obj.occgrid = world;
obj.occgrid2costmap(obj.occgrid);
% init the A* state variables
obj.reset();
end
%%%%%%%%%%%%%%%%%%%% occgrid2costmap %%%%%%%%%%%%%%%%%%%%%%%%%%%%
function occgrid2costmap(obj, occgrid, cost)
if nargin < 3
cost = 1;
end
obj.costmap = occgrid;
obj.costmap(obj.costmap==1) = Inf; % occupied cells have Inf driving cost
obj.costmap(obj.costmap==0) = cost; % unoccupied cells have driving cost
end
%%%%%%%%%%%%%%%%%%%% reset %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function reset(obj)
% reset() resets the A* planner. The next instantiation
% of plan() will perform a global replan.
obj.openlist = zeros(5,0); % the open list, one column per point
obj.openlist_maxlen = -Inf;
% keep goal in index rather than row,col format
obj.G = sub2ind(size(obj.occgrid), obj.goal(2), obj.goal(1));
obj.S = sub2ind(size(obj.occgrid), obj.start(2), obj.start(1));
obj.b = zeros(size(obj.costmap), 'uint32'); % backpointers
obj.t = zeros(size(obj.costmap), 'uint8'); % tags, default NEW
obj.t(obj.occgrid == 1) = obj.CLOSED; % put all obtacle to CLOSED
obj.h = Inf*ones(size(obj.costmap)); % path cost estimate
end
%%%%%%%%%%%%%%%%%%%% neighbours %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Y = neighbours(obj, X)
dims = size(obj.costmap);
[r,c] = ind2sub(dims, X);
% list of 8-way neighbours
Y = [r-1 r-1 r-1 r r r+1 r+1 r+1; c-1 c c+1 c-1 c+1 c-1 c c+1];
k = (min(Y)>0) & (Y(1,:)<=dims(1)) & (Y(2,:)<=dims(2));
Y = Y(:,k);
Y = sub2ind(dims, Y(1,:)', Y(2,:)')';
end
%%%%%%%%%%%%%%%%%%%% euclidean_distance %%%%%%%%%%%%%%%%%%%%%%%%
function heuristic = euclidean_distance(obj, X, Y)
[r,c] = ind2sub(size(obj.costmap), [X; Y]);
heuristic = sqrt(sum(diff([r c]).^2));
end
%%%%%%%%%%%%%%%%%%%%%% insert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function insert(obj, array, parent)
% openlist: each col contains
% openlist(1,i) = node index, not subscript
% openlist(2,i) = parent index, not subscript
% openlist(3,i) = g = path cost
% openlist(4,i) = h = heuristic
% openlist(5,i) = f = g + h
array(2,1) = parent;
obj.openlist = [obj.openlist, array];
obj.t(array(1,1)) = obj.OPEN;
end
%%%%%%%%%%%%%%%%%%%% DELETE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function DELETE(obj, X, parent, g)
% if ~obj.quiet
% fprintf('delete %d\n', X);
% end
i = find(obj.openlist(1,:) == X);
% if length(i) ~= 1
% error('D*:DELETE: state %d doesnt exist', X);
% end
obj.openlist(:,i) = []; % remove the column
obj.t(X) = obj.CLOSED;
obj.b(X) = parent;
obj.h(X) = g;
end
%%%%%%%%%%%%%%%%%%%% expand_array %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function exp_array = expand_array(obj, X, g)
% exp_array: ech col contains
% exp_array(1,i) = node index, not subscript
% exp_array(2,i) = nan
% exp_array(3,i) = g = path cost
% exp_array(4,i) = h = heuristic
% exp_array(5,i) = f = g + h
exp_array=[];
exp_count=1;
Y = obj.neighbours(X);
for Y = obj.neighbours(X)
if obj.t(Y) ~= obj.CLOSED
exp_array(1, exp_count) = Y;
exp_array(2, exp_count) = nan;
exp_array(3, exp_count) = g + obj.euclidean_distance(X, Y);
exp_array(4, exp_count) = obj.euclidean_distance(Y, obj.G);
exp_array(5, exp_count) = exp_array(3, exp_count) + exp_array(4, exp_count);
exp_count = exp_count+1;
end
end
end
%%%%%%%%%%%%%%%%%%%% MIN_STATE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [X, parent, g] = MIN_STATE(obj)
% return the index of the open state with the smallest k value
if isempty(obj.openlist)
X = [];
g = 0;
else
% find the minimum f value on the openlist
[~,i] = min(obj.openlist(5,:));
% return its index
X = obj.openlist(1,i);
parent = obj.openlist(2,i);
g = obj.openlist(3,i);
end
end
%%%%%%%%%%%%%%%%%%%%%% plan %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function plan(obj)
if ~obj.quiet
obj.plot();
hold on
end
X = obj.S;
g = 0;
obj.t(X) = obj.CLOSED;
while( X ~= obj.G)
%obj.spinner();
if ~obj.quiet
[r,c] = ind2sub(size(obj.costmap), X);
plot(c, r, 'c.', 'MarkerSize', 12);
drawnow
end
%size(obj.openlist,2)
exp_array = obj.expand_array(X, g);
for i = 1: size(exp_array,2)
% if the successor is already on the openlist
if obj.t( exp_array(1,i) ) == obj.OPEN
% find the position of this successor in the
% openlist
j = find(obj.openlist(1,:) == exp_array(1,i) );
obj.openlist(5,j) = min(obj.openlist(5,j), exp_array(5,i)); % openlist update cost
if obj.openlist(5,j) == exp_array(5,i) % it's actually updated
obj.openlist(2,j) = X;
obj.openlist(3,j) = exp_array(3,i);
obj.openlist(4,j) = exp_array(4,i);
end
% if the successor is NEW, insert to the openlist
else
obj.insert(exp_array(:,i), X);
end
end
[X, parent, g] = obj.MIN_STATE();obj.DELETE(X, parent, g);
end
%Find out the node with the smallest fn
disp('done planning')
end
%%%%%%%%%%%%%%%%%%%% path %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function pp = path(obj)
obj.plot();
hold on
robot = obj.goal;
p = [];
% iterate using the next() method until we reach the goal
while true
plot(robot(1), robot(2), 'g.', 'MarkerSize', 12);
drawnow
% move to parent on path
robot = obj.parent(robot);
% are we there yet?
if robot == obj.start
% yes, exit the loop
break
else
% no, append it to the path
p = [robot(:)'; p];
end
end
pp = p;
end
%%%%%%%%%%%%%%%%%%%%%% plot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function plot(obj)
clf
% create color map for distance field + obstacle:
% obstacle, color index = 1, red
% free space, color index > 1, greyscale
distance = obj.costmap;
% create the color map
cmap = [1 0 0; 0 0 0];
% ensure obstacles appear as red pixels
distance(obj.occgrid > 0) = 0;
% create the color map
cmap = [1 0 0; 1 1 1];
% display it with colorbar
image(distance+1, ‘CDataMapping’, ‘direct’);
set(gcf, ‘Renderer’, ‘Zbuffer’)
colormap(cmap)
% label the grid
set(gca, ‘Ydir’, ‘normal’);
xlabel(‘x’);
ylabel(‘y’);
grid on
axis equal
hold on
if ~isempty(obj.goal)
plot(obj.goal(1), obj.goal(2), ‘bd’, ‘MarkerFaceColor’, ‘b’);
end
hold on
end
%%%%%%%%%%%%%%%%%%%% spinner %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function spinner(obj)
% spinner() displays a simple ASCII progress spinner, a rotating bar.
spinchars = ‘-\|/’;
obj.spincount = obj.spincount + 1;
fprintf(‘\b%c’, spinchars( mod(obj.spincount, length(spinchars))+1 ) );
end
%%%%%%%%%%%%%%%%%%%% parent %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function n = parent(obj, current)
X = sub2ind(size(obj.costmap), current(2), current(1));
X = obj.b(X);
if X == 0
n = [];
else
[r,c] = ind2sub(size(obj.costmap), X);
n = [c;r];
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
end