======================================================================
  RRT* Implementation Template for ROS 2 Humble (C++/Python)
======================================================================

DEFINITION
----------------------------------------
RRT* (Rapidly-exploring Random Tree Star) is an asymptotically optimal sampling-based
motion planning algorithm that incrementally builds a tree of feasible robot
configurations toward a goal while iteratively rewiring paths to improve path cost.
This template provides production-ready, ROS 2 Humble–compatible implementations in
both C++ and Python, integrating with ROS 2’s lifecycle nodes, TF2, navigation
stack interfaces (e.g., nav2_msgs), and standard message types (e.g.,
geometry_msgs::msg::PoseStamped, sensor_msgs::msg::LaserScan). It abstracts low-level
RRT* logic into modular, configurable components compliant with ROS 2 best
practices—including parameterization via YAML, real-time collision checking using
costmaps or custom geometric validators, and publish/subscribe patterns for
visualization (e.g., rviz2 via nav_msgs::msg::Path).

OVERVIEW
----------------------------------------
RRT* extends the foundational RRT algorithm by introducing asymptotic optimality
through two key mechanisms: (1) local rewire—where newly added vertices search for
lower-cost neighbors within a dynamic radius and update parentage accordingly, and
(2) global rewire—where existing vertices in the neighborhood are checked for
potential cost reduction via the new vertex. In ROS 2 Humble, this translates to
tight integration with the Navigation Stack’s infrastructure: the planner node
subscribes to static and obstacle layers via Costmap2D (or a lightweight occupancy
grid abstraction), uses tf2 for frame transformations (e.g., converting poses between
map, base_link, and odom frames), and publishes standardized nav_msgs::msg::Path
messages compatible with controllers like nav2_controller. The C++ implementation
leverages modern C++17 features (e.g., std::optional, structured bindings), ROS 2’s
rclcpp_lifecycle for state management (unconfigured → configuring → active), and
thread-safe callback queues for concurrent sampling and collision checks. The Python
version (using rclpy) prioritizes rapid prototyping and educational use, employing
NumPy for vectorized nearest-neighbor searches (e.g., KDTree from scipy.spatial) and
leveraging PyYAML for configuration—while maintaining interface parity (same
parameters, topics, and QoS settings) to enable seamless A/B testing or hybrid
deployment. Both versions support configurable metrics (e.g., Euclidean vs. Dubins
path cost), termination criteria (max iterations/time, solution quality threshold),
and pluggable collision checkers (e.g., Bullet physics, PCL-based point cloud
filtering, or simple axis-aligned bounding box checks).

KEY COMPONENTS
----------------------------------------
1. RRTStarPlannerNode (lifecycle-aware ROS 2 node)
2. CollisionChecker (interface-based, supports costmap2d or custom geometry)
3. RewiringStrategy (dynamic radius computation & neighbor search)

APPLICATIONS
----------------------------------------
  - Autonomous mobile robot navigation in dynamic indoor environments
  - Multi-robot coordinated path planning with decentralized RRT* variants
  - Simulation-to-real transfer for warehouse AMRs using hardware-in-the-loop validation

KEY FORMULAS
----------------------------------------
Rewiring Radius r(n): r(n) = \gamma \left( \frac{\log n}{n} \right)^{1/d}
  -> Dynamic radius defining the neighborhood for rewiring; n is number of vertices, d is configuration space dimension, γ is problem-dependent constant ensuring asymptotic optimality
Cost-to-come g(x): g(x) = g(x_{\text{parent}}) + c(x_{\text{parent}}, x)
  -> Cumulative path cost from root to vertex x, where c(·,·) is edge cost metric (e.g., Euclidean distance or Dubins arc length)
Optimality Condition: g^*(x) = \inf \{ g(x') + c(x', x) \mid x' \in \mathcal{B}_r(x) \cap \mathcal{V} \}
  -> Optimal cost-to-come for vertex x computed over all valid neighbors x' within ball B_r(x) of radius r centered at x

RELATED CONCEPTS
----------------------------------------
  - Sampling-based Motion Planning
  - Asymptotic Optimality
  - ROS 2 Navigation Stack (nav2)
  - Costmap2D
  - TF2 Coordinate Transformations

REFERENCES
----------------------------------------
Karaman & Frazzoli (2011) — Sampling-based Algorithms for Optimal Motion Planning (https://link.springer.com/article/10.1177/0278364911406761)
ROS 2 Humble Navigation Stack Documentation (https://navigation.ros.org/)
RRT* Implementation Guidelines (OSRF Nav2 Planner Plugin Tutorial) (https://navigation.ros.org/plugin_tutorials/docs/writing_new_planner_plugin.html)

TAGS
----------------------------------------
motion-planning, ros2-humble, rrt-star, c-plus-plus, python