lib/astar: handle inconsistent heuristics better

before pruning a node we've already visited, we need to check if it has
a better cost.

lib/astar.py

@@ -37,11 +37,13 @@ def search(start, is_goal, neighbors, heuristic=None):
             return cost_so_far, this, path
 
         for c, n in neighbors(this):
-            if n not in done:
-                # calculate the "reduced cost"
-                c = cost_so_far + c + heuristic(n)
+            c = cost_so_far + c
+            if n not in done or c < done[n][0]:
+                h = heuristic(n)
+                if h is None:
+                    continue
                 i += 1
-                heappush(q, (c, i, n, this))
+                heappush(q, (c+h, i, n, this))
 
     return float('inf'), None, []