Bug in ArmNavigation/arm_obstacle_navigation def calc_heuristic_map(M, goal_node)

[Roger-Sh]

In def calc_heuristic_map(M, goal_node), the current code appears to calculate the cost from every node through 4 corners to the goal node and tries to find its min value.

for i in range(heuristic_map.shape[0]):
    for j in range(heuristic_map.shape[1]):
        heuristic_map[i, j] = min(heuristic_map[i, j],
                                  i + 1 + heuristic_map[M - 1, j],
                                  M - i + heuristic_map[0, j],
                                  j + 1 + heuristic_map[i, M - 1],
                                  M - j + heuristic_map[i, 0]
                                  )

The right calculation should be like this:

for i in range(heuristic_map.shape[0]):
    for j in range(heuristic_map.shape[1]):
        heuristic_map[i, j] = min(heuristic_map[i, j],
                                  M - i - 1 + heuristic_map[M - 1, j],
                                  i + heuristic_map[0, j],
                                  M - j - 1 + heuristic_map[i, M - 1],
                                  j + heuristic_map[i, 0]
                                  )

[AtsushiSakai]

PR is welcome