From e69e9d51205f56018ad6fb6783d4cc605b9829ec Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Thu, 29 Oct 2026 03:20:36 -0400 Subject: [PATCH 1/4] fix(informed-rrt): preserve distinct equidistant near nodes --- PathPlanning/InformedRRTStar/informed_rrt_star.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index 0483949c99..77c4891b3b 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -139,7 +139,7 @@ def find_near_nodes(self, new_node): r = 50.0 * math.sqrt(math.log(n_node) / n_node) d_list = [(node.x - new_node.x) ** 2 + (node.y - new_node.y) ** 2 for node in self.node_list] - near_inds = [d_list.index(i) for i in d_list if i <= r ** 2] + near_inds = [i for i, dist_sq in enumerate(d_list) if dist_sq <= r ** 2] return near_inds def informed_sample(self, c_max, c_min, x_center, c): From 0a5bbf8243ed74b00e904e1b90d503d9275b82cc Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Thu, 29 Oct 2026 03:20:52 -0400 Subject: [PATCH 2/4] test(informed-rrt): verify equidistant parent choice and vertex identity --- tests/test_informed_rrt_star.py | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/tests/test_informed_rrt_star.py b/tests/test_informed_rrt_star.py index 10974ecfcb..36fa2681eb 100644 --- a/tests/test_informed_rrt_star.py +++ b/tests/test_informed_rrt_star.py @@ -7,5 +7,38 @@ def test1(): m.main() + +def test_equidistant_neighbors_keep_distinct_indices_and_cheaper_parent(): + planner = m.InformedRRTStar( + start=[-1.0, 0.0], goal=[5.0, 0.0], + obstacle_list=[], rand_area=[-5.0, 5.0] + ) + left = m.Node(-1.0, 0.0) + right = m.Node(1.0, 0.0) + left.cost = 9.0 + right.cost = 1.0 + planner.node_list = [left, right] + + candidate = m.Node(0.0, 0.0) + near_indices = planner.find_near_nodes(candidate) + + assert near_indices == [0, 1] + chosen = planner.choose_parent(candidate, near_indices) + assert chosen.parent == 1 + assert chosen.cost == 2.0 + + +def test_near_nodes_preserve_equal_position_vertices(): + planner = m.InformedRRTStar( + start=[0.0, 0.0], goal=[5.0, 0.0], + obstacle_list=[], rand_area=[-5.0, 5.0] + ) + planner.node_list = [ + m.Node(1.0, 1.0), m.Node(1.0, 1.0), m.Node(100.0, 100.0) + ] + + assert planner.find_near_nodes(m.Node(0.0, 0.0)) == [0, 1] + + if __name__ == '__main__': conftest.run_this_test(__file__) From c66f42d984abb6dddd368a8b56516b130ca9539e Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Thu, 29 Oct 2026 03:21:07 -0400 Subject: [PATCH 3/4] style(tests): normalize Informed RRT* test spacing --- tests/test_informed_rrt_star.py | 1 - 1 file changed, 1 deletion(-) diff --git a/tests/test_informed_rrt_star.py b/tests/test_informed_rrt_star.py index 36fa2681eb..bd78b77144 100644 --- a/tests/test_informed_rrt_star.py +++ b/tests/test_informed_rrt_star.py @@ -7,7 +7,6 @@ def test1(): m.main() - def test_equidistant_neighbors_keep_distinct_indices_and_cheaper_parent(): planner = m.InformedRRTStar( start=[-1.0, 0.0], goal=[5.0, 0.0], From 8c364e7b9afef7cf4bf5447ad56f670d20bff187 Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Thu, 29 Oct 2026 03:21:32 -0400 Subject: [PATCH 4/4] style(informed-rrt): format neighborhood index comprehension --- PathPlanning/InformedRRTStar/informed_rrt_star.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index 77c4891b3b..fb15445f96 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -139,7 +139,9 @@ def find_near_nodes(self, new_node): r = 50.0 * math.sqrt(math.log(n_node) / n_node) d_list = [(node.x - new_node.x) ** 2 + (node.y - new_node.y) ** 2 for node in self.node_list] - near_inds = [i for i, dist_sq in enumerate(d_list) if dist_sq <= r ** 2] + near_inds = [ + i for i, dist_sq in enumerate(d_list) if dist_sq <= r ** 2 + ] return near_inds def informed_sample(self, c_max, c_min, x_center, c):