costmap 0.2.0

//! # Occupancy Grid Raycasting
//!
//! This example demonstrates 2D raycasting in an occupancy grid, a fundamental operation
//! for robotics applications such as:
//! - Laser range finder (lidar) simulation
//! - Line-of-sight checks for visibility
//! - Collision detection along a ray
//!
//! This example loads a static map and continuously rotates a ray 360 degrees,
//! visualizing hits and misses using Rerun for real-time debugging.

use std::error::Error;

use costmap::rerun_viz::{log_line3d, log_occupancy_grid, log_point3d};
use std::f32::consts::TAU;
use std::time::Duration;

use costmap::RosMapLoader;
use costmap::raycast::RayHit2D;
use glam::{Vec2, Vec3};

const DEFAULT_YAML_PATH: &str = "tests/fixtures/warehouse.yaml";

// Animation parameters
const FRAMES_PER_REV: i64 = 360;
const DELAY_MS: u64 = 30;

/// Position to cast rays from (in world frame, meters)
const RAY_ORIGIN_WORLD: Vec2 = Vec2::new(214.0, 72.0);
/// Maximum distance to check for obstacles (meters)
const MAX_RANGE_M: f32 = 30.0;

// Visualization Z-heights for layering elements in Rerun
const Z_RAY: f32 = 0.10;
const Z_ORIGIN: f32 = 0.12;
const Z_HIT: f32 = 0.13;
const Z_CENTER_MARKER: f32 = 0.15;
const Z_ORIGIN_MARKER: f32 = 0.16;

fn main() -> Result<(), Box<dyn Error>> {
    // Step 1: Load an occupancy grid from a ROS-format YAML file
    let grid = RosMapLoader::load_from_yaml(DEFAULT_YAML_PATH)?;

    // Step 2: Set up Rerun for visualization
    let rec = rerun::RecordingStreamBuilder::new("costmap_occupancy_raycast").spawn()?;
    log_occupancy_grid(&rec, "world/map", &grid, 0.0)?;

    let center = grid.info().world_center();
    log_point3d(
        &rec,
        "world/map_center",
        Vec3::new(center.x, center.y, Z_CENTER_MARKER),
        Some(rerun::Color::from_rgb(255, 255, 0)),
        Some(4.0),
    )?;

    log_point3d(
        &rec,
        "world/ray_origin_marker",
        Vec3::new(RAY_ORIGIN_WORLD.x, RAY_ORIGIN_WORLD.y, Z_ORIGIN_MARKER),
        Some(rerun::Color::from_rgb(0, 255, 255)),
        Some(4.0),
    )?;

    // Step 3: Continuously raycast in different directions
    let mut frame_idx: i64 = 0;

    loop {
        rec.set_time_sequence("frame", frame_idx);

        // Calculate direction vector for this iteration
        let phase = (frame_idx.rem_euclid(FRAMES_PER_REV) as f32 / FRAMES_PER_REV as f32) * TAU;
        let dir = Vec2::new(phase.cos(), phase.sin());

        // Core API: raycast_dda(origin, direction, max_range)
        // Returns Some(RaycastHit) if an occupied cell is hit, None otherwise
        let hit = grid.raycast_dda(RAY_ORIGIN_WORLD, dir, MAX_RANGE_M);
        let t = RayHit2D::distance_or(hit, MAX_RANGE_M);
        let end_world = RAY_ORIGIN_WORLD + dir * t;

        log_raycast_frame(
            &rec,
            RAY_ORIGIN_WORLD,
            end_world,
            hit.is_some(),
            Z_RAY,
            Z_ORIGIN,
            Z_HIT,
        )?;

        if DELAY_MS > 0 {
            std::thread::sleep(Duration::from_millis(DELAY_MS));
        }

        frame_idx = frame_idx.wrapping_add(1);
    }
}

// Helper function for Rerun visualization - not essential to understanding the library
fn log_raycast_frame(
    rec: &rerun::RecordingStream,
    origin: Vec2,
    end: Vec2,
    hit: bool,
    z_ray: f32,
    z_origin: f32,
    z_hit: f32,
) -> Result<(), Box<dyn Error>> {
    log_line3d(
        rec,
        "world/ray",
        Vec3::new(origin.x, origin.y, z_ray),
        Vec3::new(end.x, end.y, z_ray),
        Some(rerun::Color::from_rgb(255, 0, 0)),
        Some(1.0),
    )?;

    log_point3d(
        rec,
        "world/ray_origin",
        Vec3::new(origin.x, origin.y, z_origin),
        Some(rerun::Color::from_rgb(0, 128, 255)),
        Some(3.0),
    )?;

    if hit {
        log_point3d(
            rec,
            "world/ray_hit",
            Vec3::new(end.x, end.y, z_hit),
            Some(rerun::Color::from_rgb(0, 255, 0)),
            Some(3.0),
        )?;
    } else {
        rec.log("world/ray_hit", &rerun::Points3D::clear_fields())?;
    }

    Ok(())
}