mecha10-core 0.8.2

//! Shared sensor message types
//!
//! This module contains common sensor message types used across drivers and nodes.
//! Previously these were duplicated in each driver, now they are centralized here.

use crate::messages::Message;
use schemars::JsonSchema;
use serde::{Deserialize, Serialize};

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct RgbImage {
    pub timestamp: u64,
    pub frame_id: u64,
    pub width: u32,
    pub height: u32,
    pub encoding: String,
    pub data: Vec<u8>,
}

impl Message for RgbImage {}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct DepthImage {
    pub timestamp: u64,
    pub frame_id: u64,
    pub width: u32,
    pub height: u32,
    pub data: Vec<f32>,
}

impl Message for DepthImage {}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct CameraInfo {
    pub timestamp: u64,
    pub width: u32,
    pub height: u32,
    pub fps: f32,
}

impl Message for CameraInfo {}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct LaserScan {
    pub timestamp: u64,
    pub scan_id: u64,
    pub angle_min: f32,
    pub angle_max: f32,
    pub angle_increment: f32,
    pub range_min: f32,
    pub range_max: f32,
    pub ranges: Vec<f32>,
    pub intensities: Vec<f32>,
}

impl Message for LaserScan {}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Odometry {
    pub timestamp: u64,
    pub x: f32,
    pub y: f32,
    pub theta: f32,
    pub linear_vel: f32,
    pub angular_vel: f32,
}

impl Message for Odometry {}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct ImuData {
    #[serde(default = "default_imu_id")]
    pub imu_id: String,
    pub timestamp: u64,
    pub roll: f32,
    pub pitch: f32,
    pub yaw: f32,
    pub angular_velocity_x: f32,
    pub angular_velocity_y: f32,
    pub angular_velocity_z: f32,
    pub linear_acceleration_x: f32,
    pub linear_acceleration_y: f32,
    pub linear_acceleration_z: f32,
}

fn default_imu_id() -> String {
    "imu-0".to_string()
}

impl Message for ImuData {}

#[derive(Debug, Default, Clone, Copy, PartialEq, Eq, Serialize, Deserialize, JsonSchema)]
#[serde(try_from = "u16", into = "u16")]
#[schemars(with = "u16")]
pub enum GyroScale {
    #[default]
    Dps250 = 250,
    Dps500 = 500,
    Dps1000 = 1000,
    Dps2000 = 2000,
}

#[derive(Debug, Default, Clone, Copy, PartialEq, Eq, Serialize, Deserialize, JsonSchema)]
#[serde(try_from = "u8", into = "u8")]
#[schemars(with = "u8")]
pub enum AccelScale {
    #[default]
    G2 = 2,
    G4 = 4,
    G8 = 8,
    G16 = 16,
}

macro_rules! impl_scale_enum {
    ($ty:ident: $repr:ty { $($variant:ident = $val:literal),+ $(,)? }, $field:literal, $values:literal, $unit:literal) => {
        impl TryFrom<$repr> for $ty {
            type Error = String;

            fn try_from(value: $repr) -> Result<Self, Self::Error> {
                match value {
                    $($val => Ok(Self::$variant),)+
                    other => Err(format!("invalid {} {other}: must be one of {} ({})", $field, $values, $unit)),
                }
            }
        }

        impl From<$ty> for $repr {
            fn from(value: $ty) -> Self {
                value as $repr
            }
        }

        impl std::fmt::Display for $ty {
            fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
                write!(f, "{}", <$repr>::from(*self))
            }
        }
    };
}

impl_scale_enum!(
    GyroScale: u16 { Dps250 = 250, Dps500 = 500, Dps1000 = 1000, Dps2000 = 2000 },
    "gyro_scale", "250, 500, 1000, 2000", "dps"
);
impl_scale_enum!(
    AccelScale: u8 { G2 = 2, G4 = 4, G8 = 8, G16 = 16 },
    "accel_scale", "2, 4, 8, 16", "g"
);

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

    #[test]
    fn gyro_scale_try_from_valid() {
        for (raw, expected) in [
            (250u16, GyroScale::Dps250),
            (500, GyroScale::Dps500),
            (1000, GyroScale::Dps1000),
            (2000, GyroScale::Dps2000),
        ] {
            assert_eq!(GyroScale::try_from(raw).unwrap(), expected);
            assert_eq!(u16::from(expected), raw);
        }
    }

    #[test]
    fn gyro_scale_try_from_invalid() {
        let err = GyroScale::try_from(100u16).unwrap_err();
        assert!(err.contains("invalid gyro_scale 100"));
    }

    #[test]
    fn accel_scale_try_from_valid() {
        for (raw, expected) in [
            (2u8, AccelScale::G2),
            (4, AccelScale::G4),
            (8, AccelScale::G8),
            (16, AccelScale::G16),
        ] {
            assert_eq!(AccelScale::try_from(raw).unwrap(), expected);
            assert_eq!(u8::from(expected), raw);
        }
    }

    #[test]
    fn accel_scale_try_from_invalid() {
        let err = AccelScale::try_from(32u8).unwrap_err();
        assert!(err.contains("invalid accel_scale 32"));
    }

    #[test]
    fn display_formats_as_bare_number() {
        assert_eq!(GyroScale::Dps500.to_string(), "500");
        assert_eq!(AccelScale::G8.to_string(), "8");
    }

    #[test]
    fn defaults_match_datasheet_power_on_state() {
        assert_eq!(GyroScale::default(), GyroScale::Dps250);
        assert_eq!(AccelScale::default(), AccelScale::G2);
    }
}