costmap

A Rust library for 2D costmaps, occupancy grids, and raycasting — aimed at robotics navigation.

The goal is to provide a standalone, composable alternative to costmap_2d from Nav2, implemented as an independent Rust crate with no ROS dependency. The Rust robotics ecosystem needs more self-contained libraries that can be mixed and matched outside of any single framework.

Status: Work in progress. The core grid, raycasting, coordinate transforms, inflation, and ROS2 map loading are functional. The Nav2 compatibility layer and advanced costmap features are partially stubbed.

Note: This project has also been a testbed for AI-assisted code generation. Much of the code has been written or refined with the help of LLMs.

Goals

• Easy to use — clear API with sensible defaults; load a map and start querying in a few lines.
• Performant — match or exceed the performance of Nav2's C++ costmap_2d implementation.
• Composable — no framework lock-in. Use the grid on its own, plug in your own layers, or integrate with any robotics stack.

Who is this for?

This library is designed for:

• Rust robotics developers building mobile robots, autonomous vehicles, or drone navigation systems
• ROS2 users wanting to prototype or deploy navigation algorithms in pure Rust
• Game developers needing efficient 2D spatial queries and line-of-sight calculations
• Researchers experimenting with costmap algorithms without the overhead of a full robotics framework

Features

• Rolling window costmaps — update_origin shifts the grid while preserving overlapping data, enabling efficient robot-centered local costmaps.
• Raycasting — DDA and grid-step algorithms for sensor simulation and collision detection in world coordinates.
• Obstacle inflation — expand obstacles with pluggable cost functions (linear, inscribed exponential) for safe navigation.
• Line and polygon iterators — iterate cells along a ray or inside a convex polygon, with read/write access.
• 2D occupancy grids & costmaps — Grid2d<i8> and Grid2d<u8> with configurable resolution, origin, and coordinate transforms.
• Nav2-compatible semantics — lethal / inscribed / free / unknown cost values matching costmap_2d conventions.
• ROS2 map loading — load occupancy grids from the standard YAML + image format.
• Rerun visualisation (optional) — log grids and costmaps as textured 3D planes with the RViz colour palette.
• Nav2 compatibility layer (optional) — a Costmap2D-like API surface for incremental migration.

Installation

Add to your Cargo.toml:

[dependencies]
costmap = "0.1.0"

# Optional features
costmap = { version = "0.1.0", features = ["rerun"] }

Quick start

Load a map and perform raycasting for obstacle detection:

use costmap::RosMapLoader;
use glam::Vec2;

fn main() -> Result<(), Box<dyn std::error::Error>> {
    // Load a ROS2 map (YAML + image format)
    let grid = RosMapLoader::load_from_yaml("map.yaml")?;

    // Raycast from origin in a given direction
    let origin = Vec2::new(10.0, 10.0);  // meters, world coordinates
    let direction = Vec2::new(1.0, 0.0); // unit vector
    let max_range = 30.0;

    // Check for obstacles along the ray
    if let Some(hit) = grid.raycast_dda(&origin, &direction, max_range) {
        println!("Hit obstacle at distance: {:.2}m", hit.hit_distance);
        println!("Cell coordinates: {:?}", hit.cell);
    } else {
        println!("No obstacles within range");
    }

    Ok(())
}

Examples

Run the examples with the rerun feature enabled:

cargo run --example rerun_occupancy_raycast --features rerun
cargo run --example rerun_local_costmap_lidar --features rerun

rerun_occupancy_raycast

Demonstrates the core raycasting API with an animated 360° sweep. Shows how to:

• Load ROS2 map files
• Perform DDA raycasting for obstacle detection
• Handle hit/miss cases

rerun_local_costmap_lidar

Complete local costmap workflow for mobile robot navigation. Shows how to:

• Create and update rolling window costmaps
• Integrate simulated lidar data using clear_ray()
• Apply inflation layers for safe path planning
• Move the costmap origin to follow the robot

Roadmap

Core

• 2D grid storage with bounds-checked access
• World / map coordinate transforms
• Row-major cell iterators (immutable and mutable)
• Origin updates with data preservation (rolling window)
• Configurable fill value for new / reset cells
• Map resize and region reset
• Sparse active-cell iterators
• Region queries (rect, circle)
• Resolution resampling

Raycasting

• Grid-step raycast (2D)
• DDA raycast (2D)
• Line iterators (position, value, mutable value)
• clear_ray — clear along a ray with optional endpoint marking
• 3D raycast variants
• Configurable hit thresholds

Costmap

• Costmap constants (free / inscribed / lethal / unknown)
• Inflation with pluggable cost functions (linear, inscribed exponential)
• Unknown-cell preservation through inflation
• Polygon iterators and footprint cost queries
• Full Nav2 layer lifecycle (initialize, reset, update bounds/costs)
• Obstacle clearing by footprint
• Cost queries at pose

Layers

• Layered grid wrapper with metadata
• Aggregation policy trait
• Per-layer get/set by index and name
• Layer masks and filtering

IO

• ROS2 map loader (YAML + image)
• ROS2 map writer
• Other formats (PGM/PNG with metadata sidecar)

Visualisation

• Greyscale image export for occupancy grids
• Rerun: log occupancy grids and costmaps as textured planes
• Rerun: RViz-compatible costmap colour palette
• Pixel-buffer viewer (optional viewer feature)

Contributing

Contributions are welcome! Please feel free to:

• Open issues for bugs, feature requests, or questions
• Submit pull requests (please open an issue first for major changes)
• Improve documentation and examples
• Share your use cases and feedback

When contributing code, please ensure it follows the existing style and includes appropriate tests.

License

Licensed under either of

• Apache License, Version 2.0 (LICENSE-APACHE or http://www.apache.org/licenses/LICENSE-2.0)
• MIT license (LICENSE-MIT or http://opensource.org/licenses/MIT)

at your option.