fix: arm_obstacle_navigation calc_heuristic_map node through 4 corners error (AtsushiSakai#1034)

* fix: arm_obstacle_navigation calc_heuristic_map node through 4 corners error

* style: remove trailing whitespace

Author: Roger-Sh

ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py

@@ -203,10 +203,10 @@ def calc_heuristic_map(M, goal_node):
     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]
+                                      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]
                                       )
 
     return heuristic_map

ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py

@@ -105,7 +105,7 @@ def get_occupancy_grid(arm, obstacles):
     Args:
         arm: An instance of NLinkArm
         obstacles: A list of obstacles, with each obstacle defined as a list
-                   of xy coordinates and a radius. 
+                   of xy coordinates and a radius.
 
     Returns:
         Occupancy grid in joint space
@@ -234,10 +234,10 @@ def calc_heuristic_map(M, goal_node):
     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]
+                                      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]
                                       )
 
     return heuristic_map