0
$\begingroup$

I currently try implementing the D* Lite Algorithm for path-planning (see also here) to get a grasp on it. I found two implementations on the web, both for C/C++, but somehow couldn't entirely follow the ideas since they seem to differ more than expected from the pseudo code in the whitepapers. I especially use the following two papers: Koening,S.;Likhachev,M. - D* Lite, 2002 Koenig & Likkachev, Fast replanning for Navigation in Unknown Terrain, IEEE Transactions on Robotics, Vol. 21, No. 3, June 2005

I tried implementing the optimized version of D* Lite from the first whitepaper (p.5,Fig.4) and for "debugging" I use the example as shown and explained in the second whitepaper (p.6,Fig.6 and Fig.7). All work is done in MatLab (easier for exchanging code with others).

So far I got the code running to find the initial shortest path by running computeShortestPath() once. But now I am stuck at lines {36''} and {37''} of the pseudo-code: 

{36”} Scan graph for changed edge costs;
{37”} if any edge costs changed

How do I detect those changes? I somehow don't seem to have a grasp on how this is being detected? In my implementation so far, I mainly used 3 matrices. One matrix of same size as the grid map containing all rhs-values. One matrix of same size containing similarly all g-values. And one matrix with variable row count for the priority queue with the first two columns being the priority keys and the third and fourth row being the x- and y-coordinates.

Comparing my results, I get the same result for the first run of computeShortestPath() in Step5 as seen in the second whitepaper, p.6 Fig.6. Moving the robot one step also isn't that a problem. But I really have no clue how the next step of scanning for changed edge costs should be implemented.

Thanks for any hint, advice and/or help!!!

Improve this question
$\endgroup$
1
  • $\begingroup$ While searching answers for the same question on 'Lifelong Planning A*' (LPA*) I found this Applet here: homepages.dcc.ufmg.br/~lhrios/applet_d_lite/index.html Playing around with it brought me to the idea: Does 'scanning for changed costs' mean just to locally compare the possible Successor Nodes with the ones in the known map (which can differ from the real map if we find a new, unknown obstacle)? $\endgroup$
    – EliteTUM
    Sep 19, 2012 at 11:34

1 Answer 1

Reset to default
1
$\begingroup$

In real-world code, you almost never have to "scan the graph for changes." Your graph only changes when you change it in the code, so you already know exactly when and where it can change!

One common way of implementing this is to have a OnGraphChanged callback in your Graph class, which can be setup to call the OnGraphChanged method in your PathFinder class. Then anywhere the graph changes in your Graph class, make sure the OnGraphChanged callback is called.

Improve this answer
$\endgroup$
6
  • $\begingroup$ Thanks for the hint, that's what I thought after "playing around" with the Applet I gave the link to above. Using MatLab in my current implementation, callbacks aren't any option. I implemented it now by having a "true map" and a "known map" and letting the robot scan it's environment after every move. For now I just let it "scan" by checking all possible successors and comparing the true successor with the one in the known map and change the known map if needed. $\endgroup$
    – EliteTUM
    Sep 19, 2012 at 16:35
  • $\begingroup$ @EliteTUM: Matlab supports callbacks. See mathworks.com/help/matlab/ref/functions.html $\endgroup$
    – BlueRaja - Danny Pflughoeft
    Sep 19, 2012 at 17:33
  • $\begingroup$ Oh ok, didn't know about that. For now I tried implementing it manually without callback. Unfortunately, I somehow get wrong results not knowing why. Maybe someone might care looking over my code? The errors must be somewhere in the functions scanChangedEdgeCost() und myDStarSearch() (Lines 131-163), but not sure what I am calculating wrong or better to say how to calculate the updated matrices correctly. See and Download Code at: snipt.org/vLgi8 $\endgroup$
    – EliteTUM
    Sep 19, 2012 at 20:37
  • $\begingroup$ @EliteTUM: This is exactly the case that unit-tests excel at :) Try writing some simple automated tests, and make sure the pathfinder returns the correct result in those cases. I'd like to point out, however, since you're scanning the entire graph anyways at each step, you are completely negating the speed advantages DLite gives you - you might as well just do a brute-force *(breadth-first search/djikstra's), and it would probably be faster! $\endgroup$
    – BlueRaja - Danny Pflughoeft
    Sep 19, 2012 at 23:00
  • $\begingroup$ Actually, I only scan the successor nodes of the current Node my robot moved to (this happens in scanChangedEdgeCost(...) ). AND ONLY if a difference between the so far known map and the true map within those successor nodes is detected, I replan. For now, I just want to implement the functionality. Later on I will try to move to an automated replaning - as you suggested - using callbacks. Actually, I've done quite a lot of unit testing allready. But looks like I'll have to re-read the papers on D*-Lite. $\endgroup$
    – EliteTUM
    Sep 20, 2012 at 11:50

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.