fix: set rrt_star to find the shortest path (AtsushiSakai#834)

* fix: set rrt_star to find the shortest path

* fix: code style white space

Author: TKONIY

PathPlanning/RRTStar/rrt_star.py

@@ -163,9 +163,13 @@ def search_best_goal_node(self):
         if not safe_goal_inds:
             return None
 
-        min_cost = min([self.node_list[i].cost for i in safe_goal_inds])
-        for i in safe_goal_inds:
-            if self.node_list[i].cost == min_cost:
+        safe_goal_costs = [self.node_list[i].cost +
+                           self.calc_dist_to_goal(self.node_list[i].x, self.node_list[i].y)
+                           for i in safe_goal_inds]
+
+        min_cost = min(safe_goal_costs)
+        for i, cost in zip(safe_goal_inds, safe_goal_costs):
+            if cost == min_cost:
                 return i
 
         return None