Main Site Documentation

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)
                        {
                            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:


#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

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:


#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) :smiley:


#7

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


#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.