bug: walk back and forth at some enclosed case

[k88936]

thanks for your nice project, from which I have learned a lot.
but recently I found some bad case:

a case for 4-dir walk

a case for 8-dir walk

the scene setting:

    x_dim = 20
    y_dim = 20
    start = (3, 3)
    goal = (19, 19)

instead of crash through: assert (self.rhs[self.s_start] != float('inf')), "There is no known path!"
it fell into a endless loop of walk back and force between 2 points

but interestingly another project also has the problem:

and log:

our maintainer seems to noticed this long before:
https://github.com/Sollimann/Dstar-lite-pathplanner/blob/fc3f5cafea53072a750565fbca0642d1e6c0c8ec/README.md#known-bugs-please-contribute

and is it fixed in #3 ?

[k88936]

I found the cause , I will raise a pr later

[pumpzcc]

I found the cause , I will raise a pr later

Any idea how to fix this problem?

[k88936]

1. there is some error in argo implement for sure.: Fix algo implement error #15

2. walk back and force:
   that is because the g is not update correctly if we use the origin end cond of compute_shortest_path in the paper.(at least in this repo's code. i can not prove if it is a bug in paper) so in some situation. g is not equal to rhs after compute_shortest_path and some bad case will occur. for a real example I had encountered:
   the grids blew is to show the g cost, a 'w' means wall, 4dir move
   | w | 30 | w |
   | 30* | 29* | w |
   | 29 | w | w |
   of course the 29* is computed incorrectly, which should be 31. and from point 30* it looks for lowest neighbor: (maybe 29 or 29* but what if it choose) 29* then it goto 29*, it find the lowest neighbor: (maybe 30 or 30* but what if it choose) 30* . then it is trapped between the two grids.
   my temp bypass is iter till priority queue empty. but it will cause another infinite loop, see next:

3. infinite loop
   and the algo does not consider how to wait a enclosed space to be open again, it simply exits. the actual situation is the g and rhs value inside will try to increase via each other until they are greater than the cost of wall(inf) which is endless and impossible.
   just like: "Step on your right foot with your left foot and ascend into the sky." ( which is the idiom "左脚踩右脚" in zh.)
   my temp bypass is to limit max iter times of compute_shortest_path.

I modified a version also for other reason,( which maybe hard to merge )

https://github.com/k88936/Liangjian/blob/ad68e528788d1321ce2946231f498e5d702ecd15/algorithm/multi_robot/python_demo/d_star_lite.py