pamoja_kit/

navigate.rs

//! Steering a robot toward a waypoint, and stopping before an obstacle.

use crate::motion::{clamp, magnitude, Twist};
use crate::Coordinate;
use core::f32::consts::PI;
use libm::cosf;

// Wraps an angle in degrees into the half-open interval `(-180, 180]`.
fn wrap_deg_180(angle: f32) -> f32 {
    let mut a = angle % 360.0;
    if a > 180.0 {
        a -= 360.0;
    } else if a <= -180.0 {
        a += 360.0;
    }
    a
}

// `f64::abs` lives in `std`, so this `no_std` crate takes an f64 magnitude by hand.
fn magnitude_f64(value: f64) -> f64 {
    if value < 0.0 {
        -value
    } else {
        value
    }
}

/// The steering command toward a waypoint, with the geometry behind it.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Guidance {
    /// The body twist to drive: a forward speed and a yaw rate toward the target, positive
    /// turning left as [`Twist`] has it.
    pub twist: Twist,
    /// The remaining distance to the target, in meters.
    pub distance_m: f64,
    /// The heading error to the target, in degrees, in `(-180, 180]`: compass degrees, so
    /// positive when the target lies clockwise of the heading, to the robot's right.
    pub heading_error_deg: f32,
    /// Whether the target is within the arrival radius.
    pub arrived: bool,
}

/// Guides a robot from waypoint to waypoint by GPS-style coordinates (carrot following).
///
/// This is the "go to that point" primitive behind a patrol route, a return-to-base, or a field
/// pass: given where the robot is, which way it faces, and the next waypoint, it produces the
/// twist to get there. It turns toward the target in proportion to the heading error and slows
/// the forward speed as that error grows (by the cosine of the error), so the robot pivots toward
/// a target behind it before driving off, rather than swinging wide. The caller holds the list of
/// waypoints and advances to the next once [`Guidance::arrived`] is set, which keeps this
/// allocation-free.
///
/// Headings are compass degrees, clockwise from north, while the [`Twist`] it returns turns left
/// for a positive yaw rate, so a target to the robot's right gives a negative yaw rate.
///
/// # Examples
///
/// ```
/// use pamoja_kit::{Coordinate, WaypointFollower};
///
/// // Cruise 1.5 m/s, arrive within 3 m, turn at 1.5 rad per rad of error, cap 1 rad/s.
/// let follower = WaypointFollower::new(1.5, 3.0, 1.5, 1.0);
///
/// // At the equator/prime meridian facing east (90 deg), with the target due east.
/// let here = Coordinate::new(0.0, 0.0);
/// let target = Coordinate::new(0.0, 0.01);
/// let g = follower.guide(here, 90.0, target);
/// assert!(g.heading_error_deg.abs() < 1e-3); // already pointed at it
/// assert!((g.twist.vx - 1.5).abs() < 1e-3); // so drive at cruise
/// assert!(!g.arrived);
/// ```
#[derive(Clone, Copy, Debug)]
pub struct WaypointFollower {
    cruise: f32,
    arrival_m: f64,
    heading_gain: f32,
    max_angular: f32,
}

impl WaypointFollower {
    /// Creates a follower with the given speeds and tolerances.
    ///
    /// # Arguments
    ///
    /// * `cruise` - the forward speed when pointed at the target; its magnitude is used.
    /// * `arrival_m` - how close, in meters, counts as arrived; its magnitude is used.
    /// * `heading_gain` - yaw rate commanded per radian of heading error; its magnitude is used.
    /// * `max_angular` - the largest yaw rate to command; its magnitude is used.
    ///
    /// # Returns
    ///
    /// The follower.
    pub fn new(cruise: f32, arrival_m: f64, heading_gain: f32, max_angular: f32) -> Self {
        Self {
            cruise: magnitude(cruise),
            arrival_m: magnitude_f64(arrival_m),
            heading_gain: magnitude(heading_gain),
            max_angular: magnitude(max_angular),
        }
    }

