Pathfinding algorithm creating loops

Question

I've implemented the D*-Lite algorithm (here's a description, it's an algorithm for doing pathfinding when edge costs change over time), but I'm having problems with doing the edge cost updates. It works mostly, but sometimes it gets stuck in a loop, going back and forth between two vertices. I'm trying to create a test case which exhibits this behaviour, at the moment it happens for some cases when used in a large application, which makes it difficult to debug.

I'll get a test case up as soon as I can, but maybe someone can spot the error I've done going from pseudo to C++ right away.(There is a test case included below) The article presents an optimized version, Figure 4, which is the one I've implemented. The pseudocode is pasted below.

The source for my implementation is availible here.

If it helps, I'm using these types in my code:

struct VertexProperties { double x, y; };
typedef boost::adjacency_list<boost::vecS, 
                              boost::vecS, 
                              boost::undirectedS,
                              VertexProperties,
                              boost::property<boost::edge_weight_t, double> > Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
typedef DStarEuclidianHeuristic<Graph, Vertex> Heuristic; 
typedef DStarPathfinder<Graph, Heuristic> DStarPathfinder;

If any more information about usage is needed just ask, there's just too much to paste.

Pseudo code for D*-Lite:

procedure CalculateKey(s)
{01”} return [min(g(s), rhs(s)) + h(s_start, s) + km;min(g(s), rhs(s))];

procedure Initialize()
{02”} U = ∅;
{03”} km = 0;
{04”} for all s ∈ S rhs(s) = g(s) = ∞;
{05”} rhs(s_goal) = 0;
{06”} U.Insert(s_goal, [h(s_start, s_goal); 0]);

procedure UpdateVertex(u)
{07”} if (g(u) != rhs(u) AND u ∈ U) U.Update(u,CalculateKey(u));
{08”} else if (g(u) != rhs(u) AND u /∈ U) U.Insert(u,CalculateKey(u));
{09”} else if (g(u) = rhs(u) AND u ∈ U) U.Remove(u);

