From be082e68b65954f532afb21f799e846b76e34064 Mon Sep 17 00:00:00 2001 From: yunyv <62236062+yunyv@users.noreply.github.com> Date: Mon, 19 Oct 2026 17:59:50 -0600 Subject: [PATCH 1/2] Refactor theta_star.py for improved path planningFix ThetaStar grid allocation and bounds validation Updated planning method and collision check logic. Changed width calculations to use math.ceil for better accuracy. --- PathPlanning/ThetaStar/theta_star.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/PathPlanning/ThetaStar/theta_star.py b/PathPlanning/ThetaStar/theta_star.py index 73a1448b28..5ba620073e 100644 --- a/PathPlanning/ThetaStar/theta_star.py +++ b/PathPlanning/ThetaStar/theta_star.py @@ -48,7 +48,7 @@ def __str__(self): return str(self.x) + "," + str(self.y) + "," + str( self.cost) + "," + str(self.parent_index) - def planning(self, sx, sy, gx, gy): + def planning(self, sx, sy, gx, gy): """ Theta star path search @@ -155,7 +155,7 @@ def planning(self, sx, sy, gx, gy): else: if open_set[n_id].cost > node.cost: # This path is the best until now. record it - open_set[n_id] = node + open_set[n_id] = node rx, ry = self.calc_final_path(goal_node, closed_set) @@ -243,6 +243,10 @@ def verify_node(self, node): elif py >= self.max_y: return False + # Floating-point world bounds can admit an index past the grid. + if not (0 <= node.x < self.x_width and 0 <= node.y < self.y_width): + return False + # collision check if self.obstacle_map[node.x][node.y]: return False @@ -260,8 +264,8 @@ def calc_obstacle_map(self, ox, oy): print("max_x:", self.max_x) print("max_y:", self.max_y) - self.x_width = round((self.max_x - self.min_x) / self.resolution) - self.y_width = round((self.max_y - self.min_y) / self.resolution) + self.x_width = math.ceil((self.max_x - self.min_x) / self.resolution) + self.y_width = math.ceil((self.max_y - self.min_y) / self.resolution) print("x_width:", self.x_width) print("y_width:", self.y_width) @@ -343,3 +347,4 @@ def main(): if __name__ == '__main__': main() + From 73c1091963f9d4731c72f4dfcaed7afba68aef16 Mon Sep 17 00:00:00 2001 From: yunyv <62236062+yunyv@users.noreply.github.com> Date: Mon, 19 Oct 2026 18:02:42 -0600 Subject: [PATCH 2/2] Add ThetaStar grid-boundary regression tests Cover non-divisible extents, shifted origins, obstacle inflation, and floating-point grid boundaries. --- tests/test_theta_star.py | 51 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 50 insertions(+), 1 deletion(-) diff --git a/tests/test_theta_star.py b/tests/test_theta_star.py index 10ceae44e3..0d7c66fc6e 100644 --- a/tests/test_theta_star.py +++ b/tests/test_theta_star.py @@ -1,4 +1,7 @@ +import math + import conftest +import pytest from PathPlanning.ThetaStar import theta_star as m @@ -7,5 +10,51 @@ def test_1(): m.main() +@pytest.mark.parametrize("min_x,min_y,width,height,resolution", [ + (0, 0, 10, 10, 3), + (-5, -8, 10, 10, 3), + (7, 4, 10, 10, 3), + (0, 0, 10, 12, 3), + (0, 0, 12, 10, 3), + (0, 0, 10, 10, 4), + (0, 0, 10, 10, 1.5), + (0, 0, 10, 10, 2), +]) +def test_non_divisible_grid_extent(min_x, min_y, width, height, resolution): + m.show_animation = False + planner = m.ThetaStarPlanner( + [min_x, min_x + width], [min_y, min_y + height], resolution, 0.1) + expected_x = len([i for i in range(width + 1) if i * resolution < width]) + expected_y = len([i for i in range(height + 1) if i * resolution < height]) + assert planner.x_width == expected_x + assert planner.y_width == expected_y + last_x = min_x + (expected_x - 1) * resolution + last_y = min_y + (expected_y - 1) * resolution + inner_x = min_x + resolution + inner_y = min_y + resolution + for sx, sy, gx, gy in [(last_x, last_y, inner_x, inner_y), + (inner_x, inner_y, last_x, last_y)]: + rx, ry = planner.planning(sx, sy, gx, gy) + assert (rx[0], ry[0]) == (gx, gy) + assert (rx[-1], ry[-1]) == (sx, sy) + assert all(min_x <= x < min_x + width for x in rx) + assert all(min_y <= y < min_y + height for y in ry) + + +def test_last_grid_column_checks_inflated_obstacles(): + planner = m.ThetaStarPlanner([0, 10, 9], [0, 10, 6], 3, 3.1) + assert not planner.verify_node(planner.Node(3, 2, 0, -1)) + assert not planner.verify_node(planner.Node(3, 1, 0, -1)) + assert planner.verify_node(planner.Node(3, 0, 0, -1)) + + +@pytest.mark.parametrize("resolution", [0.2, math.nextafter(0.2, 0)]) +def test_grid_bounds_with_floating_point_resolution(resolution): + planner = m.ThetaStarPlanner([0, 1], [0, 1], resolution, 0.01) + for x, y in [(planner.x_width, 1), (1, planner.y_width), (-1, 1), (1, -1)]: + assert not planner.verify_node(planner.Node(x, y, 0, -1)) + assert planner.verify_node(planner.Node(4, 3, 0, -1)) + + if __name__ == '__main__': - conftest.run_this_test(__file__) \ No newline at end of file + conftest.run_this_test(__file__)