RustRobotics

This package is a rust implementation of PythonRobotics, featuring 100+ unique robotics algorithms across localization, mapping, SLAM, planning, control, and mission-level behavior.

Build

git clone https://github.com/rsasaki0109/RustRobotics.git
cd RustRobotics
cargo build --workspace
cargo test --workspace

Workspace Structure

crates/
├── rust_robotics_core/          — Core types, traits, errors
├── rust_robotics_planning/      — Path planning (A*, DWA, RRT, PRM, etc.)
├── rust_robotics_localization/  — Localization (EKF, UKF, PF, Histogram)
├── rust_robotics_control/       — Control & path tracking (Pure Pursuit, LQR, MPC, etc.)
├── rust_robotics_mapping/       — Mapping (NDT, Gaussian Grid, Ray Casting)
├── rust_robotics_slam/          — SLAM (EKF-SLAM, FastSLAM, Graph SLAM, ICP)
├── rust_robotics_viz/           — Visualization (gnuplot wrapper)
├── ros2_nodes/                  — ROS2 navigation nodes (safe_drive-based)
└── rust_robotics/               — Umbrella crate (feature-gated re-exports)

Usage as Library

# Use the full library
[dependencies]
rust_robotics = "0.1"

# Or use individual crates
[dependencies]
rust_robotics_planning = "0.1"
rust_robotics_localization = "0.1"

Types are re-exported at the crate root for convenience:

use rust_robotics_planning::{AStarPlanner, AStarConfig, DWAPlanner};
use rust_robotics_localization::{EKFConfig, EKFLocalizer};
use rust_robotics_control::{PurePursuitController, StanleyController};

Run (Example)

# Headless (no GUI dependencies)
cargo run -p rust_robotics --example headless_grid_planners --features planning
cargo run -p rust_robotics --example headless_localizers --features localization
cargo run -p rust_robotics --example headless_navigation_loop --features "planning,localization,control"
cargo run -p rust_robotics --example headless_mission_recovery --features "planning,localization,control"

# Visualization (requires gnuplot)
cargo run -p rust_robotics --example a_star --features "planning,viz"
cargo run -p rust_robotics --example jps --features "planning,viz"
cargo run -p rust_robotics --example rear_wheel_feedback --features "control,viz"

dora-rs dataflow example

The workspace also includes a minimal dora-rs planning demo that wraps the existing headless A* planner in a dora node and sends a structured JSON path report to a sink node.

dora run crates/rust_robotics/examples/dora_path_planning_dataflow.yml

This example requires the dora runtime/CLI to be installed and uses the feature-gated dora support in crates/rust_robotics.

ROS2 Integration

The workspace includes ready-to-use ROS2 navigation nodes built with safe_drive (Rust ROS2 bindings).

Path Planner (A*)
DWA Local Planner
SLAM Node
SLAM Corrected Odom (optional)
EKF Localizer
Waypoint Navigator

                    TurtleBot3 Gazebo
                 /scan  /odom  /cmd_vel
                    |      |       ^
                    v      |       |
               +-----------+       |
               | slam_node | ----- +
               +-----------+    /map
                      |
                      v
             +-------------------+
             | path_planner_node | ---> /planned_path
             +-------------------+             |
                ^           ^                  v
                |           |           +--------------+
         /ekf_odom     /goal_pose ---> | dwa_planner |
                ^                      +--------------+
                |
      +----------------------+
      | ekf_localizer_node   |
      +----------------------+
                ^
                |
      +-------------------------+
      | waypoint_navigator_node |
      +-------------------------+

Demo video: docs/gazebo_demo.mp4

See docs/ros2_integration.md for details.

source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=42  # optional but recommended if other ROS graphs are already running

