Looking for pathfinding code

I 'm currently trying to build an autonomous robot which must be able to navigate inside an unknow environnement. I would like to implement some king of trajectory maker or “path finding” to determine the best way to reach one point from another based the current position and some detected obstacles.

I ve found several algorithm in .net like the one : Shortest Path Problem: Dijkstra's Algorithm - CodeProject but can’t find any specifcly written for NETMF.

Does anyone has ever seen one ?

Thanks

Pacman, welcome to the community!

I highly recommend you take Stanford’s free AI class if you’re interested in learning more about this type of problem.

https://www.ai-class.com/

hey i’ve finally been able to implemente a astar algorithm on my Panda2. It’s working well but the time needed to obtain the solution is too long…
I need to compute almost 2 sec to reach my final destination in an array of 5x5 point.

i’m thinking about RLP but it seems that i would only got 10kb of RAM form the algithm to run? Is that right? Even in C i’m afraid it won’t be enought.

Do you think this the limitatio of Panda and i should upgrade to an hydra, spider or even new cerberrus.

You can take a look at the source code right here : http://mlrobotic.codeplex.com/SourceControl/changeset/view/13680#146389 (sorry it’s in french)…

There’s almost always room for improvement in code. I’ll look at it tonight and see if I can suggest anything. However, A* is a processor heavy algorithm so you would definitely benefit with an upgrade to Cerberus or Hydra regardless.

Do not hesitate to send me your feeback !!!

I already reduce compute by 50% when i replaced the “foreach” by “for”

i made a benchmark proc which output the time to process the function

My record is 2 sec in debug mode and 1.5 sec in release mode with LCD to see the time

Will do. I’ll test it on a Hydra tonight and let you know the results. Can I run the code as is or is it depend on any hardware?

the part of code with the astar do not depend of any hardware but the whole project do…

I ran it on my Panda-II (72 Mhz) and my Hydra (200 Mhz). I expected Hydra to do better. I’m going to see if there’s anything in the code I can spot now.

:: Panda-II ::
00:00:02.1065968
00:00:02.1059733
00:00:02.1068813
00:00:02.1047164
00:00:02.1060948
00:00:02.1066918

:: Hydra ::
00:00:01.3550938
00:00:01.3488750
00:00:01.3450313
00:00:01.3545938
00:00:01.3536250
00:00:01.3532500
00:00:01.3494062
00:00:01.3485312

You have a lot of IF blocks in there checking for debug mode. I wrapped them in #if DEBUG conditions and ran a “release” build and got the time on the Hydra down to…

:: Hydra - added #if DEBUG
00:00:01.0918750
00:00:01.0846563
00:00:01.0883125
00:00:01.0861875
00:00:01.0875625

ex.

#if DEBUG
                             if (debug) Debug.Print("Next Vertex: " + next.Position.debugValue());
 #endif

Still looking…

I think you can help things more by eliminating ArrayList where possible. For example, I refactored your Vector class as below and the time is now:

:: Hydra - Refactored Vertex to use static buffer for adjacent points.
00:00:01.0166250
00:00:01.0151563
00:00:01.0130313
00:00:01.0097500
00:00:01.0132500

I’ve got to hit the sack but I think there are probably a few more places where you can eliminate ArrayLists as I’ve done here and trim off a little more. Your code is really clean. You’re going to have to get really creative to get it much faster I’m afraid. If you think you’ll go to Hydra at some point then I would focus on looking at ways you can use more memory and less shuffling since it has so much RAM. You might consider writing a custom Array classes that allocate blocks of items (say 10 at a time) instead of pushing & popping one at a time.

Hope this helps. I had fun! :slight_smile:


namespace MLRobotic.IA.PathFinding
{
    public class Vertex 
    {
        private const int MAX_ADJACENT_POINTS = 9;
        private readonly Point[] _adjBuffer = new Point[MAX_ADJACENT_POINTS];     // 9 is the max points that could be returned.

        /// <summary>
        /// Initializes new instance of Vertex
        /// </summary>

        /// <param name="position">The vertex location on the 2d container</param>
        /// <param name="walkable">Indicates whether the vertex can be visited (false if the vertex can not be)</param>
        public Vertex(Point position)
        {
            this.Position = position;
        }

        /// <summary>
        /// The vertex location on the two-dimensional container
        /// </summary>
        public Point Position
        {
            get;
            set;
        }

        /// <summary>
        /// Gets an array of the vertex's adjacent vertices
        /// </summary>
        public Point[] Adjacents
        {
            get
            {
                var currPoint = 0;
                for (var x = this.Position.X - 1; x <= this.Position.X + 1; x++)
                {
                    for (var y = this.Position.Y - 1; y <= this.Position.Y + 1; y++)
                    {
                        if ((x < 0) || (x >= AireDeJeu.longueur) || (y < 0) || (y >= AireDeJeu.largeur)) continue;
                        if ((((x == this.Position.X) && (y == this.Position.Y)))) continue;
                        //Debug.Print("Adj de: (" + this.Position.X + "," + this.Position.Y + ") sont (" + x + "," + y + ")");
                        _adjBuffer[currPoint++] = (new Point(x, y)); 
                    }
                }
                // Return the used items from buffer.
                var ret = new Point[currPoint];
                for (var n = 0; n < currPoint; n++)
                {
                    ret[n] = _adjBuffer[n];
                }
                return ret;
            }
        }

