cu-rrt-star: anytime RRT* planner component, with a closed-loop example - #1247
Open
Scofield626 wants to merge 11 commits into
Open
cu-rrt-star: anytime RRT* planner component, with a closed-loop example#1247Scofield626 wants to merge 11 commits into
Scofield626 wants to merge 11 commits into
Conversation
Same seeded RRT* task on two nodes: a quick policy (2 quanta, quality target) and a thorough one (24 quanta, stall bound). base() publishes the first path, each refine() runs one block of iterations and republishes only a shorter one.
write_path shortcuts the tree path instead of dropping its tail, so every published segment is collision free; a path that still does not fit is not published at all. gamma: 0.0 derives the rewiring constant from the free area, which lifts the thorough planner from q=0.90 to q=0.96. The example's self-check now runs as a test, and a planner node exposes a small debug state instead of its whole tree.
|
Hi! Thanks for opening this pull request. Because this is your first time contributing to this repository, please read our contributor guide: |
Collaborator
|
Can we have a proper copper component for it? and add an example using it? |
Scofield626
changed the title
gbin
requested changes
Aug 4, 2026
| )] | ||
| pub struct Obstacle { | ||
| pub center: Point2, | ||
| pub radius: f32, |
| Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect, | ||
| )] | ||
| pub struct Point2 { | ||
| pub x: f32, |
| #[derive( | ||
| Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect, | ||
| )] | ||
| pub struct Point2 { |
Collaborator
There was a problem hiding this comment.
it cannot be unified with what we have as a standard struct?
| } | ||
|
|
||
| impl Obstacle { | ||
| pub const fn new(center: Point2, radius: f32) -> Self { |
Collaborator
There was a problem hiding this comment.
is the algorithm mandate round obstacles?
| /// carries the one it must solve. | ||
| #[derive(Default, Debug, Clone, Encode, Decode, Serialize, Deserialize, Reflect)] | ||
| pub struct World { | ||
| pub width: f32, |
| pub world: World, | ||
| pub start: Point2, | ||
| pub goal: Point2, | ||
| pub seed: u64, |
Collaborator
There was a problem hiding this comment.
what is going on here? even with the explanation for the love of my life I don't understand why any random seed would land in a live message.
| pub iterations: u32, | ||
| pub tree_size: u32, | ||
| /// Nodes on the best path before it is shortcut for publication. | ||
| pub tree_path_len: u32, |
| let mut waypoints = [Point2::default(); MAX_WAYPOINTS]; | ||
| // A path too long to represent is not published: the output keeps | ||
| // the last valid one and a later quantum tries again. | ||
| if let Some(len) = planner.write_path(&mut waypoints) { |
Collaborator
There was a problem hiding this comment.
maybe: update the next output in place and just copy it last minute instead of a custom copy here ? (might save some copy time, do it only if it does)
| /// The fields are read through `Reflect`, which the compiler cannot see. | ||
| #[allow(dead_code)] | ||
| #[derive(Default, Debug, Reflect)] | ||
| pub struct PlannerDebugState { |
Collaborator
There was a problem hiding this comment.
is this for only when the debug feature is active?
|
|
||
| ### Input and output | ||
|
|
||
| - Input: `cu_rrt_star::PlanRequest` — the world (up to `MAX_OBSTACLES` (16) |
Collaborator
There was a problem hiding this comment.
templatign it to make it 2d and 3d would be difficult?
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
An RRT* planner (Karaman and Frazzoli 2011, with the branch-and-bound pruning of the anytime RRT* from "Anytime ROS2: Timely Task Completion in Non-preemptible Robotic Systems", RTAS 2026) as the first real algorithm on
CuAnytimeTask.components/tasks/cu_rrt_star— the component.base()grows a tree until a first path; eachrefine()runs one block of iterations and republishes only a shorter path. Every published path is drivable as published. Quality is straight-line distance over path cost, so aquality_targetis portable between maps. Remote debug reads a small projection, not the tree.components/tasks/cu_rrt_star/tests— one application running the same planner under a quick (max_refines: 2,quality_target: 0.93) and a thorough (max_refines: 24,max_stall: 4) policy on the same seed, so the thorough path can never be longer.examples/cu_anytime_rrt_star— closed-loop navigation: a simulated robot patrols the depot map, replans from its live pose every copperlist, drawn in Rerun.11 tests cover the planner geometry, determinism, the anytime contract, and the dual-policy application run.