Navigation Messages

Messages for goal-based navigation: send a target pose, receive a planned path, and follow waypoints. These form the interface between high-level commands ("go to the kitchen") and low-level control (CmdVel).

A navigation target with position, orientation, and tolerances.

Rust

use horus::prelude::*;

let goal = NavGoal {
    target_pose: Pose2D { x: 5.0, y: 3.0, theta: 0.0 },
    tolerance_position: 0.1,    // 10cm position tolerance
    tolerance_angle: 0.05,      // ~3° heading tolerance
    timeout_seconds: 30.0,      // 30s to reach goal (0 = no limit)
    timestamp_ns: 0,
};

let topic: Topic<NavGoal> = Topic::new("nav.goal")?;
topic.send(goal);

Fields

FieldTypeUnitDescription
target_posePose2Dm, radTarget position and heading
tolerance_positionf64mAcceptable position error
tolerance_anglef64radAcceptable heading error
timeout_secondsf64sMax time to reach goal (0 = unlimited)
timestamp_nsu64nsTimestamp

A sequence of waypoints computed by a path planner. Each waypoint has position, heading, and speed.

Rust

use horus::prelude::*;

// Subscribe to planned paths from the planner node
let path_sub: Topic<NavPath> = Topic::new("nav.path")?;

if let Some(path) = path_sub.recv() {
    for i in 0..path.waypoint_count as usize {
        let wp = &path.waypoints[i];
        // Follow waypoint: wp.x, wp.y, wp.theta, wp.velocity
    }
}

Key Fields

FieldTypeDescription
waypoints[Waypoint; 256]Array of waypoints (max 256)
waypoint_countu16Number of valid waypoints
total_lengthf64Total path length in meters
estimated_timef64Estimated completion time in seconds

PathPlan — Compact Path Representation

A flat-array path format optimized for zero-copy IPC. Stores waypoints as packed [x, y, theta] triples.

Rust

use horus::prelude::*;

let plan_sub: Topic<PathPlan> = Topic::new("nav.plan")?;

if let Some(plan) = plan_sub.recv() {
    let n = plan.waypoint_count as usize;
    for i in 0..n {
        let x = plan.waypoint_data[i * 3];
        let y = plan.waypoint_data[i * 3 + 1];
        let theta = plan.waypoint_data[i * 3 + 2];
    }
}

Key Fields

FieldTypeDescription
waypoint_data[f32; 768]Packed [x, y, theta] × 256 waypoints
goal_pose[f32; 3]Target [x, y, theta]
waypoint_countu16Number of valid waypoints

CostMap — Navigation Cost Grid

An inflated cost grid built on top of an OccupancyGrid. Used by planners to avoid obstacles with safety margins.

Key Fields

FieldTypeDescription
occupancy_gridOccupancyGridBase grid data
costsVec<u8>Cell costs (0=free, 254=inscribed, 255=lethal)
inflation_radiusf32Obstacle inflation radius in meters
cost_scaling_factorf32Exponential cost decay factor
User/Planner → NavGoal → Path Planner → NavPath/PathPlan → Path Follower → CmdVel → Drive
                              ↑
                          CostMap (from OccupancyGrid + LiDAR)