    /// Produces the steering command from the robot's position and heading to a target.
    ///
    /// # Arguments
    ///
    /// * `here` - the robot's current coordinate.
    /// * `heading_deg` - the robot's heading in degrees clockwise from north (a compass course).
    /// * `target` - the waypoint to head toward.
    ///
    /// # Returns
    ///
    /// The [`Guidance`]; once within the arrival radius the twist is zero and `arrived` is set.
    /// A heading or coordinate that is not a finite number also gives a zero twist, so a lost
    /// fix stops the robot rather than steering it by NaN.
    pub fn guide(&self, here: Coordinate, heading_deg: f32, target: Coordinate) -> Guidance {
        let distance_m = here.distance_to(target);
        let bearing_deg = here.bearing_to(target) as f32;
        let heading_error_deg = wrap_deg_180(bearing_deg - heading_deg);

        if distance_m <= self.arrival_m {
            return Guidance {
                twist: Twist::zero(),
                distance_m,
                heading_error_deg,
                arrived: true,
            };
        }

        let error_rad = heading_error_deg * (PI / 180.0);
        let angular = clamp(
            -self.heading_gain * error_rad,
            -self.max_angular,
            self.max_angular,
        );
        let facing = cosf(error_rad);
        let forward = if facing > 0.0 {
            self.cruise * facing
        } else {
            0.0
        };

        let twist = if forward.is_finite() && angular.is_finite() {
            Twist::planar(forward, angular)
        } else {
            Twist::zero()
        };
        Guidance {
            twist,
            distance_m,
            heading_error_deg,
            arrived: false,
        }
    }
}

