fix: arm_obstacle_navigation calc_heuristic_map node through 4 corners error

[Roger-Sh]

What does this implement/fix?

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. However, the cost from current node to 4 corners of the map is wrong. This commit is meant to fix the calculation of the cost.