cargo build --release --manifest-path ros2_nodes/path_planner_node/Cargo.toml
cargo build --release --manifest-path ros2_nodes/dwa_planner_node/Cargo.toml
cargo build --release --manifest-path ros2_nodes/slam_node/Cargo.toml
cargo build --release --manifest-path ros2_nodes/ekf_localizer_node/Cargo.toml
cargo build --release --manifest-path ros2_nodes/waypoint_navigator_node/Cargo.toml

export TURTLEBOT3_MODEL=burger
./ros2_nodes/launch/run_gazebo_demo.sh

# Multi-goal mission demo
WAYPOINT_NAV_FRAME=relative_start \
WAYPOINT_NAV_WAYPOINTS="0.4,0.0;0.1,0.4" \
  ./ros2_nodes/launch/run_gazebo_mission_demo.sh

run_gazebo_mission_demo.sh defaults to WAYPOINT_NAV_FRAME=relative_start, so the mission waypoints above are interpreted as offsets from the first odom pose observed by waypoint_navigator_node. The wrapper's default mission is a conservative two-waypoint route that was verified in TurtleBot3 world: (0.4, 0.0) -> (0.1, 0.4).

waypoint_navigator_node now includes a simple recovery state machine. If the active waypoint stays outside tolerance without measurable odom progress for WAYPOINT_NAV_STUCK_TIMEOUT seconds, it issues navigation_cancel, waits briefly, rotates in place, backs off, then republishes the active waypoint. The main tuning knobs are WAYPOINT_NAV_MAX_RECOVERY_ATTEMPTS, WAYPOINT_NAV_RECOVERY_ROTATE_SECONDS, WAYPOINT_NAV_RECOVERY_BACKOFF_SECONDS, and WAYPOINT_NAV_RECOVERY_BACKOFF_SPEED.

For observability, navigation_demo.launch.py now also exposes:

ENABLE_RVIZ=true ./ros2_nodes/launch/run_gazebo_demo.sh to open RViz with navigation_demo.rviz
ENABLE_GAZEBO_GUI=false ./ros2_nodes/launch/run_gazebo_demo.sh for a headless Gazebo server run
dynamic TF from the selected nav odom topic to the robot base frame via odom_tf_broadcaster.py
/mission_status (std_msgs/String) for mission / recovery state summaries
/mission_markers (visualization_msgs/MarkerArray) for the route, active goal, and status text

By default, the current Gazebo demo uses odom as its honest global frame: slam_node publishes /map in the raw odom frame it actually integrates against, path_planner_node republishes /planned_path in that same frame, and waypoint_navigator_node publishes /goal_pose plus /mission_markers in RUST_NAV_GLOBAL_FRAME=odom. If you still want the old RViz alias, you can opt into PUBLISH_MAP_ODOM_TF=true to add a legacy identity map -> odom transform.

If you want the experimental corrected SLAM frame, enable:

ENABLE_SLAM_CORRECTED_FRAME=true ./ros2_nodes/launch/run_gazebo_mission_demo.sh

That switches the mission stack to NAV_ODOM_TOPIC=/slam_odom and NAV_GLOBAL_FRAME=map. In this mode, slam_node publishes /slam_pose plus /slam_odom, the map is integrated in map, and map_odom_tf_broadcaster.py estimates a dynamic map -> odom transform from /slam_odom against the raw odom stream. The corrected pose update is quality-gated: high-error, low-motion, or outlier ICP deltas fall back to pure odom, and medium-quality matches are attenuated instead of applied at the full blend factor.

Corrected mode also exposes two extra observability topics:

/slam_diagnostics (std_msgs/String) with per-scan ICP convergence, error, blend_alpha, gate_reason, and applied correction deltas
/slam_ground_truth_status (std_msgs/String) with relative-start Gazebo ground-truth error metrics derived from gz topic -e -t /world/default/dynamic_pose/info --json-output

The ground-truth monitor defaults to the spawned model name (GROUND_TRUTH_ENTITY_NAME=$TURTLEBOT3_MODEL) and compares /ekf_odom plus /slam_odom against the model pose after subtracting the first ground-truth sample. You can override the Gazebo source with GROUND_TRUTH_GZ_POSE_TOPIC if needed.