/// Stops forward motion when an obstacle is within the stopping distance, leaving turning free.
///
/// This is the simplest reliable safety reflex for a robot with a forward range sensor: hold the
/// requested rotation so the robot can still turn away, but cut `vx` and `vy` to zero once the
/// nearest reading falls inside the stop distance, so it does not drive into what it sees.
///
/// # Arguments
///
/// * `twist` - the requested body motion.
/// * `range_m` - the nearest measured range ahead, in meters.
/// * `stop_distance_m` - the range at or below which forward motion is cut; its magnitude is used.
///
/// # Returns
///
/// The original twist when the way is clear, or one with no translation (rotation preserved) when
/// an obstacle is within range. A range that is not a number, which a sensor that saw nothing
/// it could measure can report, counts as an obstacle: the way is clear only when a reading says
/// so.
///
/// # Examples
///
/// ```
/// use pamoja_kit::{obstacle_stop, Twist};
///
/// let driving = Twist::new(1.0, 0.0, 0.5);
/// // Clear ahead: unchanged.
/// assert_eq!(obstacle_stop(driving, 2.0, 0.5), driving);
/// // Obstacle at 0.3 m: forward cut, turn kept so it can escape.
/// assert_eq!(obstacle_stop(driving, 0.3, 0.5), Twist::new(0.0, 0.0, 0.5));
/// ```
pub fn obstacle_stop(twist: Twist, range_m: f32, stop_distance_m: f32) -> Twist {
    if range_m > magnitude(stop_distance_m) {
        twist
    } else {
        Twist::new(0.0, 0.0, twist.omega)
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn it_drives_at_cruise_when_pointed_at_the_target() {
        let follower = WaypointFollower::new(1.5, 3.0, 1.5, 1.0);
        let here = Coordinate::new(0.0, 0.0);
        let target = Coordinate::new(0.0, 0.01); // due east
        let g = follower.guide(here, 90.0, target); // facing east
        assert!(g.heading_error_deg.abs() < 1e-3);
        assert!((g.twist.vx - 1.5).abs() < 1e-3);
        assert!(g.twist.omega.abs() < 1e-3);
        assert!(!g.arrived);
    }

    #[test]
    fn it_pivots_without_driving_when_the_target_is_behind() {
        let follower = WaypointFollower::new(1.5, 3.0, 1.5, 1.0);
        let here = Coordinate::new(0.0, 0.0);
        let target = Coordinate::new(0.0, 0.01); // due east (bearing 90)
        let g = follower.guide(here, 270.0, target); // facing west: 180 deg error
        assert!(g.twist.vx.abs() < 1e-6); // cosine of 180 is negative -> no forward
        assert!(g.twist.omega.abs() > 0.0); // but it turns
    }

    #[test]
    fn it_turns_toward_the_side_the_target_is_on() {
        let follower = WaypointFollower::new(1.5, 3.0, 1.5, 1.0);
        let here = Coordinate::new(0.0, 0.0);
        let east = Coordinate::new(0.0, 0.01);
        let north = Coordinate::new(0.01, 0.0);
        // Facing north with the target east, it is to the right: turn clockwise.
        assert!(follower.guide(here, 0.0, east).twist.omega < 0.0);
        // Facing east with the target north, it is to the left: turn counter-clockwise.
        assert!(follower.guide(here, 90.0, north).twist.omega > 0.0);
    }

    #[test]
    fn a_robot_that_follows_the_guidance_comes_to_face_the_target() {
        let follower = WaypointFollower::new(0.0, 3.0, 1.5, 1.0);
        let here = Coordinate::new(0.0, 0.0);
        let east = Coordinate::new(0.0, 0.01);
        // Start facing north and turn as told: a counter-clockwise yaw rate lowers a
        // compass heading.
        let mut heading_deg = 0.0f32;
        for _ in 0..200 {
            let omega = follower.guide(here, heading_deg, east).twist.omega;
            heading_deg -= omega * 0.05 * (180.0 / PI);
        }
        assert!(
            (wrap_deg_180(heading_deg - 90.0)).abs() < 0.5,
            "ended at {heading_deg}"
        );
    }

    #[test]
    fn it_reports_arrival_inside_the_radius() {
        let follower = WaypointFollower::new(1.5, 50.0, 1.5, 1.0);
        let here = Coordinate::new(0.0, 0.0);
        let target = Coordinate::new(0.0, 0.0001); // about 11 m east, inside 50 m
        let g = follower.guide(here, 90.0, target);
        assert!(g.arrived);
        assert_eq!(g.twist, Twist::zero());
    }

    #[test]
    fn the_angular_command_is_capped() {
        let follower = WaypointFollower::new(1.0, 1.0, 10.0, 0.5); // huge gain, small cap
        let here = Coordinate::new(0.0, 0.0);
        let target = Coordinate::new(0.0001, 0.0); // due north, 90 deg off
        let g = follower.guide(here, 90.0, target); // facing east
        assert!((g.twist.omega.abs() - 0.5).abs() < 1e-6); // clamped to the cap
    }

    #[test]
    fn a_lost_fix_or_heading_stops_the_robot() {
        let follower = WaypointFollower::new(1.5, 3.0, 1.5, 1.0);
        let here = Coordinate::new(0.0, 0.0);
        let target = Coordinate::new(0.0, 0.01);
        let lost_fix = follower.guide(Coordinate::new(f64::NAN, 0.0), 90.0, target);
        assert_eq!(lost_fix.twist, Twist::zero());
        assert!(!lost_fix.arrived);
        let lost_heading = follower.guide(here, f32::NAN, target);
        assert_eq!(lost_heading.twist, Twist::zero());
    }

    #[test]
    fn a_range_that_is_not_a_number_counts_as_an_obstacle() {
        let driving = Twist::new(1.0, 0.0, 0.5);
        assert_eq!(
            obstacle_stop(driving, f32::NAN, 0.5),
            Twist::new(0.0, 0.0, 0.5)
        );
        assert_eq!(obstacle_stop(driving, f32::INFINITY, 0.5), driving);
    }
}