mecha10-core 0.6.5

//! Shared kinematics helpers for wheeled-robot odometry.
//!
//! `motor_fake` (the simulated motor driver) and `motor` (the real motor
//! node) both integrate a simple differential-drive pose update from a
//! commanded linear/angular velocity each odometry tick. This module factors
//! that shared math out so both crates call the same function instead of
//! carrying two copies of the same integration + angle-normalization logic.

use std::f32::consts::PI;

/// A 2D robot pose: position plus heading (radians).
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Pose2D {
    pub x: f32,
    pub y: f32,
    pub theta: f32,
}

/// Integrate a differential-drive pose forward by `dt` seconds given a
/// commanded linear and angular velocity, normalizing the resulting heading
/// to `[-PI, PI]`.
pub fn integrate_differential_drive(pose: Pose2D, linear: f32, angular: f32, dt: f32) -> Pose2D {
    let delta_theta = angular * dt;
    let delta_x = linear * dt * pose.theta.cos();
    let delta_y = linear * dt * pose.theta.sin();

    let mut theta = pose.theta + delta_theta;
    while theta > PI {
        theta -= 2.0 * PI;
    }
    while theta < -PI {
        theta += 2.0 * PI;
    }

    Pose2D {
        x: pose.x + delta_x,
        y: pose.y + delta_y,
        theta,
    }
}

/// Shared "tick the odometry forward" behavior for anything that tracks a 2D
/// differential-drive pose by dead-reckoning a commanded linear/angular
/// velocity each tick (`motor_fake` and `motor` both do exactly this).
///
/// Implementers only need to expose their pose/command/rate fields; the
/// default [`Self::update_odometry`] method does the actual integration via
/// [`integrate_differential_drive`], so the dead-reckoning logic itself
/// lives in exactly one place instead of being copied per node/driver.
pub trait DifferentialDriveOdometry {
    /// Current tracked pose.
    fn odom_pose(&self) -> Pose2D;
    /// Store a newly-integrated pose.
    fn set_odom_pose(&mut self, pose: Pose2D);
    /// Currently commanded (linear, angular) velocity.
    fn commanded_velocity(&self) -> (f32, f32);
    /// Odometry tick rate in Hz (used to derive `dt`).
    fn odom_rate_hz(&self) -> f32;

    /// Integrate one odometry tick forward and store the result.
    fn update_odometry(&mut self) {
        let dt = 1.0 / self.odom_rate_hz();
        let (linear, angular) = self.commanded_velocity();
        let pose = integrate_differential_drive(self.odom_pose(), linear, angular, dt);
        self.set_odom_pose(pose);
    }
}

/// Generate a [`DifferentialDriveOdometry`] impl for a struct with `x`/`y`/`theta`,
/// `cmd_linear`/`cmd_angular`, and `config.odom_rate_hz` fields - the exact layout `motor_fake`
/// and `motor` both use to track dead-reckoned pose.
///
/// Eliminates the `odom_pose`/`set_odom_pose`/`commanded_velocity`/`odom_rate_hz` boilerplate
/// that was hand-duplicated between those two crates' `DifferentialDriveOdometry` impls (every
/// method's body was byte-for-byte identical, since both structs happen to name their pose,
/// command, and config fields the same way).
///
/// # Examples
///
/// ```rust
/// use mecha10_core::differential_drive_odometry_impl;
/// use mecha10_core::kinematics::DifferentialDriveOdometry;
///
/// struct Config {
///     odom_rate_hz: f32,
/// }
///
/// struct SimMotor {
///     x: f32,
///     y: f32,
///     theta: f32,
///     cmd_linear: f32,
///     cmd_angular: f32,
///     config: Config,
/// }
///
/// differential_drive_odometry_impl!(SimMotor);
///
/// let motor = SimMotor {
///     x: 0.0,
///     y: 0.0,
///     theta: 0.0,
///     cmd_linear: 1.0,
///     cmd_angular: 0.0,
///     config: Config { odom_rate_hz: 10.0 },
/// };
/// assert_eq!(motor.odom_rate_hz(), 10.0);
/// ```
#[macro_export]
macro_rules! differential_drive_odometry_impl {
    ($struct:ident) => {
        impl $crate::kinematics::DifferentialDriveOdometry for $struct {
            fn odom_pose(&self) -> $crate::kinematics::Pose2D {
                $crate::kinematics::Pose2D {
                    x: self.x,
                    y: self.y,
                    theta: self.theta,
                }
            }

            fn set_odom_pose(&mut self, pose: $crate::kinematics::Pose2D) {
                self.x = pose.x;
                self.y = pose.y;
                self.theta = pose.theta;
            }

            fn commanded_velocity(&self) -> (f32, f32) {
                (self.cmd_linear, self.cmd_angular)
            }

            fn odom_rate_hz(&self) -> f32 {
                self.config.odom_rate_hz
            }
        }
    };
}

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

    #[test]
    fn integrates_straight_line_motion() {
        let pose = Pose2D {
            x: 0.0,
            y: 0.0,
            theta: 0.0,
        };
        let next = integrate_differential_drive(pose, 1.0, 0.0, 1.0);
        assert!((next.x - 1.0).abs() < 1e-6);
        assert!((next.y - 0.0).abs() < 1e-6);
        assert!((next.theta - 0.0).abs() < 1e-6);
    }

    #[test]
    fn normalizes_theta_above_pi() {
        let pose = Pose2D {
            x: 0.0,
            y: 0.0,
            theta: PI - 0.1,
        };
        let next = integrate_differential_drive(pose, 0.0, 1.0, 1.0);
        assert!(next.theta <= PI && next.theta >= -PI);
    }

    #[test]
    fn normalizes_theta_below_neg_pi() {
        let pose = Pose2D {
            x: 0.0,
            y: 0.0,
            theta: -PI + 0.1,
        };
        let next = integrate_differential_drive(pose, 0.0, -1.0, 1.0);
        assert!(next.theta <= PI && next.theta >= -PI);
    }

    struct FakeOdom {
        pose: Pose2D,
        linear: f32,
        angular: f32,
        rate_hz: f32,
    }

    impl DifferentialDriveOdometry for FakeOdom {
        fn odom_pose(&self) -> Pose2D {
            self.pose
        }
        fn set_odom_pose(&mut self, pose: Pose2D) {
            self.pose = pose;
        }
        fn commanded_velocity(&self) -> (f32, f32) {
            (self.linear, self.angular)
        }
        fn odom_rate_hz(&self) -> f32 {
            self.rate_hz
        }
    }

    #[test]
    fn default_update_odometry_matches_manual_integration() {
        let mut odom = FakeOdom {
            pose: Pose2D {
                x: 0.0,
                y: 0.0,
                theta: 0.0,
            },
            linear: 2.0,
            angular: 0.0,
            rate_hz: 10.0,
        };

        odom.update_odometry();

        let expected = integrate_differential_drive(
            Pose2D {
                x: 0.0,
                y: 0.0,
                theta: 0.0,
            },
            2.0,
            0.0,
            0.1,
        );
        assert_eq!(odom.pose, expected);
    }
}