For a local ROS2/Gazebo regression check, run:

ROS_DOMAIN_ID=89 ENABLE_RVIZ=false ENABLE_GAZEBO_GUI=false ./ros2_nodes/launch/run_navigation_smoke_test.sh

The smoke script launches the mission demo, verifies /map, /planned_path, and typed /mission_markers all match the nav odom frame, checks the dynamic nav odom TF, and waits for mission complete -> goal cleared -> stop command in the navigation logs.

To smoke-test the corrected SLAM frame as well:

ROS_DOMAIN_ID=90 ENABLE_RVIZ=false ENABLE_GAZEBO_GUI=false ENABLE_SLAM_CORRECTED_FRAME=true \
  ./ros2_nodes/launch/run_navigation_smoke_test.sh

In corrected mode, the same script also verifies dynamic map -> odom.

Benchmarks

Rust vs Python Speed Comparison

Algorithm	Rust (ms)	Python (ms)	Speedup
A* (100x100)	4.0	924.5	231x
EKF (1000 steps)	0.19	103.1	543x
RRT (100 runs)	0.12	5.7	46x
CubicSpline (1000 runs)	0.92	6.9	7.5x

Any-Angle Planner Comparison (160 MovingAI scenarios)

Planner	Path Quality vs Theta*	Speed vs Theta*
Theta*	baseline	baseline
Lazy Theta*	same (+0.01%)	1.7x faster (p=0.025)
A*+optimize_path	same (+0.27%)	2.3x faster

Grid Planner Benchmark (50x50)

cargo bench -p rust_robotics_planning --bench unified_planning_benchmark
cargo bench -p rust_robotics_planning --bench jps_crossover_benchmark

Localization
Mapping
SLAM
Path Planning
Path Tracking
Inverted Pendulum
- LQR Control
Arm Navigation
- Two Joint Arm, N-Joint Arm IK, Arm Obstacle Navigation
Aerial Navigation
- 3D Grid A*, Drone 3D Trajectory Following, Drone Minimum-Snap Trajectory
Mission Planning
- Behavior Tree, State Machine
ROS2 Integration

Localization

Extended Kalman Filter Localization

Red:GPS, Brue:Ground Truth, Green:EKF, Yellow:Dead Reckoning

src

Particle Filter Localization

Blue: GPS, Red: Ground Truth, Green: Particle Filter, Yellow: Dead Reckoning

src

Unscented Kalman Filter Localization

Blue: Ground Truth, Red: UKF Estimate, Black: Dead Reckoning, Green: GPS Observations, Red Ellipse: Uncertainty

src

Histogram Filter Localization

Grid-based probabilistic localization using RFID landmarks. The algorithm maintains a probability distribution over a 2D grid and updates it based on motion and observations.

Blue: True path, Orange: Dead Reckoning, Green: Histogram Filter estimate, Black: RFID landmarks

src

Cubature Kalman Filter

Cubature Kalman Filter (CKF) using 3rd-degree spherical-radial cubature rule. Achieves the same accuracy as UKF but 30% faster with zero tuning parameters (no alpha/beta/kappa). Recommended as the default over UKF for typical robotics scenarios.

src

Ensemble Kalman Filter

Stochastic ensemble-based Kalman filter. Maintains an ensemble of state particles and updates them using the Kalman gain computed from ensemble statistics.

src

Adaptive Filter

Automatically switches between EKF (fast, linear) and CKF (robust, nonlinear) based on Normalized Innovation Squared (NIS). When innovation exceeds the chi-squared threshold, switches to CKF for better nonlinearity handling.

src

Complementary Filter

Fuses high-frequency prediction (control/gyro) with low-frequency measurement (position sensor) using a tunable blending factor alpha. Simple, fast, and effective for IMU fusion.

src

Iterated EKF

Improves EKF accuracy by iterating the update step linearization. Re-linearizes the observation model around the updated state estimate multiple times until convergence.