        /// <summary>
        /// Vertex parent dans le chemin
        /// </summary>
        public Vertex Parent;


        /// <summary>
        /// Longueur du chemin le plus court depuis le depart jusqu'à ce vertex
        /// </summary>
        public int G;



        /// <summary>
        /// Estimation de la distance du Vertex par rapport à l'arrivée
        /// </summary>
        public int H;

        public int CompareTo(Vertex obj)
        {
            return ((Vertex)obj).Position.CompareTo(this.Position);
        }
    }
}

Oh, yea. I forgot to mention. I ran your original code against the emulator (2.66Ghz Quad core PC) and I got these results. So, I doubt any substantial gains can be found w/o some really creative thinking :slight_smile:

:: Emulator (PC) ::
00:00:00.4070000
00:00:00.4120000
00:00:00.3810000
00:00:00.3820000

Wahou, thanks a lot you just saved me the buying of an Hydra…

I will try your code and the advice you gave to see it on my own panda.

What do you think about RLP? I guess i would have better performance right? Do i have access to all the RAM with RLP or is it even a smaller than in NETMF?

Thanks again.

I see some more optimization potential in the Vertex class above.

public Point Position
        {
            get;
            set;
        }

I think it would be faster as a variable and not a property.

public Point[] Adjacents
        {
            get
            {

On each get on the above property you do some computations. You should do some caching and recompute only when necessary (position changed?).

for (var x = this.Position.X - 1; x <= this.Position.X + 1; x++)
                {
                    for (var y = this.Position.Y - 1; y <= this.Position.Y + 1; y++)
                    {
                        if ((x < 0) || (x >= AireDeJeu.longueur) || (y < 0) || (y >= AireDeJeu.largeur)) continue;
                        if ((((x == this.Position.X) && (y == this.Position.Y)))) continue;
                    }
                }

could improve to:

for (var x = this.Position.X - 1; x <= this.Position.X + 1; x++)
                {
                    if ((x < 0) || (x >= AireDeJeu.longueur)) continue;
                    for (var y = this.Position.Y - 1; y <= this.Position.Y + 1; y++)
                    {
                        if ((y < 0) || (y >= AireDeJeu.largeur)) continue;
                        if ((((x == this.Position.X) && (y == this.Position.Y)))) continue;
                    }
                }

In this way, you make less comparisons since x changes only in the first for and y in the second one.

_adjBuffer[currPoint++] = (new Point(x, y)); 

you recreate a Point in a position every time you do a get on that property. If your memory allows it, create all the points on init and afterwards just set the X and the Y in the desired position or do the following:

if (_adjBuffer[currPoint] == null) _adjBuffer[currPoint++] = new Point(x,y);
else
{
 _adjBuffer[currPoint].X = x;
 _adjBuffer[currPoint++].Y = y;
}
// Return the used items from buffer.
                var ret = new Point[currPoint];
                for (var n = 0; n < currPoint; n++)
                {
                    ret[n] = _adjBuffer[n];
                }
                return ret;

It would be a lot more faster to return _adjBuffer than recreating an array with currPoint. Of course, you would need to know how many elements are used in the buffer. In this case you could do something like this:


_adjBuffer[currPoint] = null;
return _adjBuffer;

and when you iterate trough Adjacents, stop when you find a null.

thanks guys, give me the weekend to update the source code on codeplex and to run some test on all you improvements !!!

I ve just tested all your optimization. It really great. The benchmark on my Panda 2 is now at 1.05 sec par iteration !!!

Things is it’s just a pathfinding into a 5x5 square. My goal is to go 20x30 and under 0.5sec.

I think it’s not realistic i can go any further. My panda does have enought RAM to go throught a pathfinding of this size and i think the time on a Hydra will be over 5sec.

I last question is : Should i go RLP for this??? Or is it pointless to do this kind of calculation on this kind of board?

Thanks again.
FYI : i you want to see the robot i m working on, you can see the work in progress on the motor control part here : http://blog.mlrobotic.com/2012/02/asservissement-net/

Very nice robot! Sorry, I don’t speak French… What are you using to maintain direction? Is it a compass or is it just responding to gyros?

Have you checked in your changes to CodePlex? I’d like to try it again on Hydra tonight. You will definitely get much better performance using RLP/RLPLite.

To maintain direction we use encoder on our weel. The video you saw was a test to see if the robot is able to keep is position when you push it away. The main goal si to be able to do some straight line in a close environnement where you can’t use gyro or compas because it s not accurate enought. The thing is the robot has to be autonomous…

I just checked in everything on codeplex, you can take a look.

But how much RAM do i dispose on RLP / RLP lite? all of it?

Ah… I see. It appears to be responding very well. I’ll look at the new code tonight.

If you’re following the other threads then you’ll know I’m just in the past few days making my first attempt at RLP/RLPLite. So, I’m not the person to tell anything on the subject yet :frowning: I do know that on the Hydra RLPLite is allotted 1MB of memory. I’m not sure about Panda-II.

On Panda II RLP reserves 10KB of memory. Your loaded binaries cannot be larger than that.