From 874ebff495b088e0aee48ad29e352548dee65e8d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Yangshen=E2=9A=A1Deng?= Date: Tue, 16 May 2023 20:43:57 +0800 Subject: [PATCH] fix: set rrt_star to find the shortest path (#834) * fix: set rrt_star to find the shortest path * fix: code style white space --- PathPlanning/RRTStar/rrt_star.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) 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