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

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).