github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
* fix: set rrt_star to find the shortest path

* fix: code style white space
This commit is contained in:
Yangshen⚡Deng
2023-05-16 20:43:57 +08:00
committed by GitHub
parent dd6d046bf9
commit 874ebff495
1 changed files with 7 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
PathPlanning/RRTStar/rrt_star.py
View File

@@ -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