# Path Planning - Greedy Best First Search

I'm working on a path planning algorithm that will be converted to RobotC. I'm trying to optimize it so that it uses the least amount of memory, as the robot it will be implemented on has supposedly as little as 15k available memory.

It may just be the nature of the algorithm used, but it is apparent that it is making some bad choices when it gets to the middle left part of the grid. Any idea how I might optimize this a bit? Perhaps using a real distance measurement? This is important because the program searching the entire map could result in running out of memory. I understand that this probably can't be completely prevented, but I'd like to mitigate it as much as possible.

Extracted algorithm from code snippet below

setTimeout is for visual feedback / understanding of algorithm.

function scanCells() {
if (openCells > 0) {
setTimeout(function() {
if (current == undefined) {
finished();
return;
}
open[current.x][current.y] = false;
$(td[current.x][current.y]).css({ "background-color": '#FFFF00' }); if (current.x == goal.x && current.y == goal.y) { pathFound = true; finished(); return; } var nodeDistances = { node1: 1000, node2 : 1000, node3 : 1000, node4 : 1000 }; // Get the best node. // Search adjacent tiles. var distance = 1000; var node = {}; node.x = -1; node.y = -1; if (current.x - 1 >= 0 && open[current.x - 1][current.y]) { node.x = current.x - 1; node.y = current.y; distance = genericDistance(current.x - 1, current.y, goal.x, goal.y); } if (current.y - 1 >= 0 && open[current.x][current.y - 1] && (genericDistance(current.x, current.y - 1, goal.x, goal.y) < distance)) { node.x = current.x; node.y = current.y - 1; distance = genericDistance(current.x, current.y - 1, goal.x, goal.y); } if (current.x + 1 < 10 && open[current.x + 1][current.y] && genericDistance(current.x + 1, current.y, goal.x, goal.y) < distance) { node.x = current.x + 1; node.y = current.y; distance = genericDistance(current.x + 1, current.y, goal.x, goal.y); } if (current.y + 1 < 10 && open[current.x][current.y + 1] && genericDistance(current.x, current.y + 1, goal.x, goal.y) < distance) { node.x = current.x; node.y = current.y + 1; distance = genericDistance(current.x, current.y + 1, goal.x, goal.y); } if (node.x == -1 || node.y == -1) { // We need to step backwards until a node is available. //path[pathIndex] = null; path[pathIndex - 1] = null; pathIndex -= 1; current = path[pathIndex - 1]; scanCells(); return; //continue; }$(td[current.x][current.y]).css({ "background-color": '#AAAA00' });
current = node;
path[pathIndex++] = current;
scanCells();
}, 1000);
}
else {
finished();
}
}
scanCells();


var grid = [];
var open = [];
var current = {};
var start =  {};
var goal = {};
var openCells = 0;
var path = [];
var pathIndex = 0;

// init grid
for (var x = 0; x < 10; x++) {
grid[x] = [];
open[x] = [];
for (var y = 0; y < 10; y++) {
grid[x][y] = 0;
open[x][y] = true;
openCells++;
}
}

open[3][5] = false;
//open[2][5] = false;
//open[2][4] = false;
open[2][3] = false;
open[3][3] = false;
open[8][8] = false;
open[8][6] = false;
open[8][7] = false;
open[8][5] = false;
open[8][4] = false;
open[8][3] = false;
open[8][2] = false;
open[8][1] = false;
open[6][1] = false;
open[6][2] = false;
open[6][3] = false;
open[4][5] = false;
open[4][6] = false;
open[5][4] = false;
open[6][4] = false;
open[7][4] = false;
open[4][7] = false;
open[4][8] = false;
open[4][9] = false;
open[7][8] = false;
open[6][8] = false;
open[5][6] = false;
open[6][6] = false;
open[1][0] = false;
open[1][1] = false;
open[1][2] = false;
open[1][3] = false;
open[1][4] = false;
open[1][5] = false;
open[1][6] = false;
open[1][7] = false;
open[1][8] = false;

open[0][0] = false;
openCells--;

start.x = 0;
start.y = 0;

goal.x = 5;
goal.y = 5;
var pathFound = false;

function genericDistance(x1, y1, x2, y2) {
return Math.abs(x2 - x1) + Math.abs(y2 - y1);
}

