mecha10-core 0.8.0

//! 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;

#[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,
    }
}

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);
    }
}

#[macro_export]
macro_rules! differential_drive_odometry_impl {
    ($struct:ident) => {
        $crate::differential_drive_odometry_impl!($struct, config.odom_rate_hz);
    };
    ($struct:ident, $($rate_path:tt)+) => {
        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.$($rate_path)+
            }
        }
    };
}

#[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);
    }
}