Wavefront Algorith for Robot Navigation (Autopilot)

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)
                        {
                            if (IsAdjacentLocOfType(x, y, LocationType.Source))
                            {
                                source = GetAdjacentLowestCell(x, y);
                                objGrid[x, y] = 2;
                                hitSource = true;
                            }
                            else
                            {
                                int adjCnt = GetAdjacentLowestCount(x, y);
                                if (adjCnt != (int)LocationType.Wall)
                                    objGrid[x, y] = adjCnt + 1;
                            }
                        }
                        else if (objGrid[x, y] == (int)LocationType.Destination)
                        {
                            destination = new Location(x, y);
                            int adjCnt = GetAdjacentLowestCount(x, y);
                            if (adjCnt != (int)LocationType.Wall)
                            {
                                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)
        {
            Location lowestCell = GetAdjacentLowestCell(pXCord, 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();
            }
            Console.ReadLine();

        }
    }
}


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 :slight_smile:

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.

FEZ Masters can now create tags.

I will add comments to the code soon.
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. :slight_smile:

Thanks Josh.

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

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

Also available on
[url]http://www.fezzer.com/project/224/wavefront-algorith-for-robot-navigation-/[/url]

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.

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.