// Draw grid / path
var table = $("<table></table>").appendTo("body"); var tr = []; var td = []; for (var x = 0; x < 10; x++) { tr[x] =$("<tr></tr>").appendTo(table);
td[x] = [];
for (var y = 0; y < 10; y++) {
td[x][y] = $("<td></td>").appendTo(tr[x]).css({ width : "50px", height : "50px", border : "1px solid #000000" }); } } for (var x = 0; x < 10; x++) { for (var y = 0; y < 10; y++) { var t = '#FF0000'; if (open[x][y]) t = '#00FF00'; td[x][y].css({ "background-color": t }); } } td[goal.x][goal.y].css({ "background-color" : '#0000FF' }); current = start; path[pathIndex++] = current; //while (openCells > 0) { function scanCells() { if (openCells > 0) { setTimeout(function() { if (current == undefined) { finished(); return; } open[current.x][current.y] = false;$(td[current.x][current.y]).css({ "background-color": '#FFFF00' });

if (current.x == goal.x && current.y == goal.y) {
pathFound = true;
finished();
return;
}

var nodeDistances = {
node1: 1000,
node2 : 1000,
node3 : 1000,
node4 : 1000
};

// Get the best node.
// Search adjacent tiles.
var distance = 1000;

var node = {};
node.x = -1;
node.y = -1;
if (current.x - 1 >= 0 && open[current.x - 1][current.y]) {
node.x = current.x - 1;
node.y = current.y;

distance = genericDistance(current.x - 1, current.y, goal.x, goal.y);
}

if (current.y - 1 >= 0 && open[current.x][current.y - 1] && (genericDistance(current.x, current.y - 1, goal.x, goal.y) < distance)) {
node.x = current.x;
node.y = current.y - 1;
distance = genericDistance(current.x, current.y - 1, goal.x, goal.y);
}

if (current.x + 1 < 10 && open[current.x + 1][current.y] && genericDistance(current.x + 1, current.y, goal.x, goal.y) < distance) {
node.x = current.x + 1;
node.y = current.y;
distance = genericDistance(current.x + 1, current.y, goal.x, goal.y);
}

if (current.y + 1 < 10 && open[current.x][current.y + 1] && genericDistance(current.x, current.y + 1, goal.x, goal.y) < distance) {
node.x = current.x;
node.y = current.y + 1;
distance = genericDistance(current.x, current.y + 1, goal.x, goal.y);
}

if (node.x == -1 || node.y == -1) {
// We need to step backwards until a node is available.
//path[pathIndex] = null;
path[pathIndex - 1] = null;
pathIndex -= 1;
current = path[pathIndex - 1];
scanCells();
return;
//continue;
}
\$(td[current.x][current.y]).css({ "background-color": '#AAAA00' });
current = node;
path[pathIndex++] = current;
scanCells();
}, 250);
}
else {
finished();
}
}
scanCells();