src

Information Filter

Dual of the Kalman Filter operating in information space (inverse covariance). Update step is additive in information form, making multi-sensor fusion natural.

src

Square Root UKF

UKF variant that propagates Cholesky factors instead of full covariance matrices. Improves numerical stability and guarantees positive semi-definiteness.

src

Monte Carlo Localization

Adaptive Particle Filter with KLD-sampling. Automatically adjusts particle count based on posterior complexity — more particles for multi-modal distributions, fewer after convergence.

src

Mapping

NDT Map

src

Gaussian Grid Map

Occupancy grid mapping using Gaussian distribution. Higher probability near obstacles.

src

Ray Casting Grid Map

Occupancy grid mapping using ray casting. Free space (0.5), Occupied (1.0), Unknown (0.0).

src

DBSCAN Clustering

Density-based spatial clustering that finds arbitrary-shaped clusters and identifies outliers (noise). No need to specify number of clusters in advance.

src

Line Extraction

Extracts line segments from 2D scan data using the Split-and-Merge (Iterative End Point Fit) algorithm. Used for feature extraction in indoor environments.

src

Occupancy Grid Map

Probabilistic occupancy grid using log-odds representation. Updates cells via Bresenham ray casting — free along rays, occupied at endpoints.

src

Gaussian Process Regression

GP regression with RBF kernel for terrain/surface mapping from sparse measurements. Provides predictions with uncertainty estimates.

src

SLAM

Iterative Closest Point (ICP) Matching

Red: Reference points, Blue: Initial points, Green: Aligned points

src

FastSLAM 1.0

Particle filter based SLAM (Simultaneous Localization and Mapping). Each particle maintains its own map of landmarks using EKF.

Blue: True path, Yellow: Dead Reckoning, Green: FastSLAM estimate, Black: True landmarks, Cyan: Estimated landmarks

src

EKF SLAM

Extended Kalman Filter based SLAM. Maintains a joint state vector of robot pose and landmark positions with full covariance matrix.

src

FastSLAM 2.0

Improved particle filter SLAM that incorporates the latest observation into the proposal distribution before sampling, producing better particle diversity than FastSLAM 1.0.

src

Graph-Based SLAM

Pose graph optimization for SLAM. Constructs a graph of robot poses connected by odometry and observation constraints, then optimizes the graph using iterative methods.

src

Pose Graph Optimization

2D pose graph optimization using Gauss-Newton iteration. Core backend for graph-based SLAM — optimizes a graph of robot poses connected by odometry and loop closure constraints.

src

Correlative Scan Matching

Brute-force correlative scan matcher that searches over a discretized pose space. More robust to initial pose errors than ICP — useful as a SLAM front-end.

src

Path Planning

A* Algorithm

Blue: Start, Red: Goal, Green: Path, Gray: Obstacles

src

cargo run -p rust_robotics --example a_star --features "planning,viz"

Theta* Algorithm

Any-angle path planning algorithm. Unlike A* which restricts movement to grid edges, Theta* allows paths at any angle by checking line-of-sight between nodes.

src

cargo run -p rust_robotics --example theta_star --features "planning,viz"

Lazy Theta*

Lazy Theta* defers line-of-sight checks until node expansion, reducing redundant visibility tests. Achieves the same path quality as Theta* while being 1.7x faster (p=0.025 on 160 MovingAI scenarios).

src

Enhanced Lazy Theta*

Extends Lazy Theta* with wider parent selection at expansion time. Uses 2-ring neighborhood search and ancestor chain walks to find better any-angle shortcuts. Achieves near-optimal paths (+0.11% vs visibility-graph optimal on 50x50 grids).

src

Anya (Optimal Any-Angle)

Optimal any-angle pathfinding using visibility-graph Dijkstra on all free cells. Guarantees the shortest any-angle path. Used as the optimality baseline for evaluating Theta* variants.

src

Path Smoothing

