From 35b080095a4fc50b0d584625afbb778547208219 Mon Sep 17 00:00:00 2001 From: blessedefe4real Date: Thu, 5 Nov 2026 20:19:00 +0000 Subject: [PATCH 1/2] Draw RRT obstacles as data-coordinate circles (#1396) Obstacles were drawn with plt.plot(ox, oy, "ok", ms=30 * size). Marker size is in points, so the circles did not match the obstacle radius used for collision checking and changed apparent size when the figure was resized or made fullscreen. Draw each obstacle with plt.Circle((ox, oy), size) instead, so it is sized in data units and matches the collision radius at any figure size. Applies to RRT Dubins, RRT* Dubins, RRT* Reeds-Shepp, Informed RRT*, Batch Informed RRT* and LQR-RRT*. --- PathPlanning/BatchInformedRRTStar/batch_informed_rrt_star.py | 2 +- PathPlanning/InformedRRTStar/informed_rrt_star.py | 2 +- PathPlanning/LQRRRTStar/lqr_rrt_star.py | 2 +- PathPlanning/RRTDubins/rrt_dubins.py | 2 +- PathPlanning/RRTStarDubins/rrt_star_dubins.py | 2 +- PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrt_star.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrt_star.py index 92c3a58761..ae39ea7331 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrt_star.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrt_star.py @@ -579,7 +579,7 @@ def draw_graph(self, xCenter=None, cBest=None, cMin=None, eTheta=None, plt.plot([start[0], start[1]], [end[0], end[1]], "-g") for (ox, oy, size) in self.obstacleList: - plt.plot(ox, oy, "ok", ms=30 * size) + plt.gca().add_patch(plt.Circle((ox, oy), size, color="k")) plt.plot(self.start[0], self.start[1], "xr") plt.plot(self.goal[0], self.goal[1], "xr") diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index 0483949c99..2ba305c3e5 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -289,7 +289,7 @@ def draw_graph(self, x_center=None, c_best=None, c_min=None, e_theta=None, [node.y, self.node_list[node.parent].y], "-g") for (ox, oy, size) in self.obstacle_list: - plt.plot(ox, oy, "ok", ms=30 * size) + plt.gca().add_patch(plt.Circle((ox, oy), size, color="k")) plt.plot(self.start.x, self.start.y, "xr") plt.plot(self.goal.x, self.goal.y, "xr") diff --git a/PathPlanning/LQRRRTStar/lqr_rrt_star.py b/PathPlanning/LQRRRTStar/lqr_rrt_star.py index 0ed08123ea..58f4cff0d7 100644 --- a/PathPlanning/LQRRRTStar/lqr_rrt_star.py +++ b/PathPlanning/LQRRRTStar/lqr_rrt_star.py @@ -110,7 +110,7 @@ def draw_graph(self, rnd=None): plt.plot(node.path_x, node.path_y, "-g") for (ox, oy, size) in self.obstacle_list: - plt.plot(ox, oy, "ok", ms=30 * size) + plt.gca().add_patch(plt.Circle((ox, oy), size, color="k")) plt.plot(self.start.x, self.start.y, "xr") plt.plot(self.end.x, self.end.y, "xr") diff --git a/PathPlanning/RRTDubins/rrt_dubins.py b/PathPlanning/RRTDubins/rrt_dubins.py index f938419f35..c4cd284173 100644 --- a/PathPlanning/RRTDubins/rrt_dubins.py +++ b/PathPlanning/RRTDubins/rrt_dubins.py @@ -114,7 +114,7 @@ def draw_graph(self, rnd=None): # pragma: no cover plt.plot(node.path_x, node.path_y, "-g") for (ox, oy, size) in self.obstacle_list: - plt.plot(ox, oy, "ok", ms=30 * size) + plt.gca().add_patch(plt.Circle((ox, oy), size, color="k")) plt.plot(self.start.x, self.start.y, "xr") plt.plot(self.end.x, self.end.y, "xr") diff --git a/PathPlanning/RRTStarDubins/rrt_star_dubins.py b/PathPlanning/RRTStarDubins/rrt_star_dubins.py index 7c52879b7c..056c2c4cde 100644 --- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py +++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py @@ -119,7 +119,7 @@ def draw_graph(self, rnd=None): plt.plot(node.path_x, node.path_y, "-g") for (ox, oy, size) in self.obstacle_list: - plt.plot(ox, oy, "ok", ms=30 * size) + plt.gca().add_patch(plt.Circle((ox, oy), size, color="k")) plt.plot(self.start.x, self.start.y, "xr") plt.plot(self.end.x, self.end.y, "xr") diff --git a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py index c4c3e7c8a8..7d1ea23d19 100644 --- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py +++ b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py @@ -133,7 +133,7 @@ def draw_graph(self, rnd=None): plt.plot(node.path_x, node.path_y, "-g") for (ox, oy, size) in self.obstacle_list: - plt.plot(ox, oy, "ok", ms=30 * size) + plt.gca().add_patch(plt.Circle((ox, oy), size, color="k")) plt.plot(self.start.x, self.start.y, "xr") plt.plot(self.end.x, self.end.y, "xr") From ef891058868a3ce00d35ec1717ea58729c31e6e4 Mon Sep 17 00:00:00 2001 From: blessedefe4real Date: Thu, 5 Nov 2026 20:41:30 +0000 Subject: [PATCH 2/2] Add regression tests for obstacle drawing radius (#1396) Each affected planner's draw_graph must draw one Circle per obstacle with the obstacle's center and collision radius. These tests fail with the previous marker-based drawing. --- tests/test_batch_informed_rrt_star.py | 17 +++++++++++++++++ tests/test_informed_rrt_star.py | 18 ++++++++++++++++++ tests/test_lqr_rrt_star.py | 18 ++++++++++++++++++ tests/test_rrt_dubins.py | 18 ++++++++++++++++++ tests/test_rrt_star_dubins.py | 18 ++++++++++++++++++ tests/test_rrt_star_reeds_shepp.py | 18 ++++++++++++++++++ 6 files changed, 107 insertions(+) diff --git a/tests/test_batch_informed_rrt_star.py b/tests/test_batch_informed_rrt_star.py index cf0a9827a8..7bbf8dacdf 100644 --- a/tests/test_batch_informed_rrt_star.py +++ b/tests/test_batch_informed_rrt_star.py @@ -10,5 +10,22 @@ def test_1(): m.main(maxIter=10) +def test_obstacles_are_drawn_with_their_radius_in_data_units(monkeypatch): + # Obstacles must be drawn as circles whose radius matches the + # collision radius, independent of the figure size (issue #1396). + monkeypatch.setattr(m.plt, "pause", lambda interval: None) + obstacle_list = [(5.0, 5.0, 1.0), (3.0, 6.0, 2.0)] + planner = m.BITStar(start=[0.0, 0.0], goal=[10.0, 10.0], obstacleList=obstacle_list, randArea=[-2.0, 15.0]) + + m.plt.figure(figsize=(12, 12)) + planner.draw_graph(samples=[]) + + circles = [patch for patch in m.plt.gca().patches + if isinstance(patch, m.plt.Circle)] + m.plt.close("all") + assert [(circle.center, circle.radius) for circle in circles] == [ + ((ox, oy), size) for (ox, oy, size) in obstacle_list] + + if __name__ == '__main__': conftest.run_this_test(__file__) diff --git a/tests/test_informed_rrt_star.py b/tests/test_informed_rrt_star.py index 10974ecfcb..f1373d9ade 100644 --- a/tests/test_informed_rrt_star.py +++ b/tests/test_informed_rrt_star.py @@ -7,5 +7,23 @@ def test1(): m.main() +def test_obstacles_are_drawn_with_their_radius_in_data_units(monkeypatch): + # Obstacles must be drawn as circles whose radius matches the + # collision radius, independent of the figure size (issue #1396). + monkeypatch.setattr(m.plt, "pause", lambda interval: None) + obstacle_list = [(5.0, 5.0, 1.0), (3.0, 6.0, 2.0)] + planner = m.InformedRRTStar([0.0, 0.0], [10.0, 10.0], obstacle_list, [-2.0, 15.0]) + planner.node_list = [planner.start] + + m.plt.figure(figsize=(12, 12)) + planner.draw_graph() + + circles = [patch for patch in m.plt.gca().patches + if isinstance(patch, m.plt.Circle)] + m.plt.close("all") + assert [(circle.center, circle.radius) for circle in circles] == [ + ((ox, oy), size) for (ox, oy, size) in obstacle_list] + + if __name__ == '__main__': conftest.run_this_test(__file__) diff --git a/tests/test_lqr_rrt_star.py b/tests/test_lqr_rrt_star.py index 444b4616b8..36c56434f3 100644 --- a/tests/test_lqr_rrt_star.py +++ b/tests/test_lqr_rrt_star.py @@ -10,5 +10,23 @@ def test1(): m.main(maxIter=5) +def test_obstacles_are_drawn_with_their_radius_in_data_units(monkeypatch): + # Obstacles must be drawn as circles whose radius matches the + # collision radius, independent of the figure size (issue #1396). + monkeypatch.setattr(m.plt, "pause", lambda interval: None) + obstacle_list = [(5.0, 5.0, 1.0), (3.0, 6.0, 2.0)] + planner = m.LQRRRTStar([0.0, 0.0], [10.0, 10.0], obstacle_list, [-2.0, 15.0]) + planner.node_list = [planner.start] + + m.plt.figure(figsize=(12, 12)) + planner.draw_graph() + + circles = [patch for patch in m.plt.gca().patches + if isinstance(patch, m.plt.Circle)] + m.plt.close("all") + assert [(circle.center, circle.radius) for circle in circles] == [ + ((ox, oy), size) for (ox, oy, size) in obstacle_list] + + if __name__ == '__main__': conftest.run_this_test(__file__) diff --git a/tests/test_rrt_dubins.py b/tests/test_rrt_dubins.py index 66130484bc..0932baa8a4 100644 --- a/tests/test_rrt_dubins.py +++ b/tests/test_rrt_dubins.py @@ -7,5 +7,23 @@ def test1(): m.main() +def test_obstacles_are_drawn_with_their_radius_in_data_units(monkeypatch): + # Obstacles must be drawn as circles whose radius matches the + # collision radius, independent of the figure size (issue #1396). + monkeypatch.setattr(m.plt, "pause", lambda interval: None) + obstacle_list = [(5.0, 5.0, 1.0), (3.0, 6.0, 2.0)] + planner = m.RRTDubins([0.0, 0.0, 0.0], [10.0, 10.0, 0.0], obstacle_list, [-2.0, 15.0]) + planner.node_list = [planner.start] + + m.plt.figure(figsize=(12, 12)) + planner.draw_graph() + + circles = [patch for patch in m.plt.gca().patches + if isinstance(patch, m.plt.Circle)] + m.plt.close("all") + assert [(circle.center, circle.radius) for circle in circles] == [ + ((ox, oy), size) for (ox, oy, size) in obstacle_list] + + if __name__ == '__main__': conftest.run_this_test(__file__) diff --git a/tests/test_rrt_star_dubins.py b/tests/test_rrt_star_dubins.py index c55ca23e43..65f2dc5d2f 100644 --- a/tests/test_rrt_star_dubins.py +++ b/tests/test_rrt_star_dubins.py @@ -7,5 +7,23 @@ def test1(): m.main() +def test_obstacles_are_drawn_with_their_radius_in_data_units(monkeypatch): + # Obstacles must be drawn as circles whose radius matches the + # collision radius, independent of the figure size (issue #1396). + monkeypatch.setattr(m.plt, "pause", lambda interval: None) + obstacle_list = [(5.0, 5.0, 1.0), (3.0, 6.0, 2.0)] + planner = m.RRTStarDubins([0.0, 0.0, 0.0], [10.0, 10.0, 0.0], obstacle_list, [-2.0, 15.0]) + planner.node_list = [planner.start] + + m.plt.figure(figsize=(12, 12)) + planner.draw_graph() + + circles = [patch for patch in m.plt.gca().patches + if isinstance(patch, m.plt.Circle)] + m.plt.close("all") + assert [(circle.center, circle.radius) for circle in circles] == [ + ((ox, oy), size) for (ox, oy, size) in obstacle_list] + + if __name__ == '__main__': conftest.run_this_test(__file__) diff --git a/tests/test_rrt_star_reeds_shepp.py b/tests/test_rrt_star_reeds_shepp.py index 11157aa57a..06981e9aa9 100644 --- a/tests/test_rrt_star_reeds_shepp.py +++ b/tests/test_rrt_star_reeds_shepp.py @@ -42,5 +42,23 @@ def test_too_big_step_size(): assert path is None +def test_obstacles_are_drawn_with_their_radius_in_data_units(monkeypatch): + # Obstacles must be drawn as circles whose radius matches the + # collision radius, independent of the figure size (issue #1396). + monkeypatch.setattr(m.plt, "pause", lambda interval: None) + obstacle_list = [(5.0, 5.0, 1.0), (3.0, 6.0, 2.0)] + planner = m.RRTStarReedsShepp([0.0, 0.0, 0.0], [10.0, 10.0, 0.0], obstacle_list, [-2.0, 15.0]) + planner.node_list = [planner.start] + + m.plt.figure(figsize=(12, 12)) + planner.draw_graph() + + circles = [patch for patch in m.plt.gca().patches + if isinstance(patch, m.plt.Circle)] + m.plt.close("all") + assert [(circle.center, circle.radius) for circle in circles] == [ + ((ox, oy), size) for (ox, oy, size) in obstacle_list] + + if __name__ == '__main__': conftest.run_this_test(__file__)