diff --git a/PathPlanning/RRTStar/rrt_star.py b/PathPlanning/RRTStar/rrt_star.py
index b2884fe2..dcb1a066 100644
--- a/PathPlanning/RRTStar/rrt_star.py
+++ b/PathPlanning/RRTStar/rrt_star.py
@@ -163,9 +163,13 @@ class RRTStar(RRT):
         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