mecha10_core/messages/

perception.rs

// Perception Message Types

use serde::{Deserialize, Serialize};

/// Object detection result
///
/// Represents a detected object with bounding box and classification
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Detection {
    /// Timestamp in microseconds since epoch
    pub timestamp: u64,

    /// Class ID
    pub class_id: u32,

    /// Class name (e.g., "person", "cup", "table")
    pub class_name: String,

    /// Confidence score (0.0 to 1.0)
    pub confidence: f32,

    /// Bounding box in image coordinates
    pub bbox: BoundingBox,

    /// Optional tracking ID (for multi-object tracking)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub track_id: Option<u32>,

    /// Optional 3D position (if depth available)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub position_3d: Option<[f32; 3]>,
}

/// 2D Bounding box
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct BoundingBox {
    /// X coordinate of top-left corner
    pub x: f32,

    /// Y coordinate of top-left corner
    pub y: f32,

    /// Width of the box
    pub width: f32,

    /// Height of the box
    pub height: f32,
}

impl BoundingBox {
    /// Create a new bounding box
    pub fn new(x: f32, y: f32, width: f32, height: f32) -> Self {
        Self { x, y, width, height }
    }

    /// Get the center point of the box
    pub fn center(&self) -> (f32, f32) {
        (self.x + self.width / 2.0, self.y + self.height / 2.0)
    }

    /// Get the area of the box
    pub fn area(&self) -> f32 {
        self.width * self.height
    }

    /// Calculate Intersection over Union (IoU) with another box
    pub fn iou(&self, other: &BoundingBox) -> f32 {
        let x1 = self.x.max(other.x);
        let y1 = self.y.max(other.y);
        let x2 = (self.x + self.width).min(other.x + other.width);
        let y2 = (self.y + self.height).min(other.y + other.height);

        if x2 < x1 || y2 < y1 {
            return 0.0;
        }

        let intersection = (x2 - x1) * (y2 - y1);
        let union = self.area() + other.area() - intersection;

        intersection / union
    }
}

/// 3D Pose (position + orientation)
///
/// Represents a 6-DOF pose in 3D space
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Pose {
    /// Timestamp in microseconds since epoch
    pub timestamp: u64,

    /// Position (x, y, z) in meters
    pub position: [f32; 3],

    /// Orientation as quaternion (x, y, z, w)
    pub orientation: [f32; 4],

    /// Optional frame ID
    #[serde(skip_serializing_if = "Option::is_none")]
    pub frame_id: Option<String>,

    /// Optional covariance matrix (6x6, row-major) - stored as Vec for serde compatibility
    #[serde(skip_serializing_if = "Option::is_none")]
    pub covariance: Option<Vec<f32>>,
}

// Helper methods for Detection
impl Detection {
    /// Creates a new detection
    pub fn new(class_id: u32, class_name: String, confidence: f32, bbox: BoundingBox) -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            class_id,
            class_name,
            confidence,
            bbox,
            track_id: None,
            position_3d: None,
        }
    }

    /// Checks if confidence is above a threshold
    pub fn is_confident(&self, threshold: f32) -> bool {
        self.confidence >= threshold
    }

    /// Returns the center of the bounding box
    pub fn center(&self) -> (f32, f32) {
        self.bbox.center()
    }

    /// Returns the area of the bounding box
    pub fn area(&self) -> f32 {
        self.bbox.area()
    }

    /// Checks if this detection has 3D position information
    pub fn has_3d_position(&self) -> bool {
        self.position_3d.is_some()
    }

    /// Returns the 3D distance from origin (if position available)
    pub fn distance_3d(&self) -> Option<f32> {
        self.position_3d
            .map(|pos| (pos[0] * pos[0] + pos[1] * pos[1] + pos[2] * pos[2]).sqrt())
    }

    /// Returns the age of this detection in microseconds
    pub fn age_micros(&self) -> u64 {
        crate::prelude::now_micros().saturating_sub(self.timestamp)
    }
}