Post-processing pipeline for grid-based paths: greedy LOS shortcutting followed by iterative waypoint relaxation. Transforms grid-constrained A* paths into near-optimal any-angle paths. A*+optimize_path achieves 2.3x speedup over Theta* with equivalent path quality.

src

Jump Point Search (JPS)

Optimized pathfinding algorithm for uniform-cost grids. Reduces the number of nodes to explore by identifying and jumping to key "jump points" instead of examining all neighbors.

src

cargo run -p rust_robotics --example jps --features "planning,viz"

Bezier Path Planning

Blue: Start, Red: Goal, Green: Path

src

Cubic Spline

Black: Control points, Green: Path

src

Dynamic Window Approach

Black: Obstacles, Green: Trajectory, Yellow: Predicted trajectory

src

D* Lite

Blue: Start, Red: Goal, Green: Path, Black: Obstacles

src

D* Lite is an incremental heuristic search algorithm for path planning in dynamic environments. It's particularly efficient for replanning when the environment changes.

Dijkstra Algorithm

src

Informed RRT*

Blue: Start, Red: Goal, Green: Path, Black: Tree

src

Model Predictive Trajectory Generator

Green: Path

src

Potential Field Algorithm

Blue: Start, Red: Goal, Green: Path, Gray: Obstacles

src

Quintic Polynomials

Blue: Start, Red: Goal, Green: Path

src

Rapidly-Exploring Random Trees (RRT)

Sampling-based path planning algorithm that builds a tree by randomly sampling the configuration space.

Blue: Start, Red: Goal, Green: Path, Gray: Tree

src

RRT*

Optimized version of RRT that rewires the tree to find shorter paths. Asymptotically optimal.

Blue: Start, Red: Goal, Green: Path, Gray: Tree

src

Reeds-Shepp Path

Blue: Start, Red: Goal, Green: Path

src

Dubins Path

Shortest path for non-holonomic vehicles with bounded turning radius. Computes optimal paths composed of circular arcs and straight segments (6 types: LSL, RSR, LSR, RSL, RLR, LRL).

src

PRM (Probabilistic Road-Map)

Sampling-based path planning using random samples and k-nearest neighbor connections.

Blue: Start, Red: Goal, Green: Path, Gray: Samples and edges, Black: Obstacles

src

Voronoi Road-Map

Path planning using Voronoi diagram vertices as waypoints. Provides paths that maximize clearance from obstacles.

Blue: Start, Red: Goal, Green: Path, Cyan: Voronoi vertices, Black: Obstacles

src

Frenet Optimal Trajectory

Optimal trajectory planning in Frenet coordinate frame. Widely used in autonomous driving for lane keeping and obstacle avoidance.

Gray: Reference path, Green: Optimal trajectory, Black: Obstacles, Red: Vehicle

src

State Lattice Planner

Lattice-based motion planning that searches over a pre-computed set of motion primitives. Generates smooth, dynamically feasible trajectories by connecting state lattice primitives.

src

cargo run -p rust_robotics --example state_lattice --features "planning,viz"

Path Tracking

LQR Steer Control

Black: Planned path, Green: Tracked path

src

Move to Pose

Green: Path, Red: Start and Goal

src

Pure Pursuit

Black: Planned path, Green: Tracked path

src

Stanley Control

Black: Planned path, Green: Tracked path

src

Rear Wheel Feedback Control

Path tracking using rear wheel feedback steering control. Combines heading error and lateral error with path curvature feedforward.

Blue: Reference path, Red: Vehicle trajectory, Green: Waypoints

src

cargo run -p rust_robotics --example rear_wheel_feedback --features "control,viz"

MPC (Model Predictive Control)

Model Predictive Control for path tracking using linearized bicycle model. Predicts future states and optimizes control inputs over a horizon.

Gray: Reference path, Blue: Tracked trajectory, Green: Prediction horizon, Red: Vehicle

src

Inverted Pendulum