procedure ComputeShortestPath()
{10”} while (U.TopKey() < CalculateKey(s_start) OR rhs(s_start) > g(s_start))
{11”} u = U.Top();
{12”} k_old = U.TopKey();
{13”} k_new = CalculateKey(u));
{14”} if(k_old < k_new)
{15”}   U.Update(u, k_new);
{16”} else if (g(u) > rhs(u))
{17”}   g(u) = rhs(u);
{18”}   U.Remove(u);
{19”}   for all s ∈ Pred(u)
{20”}   if (s != s_goal) rhs(s) = min(rhs(s), c(s, u) + g(u));
{21”}   UpdateVertex(s);
{22”} else
{23”}   g_old = g(u);
{24”}   g(u) = ∞;
{25”}   for all s ∈ Pred(u) ∪ {u}
{26”}   if (rhs(s) = c(s, u) + g_old)
{27”}     if (s != s_goal) rhs(s) = min s'∈Succ(s)(c(s, s') + g(s'));
{28”}   UpdateVertex(s);

procedure Main()
{29”} s_last = s_start;
{30”} Initialize();
{31”} ComputeShortestPath();
{32”} while (s_start != s_goal)
{33”} /* if (g(s_start) = ∞) then there is no known path */
{34”}   s_start = argmin s'∈Succ(s_start)(c(s_start, s') + g(s'));
{35”}   Move to s_start;
{36”}   Scan graph for changed edge costs;
{37”}   if any edge costs changed
{38”}     km = km + h(s_last, s_start);
{39”}     s_last = s_start;
{40”}     for all directed edges (u, v) with changed edge costs
{41”}       c_old = c(u, v);
{42”}       Update the edge cost c(u, v);
{43”}       if (c_old > c(u, v))
{44”}         if (u != s_goal) rhs(u) = min(rhs(u), c(u, v) + g(v));
{45”}       else if (rhs(u) = c_old + g(v))
{46”}         if (u != s_goal) rhs(u) = min s'∈Succ(u)(c(u, s') + g(s'));
{47”}       UpdateVertex(u);
{48”}     ComputeShortestPath()

---

EDIT:

I've succeeded in creating a test-case which shows the erronous behaviour. Running this along with the code in the pastebin, it will hang up in the last get_path call, going back and forth between nodes 1 and 2. It seems to me it is because the node 3 is never touched, and so going that way has an infinite cost.

#include <cmath>
#include <boost/graph/adjacency_list.hpp>
#include "dstar_search.h"

template <typename Graph, typename Vertex>
struct DStarEuclidianHeuristic {
    DStarEuclidianHeuristic(const Graph& G_) : G(G_) {}

    double operator()(const Vertex& u, const Vertex& v) {
        double dx = G[u].x - G[v].x;
        double dy = G[u].y - G[v].y;
        double len = sqrt(dx*dx+dy*dy);
        return len;
    }

    const Graph& G;
};

struct VertexProp {
    double x, y;
};

int main() {
    typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
        VertexProp, boost::property<boost::edge_weight_t, double> > Graph;
    typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
    typedef boost::graph_traits<Graph>::edge_descriptor Edge;
    typedef DStarEuclidianHeuristic<Graph, Vertex> Heur;

    typedef boost::property_map<Graph, boost::edge_weight_t>::type WMap;

    Graph g(7);
    WMap weights = boost::get(boost::edge_weight, g);
    Edge e;
    // Create a graph
    e = boost::add_edge(0, 1, g).first;
    weights[e] = sqrt(2.);
    e = boost::add_edge(1, 2, g).first;
    weights[e] = 1;
    e = boost::add_edge(2, 3, g).first;
    weights[e] = 1;
    e = boost::add_edge(1, 4, g).first;
    weights[e] = 1;
    e = boost::add_edge(3, 4, g).first;
    weights[e] = 1;
    e = boost::add_edge(3, 5, g).first;
    weights[e] = sqrt(2.);
    e = boost::add_edge(2, 6, g).first;
    weights[e] = sqrt(2.);
    e = boost::add_edge(5, 6, g).first;
    weights[e] = 1;
    e = boost::add_edge(6, 7, g).first;
    weights[e] = 1;
    g[0].x = 1; g[0].y = 0;
    g[1].x = 0; g[1].y = 1;
    g[2].x = 0; g[2].y = 2;
    g[3].x = 1; g[3].y = 2;
    g[4].x = 1; g[4].y = 1;
    g[5].x = 2; g[5].y = 3;
    g[6].x = 1; g[6].y = 3;
    g[7].x = 1; g[7].y = 4;

    DStarPathfinder<Graph, Heur> dstar(g, Heur(g), 0, 7);
    std::list<std::pair<Edge, double>> changes;

    auto a =  dstar.get_path(); // Find the initial path, works well
    std::copy(a.begin(), a.end(), std::ostream_iterator<Vertex>(std::cout, ","));
    // Now change the cost of going from 2->6, and try to find a new path
    changes.push_back(std::make_pair(boost::edge(2, 6, g).first, 4.));
    dstar.update(changes);
    a = dstar.get_path(); // Stuck in loop
    std::copy(a.begin(), a.end(), std::ostream_iterator<Vertex>(std::cout, ","));

    return 0;
}

EDIT 2: More progress. If I replace the break condition in the while loop in ComputeShortestPath with just U != Ø (U is not empty), the path is found! It's quite slow though, since it always examines every node in the graph, which is not supposed to be neccessary. Also, since I use undirected graphs, I added some code to {40"} to update both u and v.

Answer 1

There are at least two problems with your code (not including the typenames I had to prepend to constructs like std::vector<TemplateParameter>::iterator in order to compile it).

1. You're using a non-admissible heuristic, since the diagonals cost 1 but have length √2. This prevents the second call to ComputeShortestPath from doing anything at all.

2. The update method of the heap you're using (which is private by convention to Boost and thus apparently undocumented) supports key decreases only. D* Lite needs key increases as well.

Answer 2

Unfortunately, posting pseudocode isn't really useful here since the pseudocode may be correct but the actual implementation may be at fault.

Generally, in path finding algorithms, if you're cycling between nodes then there is a really good chance that the algorithm is not removing visited nodes from the set of potential route nodes. This is usually done by setting a flag on the node as you traverse through it and reset the flag as you back step up the search tree.