Wavefront Algorith for Robot Navigation (Autopilot)

#1

Hi everyone, I have been working on my version of an Auto Navigation Bot which takes in the start and the end location along with a grid map to find a path joining the Start and End.
Base on my reading I found that there are quiet a lot of algorithms out there but the simplest one was the Wavefront algorithm.

C# implementations is as follows :

``````
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

/*
*  This is an implementation of a modified form of the Wavefront Algorithm
*  which can be found on http://pirate.shu.edu/~wachsmut/Teaching/CSAS-Robotics/Challenges/06_Wavefront/index.html
*
* NOTE:- Adjacent cells only represent the cells which can be reached from the the source by one hop
* in any primary direction (only E-W-N-S)
*/
namespace WavefromtMotion
{
/// <summary>
/// Implementation of the Wavefront Algorithm.
/// </summary>
public class Wavefront
{
/// <summary>
/// Enum representing the various states of the grid cells.
/// </summary>
public enum LocationType
{
/// <summary>
/// Represents a wall.
/// </summary>
Wall = 255,
/// <summary>
/// Represents an empty cell (i.e. allows pass through).
/// </summary>
Empty = 0,
/// <summary>
/// Represents destination cell.
/// </summary>
Destination = 254,
/// <summary>
/// Represents source cell.
/// </summary>
Source = 1
}

/// <summary>
/// Struct representing a location on the map grid.
/// </summary>
public struct Location
{
/// <summary>
/// Coordinates.
/// </summary>
public int X, Y;

public static Location NullType
{
get { return new Location(-1, -1); }
}

public Location(int pX, int pY)
{
X = pX;
Y = pY;
}
public override bool Equals(object obj)
{
if (obj == null)
return (this.Equals(Location.NullType));
else
{
return ((((Location)obj).X == this.X) && (((Location)obj).Y == this.Y));
}
}
}

int[,] objGrid;
bool hitSource, hitDest;
int recurCount = 0;
Location source, destination;

/// <summary>
/// Constructor.
/// </summary>
/// <param name="pMap">Grid representing the floorplan in a two dimentional array.</param>
public Wavefront(int[,] pMap)
{
objGrid = pMap;
source = Location.NullType;
destination = Location.NullType;
}

/// <summary>
/// This method will calculate the weights for all the cells on the grid.
/// </summary>
public void GeneratePathWeights()
{
while ((!(hitDest && hitSource)) || (recurCount < objGrid.GetLength(0)))
{
recurCount++;
for (int y = 0; y < objGrid.GetLength(1); y++)
{
for (int x = 0; x < objGrid.GetLength(0); x++)
{
if (objGrid[x, y] == (int)LocationType.Wall)
continue;
else if (objGrid[x, y] == (int)LocationType.Empty)
{
{
objGrid[x, y] = 2;
hitSource = true;
}
else
{
objGrid[x, y] = adjCnt + 1;
}
}
else if (objGrid[x, y] == (int)LocationType.Destination)
{
destination = new Location(x, y);
{
hitDest = true;

}
}
}
}
}

}

/// <summary>
/// Gets the path found between the Source and the Destination.
/// </summary>
/// <returns>An array of Location, representing the path found.</returns>
/// <remarks>Null returned if no path is found.</remarks>
public Location[] GetPath()
{
if (destination.Equals(Location.NullType) || source.Equals(Location.NullType))
return null;
Location[] path = new Location[10];
bool exitLoop = false;
int locPointer = 0;
while (!exitLoop)
{

if (locPointer == 0)
{
path[locPointer] = destination;

}
else
{

path[locPointer] = GetAdjacentLowestCell(path[locPointer - 1].X, path[locPointer - 1].Y);
if (path[locPointer].Equals(source))
{
exitLoop = true;
}
}

if (!exitLoop)
{
locPointer++;
if (locPointer == path.Length - 1)
{
//Resize the array by 10 if needed.
Array.Resize<Location>(ref path, path.Length + 10);
}
}
}

//Again resige the array to only contain the actual path.
Array.Resize<Location>(ref path, locPointer + 1);
return path;
}

/// <summary>
/// Gets the adjacent cell number which has the lowest count.
/// </summary>
/// <param name="pXCord">X coordinate.</param>
/// <param name="pYCord">Y coordinate.</param>
/// <returns>Location of the lowest cell.</returns>
private Location GetAdjacentLowestCell(int pXCord, int pYCord)
{
int lowestCnt = (int)LocationType.Wall;
Location lowestCell = Location.NullType;
if (pXCord < objGrid.GetLength(0) - 1)
{
if (objGrid[pXCord + 1, pYCord] != (int)LocationType.Empty && objGrid[pXCord + 1, pYCord] != (int)LocationType.Destination)
{
if (lowestCnt > objGrid[pXCord + 1, pYCord])
{
lowestCnt = objGrid[pXCord + 1, pYCord];
lowestCell = new Location(pXCord + 1, pYCord);
}
}

}
if (pXCord != 0 && pXCord - 1 >= 0)
{
if (objGrid[pXCord - 1, pYCord] != (int)LocationType.Empty && objGrid[pXCord - 1, pYCord] != (int)LocationType.Destination)
{
if (lowestCnt > objGrid[pXCord - 1, pYCord])
{
lowestCnt = objGrid[pXCord - 1, pYCord];
lowestCell = new Location(pXCord - 1, pYCord);
}
}
}

if (pYCord < objGrid.GetLength(1) - 1)
{
if (objGrid[pXCord, pYCord + 1] != (int)LocationType.Empty && objGrid[pXCord, pYCord + 1] != (int)LocationType.Destination)
{
if (lowestCnt > objGrid[pXCord, pYCord + 1])
{
lowestCnt = objGrid[pXCord, pYCord + 1];
lowestCell = new Location(pXCord, pYCord + 1);
}
}

}

if (pYCord != 0 && pYCord - 1 >= 0)
{
if (objGrid[pXCord, pYCord - 1] != (int)LocationType.Empty && objGrid[pXCord, pYCord - 1] != (int)LocationType.Destination)
{
if (lowestCnt > objGrid[pXCord, pYCord - 1])
{
lowestCnt = objGrid[pXCord, pYCord - 1];
lowestCell = new Location(pXCord, pYCord - 1);
}
}
}

return lowestCell;
}

/// <summary>
/// Gets the lowest weight of the surrounding cells.
/// </summary>
/// <param name="pXCord">X coordinate.</param>
/// <param name="pYCord">Y coordinate.</param>
/// <returns>Lowest weight.</returns>
private int GetAdjacentLowestCount(int pXCord, int pYCord)
{
if (lowestCell.Equals(Location.NullType))
return (int)LocationType.Wall;
else
return objGrid[lowestCell.X, lowestCell.Y];
}

/// <summary>
/// Checks if any of the adjacent cells of of the required type.
/// </summary>
/// <param name="pXCord">X coordinate.</param>
/// <param name="pYCord">Y coordinate.</param>
/// <param name="pType">Type to check for.</param>
/// <returns>Boolean result of the check.</returns>
private bool IsAdjacentLocOfType(int pXCord, int pYCord, LocationType pType)
{
if (pXCord < objGrid.GetLength(0) - 1)
{
if (objGrid[pXCord + 1, pYCord] == (int)pType)
{
return true;
}

if (pXCord - 1 >= 0)
{
if (objGrid[pXCord - 1, pYCord] == (int)pType)
{
return true;
}
}
}

if (pYCord < objGrid.GetLength(1) - 1)
{
if (objGrid[pXCord, pYCord + 1] == (int)pType)
{
return true;
}

if (pYCord - 1 >= 0)
{
if (objGrid[pXCord, pYCord - 1] == (int)pType)
{
return true;
}
}
}

return false;
}

}

}

