cu-rrt-star: anytime RRT* planner component, with a closed-loop example

Cargo.toml

@@ -72,10 +72,12 @@ 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",
   "benchmarks/cu_zenoh_bridge_bench",
+  "examples/cu_anytime_rrt_star",
   "examples/cu_background_task",
   "examples/cu_baremetal_safety",
   "examples/cu_bridge_test",
@@ -244,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" }
 

components/res/cu_rng/src/lib.rs

@@ -85,6 +85,13 @@ pub struct CuRng {
     inner: ChaCha8Rng,
 }
 
+// Opaque on purpose: the stream state is not meaningful to print.
+impl core::fmt::Debug for CuRng {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        f.write_str("CuRng(ChaCha8)")
+    }
+}
+
 impl CuRng {
     /// Build a fresh RNG from the given 64-bit seed.
     pub fn from_seed(seed: u64) -> Self {

components/tasks/cu_rrt_star/Cargo.toml

@@ -0,0 +1,30 @@
+[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 }
+cu-rng = { workspace = true }
+cu29 = { workspace = true }
+serde = { workspace = true }
+
+[dev-dependencies]
+tempfile = { workspace = true }
+
+[build-dependencies]
+cu29-build = { workspace = true }

components/tasks/cu_rrt_star/README.md

@@ -0,0 +1,93 @@
+# 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` — the world (up to `MAX_OBSTACLES` (16)
+  round obstacles in a rectangle), start and goal. The map travels with the
+  job: the planner has no map of its own, so the source may change the map
+  between jobs. Internally the algorithm only sees a sampling and collision
+  trait, so the round-obstacle world is one implementation, not a mandate.
+- 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.
+- Distances and costs are `cu29-units` lengths in meters; the quality is a
+  `Ratio`.
+
+### Usage
+
+The planner draws its randomness from a `cu_rng::CuRngBundle` resource, so
+the node needs a resource and a binding:
+
+```ron
+resources: [
+    ( id: "planner_rng", provider: "cu_rng::CuRngBundle", config: {"seed": 1} ),
+],
+tasks: [
+    (
+        id: "planner",
+        type: "cu_rrt_star::RrtStarPlanner",
+        resources: { "rng": "planner_rng.rng" },
+        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. A copperlist without a request also publishes nothing, and a
+request whose start already lies on the goal converges at once with
+quality 1.0.
+
+### Determinism and debugging
+
+Randomness comes from the `CuRng` resource (ChaCha8): job N's stream is a
+pure function of the resource seed and N, and the job counter is part of the
+frozen task state, so replay reruns every job on the stream it used live. 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.

components/tasks/cu_rrt_star/build.rs

@@ -0,0 +1,3 @@
+fn main() {
+    cu29_build::setup();
+}