From 1bcf0fbee971a35a7221090d08126f3490b10b40 Mon Sep 17 00:00:00 2001 From: Scofield626 Date: Fri, 31 Jul 2026 11:24:20 +0000 Subject: [PATCH 1/7] example: anytime RRT* planner under two refinement policies 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. --- Cargo.toml | 1 + examples/cu_anytime_rrt_star/Cargo.toml | 22 + examples/cu_anytime_rrt_star/build.rs | 3 + examples/cu_anytime_rrt_star/copperconfig.ron | 75 ++ examples/cu_anytime_rrt_star/src/main.rs | 110 +++ examples/cu_anytime_rrt_star/src/rrt.rs | 654 ++++++++++++++++++ examples/cu_anytime_rrt_star/src/tasks.rs | 263 +++++++ 7 files changed, 1128 insertions(+) create mode 100644 examples/cu_anytime_rrt_star/Cargo.toml create mode 100644 examples/cu_anytime_rrt_star/build.rs create mode 100644 examples/cu_anytime_rrt_star/copperconfig.ron create mode 100644 examples/cu_anytime_rrt_star/src/main.rs create mode 100644 examples/cu_anytime_rrt_star/src/rrt.rs create mode 100644 examples/cu_anytime_rrt_star/src/tasks.rs diff --git a/Cargo.toml b/Cargo.toml index 5913ebdad42..02bab5a8409 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -76,6 +76,7 @@ members = [ "benchmarks/cu_async_cl_io_bench", "benchmarks/cu_dorabench", "benchmarks/cu_zenoh_bridge_bench", + "examples/cu_anytime_rrt_star", "examples/cu_background_task", "examples/cu_baremetal_safety", "examples/cu_bridge_test", diff --git a/examples/cu_anytime_rrt_star/Cargo.toml b/examples/cu_anytime_rrt_star/Cargo.toml new file mode 100644 index 00000000000..970ead8bfbb --- /dev/null +++ b/examples/cu_anytime_rrt_star/Cargo.toml @@ -0,0 +1,22 @@ +[package] +name = "cu-anytime-rrt-star" +description = "Example for the Copper project showing an anytime task: an RRT* planner that publishes a first path, then improves it quantum by quantum." +version.workspace = true +authors.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true +keywords.workspace = true +categories.workspace = true +homepage.workspace = true +repository.workspace = true + +publish = false + +[dependencies] +bincode = { workspace = true } +cu29 = { workspace = true } +serde = { workspace = true } + +[build-dependencies] +cu29-build = { workspace = true } diff --git a/examples/cu_anytime_rrt_star/build.rs b/examples/cu_anytime_rrt_star/build.rs new file mode 100644 index 00000000000..7cbac12abe5 --- /dev/null +++ b/examples/cu_anytime_rrt_star/build.rs @@ -0,0 +1,3 @@ +fn main() { + cu29_build::setup(); +} diff --git a/examples/cu_anytime_rrt_star/copperconfig.ron b/examples/cu_anytime_rrt_star/copperconfig.ron new file mode 100644 index 00000000000..eed9017964b --- /dev/null +++ b/examples/cu_anytime_rrt_star/copperconfig.ron @@ -0,0 +1,75 @@ +( + tasks: [ + ( + id: "goal", + type: "tasks::GoalSrc", + config: {"seed": 1}, + ), + ( + id: "quick_planner", + type: "tasks::RrtStarPlanner", + config: { + "base_iterations": 400, + "block_iterations": 256, + "step_size": 0.8, + "goal_bias": 0.05, + "goal_threshold": 0.5, + "gamma": 3.0, + "prune_interval": 512, + "max_nodes": 4000, + }, + anytime: ( + max_refines: 2, + time_budget_ms: 50.0, + quality_target: 0.85, + quality_floor: 0.05, + ), + ), + ( + id: "thorough_planner", + type: "tasks::RrtStarPlanner", + config: { + "base_iterations": 400, + "block_iterations": 256, + "step_size": 0.8, + "goal_bias": 0.05, + "goal_threshold": 0.5, + "gamma": 3.0, + "prune_interval": 512, + "max_nodes": 4000, + }, + anytime: ( + max_refines: 24, + time_budget_ms: 250.0, + max_stall: 4, + quality_floor: 0.05, + ), + ), + ( + id: "monitor", + type: "tasks::ComparisonSink", + ), + ], + cnx: [ + ( + src: "goal", + dst: "quick_planner", + msg: "crate::tasks::PlanRequest", + ), + ( + src: "goal", + dst: "thorough_planner", + msg: "crate::tasks::PlanRequest", + ), + ( + src: "quick_planner", + dst: "monitor", + msg: "crate::tasks::PlanPath", + ), + ( + src: "thorough_planner", + dst: "monitor", + msg: "crate::tasks::PlanPath", + ), + ], +) \ No newline at end of file diff --git a/examples/cu_anytime_rrt_star/src/main.rs b/examples/cu_anytime_rrt_star/src/main.rs new file mode 100644 index 00000000000..b245b1295b6 --- /dev/null +++ b/examples/cu_anytime_rrt_star/src/main.rs @@ -0,0 +1,110 @@ +//! Anytime RRT*: the same planner under two refinement policies. +//! +//! `base()` grows the tree until it has a first, crude path; every `refine()` +//! runs one more block of RRT* iterations and republishes only when the path +//! got shorter. The task reports how good the path is; the RON `anytime:` +//! policy decides how long to keep going. +//! +//! Both planner nodes are the same task type with the same RRT* `config:`, so +//! they run the same tree from the same seed. Only the policy differs: +//! +//! | node | policy | meaning | +//! |---|---|---| +//! | `quick_planner` | `max_refines: 2`, `time_budget_ms: 50`, `quality_target: 0.85` | two quanta at most, and stop early once the path is within 15% of the straight line | +//! | `thorough_planner` | `max_refines: 24`, `time_budget_ms: 250`, `max_stall: 4` | up to 24 quanta, but give up after 4 that improved nothing | +//! +//! Both carry `quality_floor: 0.05`, which drops a job that found no path at +//! all: the sink then sees no payload. +//! +//! Because both nodes start from the same seed, the thorough tree is the quick +//! tree plus more iterations, so its path is never longer - that is the anytime +//! trade-off, measured. Keeping the two `config:` blocks identical is what +//! makes the comparison below valid. + +mod rrt; +mod tasks; + +use cu29::prelude::*; +use std::fs; +use std::path::Path; + +#[copper_runtime(config = "copperconfig.ron")] +struct App {} + +const SLAB_SIZE: Option = Some(16 * 1024 * 1024); +const ITERATIONS: usize = 10; + +fn main() { + let logger_path = "logs/anytime_rrt_star.copper"; + if let Some(parent) = Path::new(logger_path).parent() + && !parent.exists() + { + fs::create_dir_all(parent).expect("Failed to create logs directory"); + } + let mut application = App::builder() + .with_log_path(logger_path, SLAB_SIZE) + .expect("Failed to setup logger.") + .build() + .expect("Failed to create application."); + application + .start_all_tasks() + .expect("Failed to start application."); + for _ in 0..ITERATIONS { + application + .run_one_iteration() + .expect("Failed to run application."); + } + application + .stop_all_tasks() + .expect("Failed to stop application."); + + let reports = tasks::REPORTS.lock().expect("reports poisoned"); + assert_eq!(reports.len(), ITERATIONS, "one report per copperlist"); + + let lower_bound = tasks::START.distance(tasks::GOAL); + println!("straight line start -> goal: {lower_bound:.2} m (quality 1.0)"); + println!("{:<5} {:>28} {:>28}", "job", "quick", "thorough"); + let mut compared = 0; + for (index, report) in reports.iter().enumerate() { + println!( + "{:<5} {:>28} {:>28}", + index, + describe(&report.quick, &report.quick_status, lower_bound), + describe(&report.thorough, &report.thorough_status, lower_bound), + ); + let (Some(quick), Some(thorough)) = (&report.quick, &report.thorough) else { + continue; + }; + compared += 1; + // More quanta on the same seed can only shorten the path. + assert!( + thorough.cost <= quick.cost + 1e-3, + "job {index}: the thorough policy published a longer path ({} vs {})", + thorough.cost, + quick.cost + ); + assert!( + thorough.cost >= lower_bound, + "job {index}: path shorter than the straight line" + ); + assert!(thorough.len >= 2, "job {index}: a path needs two waypoints"); + } + assert!( + compared >= ITERATIONS / 2, + "both planners published in only {compared} of {ITERATIONS} jobs" + ); + println!("anytime RRT* example OK: {compared}/{ITERATIONS} jobs compared"); +} + +/// One cell of the table: cost, quality and the runtime's anytime stamp. +fn describe(path: &Option, status: &str, lower_bound: f32) -> String { + match path { + Some(path) => format!( + "{:.2}m q={:.2} [{}]", + path.cost, + lower_bound / path.cost, + status + ), + None => format!("no path [{status}]"), + } +} diff --git a/examples/cu_anytime_rrt_star/src/rrt.rs b/examples/cu_anytime_rrt_star/src/rrt.rs new file mode 100644 index 00000000000..dc0fb5cd64e --- /dev/null +++ b/examples/cu_anytime_rrt_star/src/rrt.rs @@ -0,0 +1,654 @@ +//! Seeded RRT* over a 2D world of round obstacles. +//! +//! The planner knows nothing about Copper: it only exposes [`RrtStar::grow`], +//! one bounded block of iterations. `tasks.rs` calls it once from `base()` and +//! once per anytime refinement quantum. + +use bincode::{Decode, Encode}; +use cu29::prelude::*; +use serde::{Deserialize, Serialize}; + +/// Waypoints carried by a published path. Kept at 32 because serde derives +/// array impls up to that size. +pub const MAX_WAYPOINTS: usize = 32; + +/// A point of the planar world, in meters. +#[derive( + Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect, +)] +pub struct Point2 { + pub x: f32, + pub y: f32, +} + +impl Point2 { + pub const fn new(x: f32, y: f32) -> Self { + Self { x, y } + } + + /// Euclidean distance to `other`. + pub fn distance(self, other: Self) -> f32 { + let (dx, dy) = (self.x - other.x, self.y - other.y); + (dx * dx + dy * dy).sqrt() + } +} + +/// A round obstacle: the planner rejects any point or segment within `radius` +/// of `center`. +#[derive(Debug, Clone, Copy, Reflect)] +pub struct Obstacle { + pub center: Point2, + pub radius: f32, +} + +/// The rectangular world `0..width` x `0..height` and its obstacles. +#[derive(Debug, Clone, Reflect)] +pub struct World { + pub width: f32, + pub height: f32, + pub obstacles: Vec, +} + +impl World { + /// The map every planner node of the example runs on: a 10x10 m depot with + /// five pillars, placed so the straight line from start to goal is blocked. + /// A first path is therefore always a detour, and refinement has real work + /// to do. + pub fn depot() -> Self { + Self { + width: 10.0, + height: 10.0, + obstacles: vec![ + Obstacle { + center: Point2::new(3.0, 3.0), + radius: 1.2, + }, + Obstacle { + center: Point2::new(6.0, 6.0), + radius: 1.5, + }, + Obstacle { + center: Point2::new(7.0, 2.5), + radius: 1.0, + }, + Obstacle { + center: Point2::new(2.5, 7.0), + radius: 1.0, + }, + Obstacle { + center: Point2::new(5.0, 1.5), + radius: 0.8, + }, + ], + } + } + + /// True when `point` is inside the bounds and outside every obstacle. + pub fn is_free(&self, point: Point2) -> bool { + if point.x < 0.0 || point.y < 0.0 || point.x > self.width || point.y > self.height { + return false; + } + self.obstacles + .iter() + .all(|o| point.distance(o.center) > o.radius) + } + + /// True when the whole segment `a`-`b` is free. + pub fn is_free_segment(&self, a: Point2, b: Point2) -> bool { + if !self.is_free(a) || !self.is_free(b) { + return false; + } + self.obstacles + .iter() + .all(|o| distance_to_segment(a, b, o.center) > o.radius) + } +} + +/// Distance from `point` to the segment `a`-`b`. +fn distance_to_segment(a: Point2, b: Point2, point: Point2) -> f32 { + let (abx, aby) = (b.x - a.x, b.y - a.y); + let len_sq = abx * abx + aby * aby; + if len_sq <= f32::EPSILON { + return a.distance(point); + } + let t = (((point.x - a.x) * abx + (point.y - a.y) * aby) / len_sq).clamp(0.0, 1.0); + Point2::new(a.x + t * abx, a.y + t * aby).distance(point) +} + +/// Tuning knobs of the planner, all read from the node's RON `config:`. +#[derive(Debug, Clone, Copy, Reflect)] +pub struct RrtParams { + /// Longest edge the planner adds in one extension, in meters. + pub step_size: f32, + /// Probability of sampling the goal instead of a random point. + pub goal_bias: f32, + /// A node this close to the goal closes a path. + pub goal_threshold: f32, + /// Gamma of the RRT* rewiring radius `gamma * sqrt(ln n / n)`. + pub gamma: f32, + /// Branch-and-bound prune every N iterations; 0 disables pruning. + pub prune_interval: u32, + /// Hard cap on the tree size, so one job cannot grow without bound. + pub max_nodes: u32, +} + +impl Default for RrtParams { + fn default() -> Self { + Self { + step_size: 0.8, + goal_bias: 0.05, + goal_threshold: 0.5, + gamma: 3.0, + prune_interval: 512, + max_nodes: 4000, + } + } +} + +/// xorshift64*, so a given seed always replays the same tree. +#[derive(Debug, Clone, Reflect)] +pub struct Rng(u64); + +impl Rng { + pub fn new(seed: u64) -> Self { + // splitmix64 finalizer: consecutive seeds must not start on neighboring + // states, otherwise consecutive jobs explore almost the same tree. + let mut state = seed.wrapping_add(0x9E37_79B9_7F4A_7C15); + state = (state ^ (state >> 30)).wrapping_mul(0xBF58_476D_1CE4_E5B9); + state = (state ^ (state >> 27)).wrapping_mul(0x94D0_49BB_1331_11EB); + state ^= state >> 31; + // xorshift64* must never start at zero. + Self(if state == 0 { 1 } else { state }) + } + + fn next_u64(&mut self) -> u64 { + let mut x = self.0; + x ^= x >> 12; + x ^= x << 25; + x ^= x >> 27; + self.0 = x; + x.wrapping_mul(0x2545_F491_4F6C_DD1D) + } + + /// Uniform in `[0.0, 1.0)`. + pub fn next_f32(&mut self) -> f32 { + (self.next_u64() >> 40) as f32 / (1u32 << 24) as f32 + } +} + +/// One vertex of the tree. +#[derive(Debug, Clone, Reflect)] +struct TreeNode { + pos: Point2, + /// `None` for the root only. + parent: Option, + /// Path cost from the start to this node. + cost: f32, + children: Vec, +} + +/// An RRT* search for one start/goal pair. +/// +/// The tree only ever improves: `best_cost` is monotone non-increasing over +/// iterations, which is what makes the algorithm a good anytime task. +#[derive(Debug, Reflect)] +pub struct RrtStar { + world: World, + params: RrtParams, + start: Point2, + goal: Point2, + tree: Vec, + /// Node closing the best path found so far. + best_goal: Option, + /// Cost of the best path found so far, infinite until one is found. + best_cost: f32, + iterations: u32, + rng: Rng, + /// Reused between iterations to keep the search allocation-free. + scratch_near: Vec, + scratch_stack: Vec, +} + +impl RrtStar { + /// Starts a search rooted at `start`. An unreachable or blocked `start` + /// simply never grows a tree; the caller sees "no path" and the anytime + /// quality floor drops the result. + pub fn new(world: World, params: RrtParams, start: Point2, goal: Point2, seed: u64) -> Self { + let mut planner = Self { + world, + params, + start, + goal, + tree: Vec::new(), + best_goal: None, + best_cost: f32::INFINITY, + iterations: 0, + rng: Rng::new(seed), + scratch_near: Vec::new(), + scratch_stack: Vec::new(), + }; + planner.reset(start, goal, seed); + planner + } + + /// Restarts the search on a new problem, keeping the capacity the previous + /// job grew: after the first job the planner asks the allocator for much + /// less. + pub fn reset(&mut self, start: Point2, goal: Point2, seed: u64) { + self.start = start; + self.goal = goal; + self.tree.clear(); + self.tree.push(TreeNode { + pos: start, + parent: None, + cost: 0.0, + children: Vec::new(), + }); + self.best_goal = None; + self.best_cost = f32::INFINITY; + self.iterations = 0; + self.rng = Rng::new(seed); + } + + /// Runs one bounded block of `iterations` RRT* iterations. + pub fn grow(&mut self, iterations: u32) { + for _ in 0..iterations { + self.iterations += 1; + if self.tree.len() < self.params.max_nodes as usize { + self.step(); + } + if self.params.prune_interval > 0 + && self.iterations.is_multiple_of(self.params.prune_interval) + && self.best_goal.is_some() + { + self.prune(); + } + } + } + + /// Cost of the best path so far, infinite while no path is known. + pub fn best_cost(&self) -> f32 { + self.best_cost + } + + /// True once a path to the goal exists. + pub fn has_solution(&self) -> bool { + self.best_goal.is_some() + } + + pub fn tree_size(&self) -> u32 { + self.tree.len() as u32 + } + + pub fn iterations(&self) -> u32 { + self.iterations + } + + /// True when the tree is full and pruning can never free room again, so no + /// further iteration can change anything. + pub fn is_exhausted(&self) -> bool { + self.tree.len() >= self.params.max_nodes as usize + && (self.params.prune_interval == 0 || self.best_goal.is_none()) + } + + /// Shortest conceivable path: the straight line, obstacles ignored. + pub fn lower_bound(&self) -> f32 { + self.start.distance(self.goal) + } + + /// Normalized quality in `0.0..=1.0`: how close the best path is to the + /// straight-line lower bound. 0.0 means no path yet, 1.0 means the path is + /// as short as the world allows. + pub fn quality(&self) -> f32 { + if !self.has_solution() { + return 0.0; + } + (self.lower_bound() / self.best_cost).clamp(0.0, 1.0) + } + + /// Copies the best path into `out` and returns `(waypoints, truncated)`. + /// + /// A path longer than [`MAX_WAYPOINTS`] is cut after its head and still + /// ends on the goal; `truncated` says so, and the reported cost always + /// describes the whole path. + pub fn write_path(&self, out: &mut [Point2; MAX_WAYPOINTS]) -> (u32, bool) { + let Some(goal_node) = self.best_goal else { + return (0, false); + }; + let mut chain = Vec::new(); + let mut cursor = Some(goal_node); + while let Some(index) = cursor { + let node = &self.tree[index as usize]; + chain.push(node.pos); + cursor = node.parent; + } + chain.reverse(); + chain.push(self.goal); + + let truncated = chain.len() > MAX_WAYPOINTS; + let len = chain.len().min(MAX_WAYPOINTS); + out[..len].copy_from_slice(&chain[..len]); + if truncated { + out[len - 1] = self.goal; + } + (len as u32, truncated) + } + + /// One RRT* iteration: sample, steer, choose the cheapest parent, rewire + /// the neighborhood, then check whether the new node closes a better path. + fn step(&mut self) { + let sample = self.sample(); + let nearest = self.nearest(sample); + let from = self.tree[nearest as usize].pos; + let new_pos = steer(from, sample, self.params.step_size); + if !self.world.is_free_segment(from, new_pos) { + return; + } + + let radius = self.near_radius(); + let mut near = core::mem::take(&mut self.scratch_near); + near.clear(); + for (index, node) in self.tree.iter().enumerate() { + if node.pos.distance(new_pos) <= radius { + near.push(index as u32); + } + } + + // Choose the parent that gives the cheapest path to the new node. + let mut parent = nearest; + let mut cost = self.tree[nearest as usize].cost + from.distance(new_pos); + for &index in near.iter() { + let candidate = &self.tree[index as usize]; + let candidate_cost = candidate.cost + candidate.pos.distance(new_pos); + if candidate_cost < cost && self.world.is_free_segment(candidate.pos, new_pos) { + parent = index; + cost = candidate_cost; + } + } + + let new_index = self.tree.len() as u32; + self.tree.push(TreeNode { + pos: new_pos, + parent: Some(parent), + cost, + children: Vec::new(), + }); + self.tree[parent as usize].children.push(new_index); + + // Rewire: neighbors that are cheaper to reach through the new node. + for &index in near.iter() { + if index == parent { + continue; + } + let (neighbor_pos, neighbor_cost) = { + let neighbor = &self.tree[index as usize]; + (neighbor.pos, neighbor.cost) + }; + let rewired_cost = cost + neighbor_pos.distance(new_pos); + if rewired_cost < neighbor_cost + && !self.is_ancestor(index, new_index) + && self.world.is_free_segment(new_pos, neighbor_pos) + { + self.reparent(index, new_index, rewired_cost); + } + } + self.scratch_near = near; + + // Does the new node close a better path? + let to_goal = new_pos.distance(self.goal); + if to_goal <= self.params.goal_threshold + && self.world.is_free_segment(new_pos, self.goal) + && cost + to_goal < self.best_cost + { + self.best_cost = cost + to_goal; + self.best_goal = Some(new_index); + } + // Rewiring may have shortened the current best path too. + if let Some(goal_node) = self.best_goal { + let node = &self.tree[goal_node as usize]; + self.best_cost = self.best_cost.min(node.cost + node.pos.distance(self.goal)); + } + } + + /// A random point of the world, biased toward the goal. + fn sample(&mut self) -> Point2 { + if self.rng.next_f32() < self.params.goal_bias { + return self.goal; + } + Point2::new( + self.rng.next_f32() * self.world.width, + self.rng.next_f32() * self.world.height, + ) + } + + /// Index of the tree node closest to `point`. Linear on purpose: a real + /// planner would index the tree, but a flat scan keeps the example short. + fn nearest(&self, point: Point2) -> u32 { + let mut best = 0u32; + let mut best_distance = f32::INFINITY; + for (index, node) in self.tree.iter().enumerate() { + let distance = node.pos.distance(point); + if distance < best_distance { + best_distance = distance; + best = index as u32; + } + } + best + } + + /// RRT* rewiring radius `gamma * sqrt(ln n / n)`, capped at one step. + fn near_radius(&self) -> f32 { + let n = (self.tree.len() as f32).max(2.0); + (self.params.gamma * (n.ln() / n).sqrt()).min(self.params.step_size) + } + + /// True when `candidate` sits on the path from `node` up to the root. + /// + /// Rewiring an ancestor would turn the tree into a graph with a cycle, and + /// every walk over it would then loop forever. Exact arithmetic already + /// rules it out - reaching an ancestor through its own descendant is never + /// cheaper - but rounding on two nearly coincident samples must not be able + /// to break that. + fn is_ancestor(&self, candidate: u32, node: u32) -> bool { + let mut cursor = self.tree[node as usize].parent; + while let Some(index) = cursor { + if index == candidate { + return true; + } + cursor = self.tree[index as usize].parent; + } + false + } + + /// Moves `node` under `new_parent` and shifts the cost of its whole + /// subtree by the same delta. + fn reparent(&mut self, node: u32, new_parent: u32, new_cost: f32) { + if let Some(old_parent) = self.tree[node as usize].parent { + self.tree[old_parent as usize] + .children + .retain(|&child| child != node); + } + self.tree[node as usize].parent = Some(new_parent); + self.tree[new_parent as usize].children.push(node); + + let delta = new_cost - self.tree[node as usize].cost; + let mut stack = core::mem::take(&mut self.scratch_stack); + stack.clear(); + stack.push(node); + while let Some(index) = stack.pop() { + self.tree[index as usize].cost += delta; + for i in 0..self.tree[index as usize].children.len() { + stack.push(self.tree[index as usize].children[i]); + } + } + self.scratch_stack = stack; + } + + /// Branch and bound: drop every node that cannot belong to a path better + /// than the best one known. + /// + /// Walking down from the root keeps the tree consistent: a node is kept + /// only if its parent is kept, so no orphan survives the compaction. The + /// triangle inequality makes that almost free anyway - a kept node's parent + /// always satisfies the bound as well. + fn prune(&mut self) { + // The best path itself is protected: rounding must never let branch and + // bound drop the path it is bounding against. + let mut protected = vec![false; self.tree.len()]; + let mut cursor = self.best_goal; + while let Some(index) = cursor { + protected[index as usize] = true; + cursor = self.tree[index as usize].parent; + } + + let mut keep = vec![false; self.tree.len()]; + let mut stack = core::mem::take(&mut self.scratch_stack); + stack.clear(); + stack.push(0); + keep[0] = true; + while let Some(index) = stack.pop() { + for i in 0..self.tree[index as usize].children.len() { + let child = self.tree[index as usize].children[i]; + let node = &self.tree[child as usize]; + if protected[child as usize] + || node.cost + node.pos.distance(self.goal) <= self.best_cost + { + keep[child as usize] = true; + stack.push(child); + } + } + } + self.scratch_stack = stack; + + let mut remap = vec![u32::MAX; self.tree.len()]; + let mut kept = Vec::with_capacity(self.tree.len()); + for (index, node) in self.tree.iter().enumerate() { + if keep[index] { + remap[index] = kept.len() as u32; + kept.push(TreeNode { + pos: node.pos, + parent: node.parent, + cost: node.cost, + children: Vec::new(), + }); + } + } + for node in kept.iter_mut() { + node.parent = node.parent.map(|parent| remap[parent as usize]); + } + for index in 0..kept.len() { + if let Some(parent) = kept[index].parent { + kept[parent as usize].children.push(index as u32); + } + } + self.best_goal = self.best_goal.map(|goal| remap[goal as usize]); + self.tree = kept; + } +} + +/// Point at most `step_size` away from `from` in the direction of `to`. +fn steer(from: Point2, to: Point2, step_size: f32) -> Point2 { + let distance = from.distance(to); + if distance <= step_size { + return to; + } + let ratio = step_size / distance; + Point2::new( + from.x + ratio * (to.x - from.x), + from.y + ratio * (to.y - from.y), + ) +} + +#[cfg(test)] +mod tests { + use super::*; + + const START: Point2 = Point2::new(0.5, 0.5); + const GOAL: Point2 = Point2::new(9.5, 9.5); + + fn planner(seed: u64) -> RrtStar { + RrtStar::new(World::depot(), RrtParams::default(), START, GOAL, seed) + } + + #[test] + fn segment_collision_is_detected() { + let world = World::depot(); + // Straight through the pillar at (3, 3). + assert!(!world.is_free_segment(Point2::new(1.0, 1.0), Point2::new(5.0, 5.0))); + // Along the free bottom edge. + assert!(world.is_free_segment(Point2::new(0.2, 0.2), Point2::new(0.2, 9.8))); + // Endpoints out of bounds. + assert!(!world.is_free_segment(START, Point2::new(11.0, 0.5))); + } + + #[test] + fn refinement_only_improves_the_path() { + let mut planner = planner(42); + planner.grow(400); + assert!(planner.has_solution(), "no first path after the base block"); + + let mut previous = planner.best_cost(); + for _ in 0..16 { + planner.grow(256); + assert!( + planner.best_cost() <= previous + 1e-4, + "cost went up: {} then {}", + previous, + planner.best_cost() + ); + previous = planner.best_cost(); + } + assert!(planner.quality() > 0.0 && planner.quality() <= 1.0); + assert!(planner.best_cost() >= planner.lower_bound()); + } + + #[test] + fn published_path_is_valid() { + let mut planner = planner(7); + planner.grow(2000); + let mut waypoints = [Point2::default(); MAX_WAYPOINTS]; + let (len, truncated) = planner.write_path(&mut waypoints); + assert!(len >= 2, "a path has at least a start and a goal"); + assert_eq!(waypoints[0], START); + assert_eq!(waypoints[(len - 1) as usize], GOAL); + if !truncated { + let world = World::depot(); + for pair in waypoints[..len as usize].windows(2) { + assert!( + world.is_free_segment(pair[0], pair[1]), + "published path crosses an obstacle" + ); + } + } + } + + #[test] + fn same_seed_replays_the_same_tree() { + let (mut a, mut b) = (planner(11), planner(11)); + a.grow(600); + b.grow(300); + b.grow(300); + assert_eq!(a.tree_size(), b.tree_size()); + assert_eq!(a.best_cost(), b.best_cost()); + } + + #[test] + fn pruning_keeps_the_best_path_reachable() { + let mut planner = planner(3); + planner.grow(1500); + let cost_before = planner.best_cost(); + planner.prune(); + assert!(planner.has_solution(), "pruning dropped the goal node"); + // The tree stays consistent: every node still reaches the root. + for index in 0..planner.tree.len() { + let mut cursor = Some(index as u32); + let mut hops = 0; + while let Some(current) = cursor { + cursor = planner.tree[current as usize].parent; + hops += 1; + assert!(hops <= planner.tree.len(), "cycle in the tree"); + } + } + assert_eq!(planner.best_cost(), cost_before); + } +} diff --git a/examples/cu_anytime_rrt_star/src/tasks.rs b/examples/cu_anytime_rrt_star/src/tasks.rs new file mode 100644 index 00000000000..1652554623e --- /dev/null +++ b/examples/cu_anytime_rrt_star/src/tasks.rs @@ -0,0 +1,263 @@ +//! The Copper side of the example: a goal source, two RRT* anytime planners +//! under different policies, and a sink comparing what they published. + +use crate::rrt::{MAX_WAYPOINTS, Point2, RrtParams, RrtStar, World}; +use bincode::{Decode, Encode}; +use cu29::cutask_anytime::{AnytimeStatus, CuAnytimeTask, Quality, quality_from_f32}; +use cu29::prelude::*; +use serde::{Deserialize, Serialize}; +use std::sync::Mutex; + +/// Start and goal of every planning job. +pub const START: Point2 = Point2::new(0.5, 0.5); +pub const GOAL: Point2 = Point2::new(9.5, 9.5); + +/// Quality at which the path matches the straight-line lower bound: there is +/// nothing left to refine. +const CONVERGED_QUALITY: f32 = 0.999; + +/// What the sink saw, one entry per copperlist, for the checks in `main`. +pub static REPORTS: Mutex> = Mutex::new(Vec::new()); + +/// One copperlist as the sink saw it. A `None` path means the node published +/// nothing: no path yet, or a quality below the configured floor. +#[derive(Debug, Clone)] +pub struct PlanReport { + pub quick: Option, + pub quick_status: String, + pub thorough: Option, + pub thorough_status: String, +} + +/// One planning problem. The seed changes every copperlist, so each job is a +/// fresh RRT* run rather than a replay of the previous one. +#[derive(Default, Debug, Clone, Encode, Decode, Serialize, Deserialize, Reflect)] +pub struct PlanRequest { + pub start: Point2, + pub goal: Point2, + pub seed: u64, +} + +/// The best path known when the refinement window closed. +#[derive(Default, Debug, Clone, Encode, Decode, Serialize, Deserialize, Reflect)] +pub struct PlanPath { + pub waypoints: [Point2; MAX_WAYPOINTS], + /// Waypoints actually used in `waypoints`. + pub len: u32, + /// True when the path had more than [`MAX_WAYPOINTS`] waypoints and only + /// its head plus the goal is carried; `cost` still covers the whole path. + pub truncated: bool, + pub cost: f32, + /// RRT* iterations spent on this path, base block included. + pub iterations: u32, + pub tree_size: u32, +} + +/// Emits one planning problem per copperlist. +#[derive(Default, Reflect)] +pub struct GoalSrc { + seed: u64, +} + +impl Freezable for GoalSrc {} + +impl CuSrcTask for GoalSrc { + type Resources<'r> = (); + type Output<'m> = output_msg!(PlanRequest); + + fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult { + let seed = match config { + Some(config) => config.get::("seed")?.unwrap_or(1) as u64, + None => 1, + }; + Ok(Self { seed }) + } + + fn process(&mut self, ctx: &CuContext, new_msg: &mut Self::Output<'_>) -> CuResult<()> { + self.seed = self.seed.wrapping_add(1); + new_msg.set_payload(PlanRequest { + start: START, + goal: GOAL, + seed: self.seed, + }); + // A fresh Tov: the anytime policy anchors max_age_ms on it. + new_msg.tov = Tov::Time(ctx.clock.now()); + Ok(()) + } +} + +/// An RRT* planner as an anytime task. +/// +/// `base()` runs the first block of iterations and publishes the first path it +/// finds; each `refine()` runs one more block and republishes only when the +/// path got shorter. How many blocks run is the policy's call, not the task's. +#[derive(Reflect)] +pub struct RrtStarPlanner { + params: RrtParams, + /// Iterations of the base block, aiming at a first path. + base_iterations: u32, + /// Iterations of one refinement quantum. + block_iterations: u32, + /// The current job, `None` before the first `base()`. + planner: Option, + /// Cost of the path currently in the output; infinite while none was + /// published for this job. + published_cost: f32, + published_quality: f32, +} + +impl Freezable for RrtStarPlanner {} + +impl RrtStarPlanner { + /// Commits the best path of the tree when it beats the published one, and + /// returns the published quality. Leaving the output alone when nothing + /// improved is what the anytime contract asks for: the output always holds + /// the best result so far, so the runtime can publish it at any stop point. + fn publish(&mut self, output: &mut CuMsg) -> Quality { + if let Some(planner) = self.planner.as_ref() + && planner.has_solution() + && planner.best_cost() < self.published_cost + { + let mut path = PlanPath { + cost: planner.best_cost(), + iterations: planner.iterations(), + tree_size: planner.tree_size(), + ..Default::default() + }; + (path.len, path.truncated) = planner.write_path(&mut path.waypoints); + output.set_payload(path); + self.published_cost = planner.best_cost(); + self.published_quality = planner.quality(); + } + quality_from_f32(self.published_quality) + } +} + +impl CuAnytimeTask for RrtStarPlanner { + type Input<'m> = input_msg!(PlanRequest); + type Output<'m> = output_msg!(PlanPath); + type Resources<'r> = (); + type Quality = Quality; + + fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult { + let mut params = RrtParams::default(); + let mut base_iterations = 400u32; + let mut block_iterations = 256u32; + if let Some(config) = config { + if let Some(value) = config.get::("step_size")? { + params.step_size = value; + } + if let Some(value) = config.get::("goal_bias")? { + params.goal_bias = value; + } + if let Some(value) = config.get::("goal_threshold")? { + params.goal_threshold = value; + } + if let Some(value) = config.get::("gamma")? { + params.gamma = value; + } + if let Some(value) = config.get::("prune_interval")? { + params.prune_interval = value; + } + if let Some(value) = config.get::("max_nodes")? { + params.max_nodes = value; + } + if let Some(value) = config.get::("base_iterations")? { + base_iterations = value; + } + if let Some(value) = config.get::("block_iterations")? { + block_iterations = value; + } + } + Ok(Self { + params, + base_iterations, + block_iterations, + planner: None, + published_cost: f32::INFINITY, + published_quality: 0.0, + }) + } + + fn base( + &mut self, + _ctx: &CuContext, + input: &Self::Input<'_>, + output: &mut Self::Output<'_>, + ) -> CuResult> { + let request = input.payload().ok_or("rrt*: no plan request")?; + let planner = match self.planner.as_mut() { + // Restart on the previous job's memory instead of a fresh tree. + Some(planner) => { + planner.reset(request.start, request.goal, request.seed); + planner + } + None => self.planner.insert(RrtStar::new( + World::depot(), + self.params, + request.start, + request.goal, + request.seed, + )), + }; + planner.grow(self.base_iterations); + self.published_cost = f32::INFINITY; + self.published_quality = 0.0; + // Output messages are recycled: with no path yet the message must not + // still carry the previous job's path. + output.clear_payload(); + // Even with no path found the job goes on: refinement is what usually + // finds one, and a quality of 0.0 stays under any configured floor. + Ok(AnytimeStatus::Improved(self.publish(output))) + } + + fn refine( + &mut self, + _ctx: &CuContext, + output: &mut Self::Output<'_>, + ) -> CuResult> { + let planner = self + .planner + .as_mut() + .ok_or("rrt*: refine() without a job from base()")?; + if planner.is_exhausted() { + return Ok(AnytimeStatus::Converged(quality_from_f32( + self.published_quality, + ))); + } + planner.grow(self.block_iterations); + let quality = self.publish(output); + if self.published_quality >= CONVERGED_QUALITY { + // The path matches the straight line: no iteration can beat it. + return Ok(AnytimeStatus::Converged(quality)); + } + Ok(AnytimeStatus::Improved(quality)) + } +} + +/// Records what both planners published in the same copperlist. +#[derive(Default, Reflect)] +pub struct ComparisonSink; + +impl Freezable for ComparisonSink {} + +impl CuSinkTask for ComparisonSink { + type Resources<'r> = (); + type Input<'m> = input_msg!('m, PlanPath, PlanPath); + + fn new(_config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult { + Ok(Self) + } + + fn process(&mut self, _ctx: &CuContext, input: &Self::Input<'_>) -> CuResult<()> { + // Input order follows the cnx order in the RON: quick first. + let (quick, thorough): (&CuMsg, &CuMsg) = *input; + REPORTS.lock().expect("reports poisoned").push(PlanReport { + quick: quick.payload().cloned(), + quick_status: quick.metadata.status_txt.0.to_string(), + thorough: thorough.payload().cloned(), + thorough_status: thorough.metadata.status_txt.0.to_string(), + }); + Ok(()) + } +} From 3b16d19573c0870e641359381fa7a1c7402ded9f Mon Sep 17 00:00:00 2001 From: Scofield626 Date: Mon, 3 Aug 2026 10:08:58 +0000 Subject: [PATCH 2/7] example: publish drivable RRT* paths and derive gamma from the map 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. --- examples/cu_anytime_rrt_star/copperconfig.ron | 6 +- examples/cu_anytime_rrt_star/src/main.rs | 89 ++++++-- examples/cu_anytime_rrt_star/src/rrt.rs | 198 +++++++++++++++--- examples/cu_anytime_rrt_star/src/tasks.rs | 136 ++++++++++-- 4 files changed, 364 insertions(+), 65 deletions(-) diff --git a/examples/cu_anytime_rrt_star/copperconfig.ron b/examples/cu_anytime_rrt_star/copperconfig.ron index eed9017964b..0052a112bb5 100644 --- a/examples/cu_anytime_rrt_star/copperconfig.ron +++ b/examples/cu_anytime_rrt_star/copperconfig.ron @@ -14,14 +14,14 @@ "step_size": 0.8, "goal_bias": 0.05, "goal_threshold": 0.5, - "gamma": 3.0, + "gamma": 0.0, "prune_interval": 512, "max_nodes": 4000, }, anytime: ( max_refines: 2, time_budget_ms: 50.0, - quality_target: 0.85, + quality_target: 0.93, quality_floor: 0.05, ), ), @@ -34,7 +34,7 @@ "step_size": 0.8, "goal_bias": 0.05, "goal_threshold": 0.5, - "gamma": 3.0, + "gamma": 0.0, "prune_interval": 512, "max_nodes": 4000, }, diff --git a/examples/cu_anytime_rrt_star/src/main.rs b/examples/cu_anytime_rrt_star/src/main.rs index b245b1295b6..5e8916181cf 100644 --- a/examples/cu_anytime_rrt_star/src/main.rs +++ b/examples/cu_anytime_rrt_star/src/main.rs @@ -10,7 +10,7 @@ //! //! | node | policy | meaning | //! |---|---|---| -//! | `quick_planner` | `max_refines: 2`, `time_budget_ms: 50`, `quality_target: 0.85` | two quanta at most, and stop early once the path is within 15% of the straight line | +//! | `quick_planner` | `max_refines: 2`, `time_budget_ms: 50`, `quality_target: 0.93` | two quanta at most, and stop early once the path is within 7% of the straight line | //! | `thorough_planner` | `max_refines: 24`, `time_budget_ms: 250`, `max_stall: 4` | up to 24 quanta, but give up after 4 that improved nothing | //! //! Both carry `quality_floor: 0.05`, which drops a job that found no path at @@ -20,6 +20,10 @@ //! tree plus more iterations, so its path is never longer - that is the anytime //! trade-off, measured. Keeping the two `config:` blocks identical is what //! makes the comparison below valid. +//! +//! The RRT* `gamma` is `0.0` in the RON, which asks the planner to derive the +//! rewiring radius constant from the map. Pinning it to an arbitrary smaller +//! number is what makes an RRT* implementation quietly stop converging. mod rrt; mod tasks; @@ -34,8 +38,9 @@ struct App {} const SLAB_SIZE: Option = Some(16 * 1024 * 1024); const ITERATIONS: usize = 10; -fn main() { - let logger_path = "logs/anytime_rrt_star.copper"; +/// Runs the whole application once and returns what the sink saw, one entry +/// per copperlist. +fn run(logger_path: &str) -> Vec { if let Some(parent) = Path::new(logger_path).parent() && !parent.exists() { @@ -57,21 +62,40 @@ fn main() { application .stop_all_tasks() .expect("Failed to stop application."); + tasks::take_reports() +} - let reports = tasks::REPORTS.lock().expect("reports poisoned"); +/// Checks the anytime contract on what the sink saw and returns how many jobs +/// both planners published. +fn check(reports: &[tasks::PlanReport]) -> usize { assert_eq!(reports.len(), ITERATIONS, "one report per copperlist"); - + let world = rrt::World::depot(); let lower_bound = tasks::START.distance(tasks::GOAL); - println!("straight line start -> goal: {lower_bound:.2} m (quality 1.0)"); - println!("{:<5} {:>28} {:>28}", "job", "quick", "thorough"); let mut compared = 0; + for (index, report) in reports.iter().enumerate() { - println!( - "{:<5} {:>28} {:>28}", - index, - describe(&report.quick, &report.quick_status, lower_bound), - describe(&report.thorough, &report.thorough_status, lower_bound), - ); + for path in [&report.quick, &report.thorough].into_iter().flatten() { + assert!(path.len >= 2, "job {index}: a path needs two waypoints"); + assert_eq!(path.waypoints[0], tasks::START, "job {index}"); + assert_eq!( + path.waypoints[(path.len - 1) as usize], + tasks::GOAL, + "job {index}" + ); + // A published path must be drivable as published, whichever stop + // point the policy picked. + for pair in path.waypoints[..path.len as usize].windows(2) { + assert!( + world.is_free_segment(pair[0], pair[1]), + "job {index}: published path crosses an obstacle" + ); + } + assert!( + path.cost >= lower_bound, + "job {index}: path shorter than the straight line" + ); + } + let (Some(quick), Some(thorough)) = (&report.quick, &report.thorough) else { continue; }; @@ -83,16 +107,31 @@ fn main() { thorough.cost, quick.cost ); - assert!( - thorough.cost >= lower_bound, - "job {index}: path shorter than the straight line" - ); - assert!(thorough.len >= 2, "job {index}: a path needs two waypoints"); } + assert!( compared >= ITERATIONS / 2, "both planners published in only {compared} of {ITERATIONS} jobs" ); + compared +} + +fn main() { + let reports = run("logs/anytime_rrt_star.copper"); + + let lower_bound = tasks::START.distance(tasks::GOAL); + println!("straight line start -> goal: {lower_bound:.2} m (quality 1.0)"); + println!("{:<5} {:>28} {:>28}", "job", "quick", "thorough"); + for (index, report) in reports.iter().enumerate() { + println!( + "{:<5} {:>28} {:>28}", + index, + describe(&report.quick, &report.quick_status, lower_bound), + describe(&report.thorough, &report.thorough_status, lower_bound), + ); + } + + let compared = check(&reports); println!("anytime RRT* example OK: {compared}/{ITERATIONS} jobs compared"); } @@ -108,3 +147,17 @@ fn describe(path: &Option, status: &str, lower_bound: f32) -> S None => format!("no path [{status}]"), } } + +#[cfg(test)] +mod tests { + use super::*; + + /// The self-check of `main`, as a test: CI only builds the examples, so + /// without this nothing ever runs the anytime path. + #[test] + fn both_policies_publish_drivable_paths() { + let logger_path = std::env::temp_dir().join("cu_anytime_rrt_star_test.copper"); + let reports = run(logger_path.to_str().expect("non-utf8 temp dir")); + check(&reports); + } +} diff --git a/examples/cu_anytime_rrt_star/src/rrt.rs b/examples/cu_anytime_rrt_star/src/rrt.rs index dc0fb5cd64e..996af5d2e2a 100644 --- a/examples/cu_anytime_rrt_star/src/rrt.rs +++ b/examples/cu_anytime_rrt_star/src/rrt.rs @@ -3,6 +3,13 @@ //! The planner knows nothing about Copper: it only exposes [`RrtStar::grow`], //! one bounded block of iterations. `tasks.rs` calls it once from `base()` and //! once per anytime refinement quantum. +//! +//! The steps follow Karaman and Frazzoli: sample with goal bias, nearest, +//! steer, choose the cheapest parent, rewire the neighborhood, and prune by +//! branch and bound. Three points are stricter here than in a textbook write-up: +//! the final leg to the goal is collision checked and counted in the path cost, +//! rewiring refuses an ancestor so rounding cannot close a cycle, and the cost +//! shift after a rewire is iterative instead of recursive. use bincode::{Decode, Encode}; use cu29::prelude::*; @@ -102,6 +109,28 @@ impl World { .iter() .all(|o| distance_to_segment(a, b, o.center) > o.radius) } + + /// Area left free by the obstacles. Assumes every obstacle lies inside the + /// bounds and none overlap, which holds for [`World::depot`]. + pub fn free_area(&self) -> f32 { + let blocked: f32 = self + .obstacles + .iter() + .map(|o| core::f32::consts::PI * o.radius * o.radius) + .sum(); + (self.width * self.height - blocked).max(f32::EPSILON) + } + + /// The RRT* radius constant of Karaman and Frazzoli: + /// `gamma* = 2 * (1 + 1/d)^(1/d) * (free_area / zeta_d)^(1/d)`, here with + /// `d = 2` and `zeta_2 = pi`. + /// + /// A smaller constant shrinks the rewiring neighborhood below what + /// asymptotic optimality needs, and the planner degrades toward plain RRT + /// as the tree grows. + pub fn rrt_star_gamma(&self) -> f32 { + 2.0 * 1.5f32.sqrt() * (self.free_area() / core::f32::consts::PI).sqrt() + } } /// Distance from `point` to the segment `a`-`b`. @@ -124,7 +153,9 @@ pub struct RrtParams { pub goal_bias: f32, /// A node this close to the goal closes a path. pub goal_threshold: f32, - /// Gamma of the RRT* rewiring radius `gamma * sqrt(ln n / n)`. + /// Gamma of the RRT* rewiring radius `gamma * sqrt(ln n / n)`. `0.0` + /// derives it from the world through [`World::rrt_star_gamma`], which is + /// the value RRT* needs to converge to the optimum. pub gamma: f32, /// Branch-and-bound prune every N iterations; 0 disables pruning. pub prune_interval: u32, @@ -138,7 +169,7 @@ impl Default for RrtParams { step_size: 0.8, goal_bias: 0.05, goal_threshold: 0.5, - gamma: 3.0, + gamma: 0.0, prune_interval: 512, max_nodes: 4000, } @@ -213,7 +244,16 @@ impl RrtStar { /// Starts a search rooted at `start`. An unreachable or blocked `start` /// simply never grows a tree; the caller sees "no path" and the anytime /// quality floor drops the result. - pub fn new(world: World, params: RrtParams, start: Point2, goal: Point2, seed: u64) -> Self { + pub fn new( + world: World, + mut params: RrtParams, + start: Point2, + goal: Point2, + seed: u64, + ) -> Self { + if params.gamma <= 0.0 { + params.gamma = world.rrt_star_gamma(); + } let mut planner = Self { world, params, @@ -306,15 +346,36 @@ impl RrtStar { (self.lower_bound() / self.best_cost).clamp(0.0, 1.0) } - /// Copies the best path into `out` and returns `(waypoints, truncated)`. - /// - /// A path longer than [`MAX_WAYPOINTS`] is cut after its head and still - /// ends on the goal; `truncated` says so, and the reported cost always - /// describes the whole path. - pub fn write_path(&self, out: &mut [Point2; MAX_WAYPOINTS]) -> (u32, bool) { + /// Nodes on the best path before shortcutting, the goal included. Zero + /// while no path is known. + pub fn tree_path_len(&self) -> usize { let Some(goal_node) = self.best_goal else { - return (0, false); + return 0; }; + let mut len = 1; // the goal itself, which is not a tree node + let mut cursor = Some(goal_node); + while let Some(index) = cursor { + len += 1; + cursor = self.tree[index as usize].parent; + } + len + } + + /// Writes the best path into `out` and returns how many waypoints it used. + /// + /// The tree path routinely holds more nodes than [`MAX_WAYPOINTS`], so it + /// is shortcut first: from each waypoint the path jumps to the furthest + /// later one still reachable in a straight free line. Shortcutting is what + /// a planner publishes anyway, and it keeps every published segment + /// collision free - dropping the tail instead would publish a straight + /// jump across the map. + /// + /// The shortcut path is never longer than the tree path, so the reported + /// cost stays an upper bound on what the robot drives. `None` means even + /// the shortcut path does not fit; the caller then publishes nothing + /// rather than a path that cuts through an obstacle. + pub fn write_path(&self, out: &mut [Point2; MAX_WAYPOINTS]) -> Option { + let goal_node = self.best_goal?; let mut chain = Vec::new(); let mut cursor = Some(goal_node); while let Some(index) = cursor { @@ -325,13 +386,27 @@ impl RrtStar { chain.reverse(); chain.push(self.goal); - let truncated = chain.len() > MAX_WAYPOINTS; - let len = chain.len().min(MAX_WAYPOINTS); - out[..len].copy_from_slice(&chain[..len]); - if truncated { - out[len - 1] = self.goal; + let mut len = 0usize; + let mut at = 0usize; + loop { + if len == MAX_WAYPOINTS { + return None; + } + out[len] = chain[at]; + len += 1; + if at == chain.len() - 1 { + return Some(len as u32); + } + // The next tree node is always reachable - it is a tree edge - so + // the scan only looks for something further. + let mut next = at + 1; + for candidate in (at + 2)..chain.len() { + if self.world.is_free_segment(chain[at], chain[candidate]) { + next = candidate; + } + } + at = next; } - (len as u32, truncated) } /// One RRT* iteration: sample, steer, choose the cheapest parent, rewire @@ -602,24 +677,89 @@ mod tests { assert!(planner.best_cost() >= planner.lower_bound()); } + /// Every published path must be drivable, at every stop point the anytime + /// policy could pick. + /// + /// A small `gamma` is included on purpose: it barely rewires, so its tree + /// paths grow past [`MAX_WAYPOINTS`] and the shortcut is exercised rather + /// than skipped. #[test] - fn published_path_is_valid() { - let mut planner = planner(7); - planner.grow(2000); - let mut waypoints = [Point2::default(); MAX_WAYPOINTS]; - let (len, truncated) = planner.write_path(&mut waypoints); - assert!(len >= 2, "a path has at least a start and a goal"); - assert_eq!(waypoints[0], START); - assert_eq!(waypoints[(len - 1) as usize], GOAL); - if !truncated { - let world = World::depot(); - for pair in waypoints[..len as usize].windows(2) { + fn published_path_is_valid_at_every_stop_point() { + let world = World::depot(); + let mut longest_tree_path = 0; + for (seed, gamma) in (1..40u64).flat_map(|seed| [(seed, 0.0f32), (seed, 3.0)]) { + let params = RrtParams { + gamma, + ..Default::default() + }; + let mut planner = RrtStar::new(World::depot(), params, START, GOAL, seed); + planner.grow(400); + for _ in 0..24 { + planner.grow(256); + let mut waypoints = [Point2::default(); MAX_WAYPOINTS]; + let Some(len) = planner.write_path(&mut waypoints) else { + panic!("seed {seed}: the shortcut path did not fit"); + }; + longest_tree_path = longest_tree_path.max(planner.tree_path_len()); + assert!(len >= 2, "a path has at least a start and a goal"); + assert_eq!(waypoints[0], START); + assert_eq!(waypoints[(len - 1) as usize], GOAL); + for pair in waypoints[..len as usize].windows(2) { + assert!( + world.is_free_segment(pair[0], pair[1]), + "seed {seed}: published path crosses an obstacle" + ); + } + // The shortcut only removes waypoints it can bypass in a + // straight free line, so it never lengthens the path. + let published: f32 = waypoints[..len as usize] + .windows(2) + .map(|pair| pair[0].distance(pair[1])) + .sum(); assert!( - world.is_free_segment(pair[0], pair[1]), - "published path crosses an obstacle" + published <= planner.best_cost() + 1e-3, + "seed {seed}: shortcut path {published} longer than the cost {}", + planner.best_cost() ); } } + assert!( + longest_tree_path > MAX_WAYPOINTS, + "the tree path never outgrew MAX_WAYPOINTS, so the shortcut was never exercised" + ); + } + + /// A gamma below the value the free area implies shrinks the rewiring + /// neighborhood, and refinement then converges to a worse path. + #[test] + fn derived_gamma_beats_an_arbitrary_one() { + let world = World::depot(); + let derived = world.rrt_star_gamma(); + assert!( + (12.0..13.0).contains(&derived), + "gamma for the depot map should be near 12.4, got {derived}" + ); + + let cost_at = |gamma: f32| { + let mut total = 0.0; + for seed in 1..40u64 { + let params = RrtParams { + gamma, + ..Default::default() + }; + let mut planner = RrtStar::new(World::depot(), params, START, GOAL, seed); + planner.grow(400); + for _ in 0..24 { + planner.grow(256); + } + total += planner.best_cost(); + } + total + }; + assert!( + cost_at(0.0) < cost_at(3.0), + "the derived gamma should refine to a shorter path than a small one" + ); } #[test] diff --git a/examples/cu_anytime_rrt_star/src/tasks.rs b/examples/cu_anytime_rrt_star/src/tasks.rs index 1652554623e..d49b68a8552 100644 --- a/examples/cu_anytime_rrt_star/src/tasks.rs +++ b/examples/cu_anytime_rrt_star/src/tasks.rs @@ -17,7 +17,12 @@ pub const GOAL: Point2 = Point2::new(9.5, 9.5); const CONVERGED_QUALITY: f32 = 0.999; /// What the sink saw, one entry per copperlist, for the checks in `main`. -pub static REPORTS: Mutex> = Mutex::new(Vec::new()); +static REPORTS: Mutex> = Mutex::new(Vec::new()); + +/// Takes the reports collected so far, leaving the sink ready for a new run. +pub fn take_reports() -> Vec { + core::mem::take(&mut *REPORTS.lock().expect("reports poisoned")) +} /// One copperlist as the sink saw it. A `None` path means the node published /// nothing: no path yet, or a quality below the configured floor. @@ -39,14 +44,17 @@ pub struct PlanRequest { } /// The best path known when the refinement window closed. +/// +/// Every consecutive pair of waypoints is collision free, so the path can be +/// driven as published. #[derive(Default, Debug, Clone, Encode, Decode, Serialize, Deserialize, Reflect)] pub struct PlanPath { pub waypoints: [Point2; MAX_WAYPOINTS], /// Waypoints actually used in `waypoints`. pub len: u32, - /// True when the path had more than [`MAX_WAYPOINTS`] waypoints and only - /// its head plus the goal is carried; `cost` still covers the whole path. - pub truncated: bool, + /// Cost of the RRT* tree path, which is what the anytime quality scores. + /// The published waypoints are a shortcut of it, so this is an upper bound + /// on the distance actually driven. pub cost: f32, /// RRT* iterations spent on this path, base block included. pub iterations: u32, @@ -80,12 +88,32 @@ impl CuSrcTask for GoalSrc { goal: GOAL, seed: self.seed, }); - // A fresh Tov: the anytime policy anchors max_age_ms on it. + // A fresh Tov per job. An anytime node reads it as the age anchor when + // its policy sets max_age_ms; neither planner here does, so both + // anchor on their own job start instead. new_msg.tov = Tov::Time(ctx.clock.now()); Ok(()) } } +/// What a remote debugger sees of a planner node: the progress of the job, +/// not the thousands of tree nodes behind it. +/// +/// The fields are read through `Reflect`, which the compiler cannot see. +#[allow(dead_code)] +#[derive(Default, Debug, Reflect)] +pub struct PlannerDebugState { + pub iterations: u32, + pub tree_size: u32, + /// Nodes on the best path before it is shortcut for publication. + pub tree_path_len: u32, + /// Cost of the best path in the tree, `f32::INFINITY` while there is none. + pub best_cost: f32, + /// Cost of the path currently in the output. + pub published_cost: f32, + pub published_quality: f32, +} + /// An RRT* planner as an anytime task. /// /// `base()` runs the first block of iterations and publishes the first path it @@ -118,19 +146,36 @@ impl RrtStarPlanner { && planner.has_solution() && planner.best_cost() < self.published_cost { - let mut path = PlanPath { - cost: planner.best_cost(), - iterations: planner.iterations(), - tree_size: planner.tree_size(), - ..Default::default() - }; - (path.len, path.truncated) = planner.write_path(&mut path.waypoints); - output.set_payload(path); - self.published_cost = planner.best_cost(); - self.published_quality = planner.quality(); + 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) { + output.set_payload(PlanPath { + waypoints, + len, + cost: planner.best_cost(), + iterations: planner.iterations(), + tree_size: planner.tree_size(), + }); + self.published_cost = planner.best_cost(); + self.published_quality = planner.quality(); + } } quality_from_f32(self.published_quality) } + + /// The projected view a debug session gets instead of the whole tree. + fn debug_state(&self) -> PlannerDebugState { + let planner = self.planner.as_ref(); + PlannerDebugState { + iterations: planner.map_or(0, RrtStar::iterations), + tree_size: planner.map_or(0, RrtStar::tree_size), + tree_path_len: planner.map_or(0, |p| p.tree_path_len() as u32), + best_cost: planner.map_or(f32::INFINITY, RrtStar::best_cost), + published_cost: self.published_cost, + published_quality: self.published_quality, + } + } } impl CuAnytimeTask for RrtStarPlanner { @@ -139,6 +184,21 @@ impl CuAnytimeTask for RrtStarPlanner { type Resources<'r> = (); type Quality = Quality; + // The task struct holds a whole RRT* tree, up to `max_nodes` entries. The + // default hooks would ship all of it on every debug read, so the node + // exposes a small view instead. + fn register_debug_state_types(registry: &mut TypeRegistry) { + registry.register::(); + } + + fn debug_state_type_path() -> &'static str { + PlannerDebugState::type_path() + } + + fn with_debug_state(&self, f: impl FnOnce(&dyn bevy_reflect::Reflect) -> R) -> R { + f(&self.debug_state()) + } + fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult { let mut params = RrtParams::default(); let mut base_iterations = 400u32; @@ -261,3 +321,49 @@ impl CuSinkTask for ComparisonSink { Ok(()) } } + +#[cfg(test)] +mod tests { + use super::*; + + /// The anytime contract driven by hand: `base()` publishes a first path, + /// every `refine()` leaves the output holding the best path so far, and + /// the debug state tracks the job instead of exposing the whole tree. + #[test] + fn refinement_only_commits_improvements() { + let ctx = CuContext::new_with_clock(); + let mut task = RrtStarPlanner::new(None, ()).unwrap(); + let input = CuMsg::new(Some(PlanRequest { + start: START, + goal: GOAL, + seed: 42, + })); + let mut output = CuMsg::new(None); + + task.start(&ctx).unwrap(); + let status = task.base(&ctx, &input, &mut output).unwrap(); + assert!(matches!(status, AnytimeStatus::Improved(_))); + + let mut best = f32::INFINITY; + for _ in 0..24 { + if let AnytimeStatus::Aborted = task.refine(&ctx, &mut output).unwrap() { + panic!("the planner should not abort on a solvable map"); + } + if let Some(path) = output.payload() { + assert!( + path.cost <= best + 1e-4, + "the output regressed: {best} then {}", + path.cost + ); + best = path.cost; + } + } + assert!(output.payload().is_some(), "no path after 24 quanta"); + + let state = task.debug_state(); + assert_eq!(state.published_cost, best); + assert!(state.best_cost <= state.published_cost); + assert!(state.published_quality > 0.0); + assert!(state.iterations > 0 && state.tree_size > 0); + } +} From 9b86111bdbb525df4088aa59e5d87eb597298e11 Mon Sep 17 00:00:00 2001 From: Scofield626 Date: Mon, 3 Aug 2026 14:16:00 +0000 Subject: [PATCH 3/7] cu-rrt-star: extract the anytime RRT* planner into a component --- Cargo.toml | 2 + components/tasks/cu_rrt_star/Cargo.toml | 29 + components/tasks/cu_rrt_star/README.md | 75 +++ components/tasks/cu_rrt_star/build.rs | 3 + components/tasks/cu_rrt_star/src/lib.rs | 297 +++++++++ components/tasks/cu_rrt_star/src/rrt.rs | 794 ++++++++++++++++++++++++ 6 files changed, 1200 insertions(+) create mode 100644 components/tasks/cu_rrt_star/Cargo.toml create mode 100644 components/tasks/cu_rrt_star/README.md create mode 100644 components/tasks/cu_rrt_star/build.rs create mode 100644 components/tasks/cu_rrt_star/src/lib.rs create mode 100644 components/tasks/cu_rrt_star/src/rrt.rs diff --git a/Cargo.toml b/Cargo.toml index 02bab5a8409..ac1ed915498 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -72,6 +72,7 @@ members = [ "components/tasks/cu_pid", "components/tasks/cu_python_task", "components/tasks/cu_ratelimit", + "components/tasks/cu_rrt_star", "components/testing/cu_udp_inject", "benchmarks/cu_async_cl_io_bench", "benchmarks/cu_dorabench", @@ -245,6 +246,7 @@ cu-msp-bridge = { path = "components/bridges/cu_msp_bridge", version = "1.1.0-de cu-msp-lib = { path = "components/libs/cu_msp_lib", version = "1.1.0-dev" } cu-rp-encoder = { path = "components/sources/cu_rp_encoder", version = "1.1.0-dev" } cu-rp-sn754410-new = { path = "components/sinks/cu_rp_sn754410", version = "1.1.0-dev" } +cu-rrt-star = { path = "components/tasks/cu_rrt_star", version = "1.1.0-dev" } cu-sdlogger = { path = "components/libs/cu_sdlogger", version = "1.1.0-dev" } cu-zenoh-bridge = { path = "components/bridges/cu_zenoh_bridge", version = "1.1.0-dev" } diff --git a/components/tasks/cu_rrt_star/Cargo.toml b/components/tasks/cu_rrt_star/Cargo.toml new file mode 100644 index 00000000000..3a1e7b74473 --- /dev/null +++ b/components/tasks/cu_rrt_star/Cargo.toml @@ -0,0 +1,29 @@ +[package] +name = "cu-rrt-star" +description = "An anytime RRT* path planner component for the Copper project" +version.workspace = true +authors.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true +keywords.workspace = true +categories.workspace = true +homepage.workspace = true +repository.workspace = true +documentation = "https://docs.rs/cu-rrt-star" + +[package.metadata.copper] +kind = "task" +domains = ["algorithm/planning"] +environments = ["host"] + +[dependencies] +bincode = { workspace = true } +cu29 = { workspace = true } +serde = { workspace = true } + +[dev-dependencies] +tempfile = { workspace = true } + +[build-dependencies] +cu29-build = { workspace = true } diff --git a/components/tasks/cu_rrt_star/README.md b/components/tasks/cu_rrt_star/README.md new file mode 100644 index 00000000000..4d38393e6aa --- /dev/null +++ b/components/tasks/cu_rrt_star/README.md @@ -0,0 +1,75 @@ +# cu-rrt-star: an anytime RRT* path planner for Copper + +An RRT* planner (Karaman and Frazzoli) packaged as a Copper anytime task. +`base()` grows the tree until it has a first path and publishes it; every +`refine()` runs one more block of iterations and republishes only when the +path got shorter. The RON `anytime:` policy — not the task — decides how many +refinement quanta run. + +### Input and output + +- Input: `cu_rrt_star::PlanRequest` — start, goal and the RNG seed of the job. +- Output: `cu_rrt_star::PlanPath` — up to `MAX_WAYPOINTS` (32) waypoints. + Every consecutive pair of waypoints is collision free, so the path can be + driven as published, whichever stop point the policy picked. `cost` is the + RRT* tree path cost, an upper bound on the distance actually driven. + +### Usage + +```ron +( + id: "planner", + type: "cu_rrt_star::RrtStarPlanner", + config: { + "base_iterations": 400, + "block_iterations": 256, + "gamma": 0.0, + }, + anytime: ( + max_refines: 16, + time_budget_ms: 30.0, + max_stall: 4, + quality_floor: 0.05, + ), +), +``` + +### Configuration + +- `base_iterations` (default 400): iterations of the base block, aiming at a + first path. +- `block_iterations` (default 256): iterations of one refinement quantum. +- `step_size` (default 0.8): longest edge added in one extension, in meters. +- `goal_bias` (default 0.05): probability of sampling the goal. +- `goal_threshold` (default 0.5): a node this close to the goal closes a path. +- `gamma` (default 0.0): rewiring radius constant; `0.0` derives it from the + map, which is the value RRT* needs to converge to the optimum. +- `prune_interval` (default 512): branch-and-bound prune every N iterations; + 0 disables pruning. +- `max_nodes` (default 4000): hard cap on the tree size. + +### Anytime policy + +All the knobs of the RON `anytime:` block apply: `max_refines`, +`time_budget_ms`, `max_age_ms`, `quality_target`, `quality_floor`, +`max_stall`. The reported quality is `straight-line distance / best path +cost` in `0.0..=1.0`: 0.0 means no path yet, 1.0 means the path is as short +as the world allows, so a `quality_target` is portable between maps. + +A job that found no path reports quality 0.0, which stays under any +configured `quality_floor`: the node then publishes nothing for that +copperlist. + +### Determinism and debugging + +The planner uses a seeded xorshift64* generator: the same seed replays the +same tree. A remote debug session sees a small `PlannerDebugState` projection +(iterations, tree size, best cost, published quality) instead of the whole +tree. + +### See also + +- `examples/cu_anytime_rrt_star`: a closed-loop navigation demo with a Rerun + viewer. +- `tests/`: the same planner under a quick and a thorough policy, compared + per copperlist. diff --git a/components/tasks/cu_rrt_star/build.rs b/components/tasks/cu_rrt_star/build.rs new file mode 100644 index 00000000000..7cbac12abe5 --- /dev/null +++ b/components/tasks/cu_rrt_star/build.rs @@ -0,0 +1,3 @@ +fn main() { + cu29_build::setup(); +} diff --git a/components/tasks/cu_rrt_star/src/lib.rs b/components/tasks/cu_rrt_star/src/lib.rs new file mode 100644 index 00000000000..477b8c32d68 --- /dev/null +++ b/components/tasks/cu_rrt_star/src/lib.rs @@ -0,0 +1,297 @@ +//! An RRT* path planner as a Copper anytime task. +//! +//! [`RrtStarPlanner`] consumes a [`PlanRequest`] and publishes a [`PlanPath`]. +//! `base()` grows the tree until it has a first, crude path; every `refine()` +//! runs one more block of RRT* iterations and republishes only when the path +//! got shorter. The task reports how good the path is; the RON `anytime:` +//! policy decides how long to keep going. + +mod rrt; + +pub use rrt::*; + +use bincode::{Decode, Encode}; +use cu29::cutask_anytime::{AnytimeStatus, CuAnytimeTask, Quality, quality_from_f32}; +use cu29::prelude::*; +use serde::{Deserialize, Serialize}; + +/// Quality at which the path matches the straight-line lower bound: there is +/// nothing left to refine. +const CONVERGED_QUALITY: f32 = 0.999; + +/// One planning problem. A different seed per job makes each job a fresh RRT* +/// run rather than a replay of the previous one. +#[derive(Default, Debug, Clone, Encode, Decode, Serialize, Deserialize, Reflect)] +pub struct PlanRequest { + pub start: Point2, + pub goal: Point2, + pub seed: u64, +} + +/// The best path known when the refinement window closed. +/// +/// Every consecutive pair of waypoints is collision free, so the path can be +/// driven as published. +#[derive(Default, Debug, Clone, Encode, Decode, Serialize, Deserialize, Reflect)] +pub struct PlanPath { + pub waypoints: [Point2; MAX_WAYPOINTS], + /// Waypoints actually used in `waypoints`. + pub len: u32, + /// Cost of the RRT* tree path, which is what the anytime quality scores. + /// The published waypoints are a shortcut of it, so this is an upper bound + /// on the distance actually driven. + pub cost: f32, + /// RRT* iterations spent on this path, base block included. + pub iterations: u32, + pub tree_size: u32, +} + +/// What a remote debugger sees of a planner node: the progress of the job, +/// not the thousands of tree nodes behind it. +/// +/// The fields are read through `Reflect`, which the compiler cannot see. +#[allow(dead_code)] +#[derive(Default, Debug, Reflect)] +pub struct PlannerDebugState { + pub iterations: u32, + pub tree_size: u32, + /// Nodes on the best path before it is shortcut for publication. + pub tree_path_len: u32, + /// Cost of the best path in the tree, `f32::INFINITY` while there is none. + pub best_cost: f32, + /// Cost of the path currently in the output. + pub published_cost: f32, + pub published_quality: f32, +} + +/// An RRT* planner as an anytime task. +/// +/// `base()` runs the first block of iterations and publishes the first path it +/// finds; each `refine()` runs one more block and republishes only when the +/// path got shorter. How many blocks run is the policy's call, not the task's. +#[derive(Reflect)] +pub struct RrtStarPlanner { + params: RrtParams, + /// Iterations of the base block, aiming at a first path. + base_iterations: u32, + /// Iterations of one refinement quantum. + block_iterations: u32, + /// The current job, `None` before the first `base()`. + planner: Option, + /// Cost of the path currently in the output; infinite while none was + /// published for this job. + published_cost: f32, + published_quality: f32, +} + +// All mutable state is per-job and re-initialized by `base()` at the start of +// every copperlist, so there is nothing to snapshot for a foreground node. +impl Freezable for RrtStarPlanner {} + +impl RrtStarPlanner { + /// Commits the best path of the tree when it beats the published one, and + /// returns the published quality. Leaving the output alone when nothing + /// improved is what the anytime contract asks for: the output always holds + /// the best result so far, so the runtime can publish it at any stop point. + fn publish(&mut self, output: &mut CuMsg) -> Quality { + if let Some(planner) = self.planner.as_ref() + && planner.has_solution() + && planner.best_cost() < self.published_cost + { + 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) { + output.set_payload(PlanPath { + waypoints, + len, + cost: planner.best_cost(), + iterations: planner.iterations(), + tree_size: planner.tree_size(), + }); + self.published_cost = planner.best_cost(); + self.published_quality = planner.quality(); + } + } + quality_from_f32(self.published_quality) + } + + /// The projected view a debug session gets instead of the whole tree. + fn debug_state(&self) -> PlannerDebugState { + let planner = self.planner.as_ref(); + PlannerDebugState { + iterations: planner.map_or(0, RrtStar::iterations), + tree_size: planner.map_or(0, RrtStar::tree_size), + tree_path_len: planner.map_or(0, |p| p.tree_path_len() as u32), + best_cost: planner.map_or(f32::INFINITY, RrtStar::best_cost), + published_cost: self.published_cost, + published_quality: self.published_quality, + } + } +} + +impl CuAnytimeTask for RrtStarPlanner { + type Input<'m> = input_msg!(PlanRequest); + type Output<'m> = output_msg!(PlanPath); + type Resources<'r> = (); + type Quality = Quality; + + // The task struct holds a whole RRT* tree, up to `max_nodes` entries. The + // default hooks would ship all of it on every debug read, so the node + // exposes a small view instead. + fn register_debug_state_types(registry: &mut TypeRegistry) { + registry.register::(); + } + + fn debug_state_type_path() -> &'static str { + PlannerDebugState::type_path() + } + + fn with_debug_state(&self, f: impl FnOnce(&dyn bevy_reflect::Reflect) -> R) -> R { + f(&self.debug_state()) + } + + fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult { + let mut params = RrtParams::default(); + let mut base_iterations = 400u32; + let mut block_iterations = 256u32; + if let Some(config) = config { + if let Some(value) = config.get::("step_size")? { + params.step_size = value; + } + if let Some(value) = config.get::("goal_bias")? { + params.goal_bias = value; + } + if let Some(value) = config.get::("goal_threshold")? { + params.goal_threshold = value; + } + if let Some(value) = config.get::("gamma")? { + params.gamma = value; + } + if let Some(value) = config.get::("prune_interval")? { + params.prune_interval = value; + } + if let Some(value) = config.get::("max_nodes")? { + params.max_nodes = value; + } + if let Some(value) = config.get::("base_iterations")? { + base_iterations = value; + } + if let Some(value) = config.get::("block_iterations")? { + block_iterations = value; + } + } + Ok(Self { + params, + base_iterations, + block_iterations, + planner: None, + published_cost: f32::INFINITY, + published_quality: 0.0, + }) + } + + fn base( + &mut self, + _ctx: &CuContext, + input: &Self::Input<'_>, + output: &mut Self::Output<'_>, + ) -> CuResult> { + let request = input.payload().ok_or("rrt*: no plan request")?; + let planner = match self.planner.as_mut() { + // Restart on the previous job's memory instead of a fresh tree. + Some(planner) => { + planner.reset(request.start, request.goal, request.seed); + planner + } + None => self.planner.insert(RrtStar::new( + World::depot(), + self.params, + request.start, + request.goal, + request.seed, + )), + }; + planner.grow(self.base_iterations); + self.published_cost = f32::INFINITY; + self.published_quality = 0.0; + // Output messages are recycled: with no path yet the message must not + // still carry the previous job's path. + output.clear_payload(); + // Even with no path found the job goes on: refinement is what usually + // finds one, and a quality of 0.0 stays under any configured floor. + Ok(AnytimeStatus::Improved(self.publish(output))) + } + + fn refine( + &mut self, + _ctx: &CuContext, + output: &mut Self::Output<'_>, + ) -> CuResult> { + let planner = self + .planner + .as_mut() + .ok_or("rrt*: refine() without a job from base()")?; + if planner.is_exhausted() { + return Ok(AnytimeStatus::Converged(quality_from_f32( + self.published_quality, + ))); + } + planner.grow(self.block_iterations); + let quality = self.publish(output); + if self.published_quality >= CONVERGED_QUALITY { + // The path matches the straight line: no iteration can beat it. + return Ok(AnytimeStatus::Converged(quality)); + } + Ok(AnytimeStatus::Improved(quality)) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + const START: Point2 = Point2::new(0.5, 0.5); + const GOAL: Point2 = Point2::new(9.5, 9.5); + + /// The anytime contract driven by hand: `base()` publishes a first path, + /// every `refine()` leaves the output holding the best path so far, and + /// the debug state tracks the job instead of exposing the whole tree. + #[test] + fn refinement_only_commits_improvements() { + let ctx = CuContext::new_with_clock(); + let mut task = RrtStarPlanner::new(None, ()).unwrap(); + let input = CuMsg::new(Some(PlanRequest { + start: START, + goal: GOAL, + seed: 42, + })); + let mut output = CuMsg::new(None); + + task.start(&ctx).unwrap(); + let status = task.base(&ctx, &input, &mut output).unwrap(); + assert!(matches!(status, AnytimeStatus::Improved(_))); + + let mut best = f32::INFINITY; + for _ in 0..24 { + if let AnytimeStatus::Aborted = task.refine(&ctx, &mut output).unwrap() { + panic!("the planner should not abort on a solvable map"); + } + if let Some(path) = output.payload() { + assert!( + path.cost <= best + 1e-4, + "the output regressed: {best} then {}", + path.cost + ); + best = path.cost; + } + } + assert!(output.payload().is_some(), "no path after 24 quanta"); + + let state = task.debug_state(); + assert_eq!(state.published_cost, best); + assert!(state.best_cost <= state.published_cost); + assert!(state.published_quality > 0.0); + assert!(state.iterations > 0 && state.tree_size > 0); + } +} diff --git a/components/tasks/cu_rrt_star/src/rrt.rs b/components/tasks/cu_rrt_star/src/rrt.rs new file mode 100644 index 00000000000..7e2d86cce8b --- /dev/null +++ b/components/tasks/cu_rrt_star/src/rrt.rs @@ -0,0 +1,794 @@ +//! Seeded RRT* over a 2D world of round obstacles. +//! +//! The planner knows nothing about Copper: it only exposes [`RrtStar::grow`], +//! one bounded block of iterations. [`crate::RrtStarPlanner`] calls it once +//! from `base()` and once per anytime refinement quantum. +//! +//! The steps follow Karaman and Frazzoli: sample with goal bias, nearest, +//! steer, choose the cheapest parent, rewire the neighborhood, and prune by +//! branch and bound. Three points are stricter here than in a textbook write-up: +//! the final leg to the goal is collision checked and counted in the path cost, +//! rewiring refuses an ancestor so rounding cannot close a cycle, and the cost +//! shift after a rewire is iterative instead of recursive. + +use bincode::{Decode, Encode}; +use cu29::prelude::*; +use serde::{Deserialize, Serialize}; + +/// Waypoints carried by a published path. Kept at 32 because serde derives +/// array impls up to that size. +pub const MAX_WAYPOINTS: usize = 32; + +/// A point of the planar world, in meters. +#[derive( + Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect, +)] +pub struct Point2 { + pub x: f32, + pub y: f32, +} + +impl Point2 { + pub const fn new(x: f32, y: f32) -> Self { + Self { x, y } + } + + /// Euclidean distance to `other`. + pub fn distance(self, other: Self) -> f32 { + let (dx, dy) = (self.x - other.x, self.y - other.y); + (dx * dx + dy * dy).sqrt() + } +} + +/// A round obstacle: the planner rejects any point or segment within `radius` +/// of `center`. +#[derive(Debug, Clone, Copy, Reflect)] +pub struct Obstacle { + pub center: Point2, + pub radius: f32, +} + +/// The rectangular world `0..width` x `0..height` and its obstacles. +#[derive(Debug, Clone, Reflect)] +pub struct World { + pub width: f32, + pub height: f32, + pub obstacles: Vec, +} + +impl World { + /// The map every planner node of the example runs on: a 10x10 m depot with + /// five pillars, placed so the straight line from start to goal is blocked. + /// A first path is therefore always a detour, and refinement has real work + /// to do. + pub fn depot() -> Self { + Self { + width: 10.0, + height: 10.0, + obstacles: vec![ + Obstacle { + center: Point2::new(3.0, 3.0), + radius: 1.2, + }, + Obstacle { + center: Point2::new(6.0, 6.0), + radius: 1.5, + }, + Obstacle { + center: Point2::new(7.0, 2.5), + radius: 1.0, + }, + Obstacle { + center: Point2::new(2.5, 7.0), + radius: 1.0, + }, + Obstacle { + center: Point2::new(5.0, 1.5), + radius: 0.8, + }, + ], + } + } + + /// True when `point` is inside the bounds and outside every obstacle. + pub fn is_free(&self, point: Point2) -> bool { + if point.x < 0.0 || point.y < 0.0 || point.x > self.width || point.y > self.height { + return false; + } + self.obstacles + .iter() + .all(|o| point.distance(o.center) > o.radius) + } + + /// True when the whole segment `a`-`b` is free. + pub fn is_free_segment(&self, a: Point2, b: Point2) -> bool { + if !self.is_free(a) || !self.is_free(b) { + return false; + } + self.obstacles + .iter() + .all(|o| distance_to_segment(a, b, o.center) > o.radius) + } + + /// Area left free by the obstacles. Assumes every obstacle lies inside the + /// bounds and none overlap, which holds for [`World::depot`]. + pub fn free_area(&self) -> f32 { + let blocked: f32 = self + .obstacles + .iter() + .map(|o| core::f32::consts::PI * o.radius * o.radius) + .sum(); + (self.width * self.height - blocked).max(f32::EPSILON) + } + + /// The RRT* radius constant of Karaman and Frazzoli: + /// `gamma* = 2 * (1 + 1/d)^(1/d) * (free_area / zeta_d)^(1/d)`, here with + /// `d = 2` and `zeta_2 = pi`. + /// + /// A smaller constant shrinks the rewiring neighborhood below what + /// asymptotic optimality needs, and the planner degrades toward plain RRT + /// as the tree grows. + pub fn rrt_star_gamma(&self) -> f32 { + 2.0 * 1.5f32.sqrt() * (self.free_area() / core::f32::consts::PI).sqrt() + } +} + +/// Distance from `point` to the segment `a`-`b`. +fn distance_to_segment(a: Point2, b: Point2, point: Point2) -> f32 { + let (abx, aby) = (b.x - a.x, b.y - a.y); + let len_sq = abx * abx + aby * aby; + if len_sq <= f32::EPSILON { + return a.distance(point); + } + let t = (((point.x - a.x) * abx + (point.y - a.y) * aby) / len_sq).clamp(0.0, 1.0); + Point2::new(a.x + t * abx, a.y + t * aby).distance(point) +} + +/// Tuning knobs of the planner, all read from the node's RON `config:`. +#[derive(Debug, Clone, Copy, Reflect)] +pub struct RrtParams { + /// Longest edge the planner adds in one extension, in meters. + pub step_size: f32, + /// Probability of sampling the goal instead of a random point. + pub goal_bias: f32, + /// A node this close to the goal closes a path. + pub goal_threshold: f32, + /// Gamma of the RRT* rewiring radius `gamma * sqrt(ln n / n)`. `0.0` + /// derives it from the world through [`World::rrt_star_gamma`], which is + /// the value RRT* needs to converge to the optimum. + pub gamma: f32, + /// Branch-and-bound prune every N iterations; 0 disables pruning. + pub prune_interval: u32, + /// Hard cap on the tree size, so one job cannot grow without bound. + pub max_nodes: u32, +} + +impl Default for RrtParams { + fn default() -> Self { + Self { + step_size: 0.8, + goal_bias: 0.05, + goal_threshold: 0.5, + gamma: 0.0, + prune_interval: 512, + max_nodes: 4000, + } + } +} + +/// xorshift64*, so a given seed always replays the same tree. +#[derive(Debug, Clone, Reflect)] +pub struct Rng(u64); + +impl Rng { + pub fn new(seed: u64) -> Self { + // splitmix64 finalizer: consecutive seeds must not start on neighboring + // states, otherwise consecutive jobs explore almost the same tree. + let mut state = seed.wrapping_add(0x9E37_79B9_7F4A_7C15); + state = (state ^ (state >> 30)).wrapping_mul(0xBF58_476D_1CE4_E5B9); + state = (state ^ (state >> 27)).wrapping_mul(0x94D0_49BB_1331_11EB); + state ^= state >> 31; + // xorshift64* must never start at zero. + Self(if state == 0 { 1 } else { state }) + } + + fn next_u64(&mut self) -> u64 { + let mut x = self.0; + x ^= x >> 12; + x ^= x << 25; + x ^= x >> 27; + self.0 = x; + x.wrapping_mul(0x2545_F491_4F6C_DD1D) + } + + /// Uniform in `[0.0, 1.0)`. + pub fn next_f32(&mut self) -> f32 { + (self.next_u64() >> 40) as f32 / (1u32 << 24) as f32 + } +} + +/// One vertex of the tree. +#[derive(Debug, Clone, Reflect)] +struct TreeNode { + pos: Point2, + /// `None` for the root only. + parent: Option, + /// Path cost from the start to this node. + cost: f32, + children: Vec, +} + +/// An RRT* search for one start/goal pair. +/// +/// The tree only ever improves: `best_cost` is monotone non-increasing over +/// iterations, which is what makes the algorithm a good anytime task. +#[derive(Debug, Reflect)] +pub struct RrtStar { + world: World, + params: RrtParams, + start: Point2, + goal: Point2, + tree: Vec, + /// Node closing the best path found so far. + best_goal: Option, + /// Cost of the best path found so far, infinite until one is found. + best_cost: f32, + iterations: u32, + rng: Rng, + /// Reused between iterations to keep the search allocation-free. + scratch_near: Vec, + scratch_stack: Vec, +} + +impl RrtStar { + /// Starts a search rooted at `start`. An unreachable or blocked `start` + /// simply never grows a tree; the caller sees "no path" and the anytime + /// quality floor drops the result. + pub fn new( + world: World, + mut params: RrtParams, + start: Point2, + goal: Point2, + seed: u64, + ) -> Self { + if params.gamma <= 0.0 { + params.gamma = world.rrt_star_gamma(); + } + let mut planner = Self { + world, + params, + start, + goal, + tree: Vec::new(), + best_goal: None, + best_cost: f32::INFINITY, + iterations: 0, + rng: Rng::new(seed), + scratch_near: Vec::new(), + scratch_stack: Vec::new(), + }; + planner.reset(start, goal, seed); + planner + } + + /// Restarts the search on a new problem, keeping the capacity the previous + /// job grew: after the first job the planner asks the allocator for much + /// less. + pub fn reset(&mut self, start: Point2, goal: Point2, seed: u64) { + self.start = start; + self.goal = goal; + self.tree.clear(); + self.tree.push(TreeNode { + pos: start, + parent: None, + cost: 0.0, + children: Vec::new(), + }); + self.best_goal = None; + self.best_cost = f32::INFINITY; + self.iterations = 0; + self.rng = Rng::new(seed); + } + + /// Runs one bounded block of `iterations` RRT* iterations. + pub fn grow(&mut self, iterations: u32) { + for _ in 0..iterations { + self.iterations += 1; + if self.tree.len() < self.params.max_nodes as usize { + self.step(); + } + if self.params.prune_interval > 0 + && self.iterations.is_multiple_of(self.params.prune_interval) + && self.best_goal.is_some() + { + self.prune(); + } + } + } + + /// Cost of the best path so far, infinite while no path is known. + pub fn best_cost(&self) -> f32 { + self.best_cost + } + + /// True once a path to the goal exists. + pub fn has_solution(&self) -> bool { + self.best_goal.is_some() + } + + pub fn tree_size(&self) -> u32 { + self.tree.len() as u32 + } + + pub fn iterations(&self) -> u32 { + self.iterations + } + + /// True when the tree is full and pruning can never free room again, so no + /// further iteration can change anything. + pub fn is_exhausted(&self) -> bool { + self.tree.len() >= self.params.max_nodes as usize + && (self.params.prune_interval == 0 || self.best_goal.is_none()) + } + + /// Shortest conceivable path: the straight line, obstacles ignored. + pub fn lower_bound(&self) -> f32 { + self.start.distance(self.goal) + } + + /// Normalized quality in `0.0..=1.0`: how close the best path is to the + /// straight-line lower bound. 0.0 means no path yet, 1.0 means the path is + /// as short as the world allows. + pub fn quality(&self) -> f32 { + if !self.has_solution() { + return 0.0; + } + (self.lower_bound() / self.best_cost).clamp(0.0, 1.0) + } + + /// Nodes on the best path before shortcutting, the goal included. Zero + /// while no path is known. + pub fn tree_path_len(&self) -> usize { + let Some(goal_node) = self.best_goal else { + return 0; + }; + let mut len = 1; // the goal itself, which is not a tree node + let mut cursor = Some(goal_node); + while let Some(index) = cursor { + len += 1; + cursor = self.tree[index as usize].parent; + } + len + } + + /// Writes the best path into `out` and returns how many waypoints it used. + /// + /// The tree path routinely holds more nodes than [`MAX_WAYPOINTS`], so it + /// is shortcut first: from each waypoint the path jumps to the furthest + /// later one still reachable in a straight free line. Shortcutting is what + /// a planner publishes anyway, and it keeps every published segment + /// collision free - dropping the tail instead would publish a straight + /// jump across the map. + /// + /// The shortcut path is never longer than the tree path, so the reported + /// cost stays an upper bound on what the robot drives. `None` means even + /// the shortcut path does not fit; the caller then publishes nothing + /// rather than a path that cuts through an obstacle. + pub fn write_path(&self, out: &mut [Point2; MAX_WAYPOINTS]) -> Option { + let goal_node = self.best_goal?; + let mut chain = Vec::new(); + let mut cursor = Some(goal_node); + while let Some(index) = cursor { + let node = &self.tree[index as usize]; + chain.push(node.pos); + cursor = node.parent; + } + chain.reverse(); + chain.push(self.goal); + + let mut len = 0usize; + let mut at = 0usize; + loop { + if len == MAX_WAYPOINTS { + return None; + } + out[len] = chain[at]; + len += 1; + if at == chain.len() - 1 { + return Some(len as u32); + } + // The next tree node is always reachable - it is a tree edge - so + // the scan only looks for something further. + let mut next = at + 1; + for candidate in (at + 2)..chain.len() { + if self.world.is_free_segment(chain[at], chain[candidate]) { + next = candidate; + } + } + at = next; + } + } + + /// One RRT* iteration: sample, steer, choose the cheapest parent, rewire + /// the neighborhood, then check whether the new node closes a better path. + fn step(&mut self) { + let sample = self.sample(); + let nearest = self.nearest(sample); + let from = self.tree[nearest as usize].pos; + let new_pos = steer(from, sample, self.params.step_size); + if !self.world.is_free_segment(from, new_pos) { + return; + } + + let radius = self.near_radius(); + let mut near = core::mem::take(&mut self.scratch_near); + near.clear(); + for (index, node) in self.tree.iter().enumerate() { + if node.pos.distance(new_pos) <= radius { + near.push(index as u32); + } + } + + // Choose the parent that gives the cheapest path to the new node. + let mut parent = nearest; + let mut cost = self.tree[nearest as usize].cost + from.distance(new_pos); + for &index in near.iter() { + let candidate = &self.tree[index as usize]; + let candidate_cost = candidate.cost + candidate.pos.distance(new_pos); + if candidate_cost < cost && self.world.is_free_segment(candidate.pos, new_pos) { + parent = index; + cost = candidate_cost; + } + } + + let new_index = self.tree.len() as u32; + self.tree.push(TreeNode { + pos: new_pos, + parent: Some(parent), + cost, + children: Vec::new(), + }); + self.tree[parent as usize].children.push(new_index); + + // Rewire: neighbors that are cheaper to reach through the new node. + for &index in near.iter() { + if index == parent { + continue; + } + let (neighbor_pos, neighbor_cost) = { + let neighbor = &self.tree[index as usize]; + (neighbor.pos, neighbor.cost) + }; + let rewired_cost = cost + neighbor_pos.distance(new_pos); + if rewired_cost < neighbor_cost + && !self.is_ancestor(index, new_index) + && self.world.is_free_segment(new_pos, neighbor_pos) + { + self.reparent(index, new_index, rewired_cost); + } + } + self.scratch_near = near; + + // Does the new node close a better path? + let to_goal = new_pos.distance(self.goal); + if to_goal <= self.params.goal_threshold + && self.world.is_free_segment(new_pos, self.goal) + && cost + to_goal < self.best_cost + { + self.best_cost = cost + to_goal; + self.best_goal = Some(new_index); + } + // Rewiring may have shortened the current best path too. + if let Some(goal_node) = self.best_goal { + let node = &self.tree[goal_node as usize]; + self.best_cost = self.best_cost.min(node.cost + node.pos.distance(self.goal)); + } + } + + /// A random point of the world, biased toward the goal. + fn sample(&mut self) -> Point2 { + if self.rng.next_f32() < self.params.goal_bias { + return self.goal; + } + Point2::new( + self.rng.next_f32() * self.world.width, + self.rng.next_f32() * self.world.height, + ) + } + + /// Index of the tree node closest to `point`. Linear on purpose: a real + /// planner would index the tree, but a flat scan keeps the example short. + fn nearest(&self, point: Point2) -> u32 { + let mut best = 0u32; + let mut best_distance = f32::INFINITY; + for (index, node) in self.tree.iter().enumerate() { + let distance = node.pos.distance(point); + if distance < best_distance { + best_distance = distance; + best = index as u32; + } + } + best + } + + /// RRT* rewiring radius `gamma * sqrt(ln n / n)`, capped at one step. + fn near_radius(&self) -> f32 { + let n = (self.tree.len() as f32).max(2.0); + (self.params.gamma * (n.ln() / n).sqrt()).min(self.params.step_size) + } + + /// True when `candidate` sits on the path from `node` up to the root. + /// + /// Rewiring an ancestor would turn the tree into a graph with a cycle, and + /// every walk over it would then loop forever. Exact arithmetic already + /// rules it out - reaching an ancestor through its own descendant is never + /// cheaper - but rounding on two nearly coincident samples must not be able + /// to break that. + fn is_ancestor(&self, candidate: u32, node: u32) -> bool { + let mut cursor = self.tree[node as usize].parent; + while let Some(index) = cursor { + if index == candidate { + return true; + } + cursor = self.tree[index as usize].parent; + } + false + } + + /// Moves `node` under `new_parent` and shifts the cost of its whole + /// subtree by the same delta. + fn reparent(&mut self, node: u32, new_parent: u32, new_cost: f32) { + if let Some(old_parent) = self.tree[node as usize].parent { + self.tree[old_parent as usize] + .children + .retain(|&child| child != node); + } + self.tree[node as usize].parent = Some(new_parent); + self.tree[new_parent as usize].children.push(node); + + let delta = new_cost - self.tree[node as usize].cost; + let mut stack = core::mem::take(&mut self.scratch_stack); + stack.clear(); + stack.push(node); + while let Some(index) = stack.pop() { + self.tree[index as usize].cost += delta; + for i in 0..self.tree[index as usize].children.len() { + stack.push(self.tree[index as usize].children[i]); + } + } + self.scratch_stack = stack; + } + + /// Branch and bound: drop every node that cannot belong to a path better + /// than the best one known. + /// + /// Walking down from the root keeps the tree consistent: a node is kept + /// only if its parent is kept, so no orphan survives the compaction. The + /// triangle inequality makes that almost free anyway - a kept node's parent + /// always satisfies the bound as well. + fn prune(&mut self) { + // The best path itself is protected: rounding must never let branch and + // bound drop the path it is bounding against. + let mut protected = vec![false; self.tree.len()]; + let mut cursor = self.best_goal; + while let Some(index) = cursor { + protected[index as usize] = true; + cursor = self.tree[index as usize].parent; + } + + let mut keep = vec![false; self.tree.len()]; + let mut stack = core::mem::take(&mut self.scratch_stack); + stack.clear(); + stack.push(0); + keep[0] = true; + while let Some(index) = stack.pop() { + for i in 0..self.tree[index as usize].children.len() { + let child = self.tree[index as usize].children[i]; + let node = &self.tree[child as usize]; + if protected[child as usize] + || node.cost + node.pos.distance(self.goal) <= self.best_cost + { + keep[child as usize] = true; + stack.push(child); + } + } + } + self.scratch_stack = stack; + + let mut remap = vec![u32::MAX; self.tree.len()]; + let mut kept = Vec::with_capacity(self.tree.len()); + for (index, node) in self.tree.iter().enumerate() { + if keep[index] { + remap[index] = kept.len() as u32; + kept.push(TreeNode { + pos: node.pos, + parent: node.parent, + cost: node.cost, + children: Vec::new(), + }); + } + } + for node in kept.iter_mut() { + node.parent = node.parent.map(|parent| remap[parent as usize]); + } + for index in 0..kept.len() { + if let Some(parent) = kept[index].parent { + kept[parent as usize].children.push(index as u32); + } + } + self.best_goal = self.best_goal.map(|goal| remap[goal as usize]); + self.tree = kept; + } +} + +/// Point at most `step_size` away from `from` in the direction of `to`. +fn steer(from: Point2, to: Point2, step_size: f32) -> Point2 { + let distance = from.distance(to); + if distance <= step_size { + return to; + } + let ratio = step_size / distance; + Point2::new( + from.x + ratio * (to.x - from.x), + from.y + ratio * (to.y - from.y), + ) +} + +#[cfg(test)] +mod tests { + use super::*; + + const START: Point2 = Point2::new(0.5, 0.5); + const GOAL: Point2 = Point2::new(9.5, 9.5); + + fn planner(seed: u64) -> RrtStar { + RrtStar::new(World::depot(), RrtParams::default(), START, GOAL, seed) + } + + #[test] + fn segment_collision_is_detected() { + let world = World::depot(); + // Straight through the pillar at (3, 3). + assert!(!world.is_free_segment(Point2::new(1.0, 1.0), Point2::new(5.0, 5.0))); + // Along the free bottom edge. + assert!(world.is_free_segment(Point2::new(0.2, 0.2), Point2::new(0.2, 9.8))); + // Endpoints out of bounds. + assert!(!world.is_free_segment(START, Point2::new(11.0, 0.5))); + } + + #[test] + fn refinement_only_improves_the_path() { + let mut planner = planner(42); + planner.grow(400); + assert!(planner.has_solution(), "no first path after the base block"); + + let mut previous = planner.best_cost(); + for _ in 0..16 { + planner.grow(256); + assert!( + planner.best_cost() <= previous + 1e-4, + "cost went up: {} then {}", + previous, + planner.best_cost() + ); + previous = planner.best_cost(); + } + assert!(planner.quality() > 0.0 && planner.quality() <= 1.0); + assert!(planner.best_cost() >= planner.lower_bound()); + } + + /// Every published path must be drivable, at every stop point the anytime + /// policy could pick. + /// + /// A small `gamma` is included on purpose: it barely rewires, so its tree + /// paths grow past [`MAX_WAYPOINTS`] and the shortcut is exercised rather + /// than skipped. + #[test] + fn published_path_is_valid_at_every_stop_point() { + let world = World::depot(); + let mut longest_tree_path = 0; + for (seed, gamma) in (1..40u64).flat_map(|seed| [(seed, 0.0f32), (seed, 3.0)]) { + let params = RrtParams { + gamma, + ..Default::default() + }; + let mut planner = RrtStar::new(World::depot(), params, START, GOAL, seed); + planner.grow(400); + for _ in 0..24 { + planner.grow(256); + let mut waypoints = [Point2::default(); MAX_WAYPOINTS]; + let Some(len) = planner.write_path(&mut waypoints) else { + panic!("seed {seed}: the shortcut path did not fit"); + }; + longest_tree_path = longest_tree_path.max(planner.tree_path_len()); + assert!(len >= 2, "a path has at least a start and a goal"); + assert_eq!(waypoints[0], START); + assert_eq!(waypoints[(len - 1) as usize], GOAL); + for pair in waypoints[..len as usize].windows(2) { + assert!( + world.is_free_segment(pair[0], pair[1]), + "seed {seed}: published path crosses an obstacle" + ); + } + // The shortcut only removes waypoints it can bypass in a + // straight free line, so it never lengthens the path. + let published: f32 = waypoints[..len as usize] + .windows(2) + .map(|pair| pair[0].distance(pair[1])) + .sum(); + assert!( + published <= planner.best_cost() + 1e-3, + "seed {seed}: shortcut path {published} longer than the cost {}", + planner.best_cost() + ); + } + } + assert!( + longest_tree_path > MAX_WAYPOINTS, + "the tree path never outgrew MAX_WAYPOINTS, so the shortcut was never exercised" + ); + } + + /// A gamma below the value the free area implies shrinks the rewiring + /// neighborhood, and refinement then converges to a worse path. + #[test] + fn derived_gamma_beats_an_arbitrary_one() { + let world = World::depot(); + let derived = world.rrt_star_gamma(); + assert!( + (12.0..13.0).contains(&derived), + "gamma for the depot map should be near 12.4, got {derived}" + ); + + let cost_at = |gamma: f32| { + let mut total = 0.0; + for seed in 1..40u64 { + let params = RrtParams { + gamma, + ..Default::default() + }; + let mut planner = RrtStar::new(World::depot(), params, START, GOAL, seed); + planner.grow(400); + for _ in 0..24 { + planner.grow(256); + } + total += planner.best_cost(); + } + total + }; + assert!( + cost_at(0.0) < cost_at(3.0), + "the derived gamma should refine to a shorter path than a small one" + ); + } + + #[test] + fn same_seed_replays_the_same_tree() { + let (mut a, mut b) = (planner(11), planner(11)); + a.grow(600); + b.grow(300); + b.grow(300); + assert_eq!(a.tree_size(), b.tree_size()); + assert_eq!(a.best_cost(), b.best_cost()); + } + + #[test] + fn pruning_keeps_the_best_path_reachable() { + let mut planner = planner(3); + planner.grow(1500); + let cost_before = planner.best_cost(); + planner.prune(); + assert!(planner.has_solution(), "pruning dropped the goal node"); + // The tree stays consistent: every node still reaches the root. + for index in 0..planner.tree.len() { + let mut cursor = Some(index as u32); + let mut hops = 0; + while let Some(current) = cursor { + cursor = planner.tree[current as usize].parent; + hops += 1; + assert!(hops <= planner.tree.len(), "cycle in the tree"); + } + } + assert_eq!(planner.best_cost(), cost_before); + } +} From 0fea739a1c3b53d9cc298a412103ed9815f1566b Mon Sep 17 00:00:00 2001 From: Scofield626 Date: Mon, 3 Aug 2026 14:18:12 +0000 Subject: [PATCH 4/7] cu-rrt-star: carry the map in the plan request --- components/tasks/cu_rrt_star/README.md | 5 +- components/tasks/cu_rrt_star/src/lib.rs | 11 +- components/tasks/cu_rrt_star/src/rrt.rs | 144 +++++++++++++++--------- 3 files changed, 103 insertions(+), 57 deletions(-) diff --git a/components/tasks/cu_rrt_star/README.md b/components/tasks/cu_rrt_star/README.md index 4d38393e6aa..7596481be4c 100644 --- a/components/tasks/cu_rrt_star/README.md +++ b/components/tasks/cu_rrt_star/README.md @@ -8,7 +8,10 @@ refinement quanta run. ### Input and output -- Input: `cu_rrt_star::PlanRequest` — start, goal and the RNG seed of the job. +- Input: `cu_rrt_star::PlanRequest` — the world (up to `MAX_OBSTACLES` (16) + round obstacles in a rectangle), start, goal and the RNG seed of the job. + The map travels with the job: the planner has no map of its own, so the + source may change the map between jobs. - Output: `cu_rrt_star::PlanPath` — up to `MAX_WAYPOINTS` (32) waypoints. Every consecutive pair of waypoints is collision free, so the path can be driven as published, whichever stop point the policy picked. `cost` is the diff --git a/components/tasks/cu_rrt_star/src/lib.rs b/components/tasks/cu_rrt_star/src/lib.rs index 477b8c32d68..f864d886c43 100644 --- a/components/tasks/cu_rrt_star/src/lib.rs +++ b/components/tasks/cu_rrt_star/src/lib.rs @@ -19,10 +19,12 @@ use serde::{Deserialize, Serialize}; /// nothing left to refine. const CONVERGED_QUALITY: f32 = 0.999; -/// One planning problem. A different seed per job makes each job a fresh RRT* -/// run rather than a replay of the previous one. +/// One planning problem. The map travels with the job, so the source owns it +/// and may change it between jobs. A different seed per job makes each job a +/// fresh RRT* run rather than a replay of the previous one. #[derive(Default, Debug, Clone, Encode, Decode, Serialize, Deserialize, Reflect)] pub struct PlanRequest { + pub world: World, pub start: Point2, pub goal: Point2, pub seed: u64, @@ -201,11 +203,11 @@ impl CuAnytimeTask for RrtStarPlanner { let planner = match self.planner.as_mut() { // Restart on the previous job's memory instead of a fresh tree. Some(planner) => { - planner.reset(request.start, request.goal, request.seed); + planner.reset(&request.world, request.start, request.goal, request.seed); planner } None => self.planner.insert(RrtStar::new( - World::depot(), + &request.world, self.params, request.start, request.goal, @@ -262,6 +264,7 @@ mod tests { let ctx = CuContext::new_with_clock(); let mut task = RrtStarPlanner::new(None, ()).unwrap(); let input = CuMsg::new(Some(PlanRequest { + world: World::depot(), start: START, goal: GOAL, seed: 42, diff --git a/components/tasks/cu_rrt_star/src/rrt.rs b/components/tasks/cu_rrt_star/src/rrt.rs index 7e2d86cce8b..e49fa2967c2 100644 --- a/components/tasks/cu_rrt_star/src/rrt.rs +++ b/components/tasks/cu_rrt_star/src/rrt.rs @@ -19,6 +19,11 @@ use serde::{Deserialize, Serialize}; /// array impls up to that size. pub const MAX_WAYPOINTS: usize = 32; +/// Obstacles carried by a [`World`]. Bounded so the map can travel inside a +/// [`crate::PlanRequest`] without allocating; must stay at or under 32 for +/// the same serde reason as [`MAX_WAYPOINTS`]. +pub const MAX_OBSTACLES: usize = 16; + /// A point of the planar world, in meters. #[derive( Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect, @@ -42,52 +47,78 @@ impl Point2 { /// A round obstacle: the planner rejects any point or segment within `radius` /// of `center`. -#[derive(Debug, Clone, Copy, Reflect)] +#[derive( + Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect, +)] pub struct Obstacle { pub center: Point2, pub radius: f32, } +impl Obstacle { + pub const fn new(center: Point2, radius: f32) -> Self { + Self { center, radius } + } +} + /// The rectangular world `0..width` x `0..height` and its obstacles. -#[derive(Debug, Clone, Reflect)] +/// +/// The obstacle storage is a fixed array so the map can travel inside a +/// [`crate::PlanRequest`]: the planner has no map of its own, every job +/// carries the one it must solve. +#[derive(Default, Debug, Clone, Encode, Decode, Serialize, Deserialize, Reflect)] pub struct World { pub width: f32, pub height: f32, - pub obstacles: Vec, + pub obstacles: [Obstacle; MAX_OBSTACLES], + /// Obstacles actually used in `obstacles`. + pub obstacle_count: u32, } impl World { - /// The map every planner node of the example runs on: a 10x10 m depot with - /// five pillars, placed so the straight line from start to goal is blocked. - /// A first path is therefore always a detour, and refinement has real work - /// to do. + /// A world from a list of obstacles. Errors when the list exceeds + /// [`MAX_OBSTACLES`]. + pub fn new(width: f32, height: f32, obstacles: &[Obstacle]) -> CuResult { + if obstacles.len() > MAX_OBSTACLES { + return Err(format!( + "rrt*: {} obstacles, the world holds at most {MAX_OBSTACLES}", + obstacles.len() + ) + .into()); + } + let mut world = Self { + width, + height, + obstacles: [Obstacle::default(); MAX_OBSTACLES], + obstacle_count: obstacles.len() as u32, + }; + world.obstacles[..obstacles.len()].copy_from_slice(obstacles); + Ok(world) + } + + /// The map the demo and the tests run on: a 10x10 m depot with five + /// pillars, placed so the straight line between opposite corners is + /// blocked. A first path is therefore always a detour, and refinement has + /// real work to do. pub fn depot() -> Self { - Self { - width: 10.0, - height: 10.0, - obstacles: vec![ - Obstacle { - center: Point2::new(3.0, 3.0), - radius: 1.2, - }, - Obstacle { - center: Point2::new(6.0, 6.0), - radius: 1.5, - }, - Obstacle { - center: Point2::new(7.0, 2.5), - radius: 1.0, - }, - Obstacle { - center: Point2::new(2.5, 7.0), - radius: 1.0, - }, - Obstacle { - center: Point2::new(5.0, 1.5), - radius: 0.8, - }, + Self::new( + 10.0, + 10.0, + &[ + Obstacle::new(Point2::new(3.0, 3.0), 1.2), + Obstacle::new(Point2::new(6.0, 6.0), 1.5), + Obstacle::new(Point2::new(7.0, 2.5), 1.0), + Obstacle::new(Point2::new(2.5, 7.0), 1.0), + Obstacle::new(Point2::new(5.0, 1.5), 0.8), ], - } + ) + .expect("the depot obstacles fit MAX_OBSTACLES") + } + + /// The obstacles in use, with a count out of range clamped rather than + /// trusted: a hand-built `World` must not be able to cause a panic here. + fn obstacle_slice(&self) -> &[Obstacle] { + &self.obstacles[..(self.obstacle_count as usize).min(MAX_OBSTACLES)] } /// True when `point` is inside the bounds and outside every obstacle. @@ -95,7 +126,7 @@ impl World { if point.x < 0.0 || point.y < 0.0 || point.x > self.width || point.y > self.height { return false; } - self.obstacles + self.obstacle_slice() .iter() .all(|o| point.distance(o.center) > o.radius) } @@ -105,7 +136,7 @@ impl World { if !self.is_free(a) || !self.is_free(b) { return false; } - self.obstacles + self.obstacle_slice() .iter() .all(|o| distance_to_segment(a, b, o.center) > o.radius) } @@ -114,7 +145,7 @@ impl World { /// bounds and none overlap, which holds for [`World::depot`]. pub fn free_area(&self) -> f32 { let blocked: f32 = self - .obstacles + .obstacle_slice() .iter() .map(|o| core::f32::consts::PI * o.radius * o.radius) .sum(); @@ -226,6 +257,9 @@ struct TreeNode { pub struct RrtStar { world: World, params: RrtParams, + /// The rewiring gamma in effect for the current job: the configured one, + /// or the one derived from the job's world when the config left it at 0. + gamma: f32, start: Point2, goal: Point2, tree: Vec, @@ -244,19 +278,11 @@ impl RrtStar { /// Starts a search rooted at `start`. An unreachable or blocked `start` /// simply never grows a tree; the caller sees "no path" and the anytime /// quality floor drops the result. - pub fn new( - world: World, - mut params: RrtParams, - start: Point2, - goal: Point2, - seed: u64, - ) -> Self { - if params.gamma <= 0.0 { - params.gamma = world.rrt_star_gamma(); - } + pub fn new(world: &World, params: RrtParams, start: Point2, goal: Point2, seed: u64) -> Self { let mut planner = Self { - world, + world: world.clone(), params, + gamma: 0.0, start, goal, tree: Vec::new(), @@ -267,14 +293,21 @@ impl RrtStar { scratch_near: Vec::new(), scratch_stack: Vec::new(), }; - planner.reset(start, goal, seed); + planner.reset(world, start, goal, seed); planner } /// Restarts the search on a new problem, keeping the capacity the previous /// job grew: after the first job the planner asks the allocator for much /// less. - pub fn reset(&mut self, start: Point2, goal: Point2, seed: u64) { + pub fn reset(&mut self, world: &World, start: Point2, goal: Point2, seed: u64) { + self.world = world.clone(); + // The map can change between jobs, so a derived gamma must follow it. + self.gamma = if self.params.gamma > 0.0 { + self.params.gamma + } else { + world.rrt_star_gamma() + }; self.start = start; self.goal = goal; self.tree.clear(); @@ -514,7 +547,7 @@ impl RrtStar { /// RRT* rewiring radius `gamma * sqrt(ln n / n)`, capped at one step. fn near_radius(&self) -> f32 { let n = (self.tree.len() as f32).max(2.0); - (self.params.gamma * (n.ln() / n).sqrt()).min(self.params.step_size) + (self.gamma * (n.ln() / n).sqrt()).min(self.params.step_size) } /// True when `candidate` sits on the path from `node` up to the root. @@ -642,7 +675,14 @@ mod tests { const GOAL: Point2 = Point2::new(9.5, 9.5); fn planner(seed: u64) -> RrtStar { - RrtStar::new(World::depot(), RrtParams::default(), START, GOAL, seed) + RrtStar::new(&World::depot(), RrtParams::default(), START, GOAL, seed) + } + + #[test] + fn world_rejects_too_many_obstacles() { + let too_many = [Obstacle::new(Point2::new(1.0, 1.0), 0.1); MAX_OBSTACLES + 1]; + assert!(World::new(10.0, 10.0, &too_many).is_err()); + assert!(World::new(10.0, 10.0, &too_many[..MAX_OBSTACLES]).is_ok()); } #[test] @@ -692,7 +732,7 @@ mod tests { gamma, ..Default::default() }; - let mut planner = RrtStar::new(World::depot(), params, START, GOAL, seed); + let mut planner = RrtStar::new(&World::depot(), params, START, GOAL, seed); planner.grow(400); for _ in 0..24 { planner.grow(256); @@ -747,7 +787,7 @@ mod tests { gamma, ..Default::default() }; - let mut planner = RrtStar::new(World::depot(), params, START, GOAL, seed); + let mut planner = RrtStar::new(&World::depot(), params, START, GOAL, seed); planner.grow(400); for _ in 0..24 { planner.grow(256); From 7a45e292f45345ab1dd064428512575557dbaab6 Mon Sep 17 00:00:00 2001 From: Scofield626 Date: Mon, 3 Aug 2026 14:19:40 +0000 Subject: [PATCH 5/7] cu-rrt-star: dual-policy integration test --- .../tasks/cu_rrt_star/tests/copperconfig.ron | 75 ++++++++ .../cu_rrt_star/tests/rrt_star_tester.rs | 182 ++++++++++++++++++ 2 files changed, 257 insertions(+) create mode 100644 components/tasks/cu_rrt_star/tests/copperconfig.ron create mode 100644 components/tasks/cu_rrt_star/tests/rrt_star_tester.rs diff --git a/components/tasks/cu_rrt_star/tests/copperconfig.ron b/components/tasks/cu_rrt_star/tests/copperconfig.ron new file mode 100644 index 00000000000..f1a2dc61b90 --- /dev/null +++ b/components/tasks/cu_rrt_star/tests/copperconfig.ron @@ -0,0 +1,75 @@ +( + tasks: [ + ( + id: "goal", + type: "GoalSrc", + config: {"seed": 1}, + ), + ( + id: "quick_planner", + type: "cu_rrt_star::RrtStarPlanner", + config: { + "base_iterations": 400, + "block_iterations": 256, + "step_size": 0.8, + "goal_bias": 0.05, + "goal_threshold": 0.5, + "gamma": 0.0, + "prune_interval": 512, + "max_nodes": 4000, + }, + anytime: ( + max_refines: 2, + time_budget_ms: 50.0, + quality_target: 0.93, + quality_floor: 0.05, + ), + ), + ( + id: "thorough_planner", + type: "cu_rrt_star::RrtStarPlanner", + config: { + "base_iterations": 400, + "block_iterations": 256, + "step_size": 0.8, + "goal_bias": 0.05, + "goal_threshold": 0.5, + "gamma": 0.0, + "prune_interval": 512, + "max_nodes": 4000, + }, + anytime: ( + max_refines: 24, + time_budget_ms: 250.0, + max_stall: 4, + quality_floor: 0.05, + ), + ), + ( + id: "monitor", + type: "ComparisonSink", + ), + ], + cnx: [ + ( + src: "goal", + dst: "quick_planner", + msg: "cu_rrt_star::PlanRequest", + ), + ( + src: "goal", + dst: "thorough_planner", + msg: "cu_rrt_star::PlanRequest", + ), + ( + src: "quick_planner", + dst: "monitor", + msg: "cu_rrt_star::PlanPath", + ), + ( + src: "thorough_planner", + dst: "monitor", + msg: "cu_rrt_star::PlanPath", + ), + ], +) diff --git a/components/tasks/cu_rrt_star/tests/rrt_star_tester.rs b/components/tasks/cu_rrt_star/tests/rrt_star_tester.rs new file mode 100644 index 00000000000..d3b4ea79243 --- /dev/null +++ b/components/tasks/cu_rrt_star/tests/rrt_star_tester.rs @@ -0,0 +1,182 @@ +//! The planner under two refinement policies, in a full application. +//! +//! Both planner nodes are the same task type with the same RRT* `config:`, so +//! they run the same tree from the same seed. Only the `anytime:` policy +//! differs: +//! +//! | node | policy | +//! |---|---| +//! | `quick_planner` | `max_refines: 2`, `time_budget_ms: 50`, `quality_target: 0.93` | +//! | `thorough_planner` | `max_refines: 24`, `time_budget_ms: 250`, `max_stall: 4` | +//! +//! Because both nodes start from the same seed, the thorough tree is the quick +//! tree plus more iterations, so its path is never longer - that is the anytime +//! trade-off, measured. Keeping the two `config:` blocks identical is what +//! makes the comparison valid. + +use cu_rrt_star::{PlanPath, PlanRequest, Point2, World}; +use cu29::prelude::*; +use std::sync::Mutex; + +#[copper_runtime(config = "tests/copperconfig.ron")] +struct DualPolicyTester {} + +/// Start and goal of every planning job. +const START: Point2 = Point2::new(0.5, 0.5); +const GOAL: Point2 = Point2::new(9.5, 9.5); + +const SLAB_SIZE: Option = Some(16 * 1024 * 1024); +const ITERATIONS: usize = 10; + +/// What the sink saw, one entry per copperlist. +static REPORTS: Mutex> = Mutex::new(Vec::new()); + +/// Takes the reports collected so far, leaving the sink ready for a new run. +fn take_reports() -> Vec { + core::mem::take(&mut *REPORTS.lock().expect("reports poisoned")) +} + +/// One copperlist as the sink saw it. A `None` path means the node published +/// nothing: no path yet, or a quality below the configured floor. +#[derive(Debug, Clone)] +struct PlanReport { + quick: Option, + thorough: Option, +} + +/// Emits one planning problem per copperlist, always on the depot map. +#[derive(Default, Reflect)] +pub struct GoalSrc { + seed: u64, +} + +impl Freezable for GoalSrc {} + +impl CuSrcTask for GoalSrc { + type Resources<'r> = (); + type Output<'m> = output_msg!(PlanRequest); + + fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult { + let seed = match config { + Some(config) => config.get::("seed")?.unwrap_or(1) as u64, + None => 1, + }; + Ok(Self { seed }) + } + + fn process(&mut self, ctx: &CuContext, new_msg: &mut Self::Output<'_>) -> CuResult<()> { + self.seed = self.seed.wrapping_add(1); + new_msg.set_payload(PlanRequest { + world: World::depot(), + start: START, + goal: GOAL, + seed: self.seed, + }); + // A fresh Tov per job. An anytime node reads it as the age anchor when + // its policy sets max_age_ms; neither planner here does, so both + // anchor on their own job start instead. + new_msg.tov = Tov::Time(ctx.clock.now()); + Ok(()) + } +} + +/// Records what both planners published in the same copperlist. +#[derive(Default, Reflect)] +pub struct ComparisonSink; + +impl Freezable for ComparisonSink {} + +impl CuSinkTask for ComparisonSink { + type Resources<'r> = (); + type Input<'m> = input_msg!('m, PlanPath, PlanPath); + + fn new(_config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult { + Ok(Self) + } + + fn process(&mut self, _ctx: &CuContext, input: &Self::Input<'_>) -> CuResult<()> { + // Input order follows the cnx order in the RON: quick first. + let (quick, thorough): (&CuMsg, &CuMsg) = *input; + REPORTS.lock().expect("reports poisoned").push(PlanReport { + quick: quick.payload().cloned(), + thorough: thorough.payload().cloned(), + }); + Ok(()) + } +} + +/// Runs the whole application once and returns what the sink saw, one entry +/// per copperlist. +fn run(logger_path: &std::path::Path) -> Vec { + let mut application = DualPolicyTester::builder() + .with_log_path(logger_path, SLAB_SIZE) + .expect("Failed to setup logger.") + .build() + .expect("Failed to create application."); + application + .start_all_tasks() + .expect("Failed to start application."); + for _ in 0..ITERATIONS { + application + .run_one_iteration() + .expect("Failed to run application."); + } + application + .stop_all_tasks() + .expect("Failed to stop application."); + take_reports() +} + +/// Checks the anytime contract on what the sink saw and returns how many jobs +/// both planners published. +fn check(reports: &[PlanReport]) -> usize { + assert_eq!(reports.len(), ITERATIONS, "one report per copperlist"); + let world = World::depot(); + let lower_bound = START.distance(GOAL); + let mut compared = 0; + + for (index, report) in reports.iter().enumerate() { + for path in [&report.quick, &report.thorough].into_iter().flatten() { + assert!(path.len >= 2, "job {index}: a path needs two waypoints"); + assert_eq!(path.waypoints[0], START, "job {index}"); + assert_eq!(path.waypoints[(path.len - 1) as usize], GOAL, "job {index}"); + // A published path must be drivable as published, whichever stop + // point the policy picked. + for pair in path.waypoints[..path.len as usize].windows(2) { + assert!( + world.is_free_segment(pair[0], pair[1]), + "job {index}: published path crosses an obstacle" + ); + } + assert!( + path.cost >= lower_bound, + "job {index}: path shorter than the straight line" + ); + } + + let (Some(quick), Some(thorough)) = (&report.quick, &report.thorough) else { + continue; + }; + compared += 1; + // More quanta on the same seed can only shorten the path. + assert!( + thorough.cost <= quick.cost + 1e-3, + "job {index}: the thorough policy published a longer path ({} vs {})", + thorough.cost, + quick.cost + ); + } + + assert!( + compared >= ITERATIONS / 2, + "both planners published in only {compared} of {ITERATIONS} jobs" + ); + compared +} + +#[test] +fn dual_policy_refines_toward_shorter_paths() { + let tmp_dir = tempfile::TempDir::new().expect("could not create a tmp dir"); + let reports = run(&tmp_dir.path().join("rrt_star_tester.copper")); + check(&reports); +} From 925bfc3531e2f7cb7fd297bf35605a481c3a356f Mon Sep 17 00:00:00 2001 From: Scofield626 Date: Mon, 3 Aug 2026 14:26:25 +0000 Subject: [PATCH 6/7] cu_anytime_rrt_star: closed-loop navigation demo on cu-rrt-star --- examples/cu_anytime_rrt_star/Cargo.toml | 5 +- examples/cu_anytime_rrt_star/copperconfig.ron | 66 +- examples/cu_anytime_rrt_star/src/main.rs | 154 +--- examples/cu_anytime_rrt_star/src/rrt.rs | 794 ------------------ examples/cu_anytime_rrt_star/src/tasks.rs | 543 +++++------- 5 files changed, 276 insertions(+), 1286 deletions(-) delete mode 100644 examples/cu_anytime_rrt_star/src/rrt.rs diff --git a/examples/cu_anytime_rrt_star/Cargo.toml b/examples/cu_anytime_rrt_star/Cargo.toml index 970ead8bfbb..f016a626f80 100644 --- a/examples/cu_anytime_rrt_star/Cargo.toml +++ b/examples/cu_anytime_rrt_star/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "cu-anytime-rrt-star" -description = "Example for the Copper project showing an anytime task: an RRT* planner that publishes a first path, then improves it quantum by quantum." +description = "Example for the Copper project: closed-loop navigation on the cu-rrt-star anytime planner, visualized with Rerun." version.workspace = true authors.workspace = true edition.workspace = true @@ -15,8 +15,9 @@ publish = false [dependencies] bincode = { workspace = true } +cu-rrt-star = { workspace = true } cu29 = { workspace = true } -serde = { workspace = true } +rerun = { workspace = true } [build-dependencies] cu29-build = { workspace = true } diff --git a/examples/cu_anytime_rrt_star/copperconfig.ron b/examples/cu_anytime_rrt_star/copperconfig.ron index 0052a112bb5..de45048392e 100644 --- a/examples/cu_anytime_rrt_star/copperconfig.ron +++ b/examples/cu_anytime_rrt_star/copperconfig.ron @@ -1,75 +1,49 @@ ( + runtime: (rate_target_hz: 20), tasks: [ ( - id: "goal", - type: "tasks::GoalSrc", - config: {"seed": 1}, - ), - ( - id: "quick_planner", - type: "tasks::RrtStarPlanner", + id: "nav", + type: "tasks::NavSim", config: { - "base_iterations": 400, - "block_iterations": 256, - "step_size": 0.8, - "goal_bias": 0.05, - "goal_threshold": 0.5, - "gamma": 0.0, - "prune_interval": 512, - "max_nodes": 4000, + "seed": 1, + "speed_mps": 1.5, }, - anytime: ( - max_refines: 2, - time_budget_ms: 50.0, - quality_target: 0.93, - quality_floor: 0.05, - ), ), ( - id: "thorough_planner", - type: "tasks::RrtStarPlanner", + id: "planner", + type: "cu_rrt_star::RrtStarPlanner", config: { "base_iterations": 400, "block_iterations": 256, - "step_size": 0.8, - "goal_bias": 0.05, - "goal_threshold": 0.5, "gamma": 0.0, - "prune_interval": 512, - "max_nodes": 4000, }, anytime: ( - max_refines: 24, - time_budget_ms: 250.0, + max_refines: 16, + time_budget_ms: 30.0, max_stall: 4, quality_floor: 0.05, ), ), ( - id: "monitor", - type: "tasks::ComparisonSink", + id: "viewer", + type: "tasks::RerunViewer", ), ], cnx: [ ( - src: "goal", - dst: "quick_planner", - msg: "crate::tasks::PlanRequest", - ), - ( - src: "goal", - dst: "thorough_planner", - msg: "crate::tasks::PlanRequest", + src: "nav", + dst: "planner", + msg: "cu_rrt_star::PlanRequest", ), ( - src: "quick_planner", - dst: "monitor", - msg: "crate::tasks::PlanPath", + src: "nav", + dst: "viewer", + msg: "cu_rrt_star::PlanRequest", ), ( - src: "thorough_planner", - dst: "monitor", - msg: "crate::tasks::PlanPath", + src: "planner", + dst: "viewer", + msg: "cu_rrt_star::PlanPath", ), ], ) \ No newline at end of file diff --git a/examples/cu_anytime_rrt_star/src/main.rs b/examples/cu_anytime_rrt_star/src/main.rs index 5e8916181cf..37fe8213615 100644 --- a/examples/cu_anytime_rrt_star/src/main.rs +++ b/examples/cu_anytime_rrt_star/src/main.rs @@ -1,31 +1,22 @@ -//! Anytime RRT*: the same planner under two refinement policies. +//! Closed-loop navigation on the `cu-rrt-star` anytime planner, visualized +//! with Rerun. //! -//! `base()` grows the tree until it has a first, crude path; every `refine()` -//! runs one more block of RRT* iterations and republishes only when the path -//! got shorter. The task reports how good the path is; the RON `anytime:` -//! policy decides how long to keep going. +//! A simulated point robot patrols the corners of the depot map. Every +//! copperlist, `tasks::NavSim` advances the robot along the newest published +//! path and asks for a fresh plan from where the robot is now; the planner +//! refines each plan for as long as its `anytime:` policy allows; and +//! `tasks::RerunViewer` draws the world, the robot, its trail, the current +//! path and the per-cycle quality and iteration curves. //! -//! Both planner nodes are the same task type with the same RRT* `config:`, so -//! they run the same tree from the same seed. Only the policy differs: +//! The viewer spawns a local Rerun instance, so install it first: +//! `cargo install rerun-cli --locked` (or `pip install rerun-sdk`). Then: +//! `cargo run --release -p cu-anytime-rrt-star`, and stop with Ctrl-C. +//! To record to a file instead, set `"rrd": "demo.rrd"` in the viewer's +//! `config:` block. //! -//! | node | policy | meaning | -//! |---|---|---| -//! | `quick_planner` | `max_refines: 2`, `time_budget_ms: 50`, `quality_target: 0.93` | two quanta at most, and stop early once the path is within 7% of the straight line | -//! | `thorough_planner` | `max_refines: 24`, `time_budget_ms: 250`, `max_stall: 4` | up to 24 quanta, but give up after 4 that improved nothing | -//! -//! Both carry `quality_floor: 0.05`, which drops a job that found no path at -//! all: the sink then sees no payload. -//! -//! Because both nodes start from the same seed, the thorough tree is the quick -//! tree plus more iterations, so its path is never longer - that is the anytime -//! trade-off, measured. Keeping the two `config:` blocks identical is what -//! makes the comparison below valid. -//! -//! The RRT* `gamma` is `0.0` in the RON, which asks the planner to derive the -//! rewiring radius constant from the map. Pinning it to an arbitrary smaller -//! number is what makes an RRT* implementation quietly stop converging. +//! The quick-vs-thorough policy comparison this example used to carry lives +//! in `components/tasks/cu_rrt_star/tests/`. -mod rrt; mod tasks; use cu29::prelude::*; @@ -36,16 +27,15 @@ use std::path::Path; struct App {} const SLAB_SIZE: Option = Some(16 * 1024 * 1024); -const ITERATIONS: usize = 10; -/// Runs the whole application once and returns what the sink saw, one entry -/// per copperlist. -fn run(logger_path: &str) -> Vec { +fn main() { + let logger_path = "logs/anytime_rrt_star.copper"; if let Some(parent) = Path::new(logger_path).parent() && !parent.exists() { fs::create_dir_all(parent).expect("Failed to create logs directory"); } + let mut application = App::builder() .with_log_path(logger_path, SLAB_SIZE) .expect("Failed to setup logger.") @@ -54,110 +44,8 @@ fn run(logger_path: &str) -> Vec { application .start_all_tasks() .expect("Failed to start application."); - for _ in 0..ITERATIONS { - application - .run_one_iteration() - .expect("Failed to run application."); - } - application - .stop_all_tasks() - .expect("Failed to stop application."); - tasks::take_reports() -} - -/// Checks the anytime contract on what the sink saw and returns how many jobs -/// both planners published. -fn check(reports: &[tasks::PlanReport]) -> usize { - assert_eq!(reports.len(), ITERATIONS, "one report per copperlist"); - let world = rrt::World::depot(); - let lower_bound = tasks::START.distance(tasks::GOAL); - let mut compared = 0; - - for (index, report) in reports.iter().enumerate() { - for path in [&report.quick, &report.thorough].into_iter().flatten() { - assert!(path.len >= 2, "job {index}: a path needs two waypoints"); - assert_eq!(path.waypoints[0], tasks::START, "job {index}"); - assert_eq!( - path.waypoints[(path.len - 1) as usize], - tasks::GOAL, - "job {index}" - ); - // A published path must be drivable as published, whichever stop - // point the policy picked. - for pair in path.waypoints[..path.len as usize].windows(2) { - assert!( - world.is_free_segment(pair[0], pair[1]), - "job {index}: published path crosses an obstacle" - ); - } - assert!( - path.cost >= lower_bound, - "job {index}: path shorter than the straight line" - ); - } - - let (Some(quick), Some(thorough)) = (&report.quick, &report.thorough) else { - continue; - }; - compared += 1; - // More quanta on the same seed can only shorten the path. - assert!( - thorough.cost <= quick.cost + 1e-3, - "job {index}: the thorough policy published a longer path ({} vs {})", - thorough.cost, - quick.cost - ); - } - - assert!( - compared >= ITERATIONS / 2, - "both planners published in only {compared} of {ITERATIONS} jobs" - ); - compared -} - -fn main() { - let reports = run("logs/anytime_rrt_star.copper"); - - let lower_bound = tasks::START.distance(tasks::GOAL); - println!("straight line start -> goal: {lower_bound:.2} m (quality 1.0)"); - println!("{:<5} {:>28} {:>28}", "job", "quick", "thorough"); - for (index, report) in reports.iter().enumerate() { - println!( - "{:<5} {:>28} {:>28}", - index, - describe(&report.quick, &report.quick_status, lower_bound), - describe(&report.thorough, &report.thorough_status, lower_bound), - ); - } - - let compared = check(&reports); - println!("anytime RRT* example OK: {compared}/{ITERATIONS} jobs compared"); -} - -/// One cell of the table: cost, quality and the runtime's anytime stamp. -fn describe(path: &Option, status: &str, lower_bound: f32) -> String { - match path { - Some(path) => format!( - "{:.2}m q={:.2} [{}]", - path.cost, - lower_bound / path.cost, - status - ), - None => format!("no path [{status}]"), - } -} - -#[cfg(test)] -mod tests { - use super::*; - - /// The self-check of `main`, as a test: CI only builds the examples, so - /// without this nothing ever runs the anytime path. - #[test] - fn both_policies_publish_drivable_paths() { - let logger_path = std::env::temp_dir().join("cu_anytime_rrt_star_test.copper"); - let reports = run(logger_path.to_str().expect("non-utf8 temp dir")); - check(&reports); + // Paced by `runtime.rate_target_hz` in the RON; stops on Ctrl-C. + if let Err(error) = application.run() { + eprintln!("Error while running: {error}"); } } diff --git a/examples/cu_anytime_rrt_star/src/rrt.rs b/examples/cu_anytime_rrt_star/src/rrt.rs deleted file mode 100644 index 996af5d2e2a..00000000000 --- a/examples/cu_anytime_rrt_star/src/rrt.rs +++ /dev/null @@ -1,794 +0,0 @@ -//! Seeded RRT* over a 2D world of round obstacles. -//! -//! The planner knows nothing about Copper: it only exposes [`RrtStar::grow`], -//! one bounded block of iterations. `tasks.rs` calls it once from `base()` and -//! once per anytime refinement quantum. -//! -//! The steps follow Karaman and Frazzoli: sample with goal bias, nearest, -//! steer, choose the cheapest parent, rewire the neighborhood, and prune by -//! branch and bound. Three points are stricter here than in a textbook write-up: -//! the final leg to the goal is collision checked and counted in the path cost, -//! rewiring refuses an ancestor so rounding cannot close a cycle, and the cost -//! shift after a rewire is iterative instead of recursive. - -use bincode::{Decode, Encode}; -use cu29::prelude::*; -use serde::{Deserialize, Serialize}; - -/// Waypoints carried by a published path. Kept at 32 because serde derives -/// array impls up to that size. -pub const MAX_WAYPOINTS: usize = 32; - -/// A point of the planar world, in meters. -#[derive( - Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect, -)] -pub struct Point2 { - pub x: f32, - pub y: f32, -} - -impl Point2 { - pub const fn new(x: f32, y: f32) -> Self { - Self { x, y } - } - - /// Euclidean distance to `other`. - pub fn distance(self, other: Self) -> f32 { - let (dx, dy) = (self.x - other.x, self.y - other.y); - (dx * dx + dy * dy).sqrt() - } -} - -/// A round obstacle: the planner rejects any point or segment within `radius` -/// of `center`. -#[derive(Debug, Clone, Copy, Reflect)] -pub struct Obstacle { - pub center: Point2, - pub radius: f32, -} - -/// The rectangular world `0..width` x `0..height` and its obstacles. -#[derive(Debug, Clone, Reflect)] -pub struct World { - pub width: f32, - pub height: f32, - pub obstacles: Vec, -} - -impl World { - /// The map every planner node of the example runs on: a 10x10 m depot with - /// five pillars, placed so the straight line from start to goal is blocked. - /// A first path is therefore always a detour, and refinement has real work - /// to do. - pub fn depot() -> Self { - Self { - width: 10.0, - height: 10.0, - obstacles: vec![ - Obstacle { - center: Point2::new(3.0, 3.0), - radius: 1.2, - }, - Obstacle { - center: Point2::new(6.0, 6.0), - radius: 1.5, - }, - Obstacle { - center: Point2::new(7.0, 2.5), - radius: 1.0, - }, - Obstacle { - center: Point2::new(2.5, 7.0), - radius: 1.0, - }, - Obstacle { - center: Point2::new(5.0, 1.5), - radius: 0.8, - }, - ], - } - } - - /// True when `point` is inside the bounds and outside every obstacle. - pub fn is_free(&self, point: Point2) -> bool { - if point.x < 0.0 || point.y < 0.0 || point.x > self.width || point.y > self.height { - return false; - } - self.obstacles - .iter() - .all(|o| point.distance(o.center) > o.radius) - } - - /// True when the whole segment `a`-`b` is free. - pub fn is_free_segment(&self, a: Point2, b: Point2) -> bool { - if !self.is_free(a) || !self.is_free(b) { - return false; - } - self.obstacles - .iter() - .all(|o| distance_to_segment(a, b, o.center) > o.radius) - } - - /// Area left free by the obstacles. Assumes every obstacle lies inside the - /// bounds and none overlap, which holds for [`World::depot`]. - pub fn free_area(&self) -> f32 { - let blocked: f32 = self - .obstacles - .iter() - .map(|o| core::f32::consts::PI * o.radius * o.radius) - .sum(); - (self.width * self.height - blocked).max(f32::EPSILON) - } - - /// The RRT* radius constant of Karaman and Frazzoli: - /// `gamma* = 2 * (1 + 1/d)^(1/d) * (free_area / zeta_d)^(1/d)`, here with - /// `d = 2` and `zeta_2 = pi`. - /// - /// A smaller constant shrinks the rewiring neighborhood below what - /// asymptotic optimality needs, and the planner degrades toward plain RRT - /// as the tree grows. - pub fn rrt_star_gamma(&self) -> f32 { - 2.0 * 1.5f32.sqrt() * (self.free_area() / core::f32::consts::PI).sqrt() - } -} - -/// Distance from `point` to the segment `a`-`b`. -fn distance_to_segment(a: Point2, b: Point2, point: Point2) -> f32 { - let (abx, aby) = (b.x - a.x, b.y - a.y); - let len_sq = abx * abx + aby * aby; - if len_sq <= f32::EPSILON { - return a.distance(point); - } - let t = (((point.x - a.x) * abx + (point.y - a.y) * aby) / len_sq).clamp(0.0, 1.0); - Point2::new(a.x + t * abx, a.y + t * aby).distance(point) -} - -/// Tuning knobs of the planner, all read from the node's RON `config:`. -#[derive(Debug, Clone, Copy, Reflect)] -pub struct RrtParams { - /// Longest edge the planner adds in one extension, in meters. - pub step_size: f32, - /// Probability of sampling the goal instead of a random point. - pub goal_bias: f32, - /// A node this close to the goal closes a path. - pub goal_threshold: f32, - /// Gamma of the RRT* rewiring radius `gamma * sqrt(ln n / n)`. `0.0` - /// derives it from the world through [`World::rrt_star_gamma`], which is - /// the value RRT* needs to converge to the optimum. - pub gamma: f32, - /// Branch-and-bound prune every N iterations; 0 disables pruning. - pub prune_interval: u32, - /// Hard cap on the tree size, so one job cannot grow without bound. - pub max_nodes: u32, -} - -impl Default for RrtParams { - fn default() -> Self { - Self { - step_size: 0.8, - goal_bias: 0.05, - goal_threshold: 0.5, - gamma: 0.0, - prune_interval: 512, - max_nodes: 4000, - } - } -} - -/// xorshift64*, so a given seed always replays the same tree. -#[derive(Debug, Clone, Reflect)] -pub struct Rng(u64); - -impl Rng { - pub fn new(seed: u64) -> Self { - // splitmix64 finalizer: consecutive seeds must not start on neighboring - // states, otherwise consecutive jobs explore almost the same tree. - let mut state = seed.wrapping_add(0x9E37_79B9_7F4A_7C15); - state = (state ^ (state >> 30)).wrapping_mul(0xBF58_476D_1CE4_E5B9); - state = (state ^ (state >> 27)).wrapping_mul(0x94D0_49BB_1331_11EB); - state ^= state >> 31; - // xorshift64* must never start at zero. - Self(if state == 0 { 1 } else { state }) - } - - fn next_u64(&mut self) -> u64 { - let mut x = self.0; - x ^= x >> 12; - x ^= x << 25; - x ^= x >> 27; - self.0 = x; - x.wrapping_mul(0x2545_F491_4F6C_DD1D) - } - - /// Uniform in `[0.0, 1.0)`. - pub fn next_f32(&mut self) -> f32 { - (self.next_u64() >> 40) as f32 / (1u32 << 24) as f32 - } -} - -/// One vertex of the tree. -#[derive(Debug, Clone, Reflect)] -struct TreeNode { - pos: Point2, - /// `None` for the root only. - parent: Option, - /// Path cost from the start to this node. - cost: f32, - children: Vec, -} - -/// An RRT* search for one start/goal pair. -/// -/// The tree only ever improves: `best_cost` is monotone non-increasing over -/// iterations, which is what makes the algorithm a good anytime task. -#[derive(Debug, Reflect)] -pub struct RrtStar { - world: World, - params: RrtParams, - start: Point2, - goal: Point2, - tree: Vec, - /// Node closing the best path found so far. - best_goal: Option, - /// Cost of the best path found so far, infinite until one is found. - best_cost: f32, - iterations: u32, - rng: Rng, - /// Reused between iterations to keep the search allocation-free. - scratch_near: Vec, - scratch_stack: Vec, -} - -impl RrtStar { - /// Starts a search rooted at `start`. An unreachable or blocked `start` - /// simply never grows a tree; the caller sees "no path" and the anytime - /// quality floor drops the result. - pub fn new( - world: World, - mut params: RrtParams, - start: Point2, - goal: Point2, - seed: u64, - ) -> Self { - if params.gamma <= 0.0 { - params.gamma = world.rrt_star_gamma(); - } - let mut planner = Self { - world, - params, - start, - goal, - tree: Vec::new(), - best_goal: None, - best_cost: f32::INFINITY, - iterations: 0, - rng: Rng::new(seed), - scratch_near: Vec::new(), - scratch_stack: Vec::new(), - }; - planner.reset(start, goal, seed); - planner - } - - /// Restarts the search on a new problem, keeping the capacity the previous - /// job grew: after the first job the planner asks the allocator for much - /// less. - pub fn reset(&mut self, start: Point2, goal: Point2, seed: u64) { - self.start = start; - self.goal = goal; - self.tree.clear(); - self.tree.push(TreeNode { - pos: start, - parent: None, - cost: 0.0, - children: Vec::new(), - }); - self.best_goal = None; - self.best_cost = f32::INFINITY; - self.iterations = 0; - self.rng = Rng::new(seed); - } - - /// Runs one bounded block of `iterations` RRT* iterations. - pub fn grow(&mut self, iterations: u32) { - for _ in 0..iterations { - self.iterations += 1; - if self.tree.len() < self.params.max_nodes as usize { - self.step(); - } - if self.params.prune_interval > 0 - && self.iterations.is_multiple_of(self.params.prune_interval) - && self.best_goal.is_some() - { - self.prune(); - } - } - } - - /// Cost of the best path so far, infinite while no path is known. - pub fn best_cost(&self) -> f32 { - self.best_cost - } - - /// True once a path to the goal exists. - pub fn has_solution(&self) -> bool { - self.best_goal.is_some() - } - - pub fn tree_size(&self) -> u32 { - self.tree.len() as u32 - } - - pub fn iterations(&self) -> u32 { - self.iterations - } - - /// True when the tree is full and pruning can never free room again, so no - /// further iteration can change anything. - pub fn is_exhausted(&self) -> bool { - self.tree.len() >= self.params.max_nodes as usize - && (self.params.prune_interval == 0 || self.best_goal.is_none()) - } - - /// Shortest conceivable path: the straight line, obstacles ignored. - pub fn lower_bound(&self) -> f32 { - self.start.distance(self.goal) - } - - /// Normalized quality in `0.0..=1.0`: how close the best path is to the - /// straight-line lower bound. 0.0 means no path yet, 1.0 means the path is - /// as short as the world allows. - pub fn quality(&self) -> f32 { - if !self.has_solution() { - return 0.0; - } - (self.lower_bound() / self.best_cost).clamp(0.0, 1.0) - } - - /// Nodes on the best path before shortcutting, the goal included. Zero - /// while no path is known. - pub fn tree_path_len(&self) -> usize { - let Some(goal_node) = self.best_goal else { - return 0; - }; - let mut len = 1; // the goal itself, which is not a tree node - let mut cursor = Some(goal_node); - while let Some(index) = cursor { - len += 1; - cursor = self.tree[index as usize].parent; - } - len - } - - /// Writes the best path into `out` and returns how many waypoints it used. - /// - /// The tree path routinely holds more nodes than [`MAX_WAYPOINTS`], so it - /// is shortcut first: from each waypoint the path jumps to the furthest - /// later one still reachable in a straight free line. Shortcutting is what - /// a planner publishes anyway, and it keeps every published segment - /// collision free - dropping the tail instead would publish a straight - /// jump across the map. - /// - /// The shortcut path is never longer than the tree path, so the reported - /// cost stays an upper bound on what the robot drives. `None` means even - /// the shortcut path does not fit; the caller then publishes nothing - /// rather than a path that cuts through an obstacle. - pub fn write_path(&self, out: &mut [Point2; MAX_WAYPOINTS]) -> Option { - let goal_node = self.best_goal?; - let mut chain = Vec::new(); - let mut cursor = Some(goal_node); - while let Some(index) = cursor { - let node = &self.tree[index as usize]; - chain.push(node.pos); - cursor = node.parent; - } - chain.reverse(); - chain.push(self.goal); - - let mut len = 0usize; - let mut at = 0usize; - loop { - if len == MAX_WAYPOINTS { - return None; - } - out[len] = chain[at]; - len += 1; - if at == chain.len() - 1 { - return Some(len as u32); - } - // The next tree node is always reachable - it is a tree edge - so - // the scan only looks for something further. - let mut next = at + 1; - for candidate in (at + 2)..chain.len() { - if self.world.is_free_segment(chain[at], chain[candidate]) { - next = candidate; - } - } - at = next; - } - } - - /// One RRT* iteration: sample, steer, choose the cheapest parent, rewire - /// the neighborhood, then check whether the new node closes a better path. - fn step(&mut self) { - let sample = self.sample(); - let nearest = self.nearest(sample); - let from = self.tree[nearest as usize].pos; - let new_pos = steer(from, sample, self.params.step_size); - if !self.world.is_free_segment(from, new_pos) { - return; - } - - let radius = self.near_radius(); - let mut near = core::mem::take(&mut self.scratch_near); - near.clear(); - for (index, node) in self.tree.iter().enumerate() { - if node.pos.distance(new_pos) <= radius { - near.push(index as u32); - } - } - - // Choose the parent that gives the cheapest path to the new node. - let mut parent = nearest; - let mut cost = self.tree[nearest as usize].cost + from.distance(new_pos); - for &index in near.iter() { - let candidate = &self.tree[index as usize]; - let candidate_cost = candidate.cost + candidate.pos.distance(new_pos); - if candidate_cost < cost && self.world.is_free_segment(candidate.pos, new_pos) { - parent = index; - cost = candidate_cost; - } - } - - let new_index = self.tree.len() as u32; - self.tree.push(TreeNode { - pos: new_pos, - parent: Some(parent), - cost, - children: Vec::new(), - }); - self.tree[parent as usize].children.push(new_index); - - // Rewire: neighbors that are cheaper to reach through the new node. - for &index in near.iter() { - if index == parent { - continue; - } - let (neighbor_pos, neighbor_cost) = { - let neighbor = &self.tree[index as usize]; - (neighbor.pos, neighbor.cost) - }; - let rewired_cost = cost + neighbor_pos.distance(new_pos); - if rewired_cost < neighbor_cost - && !self.is_ancestor(index, new_index) - && self.world.is_free_segment(new_pos, neighbor_pos) - { - self.reparent(index, new_index, rewired_cost); - } - } - self.scratch_near = near; - - // Does the new node close a better path? - let to_goal = new_pos.distance(self.goal); - if to_goal <= self.params.goal_threshold - && self.world.is_free_segment(new_pos, self.goal) - && cost + to_goal < self.best_cost - { - self.best_cost = cost + to_goal; - self.best_goal = Some(new_index); - } - // Rewiring may have shortened the current best path too. - if let Some(goal_node) = self.best_goal { - let node = &self.tree[goal_node as usize]; - self.best_cost = self.best_cost.min(node.cost + node.pos.distance(self.goal)); - } - } - - /// A random point of the world, biased toward the goal. - fn sample(&mut self) -> Point2 { - if self.rng.next_f32() < self.params.goal_bias { - return self.goal; - } - Point2::new( - self.rng.next_f32() * self.world.width, - self.rng.next_f32() * self.world.height, - ) - } - - /// Index of the tree node closest to `point`. Linear on purpose: a real - /// planner would index the tree, but a flat scan keeps the example short. - fn nearest(&self, point: Point2) -> u32 { - let mut best = 0u32; - let mut best_distance = f32::INFINITY; - for (index, node) in self.tree.iter().enumerate() { - let distance = node.pos.distance(point); - if distance < best_distance { - best_distance = distance; - best = index as u32; - } - } - best - } - - /// RRT* rewiring radius `gamma * sqrt(ln n / n)`, capped at one step. - fn near_radius(&self) -> f32 { - let n = (self.tree.len() as f32).max(2.0); - (self.params.gamma * (n.ln() / n).sqrt()).min(self.params.step_size) - } - - /// True when `candidate` sits on the path from `node` up to the root. - /// - /// Rewiring an ancestor would turn the tree into a graph with a cycle, and - /// every walk over it would then loop forever. Exact arithmetic already - /// rules it out - reaching an ancestor through its own descendant is never - /// cheaper - but rounding on two nearly coincident samples must not be able - /// to break that. - fn is_ancestor(&self, candidate: u32, node: u32) -> bool { - let mut cursor = self.tree[node as usize].parent; - while let Some(index) = cursor { - if index == candidate { - return true; - } - cursor = self.tree[index as usize].parent; - } - false - } - - /// Moves `node` under `new_parent` and shifts the cost of its whole - /// subtree by the same delta. - fn reparent(&mut self, node: u32, new_parent: u32, new_cost: f32) { - if let Some(old_parent) = self.tree[node as usize].parent { - self.tree[old_parent as usize] - .children - .retain(|&child| child != node); - } - self.tree[node as usize].parent = Some(new_parent); - self.tree[new_parent as usize].children.push(node); - - let delta = new_cost - self.tree[node as usize].cost; - let mut stack = core::mem::take(&mut self.scratch_stack); - stack.clear(); - stack.push(node); - while let Some(index) = stack.pop() { - self.tree[index as usize].cost += delta; - for i in 0..self.tree[index as usize].children.len() { - stack.push(self.tree[index as usize].children[i]); - } - } - self.scratch_stack = stack; - } - - /// Branch and bound: drop every node that cannot belong to a path better - /// than the best one known. - /// - /// Walking down from the root keeps the tree consistent: a node is kept - /// only if its parent is kept, so no orphan survives the compaction. The - /// triangle inequality makes that almost free anyway - a kept node's parent - /// always satisfies the bound as well. - fn prune(&mut self) { - // The best path itself is protected: rounding must never let branch and - // bound drop the path it is bounding against. - let mut protected = vec![false; self.tree.len()]; - let mut cursor = self.best_goal; - while let Some(index) = cursor { - protected[index as usize] = true; - cursor = self.tree[index as usize].parent; - } - - let mut keep = vec![false; self.tree.len()]; - let mut stack = core::mem::take(&mut self.scratch_stack); - stack.clear(); - stack.push(0); - keep[0] = true; - while let Some(index) = stack.pop() { - for i in 0..self.tree[index as usize].children.len() { - let child = self.tree[index as usize].children[i]; - let node = &self.tree[child as usize]; - if protected[child as usize] - || node.cost + node.pos.distance(self.goal) <= self.best_cost - { - keep[child as usize] = true; - stack.push(child); - } - } - } - self.scratch_stack = stack; - - let mut remap = vec![u32::MAX; self.tree.len()]; - let mut kept = Vec::with_capacity(self.tree.len()); - for (index, node) in self.tree.iter().enumerate() { - if keep[index] { - remap[index] = kept.len() as u32; - kept.push(TreeNode { - pos: node.pos, - parent: node.parent, - cost: node.cost, - children: Vec::new(), - }); - } - } - for node in kept.iter_mut() { - node.parent = node.parent.map(|parent| remap[parent as usize]); - } - for index in 0..kept.len() { - if let Some(parent) = kept[index].parent { - kept[parent as usize].children.push(index as u32); - } - } - self.best_goal = self.best_goal.map(|goal| remap[goal as usize]); - self.tree = kept; - } -} - -/// Point at most `step_size` away from `from` in the direction of `to`. -fn steer(from: Point2, to: Point2, step_size: f32) -> Point2 { - let distance = from.distance(to); - if distance <= step_size { - return to; - } - let ratio = step_size / distance; - Point2::new( - from.x + ratio * (to.x - from.x), - from.y + ratio * (to.y - from.y), - ) -} - -#[cfg(test)] -mod tests { - use super::*; - - const START: Point2 = Point2::new(0.5, 0.5); - const GOAL: Point2 = Point2::new(9.5, 9.5); - - fn planner(seed: u64) -> RrtStar { - RrtStar::new(World::depot(), RrtParams::default(), START, GOAL, seed) - } - - #[test] - fn segment_collision_is_detected() { - let world = World::depot(); - // Straight through the pillar at (3, 3). - assert!(!world.is_free_segment(Point2::new(1.0, 1.0), Point2::new(5.0, 5.0))); - // Along the free bottom edge. - assert!(world.is_free_segment(Point2::new(0.2, 0.2), Point2::new(0.2, 9.8))); - // Endpoints out of bounds. - assert!(!world.is_free_segment(START, Point2::new(11.0, 0.5))); - } - - #[test] - fn refinement_only_improves_the_path() { - let mut planner = planner(42); - planner.grow(400); - assert!(planner.has_solution(), "no first path after the base block"); - - let mut previous = planner.best_cost(); - for _ in 0..16 { - planner.grow(256); - assert!( - planner.best_cost() <= previous + 1e-4, - "cost went up: {} then {}", - previous, - planner.best_cost() - ); - previous = planner.best_cost(); - } - assert!(planner.quality() > 0.0 && planner.quality() <= 1.0); - assert!(planner.best_cost() >= planner.lower_bound()); - } - - /// Every published path must be drivable, at every stop point the anytime - /// policy could pick. - /// - /// A small `gamma` is included on purpose: it barely rewires, so its tree - /// paths grow past [`MAX_WAYPOINTS`] and the shortcut is exercised rather - /// than skipped. - #[test] - fn published_path_is_valid_at_every_stop_point() { - let world = World::depot(); - let mut longest_tree_path = 0; - for (seed, gamma) in (1..40u64).flat_map(|seed| [(seed, 0.0f32), (seed, 3.0)]) { - let params = RrtParams { - gamma, - ..Default::default() - }; - let mut planner = RrtStar::new(World::depot(), params, START, GOAL, seed); - planner.grow(400); - for _ in 0..24 { - planner.grow(256); - let mut waypoints = [Point2::default(); MAX_WAYPOINTS]; - let Some(len) = planner.write_path(&mut waypoints) else { - panic!("seed {seed}: the shortcut path did not fit"); - }; - longest_tree_path = longest_tree_path.max(planner.tree_path_len()); - assert!(len >= 2, "a path has at least a start and a goal"); - assert_eq!(waypoints[0], START); - assert_eq!(waypoints[(len - 1) as usize], GOAL); - for pair in waypoints[..len as usize].windows(2) { - assert!( - world.is_free_segment(pair[0], pair[1]), - "seed {seed}: published path crosses an obstacle" - ); - } - // The shortcut only removes waypoints it can bypass in a - // straight free line, so it never lengthens the path. - let published: f32 = waypoints[..len as usize] - .windows(2) - .map(|pair| pair[0].distance(pair[1])) - .sum(); - assert!( - published <= planner.best_cost() + 1e-3, - "seed {seed}: shortcut path {published} longer than the cost {}", - planner.best_cost() - ); - } - } - assert!( - longest_tree_path > MAX_WAYPOINTS, - "the tree path never outgrew MAX_WAYPOINTS, so the shortcut was never exercised" - ); - } - - /// A gamma below the value the free area implies shrinks the rewiring - /// neighborhood, and refinement then converges to a worse path. - #[test] - fn derived_gamma_beats_an_arbitrary_one() { - let world = World::depot(); - let derived = world.rrt_star_gamma(); - assert!( - (12.0..13.0).contains(&derived), - "gamma for the depot map should be near 12.4, got {derived}" - ); - - let cost_at = |gamma: f32| { - let mut total = 0.0; - for seed in 1..40u64 { - let params = RrtParams { - gamma, - ..Default::default() - }; - let mut planner = RrtStar::new(World::depot(), params, START, GOAL, seed); - planner.grow(400); - for _ in 0..24 { - planner.grow(256); - } - total += planner.best_cost(); - } - total - }; - assert!( - cost_at(0.0) < cost_at(3.0), - "the derived gamma should refine to a shorter path than a small one" - ); - } - - #[test] - fn same_seed_replays_the_same_tree() { - let (mut a, mut b) = (planner(11), planner(11)); - a.grow(600); - b.grow(300); - b.grow(300); - assert_eq!(a.tree_size(), b.tree_size()); - assert_eq!(a.best_cost(), b.best_cost()); - } - - #[test] - fn pruning_keeps_the_best_path_reachable() { - let mut planner = planner(3); - planner.grow(1500); - let cost_before = planner.best_cost(); - planner.prune(); - assert!(planner.has_solution(), "pruning dropped the goal node"); - // The tree stays consistent: every node still reaches the root. - for index in 0..planner.tree.len() { - let mut cursor = Some(index as u32); - let mut hops = 0; - while let Some(current) = cursor { - cursor = planner.tree[current as usize].parent; - hops += 1; - assert!(hops <= planner.tree.len(), "cycle in the tree"); - } - } - assert_eq!(planner.best_cost(), cost_before); - } -} diff --git a/examples/cu_anytime_rrt_star/src/tasks.rs b/examples/cu_anytime_rrt_star/src/tasks.rs index d49b68a8552..e1173581902 100644 --- a/examples/cu_anytime_rrt_star/src/tasks.rs +++ b/examples/cu_anytime_rrt_star/src/tasks.rs @@ -1,369 +1,290 @@ -//! The Copper side of the example: a goal source, two RRT* anytime planners -//! under different policies, and a sink comparing what they published. - -use crate::rrt::{MAX_WAYPOINTS, Point2, RrtParams, RrtStar, World}; +//! The Copper side of the demo: a navigation simulator ahead of the planner +//! and a Rerun viewer behind it. +//! +//! The feedback loop closes outside the graph: the viewer stores the newest +//! published path in [`LATEST_PATH`], and the simulator follows it one cycle +//! later. Foreground execution runs nav -> planner -> viewer in order within +//! one copperlist, so the hand-off is deterministic. + +use bincode::de::Decoder; +use bincode::enc::Encoder; +use bincode::error::{DecodeError, EncodeError}; use bincode::{Decode, Encode}; -use cu29::cutask_anytime::{AnytimeStatus, CuAnytimeTask, Quality, quality_from_f32}; +use cu_rrt_star::{PlanPath, PlanRequest, Point2, World}; use cu29::prelude::*; -use serde::{Deserialize, Serialize}; +use rerun::{ + Color, LineStrips2D, Points2D, Radius, RecordingStream, RecordingStreamBuilder, Scalars, +}; use std::sync::Mutex; -/// Start and goal of every planning job. -pub const START: Point2 = Point2::new(0.5, 0.5); -pub const GOAL: Point2 = Point2::new(9.5, 9.5); - -/// Quality at which the path matches the straight-line lower bound: there is -/// nothing left to refine. -const CONVERGED_QUALITY: f32 = 0.999; - -/// What the sink saw, one entry per copperlist, for the checks in `main`. -static REPORTS: Mutex> = Mutex::new(Vec::new()); - -/// Takes the reports collected so far, leaving the sink ready for a new run. -pub fn take_reports() -> Vec { - core::mem::take(&mut *REPORTS.lock().expect("reports poisoned")) -} +/// Where the robot starts, in free space on the depot map. +const START: Point2 = Point2::new(0.5, 0.5); -/// One copperlist as the sink saw it. A `None` path means the node published -/// nothing: no path yet, or a quality below the configured floor. -#[derive(Debug, Clone)] -pub struct PlanReport { - pub quick: Option, - pub quick_status: String, - pub thorough: Option, - pub thorough_status: String, -} +/// The corners the robot patrols, all in free space on the depot map. +const PATROL: [Point2; 3] = [ + Point2::new(9.5, 9.5), + Point2::new(0.5, 9.0), + Point2::new(9.0, 0.8), +]; -/// One planning problem. The seed changes every copperlist, so each job is a -/// fresh RRT* run rather than a replay of the previous one. -#[derive(Default, Debug, Clone, Encode, Decode, Serialize, Deserialize, Reflect)] -pub struct PlanRequest { - pub start: Point2, - pub goal: Point2, - pub seed: u64, -} +/// The newest path the viewer saw, followed by the simulator one cycle later. +static LATEST_PATH: Mutex> = Mutex::new(None); -/// The best path known when the refinement window closed. +/// A point robot that replans while it drives. /// -/// Every consecutive pair of waypoints is collision free, so the path can be -/// driven as published. -#[derive(Default, Debug, Clone, Encode, Decode, Serialize, Deserialize, Reflect)] -pub struct PlanPath { - pub waypoints: [Point2; MAX_WAYPOINTS], - /// Waypoints actually used in `waypoints`. - pub len: u32, - /// Cost of the RRT* tree path, which is what the anytime quality scores. - /// The published waypoints are a shortcut of it, so this is an upper bound - /// on the distance actually driven. - pub cost: f32, - /// RRT* iterations spent on this path, base block included. - pub iterations: u32, - pub tree_size: u32, -} - -/// Emits one planning problem per copperlist. -#[derive(Default, Reflect)] -pub struct GoalSrc { +/// Each copperlist it advances along the newest published path, then asks for +/// a fresh plan from wherever it is now. When it reaches the current patrol +/// goal it picks the next one. While no path is published - the first cycle, +/// or a job under the quality floor - it holds position and retries with a +/// new seed. +#[derive(Reflect)] +pub struct NavSim { + pose: Point2, + goal_index: u32, seed: u64, + speed_mps: f32, + goal_threshold: f32, + #[reflect(ignore)] + last_now: Option, } -impl Freezable for GoalSrc {} - -impl CuSrcTask for GoalSrc { - type Resources<'r> = (); - type Output<'m> = output_msg!(PlanRequest); - - fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult { - let seed = match config { - Some(config) => config.get::("seed")?.unwrap_or(1) as u64, - None => 1, - }; - Ok(Self { seed }) +impl Freezable for NavSim { + fn freeze(&self, encoder: &mut E) -> Result<(), EncodeError> { + Encode::encode(&self.pose, encoder)?; + Encode::encode(&self.goal_index, encoder)?; + Encode::encode(&self.seed, encoder) } - fn process(&mut self, ctx: &CuContext, new_msg: &mut Self::Output<'_>) -> CuResult<()> { - self.seed = self.seed.wrapping_add(1); - new_msg.set_payload(PlanRequest { - start: START, - goal: GOAL, - seed: self.seed, - }); - // A fresh Tov per job. An anytime node reads it as the age anchor when - // its policy sets max_age_ms; neither planner here does, so both - // anchor on their own job start instead. - new_msg.tov = Tov::Time(ctx.clock.now()); + fn thaw(&mut self, decoder: &mut D) -> Result<(), DecodeError> { + self.pose = Decode::decode(decoder)?; + self.goal_index = Decode::decode(decoder)?; + self.seed = Decode::decode(decoder)?; Ok(()) } } -/// What a remote debugger sees of a planner node: the progress of the job, -/// not the thousands of tree nodes behind it. -/// -/// The fields are read through `Reflect`, which the compiler cannot see. -#[allow(dead_code)] -#[derive(Default, Debug, Reflect)] -pub struct PlannerDebugState { - pub iterations: u32, - pub tree_size: u32, - /// Nodes on the best path before it is shortcut for publication. - pub tree_path_len: u32, - /// Cost of the best path in the tree, `f32::INFINITY` while there is none. - pub best_cost: f32, - /// Cost of the path currently in the output. - pub published_cost: f32, - pub published_quality: f32, -} - -/// An RRT* planner as an anytime task. -/// -/// `base()` runs the first block of iterations and publishes the first path it -/// finds; each `refine()` runs one more block and republishes only when the -/// path got shorter. How many blocks run is the policy's call, not the task's. -#[derive(Reflect)] -pub struct RrtStarPlanner { - params: RrtParams, - /// Iterations of the base block, aiming at a first path. - base_iterations: u32, - /// Iterations of one refinement quantum. - block_iterations: u32, - /// The current job, `None` before the first `base()`. - planner: Option, - /// Cost of the path currently in the output; infinite while none was - /// published for this job. - published_cost: f32, - published_quality: f32, -} - -impl Freezable for RrtStarPlanner {} - -impl RrtStarPlanner { - /// Commits the best path of the tree when it beats the published one, and - /// returns the published quality. Leaving the output alone when nothing - /// improved is what the anytime contract asks for: the output always holds - /// the best result so far, so the runtime can publish it at any stop point. - fn publish(&mut self, output: &mut CuMsg) -> Quality { - if let Some(planner) = self.planner.as_ref() - && planner.has_solution() - && planner.best_cost() < self.published_cost - { - 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) { - output.set_payload(PlanPath { - waypoints, - len, - cost: planner.best_cost(), - iterations: planner.iterations(), - tree_size: planner.tree_size(), - }); - self.published_cost = planner.best_cost(); - self.published_quality = planner.quality(); +impl NavSim { + /// Advances the pose by `budget` meters along `path`. Waypoint 0 is where + /// the path was planned from - last cycle's pose - so the walk starts at + /// waypoint 1. + fn follow(&mut self, path: &PlanPath, mut budget: f32) { + let len = path.len as usize; + let mut next = 1; + while budget > 0.0 && next < len { + let target = path.waypoints[next]; + let distance = self.pose.distance(target); + if distance <= budget { + self.pose = target; + budget -= distance; + next += 1; + } else { + let ratio = budget / distance; + self.pose = Point2::new( + self.pose.x + ratio * (target.x - self.pose.x), + self.pose.y + ratio * (target.y - self.pose.y), + ); + break; } } - quality_from_f32(self.published_quality) - } - - /// The projected view a debug session gets instead of the whole tree. - fn debug_state(&self) -> PlannerDebugState { - let planner = self.planner.as_ref(); - PlannerDebugState { - iterations: planner.map_or(0, RrtStar::iterations), - tree_size: planner.map_or(0, RrtStar::tree_size), - tree_path_len: planner.map_or(0, |p| p.tree_path_len() as u32), - best_cost: planner.map_or(f32::INFINITY, RrtStar::best_cost), - published_cost: self.published_cost, - published_quality: self.published_quality, - } } } -impl CuAnytimeTask for RrtStarPlanner { - type Input<'m> = input_msg!(PlanRequest); - type Output<'m> = output_msg!(PlanPath); +impl CuSrcTask for NavSim { type Resources<'r> = (); - type Quality = Quality; - - // The task struct holds a whole RRT* tree, up to `max_nodes` entries. The - // default hooks would ship all of it on every debug read, so the node - // exposes a small view instead. - fn register_debug_state_types(registry: &mut TypeRegistry) { - registry.register::(); - } - - fn debug_state_type_path() -> &'static str { - PlannerDebugState::type_path() - } - - fn with_debug_state(&self, f: impl FnOnce(&dyn bevy_reflect::Reflect) -> R) -> R { - f(&self.debug_state()) - } + type Output<'m> = output_msg!(PlanRequest); fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult { - let mut params = RrtParams::default(); - let mut base_iterations = 400u32; - let mut block_iterations = 256u32; + let mut seed = 1u64; + let mut speed_mps = 1.5f32; + let mut goal_threshold = 0.3f32; if let Some(config) = config { - if let Some(value) = config.get::("step_size")? { - params.step_size = value; + if let Some(value) = config.get::("seed")? { + seed = value as u64; } - if let Some(value) = config.get::("goal_bias")? { - params.goal_bias = value; + if let Some(value) = config.get::("speed_mps")? { + speed_mps = value; } if let Some(value) = config.get::("goal_threshold")? { - params.goal_threshold = value; - } - if let Some(value) = config.get::("gamma")? { - params.gamma = value; - } - if let Some(value) = config.get::("prune_interval")? { - params.prune_interval = value; - } - if let Some(value) = config.get::("max_nodes")? { - params.max_nodes = value; - } - if let Some(value) = config.get::("base_iterations")? { - base_iterations = value; - } - if let Some(value) = config.get::("block_iterations")? { - block_iterations = value; + goal_threshold = value; } } Ok(Self { - params, - base_iterations, - block_iterations, - planner: None, - published_cost: f32::INFINITY, - published_quality: 0.0, + pose: START, + goal_index: 0, + seed, + speed_mps, + goal_threshold, + last_now: None, }) } - fn base( - &mut self, - _ctx: &CuContext, - input: &Self::Input<'_>, - output: &mut Self::Output<'_>, - ) -> CuResult> { - let request = input.payload().ok_or("rrt*: no plan request")?; - let planner = match self.planner.as_mut() { - // Restart on the previous job's memory instead of a fresh tree. - Some(planner) => { - planner.reset(request.start, request.goal, request.seed); - planner + fn process(&mut self, ctx: &CuContext, new_msg: &mut Self::Output<'_>) -> CuResult<()> { + let now = ctx.clock.now(); + let dt = match self.last_now { + Some(last) => { + let CuDuration(nanos) = now - last; + nanos as f32 / 1e9 } - None => self.planner.insert(RrtStar::new( - World::depot(), - self.params, - request.start, - request.goal, - request.seed, - )), + None => 0.0, }; - planner.grow(self.base_iterations); - self.published_cost = f32::INFINITY; - self.published_quality = 0.0; - // Output messages are recycled: with no path yet the message must not - // still carry the previous job's path. - output.clear_payload(); - // Even with no path found the job goes on: refinement is what usually - // finds one, and a quality of 0.0 stays under any configured floor. - Ok(AnytimeStatus::Improved(self.publish(output))) - } + self.last_now = Some(now); - fn refine( - &mut self, - _ctx: &CuContext, - output: &mut Self::Output<'_>, - ) -> CuResult> { - let planner = self - .planner - .as_mut() - .ok_or("rrt*: refine() without a job from base()")?; - if planner.is_exhausted() { - return Ok(AnytimeStatus::Converged(quality_from_f32( - self.published_quality, - ))); + let path = LATEST_PATH.lock().expect("path poisoned").clone(); + if let Some(path) = path { + self.follow(&path, self.speed_mps * dt); } - planner.grow(self.block_iterations); - let quality = self.publish(output); - if self.published_quality >= CONVERGED_QUALITY { - // The path matches the straight line: no iteration can beat it. - return Ok(AnytimeStatus::Converged(quality)); + + if self.pose.distance(PATROL[self.goal_index as usize]) <= self.goal_threshold { + self.goal_index = (self.goal_index + 1) % PATROL.len() as u32; + // A path toward the old goal must not be driven. + *LATEST_PATH.lock().expect("path poisoned") = None; } - Ok(AnytimeStatus::Improved(quality)) + + self.seed = self.seed.wrapping_add(1); + new_msg.set_payload(PlanRequest { + world: World::depot(), + start: self.pose, + goal: PATROL[self.goal_index as usize], + seed: self.seed, + }); + new_msg.tov = Tov::Time(now); + Ok(()) } } -/// Records what both planners published in the same copperlist. -#[derive(Default, Reflect)] -pub struct ComparisonSink; +/// Draws the world, the robot and the published path into a Rerun viewer, +/// and stores the path for [`NavSim`] to follow. +#[derive(Reflect)] +#[reflect(from_reflect = false)] +pub struct RerunViewer { + #[reflect(ignore)] + rec: RecordingStream, + cycle: i64, + world_logged: bool, + trail: Vec, +} -impl Freezable for ComparisonSink {} +impl Freezable for RerunViewer {} -impl CuSinkTask for ComparisonSink { +impl CuSinkTask for RerunViewer { type Resources<'r> = (); - type Input<'m> = input_msg!('m, PlanPath, PlanPath); + type Input<'m> = input_msg!('m, PlanRequest, PlanPath); - fn new(_config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult { - Ok(Self) + fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult { + let builder = RecordingStreamBuilder::new("cu-anytime-rrt-star"); + // `rrd` writes the recording to a file instead of spawning a viewer, + // for headless runs. + let rec = match config.and_then(|c| c.get::("rrd").transpose()) { + Some(path) => builder.save(path?), + None => builder.spawn(), + } + .map_err(|e| CuError::new_with_cause("Failed to open the Rerun stream", e))?; + Ok(Self { + rec, + cycle: 0, + world_logged: false, + trail: Vec::new(), + }) } fn process(&mut self, _ctx: &CuContext, input: &Self::Input<'_>) -> CuResult<()> { - // Input order follows the cnx order in the RON: quick first. - let (quick, thorough): (&CuMsg, &CuMsg) = *input; - REPORTS.lock().expect("reports poisoned").push(PlanReport { - quick: quick.payload().cloned(), - quick_status: quick.metadata.status_txt.0.to_string(), - thorough: thorough.payload().cloned(), - thorough_status: thorough.metadata.status_txt.0.to_string(), + // Input order follows the cnx order in the RON: the request first. + let (request_msg, path_msg): (&CuMsg, &CuMsg) = *input; + let Some(request) = request_msg.payload() else { + return Ok(()); + }; + + self.cycle += 1; + self.rec.set_time_sequence("copperlist", self.cycle); + apply_tov(&self.rec, &request_msg.tov); + + if !self.world_logged { + log_world(&self.rec, &request.world)?; + self.world_logged = true; + } + + self.trail.push(request.start); + log( + &self.rec, + "world/robot", + &Points2D::new([(request.start.x, request.start.y)]) + .with_radii([Radius::new_scene_units(0.15)]) + .with_colors([Color::from([255, 140, 0])]), + )?; + log( + &self.rec, + "world/trail", + &LineStrips2D::new([self.trail.iter().map(|p| (p.x, p.y)).collect::>()]) + .with_colors([Color::from([255, 200, 120])]), + )?; + log( + &self.rec, + "world/goal", + &Points2D::new([(request.goal.x, request.goal.y)]) + .with_radii([Radius::new_scene_units(0.2)]) + .with_colors([Color::from([0, 200, 0])]), + )?; + + // An empty strip when nothing was published: a job under the quality + // floor visibly clears the path. + let strip: Vec<(f32, f32)> = path_msg.payload().map_or_else(Vec::new, |path| { + path.waypoints[..path.len as usize] + .iter() + .map(|p| (p.x, p.y)) + .collect() + }); + log( + &self.rec, + "world/path", + &LineStrips2D::new([strip]).with_colors([Color::from([0, 128, 255])]), + )?; + + // The same quality definition as the planner: straight line over cost. + let quality = path_msg.payload().map_or(0.0, |path| { + (request.start.distance(request.goal) / path.cost) as f64 }); + log(&self.rec, "curves/quality", &Scalars::single(quality))?; + let iterations = path_msg + .payload() + .map_or(0.0, |path| path.iterations as f64); + log(&self.rec, "curves/iterations", &Scalars::single(iterations))?; + + if let Some(path) = path_msg.payload() { + *LATEST_PATH.lock().expect("path poisoned") = Some(path.clone()); + } Ok(()) } } -#[cfg(test)] -mod tests { - use super::*; - - /// The anytime contract driven by hand: `base()` publishes a first path, - /// every `refine()` leaves the output holding the best path so far, and - /// the debug state tracks the job instead of exposing the whole tree. - #[test] - fn refinement_only_commits_improvements() { - let ctx = CuContext::new_with_clock(); - let mut task = RrtStarPlanner::new(None, ()).unwrap(); - let input = CuMsg::new(Some(PlanRequest { - start: START, - goal: GOAL, - seed: 42, - })); - let mut output = CuMsg::new(None); - - task.start(&ctx).unwrap(); - let status = task.base(&ctx, &input, &mut output).unwrap(); - assert!(matches!(status, AnytimeStatus::Improved(_))); +fn apply_tov(rec: &RecordingStream, tov: &Tov) { + match tov { + Tov::Time(t) => rec.set_duration_secs("tov", t.0 as f64 / 1e9), + Tov::Range(r) => rec.set_duration_secs("tov", r.start.0 as f64 / 1e9), + Tov::None => rec.reset_time(), + } +} - let mut best = f32::INFINITY; - for _ in 0..24 { - if let AnytimeStatus::Aborted = task.refine(&ctx, &mut output).unwrap() { - panic!("the planner should not abort on a solvable map"); - } - if let Some(path) = output.payload() { - assert!( - path.cost <= best + 1e-4, - "the output regressed: {best} then {}", - path.cost - ); - best = path.cost; - } - } - assert!(output.payload().is_some(), "no path after 24 quanta"); +fn log(rec: &RecordingStream, path: &str, entity: &impl rerun::AsComponents) -> CuResult<()> { + rec.log(path, entity) + .map_err(|e| CuError::new_with_cause("Failed to log to Rerun", e)) +} - let state = task.debug_state(); - assert_eq!(state.published_cost, best); - assert!(state.best_cost <= state.published_cost); - assert!(state.published_quality > 0.0); - assert!(state.iterations > 0 && state.tree_size > 0); - } +/// The static part of the scene: the world bounds and the obstacles. +fn log_world(rec: &RecordingStream, world: &World) -> CuResult<()> { + let (w, h) = (world.width, world.height); + rec.log_static( + "world/bounds", + &LineStrips2D::new([[(0.0, 0.0), (w, 0.0), (w, h), (0.0, h), (0.0, 0.0)]]) + .with_colors([Color::from([180, 180, 180])]), + ) + .map_err(|e| CuError::new_with_cause("Failed to log the world bounds", e))?; + + let obstacles = &world.obstacles[..world.obstacle_count as usize]; + rec.log_static( + "world/obstacles", + &Points2D::new(obstacles.iter().map(|o| (o.center.x, o.center.y))) + .with_radii(obstacles.iter().map(|o| Radius::new_scene_units(o.radius))) + .with_colors([Color::from([100, 100, 100])]), + ) + .map_err(|e| CuError::new_with_cause("Failed to log the obstacles", e)) } From feaae1bcfabaf78fe62b25bc010d1fcf7757bbdd Mon Sep 17 00:00:00 2001 From: Scofield626 Date: Mon, 3 Aug 2026 19:13:09 +0000 Subject: [PATCH 7/7] cu_anytime_rrt_star: patrol goals with every straight leg blocked --- components/tasks/cu_rrt_star/tests/copperconfig.ron | 2 +- examples/cu_anytime_rrt_star/src/tasks.rs | 8 +++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/components/tasks/cu_rrt_star/tests/copperconfig.ron b/components/tasks/cu_rrt_star/tests/copperconfig.ron index f1a2dc61b90..ebccfc45164 100644 --- a/components/tasks/cu_rrt_star/tests/copperconfig.ron +++ b/components/tasks/cu_rrt_star/tests/copperconfig.ron @@ -72,4 +72,4 @@ msg: "cu_rrt_star::PlanPath", ), ], -) +) \ No newline at end of file diff --git a/examples/cu_anytime_rrt_star/src/tasks.rs b/examples/cu_anytime_rrt_star/src/tasks.rs index e1173581902..a76edad8e6a 100644 --- a/examples/cu_anytime_rrt_star/src/tasks.rs +++ b/examples/cu_anytime_rrt_star/src/tasks.rs @@ -20,11 +20,13 @@ use std::sync::Mutex; /// Where the robot starts, in free space on the depot map. const START: Point2 = Point2::new(0.5, 0.5); -/// The corners the robot patrols, all in free space on the depot map. +/// The goals the robot patrols, all in free space on the depot map. They are +/// placed so a pillar blocks the straight line of every leg: each leg needs a +/// detour, and refinement has visible work on every trajectory. const PATROL: [Point2; 3] = [ Point2::new(9.5, 9.5), - Point2::new(0.5, 9.0), - Point2::new(9.0, 0.8), + Point2::new(0.5, 7.0), + Point2::new(6.5, 0.5), ]; /// The newest path the viewer saw, followed by the simulator one cycle later.