Bug report: D*

[2892510130]

Describe the bug
D* can't cope with a changed map: When I add new obstacles after the start node closes, the program gets stuck.

def run(self, start, end):
   
    rx = []
    ry = []

    self.insert(end, 0.0)

    while True:
        self.process_state()
        if start.t == "close":
            break

    start.set_state("s")
    s = start
    s = s.parent
    s.set_state("e")
    tmp = start

    # here I add new obstacles
    ox, oy = [], []
    for i in range(15, 21):
        ox.append(i)
        oy.append(40)
    self.map.set_obstacle([(i, j) for i, j in zip(ox, oy)])
    plt.pause(0.001)
    plt.plot(ox, oy, ".k")

    while tmp != end:
        tmp.set_state("*")
        rx.append(tmp.x)
        ry.append(tmp.y)
        if show_animation:
            plt.plot(rx, ry, "-r")
            plt.pause(0.01)
        if tmp.parent.state == "#":
            self.modify(tmp)
            continue
        tmp = tmp.parent
    tmp.set_state("e")

    return rx, ry

Expected behavior
D* Dynamically adjust the path.

Solution
change the 'elif' in process_state function to 'if'

def process_state(self):

      *****

      if k_old < x.h:
          for y in self.map.get_neighbors(x):
              if y.h <= k_old and x.h > y.h + x.cost(y):
                  x.parent = y
                  x.h = y.h + x.cost(y)
      if k_old == x.h: # change elif to if
          for y in self.map.get_neighbors(x):
              if y.t == "new" or y.parent == x and y.h != x.h + x.cost(y) \
                      or y.parent != x and y.h > x.h + x.cost(y):
                  y.parent = x
                  self.insert(y, x.h + x.cost(y))
                  if plot_y:
                      plt.pause(time_interval)
                      plt.plot(y.x, y.y, 'xg')

     ******

PS
I don't know whether this is the correct solution of this bug, in https://www.cs.cmu.edu/~motionplanning/lecture/AppH-astar-dstar_howie.pdf, page 36, it is 'if' rather than 'elif'.

[AtsushiSakai]

PR is welcome