``````

[line]
Sample usage:

``````using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace WavefromtMotion
{
class Program
{
static void Main(string[] args)
{
int[,] map= new int[,] {{0,0,0,255,254,0},
{0,0,0,0,0,0},
{0,0,0,255,255,0},
{0,255,0,255,0,0},
{0,0,0,0,0,0},
{1,255,0,0,0,0}};

var objfront = new Wavefront(map);
objfront.GeneratePathWeights();
Wavefront.Location[] path = objfront.GetPath();
foreach (Wavefront.Location lc in path)
{
map[lc.X,lc.Y] = -1;
}

String disp = String.Empty;
for (int x1 = 0; x1 < map.GetLength(0); x1++)
{
for (int y1 = 0; y1 < map.GetLength(1); y1++)
{
if (map[x1, y1] == (int)Wavefront.LocationType.Source)
disp = "S";
else if (map[x1, y1] == (int)Wavefront.LocationType.Wall)
disp = "X";
else if (map[x1, y1] == (int)Wavefront.LocationType.Destination)
disp = "D";
else if (map[x1, y1] == -1)
disp = "-";
else
disp = map[x1, y1].ToString();
Console.Write(disp + "\t");
}
Console.WriteLine();
}

}
}
}

``````

This algorithm can further be updated to use adapting routing (In case a new object is encountered on the floor), by scanning the environment at every cell and if a difference is found, updating the grid and re generating the path weights.

If anyone knows of a better algorithm, I would like to try it out

#2

I would put more comments in the code and post it on Fezzer.com ;D

Gus, may be we should have “algorithm” tag on Fezzer for code like this one.

#3

FEZ Masters can now create tags.

#4

It would be great thought o have an algorithm tag. I see a lot of fezzers doing some really cool work on stuff like PID controls, Tilt compensation or Encryption, it will be great if we could put them together.

#5

Thanks Josh.

I didn’t know that. But I have noticed that there are much more tags than it is used to be.

#6

Speaking of tags, please order the tags, it’s hard to find what you are looking for (or not if you use ctrl-f)

#7

#8

Tanu,

Have you tried running the code under Micro Framework? I thought that doubly dimensioned arrays weren’t allowed.

I guess I can answer my own question … “using System.Linq” tells me that it isn’t running under Micro Framework.

#9

AI,
I have not tried it on .NET Micro yet. I agree that micro does not support rectangular arrays but it can be changed to Jagged arrays which are supported.