LQR Control

Cart-pendulum animation showing LQR control stabilization. Blue: Cart, Black: Pendulum. Multiple frames overlaid to show time progression from initial angle to stabilized state.

src

Arm Navigation

Two Joint Arm Control

Two joint arm to a point control simulation using inverse kinematics.

Black: Arm links, Red: Joints (shoulder, elbow, end effector), Green: Target position

src

Aerial Navigation

3D Grid A*

Bounded 3D voxel-grid planning for aerial robots. The planner supports 6-connected or 26-connected motion and returns a collision-free waypoint sequence.

src

Drone 3D Trajectory Following

Closed-loop quadrotor waypoint tracking with quintic segments, PD thrust/attitude control, and Euler-integrated rigid-body dynamics.

src

Drone Minimum-Snap Trajectory

Seventh-order minimum-snap segment generation for drone waypoint loops. The module exposes piecewise segment generation, desired-state sampling, and a direct path into the existing quadrotor tracker.

src

Mission Planning

Behavior Tree

Behavior tree runtime for mission-level decision making with sequence, selector, condition, and action nodes backed by a shared blackboard.

src

State Machine

Finite state machine for robot behavior management with states, transitions, guards, and actions

src

Name		Name	Last commit message	Last commit date
Latest commit History 193 Commits
.devcontainer		.devcontainer
.github/workflows		.github/workflows
crates		crates
docs		docs
img		img
media		media
ros2_nodes		ros2_nodes
scripts		scripts
vendor/nearest_neighbor		vendor/nearest_neighbor
.gitignore		.gitignore
CHANGELOG.md		CHANGELOG.md
CLAUDE.md		CLAUDE.md
CONTRIBUTING.md		CONTRIBUTING.md
Cargo.lock		Cargo.lock
Cargo.toml		Cargo.toml
LICENSE		LICENSE
README.md		README.md
deny.toml		deny.toml
plan.md		plan.md

Folders and files

Latest commit

History

Repository files navigation

RustRobotics

Build

Workspace Structure

Usage as Library

Run (Example)

dora-rs dataflow example

ROS2 Integration

Benchmarks

Rust vs Python Speed Comparison

Any-Angle Planner Comparison (160 MovingAI scenarios)

Grid Planner Benchmark (50x50)

Table of Contents

Localization

Extended Kalman Filter Localization

Particle Filter Localization

Unscented Kalman Filter Localization

Histogram Filter Localization

Cubature Kalman Filter

Ensemble Kalman Filter

Adaptive Filter

Complementary Filter

Iterated EKF

Information Filter

Square Root UKF

Monte Carlo Localization

Mapping

NDT Map

Gaussian Grid Map

Ray Casting Grid Map

DBSCAN Clustering

Line Extraction

Occupancy Grid Map

Gaussian Process Regression

SLAM

Iterative Closest Point (ICP) Matching

FastSLAM 1.0

EKF SLAM

FastSLAM 2.0

Graph-Based SLAM

Pose Graph Optimization

Correlative Scan Matching

Path Planning

A* Algorithm

Theta* Algorithm

Lazy Theta*

Enhanced Lazy Theta*

Anya (Optimal Any-Angle)

Path Smoothing

Jump Point Search (JPS)

Bezier Path Planning

Cubic Spline

Dynamic Window Approach

D* Lite

Dijkstra Algorithm

Informed RRT*

Model Predictive Trajectory Generator

Potential Field Algorithm

Quintic Polynomials

Rapidly-Exploring Random Trees (RRT)

RRT*

Reeds-Shepp Path

Dubins Path

PRM (Probabilistic Road-Map)

Voronoi Road-Map

Frenet Optimal Trajectory

State Lattice Planner

Path Tracking

LQR Steer Control

Move to Pose

Pure Pursuit

Stanley Control

Rear Wheel Feedback Control

MPC (Model Predictive Control)

Inverted Pendulum

LQR Control

Arm Navigation

Packages