impl Pose {
    /// Create a new pose at origin with identity orientation
    pub fn identity() -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            position: [0.0, 0.0, 0.0],
            orientation: [0.0, 0.0, 0.0, 1.0],
            frame_id: None,
            covariance: None,
        }
    }

    /// Create a new 2D pose (x, y, yaw)
    pub fn new_2d(x: f32, y: f32, yaw: f32) -> Self {
        // Convert yaw to quaternion
        let half_yaw = yaw / 2.0;
        let qz = half_yaw.sin();
        let qw = half_yaw.cos();

        Self {
            timestamp: crate::prelude::now_micros(),
            position: [x, y, 0.0],
            orientation: [0.0, 0.0, qz, qw],
            frame_id: None,
            covariance: None,
        }
    }

    /// Extract 2D pose (x, y, yaw)
    pub fn to_2d(&self) -> (f32, f32, f32) {
        let x = self.position[0];
        let y = self.position[1];

        // Convert quaternion to yaw
        let qz = self.orientation[2];
        let qw = self.orientation[3];
        let yaw = 2.0 * qz.atan2(qw);

        (x, y, yaw)
    }

    /// Calculates distance to another pose in 3D space
    pub fn distance_to(&self, other: &Pose) -> f32 {
        let dx = self.position[0] - other.position[0];
        let dy = self.position[1] - other.position[1];
        let dz = self.position[2] - other.position[2];
        (dx * dx + dy * dy + dz * dz).sqrt()
    }

    /// Calculates 2D distance to another pose
    pub fn distance_2d_to(&self, other: &Pose) -> f32 {
        let dx = self.position[0] - other.position[0];
        let dy = self.position[1] - other.position[1];
        (dx * dx + dy * dy).sqrt()
    }

    /// Normalizes the orientation quaternion
    pub fn normalize_orientation(&mut self) {
        let q = self.orientation;
        let mag = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
        if mag > 0.0 {
            self.orientation = [q[0] / mag, q[1] / mag, q[2] / mag, q[3] / mag];
        }
    }
}

impl Default for Pose {
    fn default() -> Self {
        Self::identity()
    }
}

// Helper methods for TrackingResult
impl TrackingResult {
    /// Creates a new tracking result
    pub fn new(track_id: u32, detection: Detection) -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            track_id,
            detection,
            velocity_2d: None,
            age: 0,
            time_since_update: 0,
            confidence: 1.0,
        }
    }

    /// Checks if the track is active (recently updated)
    pub fn is_active(&self) -> bool {
        self.time_since_update == 0
    }

    /// Checks if the track is stale (not updated for several frames)
    pub fn is_stale(&self, max_age: u32) -> bool {
        self.time_since_update > max_age
    }

    /// Checks if the track is mature (tracked for enough frames)
    pub fn is_mature(&self, min_age: u32) -> bool {
        self.age >= min_age
    }

    /// Returns the speed if velocity is available
    pub fn speed(&self) -> Option<f32> {
        self.velocity_2d.map(|v| (v[0] * v[0] + v[1] * v[1]).sqrt())
    }

    /// Checks if confidence is above a threshold
    pub fn is_confident(&self, threshold: f32) -> bool {
        self.confidence >= threshold
    }
}

/// Tracking result for a single object
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct TrackingResult {
    /// Timestamp in microseconds since epoch
    pub timestamp: u64,

    /// Unique tracking ID
    pub track_id: u32,

    /// Current detection
    pub detection: Detection,

    /// Estimated velocity (x, y) in pixels/second (optional)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub velocity_2d: Option<[f32; 2]>,

    /// Number of frames this object has been tracked
    pub age: u32,

    /// Number of consecutive frames without detection
    pub time_since_update: u32,

    /// Tracking confidence (0.0 to 1.0)
    pub confidence: f32,
}