function finished() {
if (pathFound) {
// Clean up path.
// Work from the goal back to the start position, see if we can shortcut any neighboring nodes.
var pathCleanedUp = true;

while (pathCleanedUp) {
pathCleanedUp = false;
for (var x = pathIndex - 1; x > 0; x--) {
if (pathCleanedUp) break;
for (var i = 0; i < pathIndex && i < x - 1; i++) {
if (pathCleanedUp) break;
// Node comes before x and is a neighbor of x
if (genericDistance(path[i].x, path[i].y, path[x].x, path[x].y) == 1) {
pathCleanedUp = true;
// Trim the inbetween.
var newPath = [];
for (var z = 0; z <= i; z++) {
newPath[z] = { x : path[z].x, y : path[z].y };
}
var newPathIndex = z;
for (var z = x; z < pathIndex; z++) {
newPath[newPathIndex++] = { x : path[z].x, y : path[z].y };
}
path = newPath;
pathIndex = newPathIndex;
}
else if ((path[i].x != path[x].x && path[i].y == path[x].y) || (path[i].x == path[x].x && path[i].y != path[x].y)) {
console.log("Connection:");
console.log(path[i]);
console.log(path[x]);
var connected = true;
// If nodes are within line of site and no used nodes are in between.
if (path[i].x == path[x].x) {
if (path[x].x == 1 && path[x].y == 2) console.log("X shared");
// Walk the Y value over until the values are the same or the node is closed.
if (path[i].y < path[x].y) {
for (var ty = path[i].y + 1; ty < path[x].y; ty++) {
console.log("x: " + path[i].x + " y: " + ty + " open: " + open[path[i].x][ty]);
if (!open[path[i].x][ty]) {
connected = false;
break;
}
}
}
else {
for (var ty = path[i].y - 1; ty > path[x].y; ty--) {
if (!open[path[i].x][ty]) {
connected = false;
break;
}
}
}
}
else if (path[i].y == path[i].y) {
// Walk the X value over until the values are the same or the node is closed.
if (path[i].x < path[x].x) {
for (var ty = path[i].x + 1; ty < path[x].x; ty++) {
if (!open[ty][path[i].y]) {
connected = false;
break;
}
}
}
else {
for (var ty = path[i].x - 1; ty > path[x].x; ty--) {
if (!open[ty][path[i].y]) {
connected = false;
break;
}
}
}
}

if (connected) {
console.log("Connected.");
var dist = 0;

var newPath = [];
for (var z = 0; z <= i; z++) {
newPath[z] = { x : path[z].x, y : path[z].y };
}

// Connect the two paths cutting out the inbetween steps.
if (path[i].x < path[x].x) {
// Increase x until we hit path[x].x.
for (var ix = i + 1; path[i].x + dist != path[x].x; ix++) {
dist++;
newPath[ix] = { x : path[i].x + dist, y : path[i].y };
open[newPath[ix].x][newPath[ix].y] = false;
}
}
else if (path[i].x > path[x].x) {
// Increase x until we hit path[x].x.
for (var ix = i + 1; path[i].x + dist != path[x].x; ix++) {
dist--;
newPath[ix] = { x : path[i].x + dist, y : path[i].y };
open[newPath[ix].x][newPath[ix].y] = false;
}
}
else if (path[i].y < path[x].y) {
// Increase x until we hit path[x].x.
for (var ix = i + 1; path[i].y + dist != path[x].y; ix++) {
dist++;
newPath[ix] = { x : path[i].x, y : path[i].y + dist };
open[newPath[ix].x][newPath[ix].y] = false;
}
}
else if (path[i].y > path[x].y) {
// Increase x until we hit path[x].x.
for (var ix = i + 1; path[i].y + dist != path[x].y; ix++) {
dist--;
newPath[ix] = { x : path[i].x, y : path[i].y + dist };
open[newPath[ix].x][newPath[ix].y] = false;
}
}

for (var iz = x; iz < pathIndex; iz++) {
newPath[ix++] = path[iz];
}
pathCleanedUp = true;
path = newPath;
console.log(path);
pathIndex = ix;
}
}
}
}
}

for (var x = 0; x < pathIndex; x++) {
td[path[x].x][path[x].y].html("x");
}
}
else {
console.log("No path found.");
}
}
<script src="https://ajax.googleapis.com/ajax/libs/jquery/2.1.1/jquery.min.js"></script>

## 1 Answer

The pathological hunting in the left region of your test case is an unfortunate consequence of using a naïve greedy algorithm. It really is at least as good locally to move up rather than left, and that's true whether you use Manhattan distances or Euclidean distances. If you want to avoid pathological hunting, try the A* algorithm instead.

There are a few simple local improvements to your code.

• var nodeDistances is never used.
• var distance = 1000 is a magic number. var distance = Number.POSITIVE_INFINITY would be better.
• Instead of a global pathFound flag, pass it as a parameter to finished() callback. You can also take advantage of the fact that finished() returns void. The idiom would be return finished(true) or return finished(false).
• Prefer early returns wherever possible. In scanCells(), if you start with if (openCells <= 0) { return finished(false); } you could save a level of indentation in the entire body of the function. More importantly, your code would be more readable because you eliminate the element of suspense. The same advice goes for the body of your finished() function.
• 10 is a magic number. Prefer open[length] and open[0].length.
• When using path as a stack, use path.push(current = node) and path.pop().
• I actually tried using the A* algorithm first in RobotC. But just defining the grid, path, gscore and fscore maps caused an out of memory error. Should the A* use less memory than best first? Nov 26, 2013 at 19:52
• How large of a maze do you want to handle? Perhaps you would like to post your A* code for review as well. Nov 26, 2013 at 23:00