From bf7ed06786a4660c8e85c87fa049ce70427efd46 Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Mon, 12 Oct 2026 18:46:23 +0800 Subject: [PATCH 1/3] fix(dijkstra): index grid cells without subtracting world origin --- PathPlanning/Dijkstra/dijkstra.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/Dijkstra/dijkstra.py b/PathPlanning/Dijkstra/dijkstra.py index f5a4703910..2fee6e928d 100644 --- a/PathPlanning/Dijkstra/dijkstra.py +++ b/PathPlanning/Dijkstra/dijkstra.py @@ -143,7 +143,7 @@ def calc_xy_index(self, position, minp): return round((position - minp) / self.resolution) def calc_index(self, node): - return (node.y - self.min_y) * self.x_width + (node.x - self.min_x) + return node.y * self.x_width + node.x def verify_node(self, node): px = self.calc_position(node.x, self.min_x) From 452164c49f7c0585dcc7c838c1e164fba264cb68 Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Mon, 12 Oct 2026 18:46:37 +0800 Subject: [PATCH 2/3] test(dijkstra): cover grid indices with offset maps and non-unit resolutions --- tests/test_dijkstra.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/tests/test_dijkstra.py b/tests/test_dijkstra.py index 40404153d8..dce6c2a8ae 100644 --- a/tests/test_dijkstra.py +++ b/tests/test_dijkstra.py @@ -7,5 +7,23 @@ def test_1(): m.main() +def test_calc_index_uses_grid_coordinates(): + # Grid indices are relative to the map origin and measured in cells. + for min_x, min_y, resolution in ((-4.0, 3.0, 0.5), (6.0, -5.0, 2.0)): + ox = [min_x, min_x + 10.0, min_x, min_x + 10.0] + oy = [min_y, min_y, min_y + 10.0, min_y + 10.0] + planner = m.DijkstraPlanner(ox, oy, resolution, robot_radius=0.0) + + assert planner.calc_xy_index(min_x + 2 * resolution, min_x) == 2 + assert planner.calc_xy_index(min_y + 3 * resolution, min_y) == 3 + assert planner.calc_index(planner.Node(0, 0, 0.0, -1)) == 0 + assert planner.calc_index(planner.Node(2, 3, 0.0, -1)) == ( + 3 * planner.x_width + 2 + ) + assert planner.calc_index( + planner.Node(planner.x_width - 1, planner.y_width - 1, 0.0, -1) + ) == planner.x_width * planner.y_width - 1 + + if __name__ == '__main__': conftest.run_this_test(__file__) From 9e96bf4d2742e9fc8c6fe5d3e037b8365e0dc980 Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Wed, 28 Oct 2026 02:16:21 -0400 Subject: [PATCH 3/3] test(dijkstra): check start-node reconstruction at offset map origin --- tests/test_dijkstra.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/tests/test_dijkstra.py b/tests/test_dijkstra.py index dce6c2a8ae..4f6d3229e2 100644 --- a/tests/test_dijkstra.py +++ b/tests/test_dijkstra.py @@ -25,5 +25,20 @@ def test_calc_index_uses_grid_coordinates(): ) == planner.x_width * planner.y_width - 1 + +def test_planning_preserves_start_when_grid_key_would_be_negative_one(): + # The old index formula gave cell (0, 1) key -1, which is also the + # parent-chain sentinel, so path reconstruction stopped before the start. + m.show_animation = False + planner = m.DijkstraPlanner( + [1.0, 5.0, 1.0, 5.0], [1.0, 1.0, 5.0, 5.0], + 1.0, 0.0 + ) + + rx, ry = planner.planning(1.0, 2.0, 1.0, 3.0) + + assert list(zip(rx, ry)) == [(1.0, 3.0), (1.0, 2.0)] + + if __name__ == '__main__': conftest.run_this_test(__file__)