817 lines
23 KiB
Rust
817 lines
23 KiB
Rust
//! Autonomous vehicle perception module
|
|
//!
|
|
//! Provides:
|
|
//! - LiDAR point cloud processing and segmentation
|
|
//! - Sensor fusion (camera, LiDAR, radar integration)
|
|
//! - Path planning integration with perception
|
|
//! - Object tracking and trajectory prediction
|
|
//! - Safety-critical validation and verification
|
|
|
|
// Modules to be implemented
|
|
// pub mod lidar;
|
|
// pub mod fusion;
|
|
// pub mod tracking;
|
|
// pub mod planning;
|
|
// pub mod safety;
|
|
// pub mod calibration;
|
|
|
|
// Placeholder structs for unimplemented modules
|
|
/// Path plan result
|
|
#[derive(Debug, Clone)]
|
|
pub struct PathPlan {
|
|
pub waypoints: Vec<[f32; 3]>,
|
|
pub trajectory_cost: f32,
|
|
}
|
|
|
|
/// Placeholder PathPlanner
|
|
#[derive(Debug, Clone)]
|
|
pub struct PathPlanner {
|
|
// Placeholder implementation
|
|
}
|
|
|
|
impl PathPlanner {
|
|
/// Plan a path based on tracked objects and sensor frame
|
|
pub fn plan(
|
|
&mut self,
|
|
_objects: &[TrackedObject],
|
|
_frame: &SensorFrame,
|
|
) -> VisionResult<PathPlan> {
|
|
// Placeholder implementation
|
|
Ok(PathPlan {
|
|
waypoints: vec![],
|
|
trajectory_cost: 0.0,
|
|
})
|
|
}
|
|
}
|
|
|
|
use crate::detection::three_d::{BoundingBox3D, Detection3DResult, PointCloud};
|
|
use crate::{DetectionResult, VisionError, VisionResult};
|
|
use rtx_tensor::Tensor;
|
|
use std::collections::HashMap;
|
|
use tracing::debug;
|
|
|
|
/// Autonomous driving sensor types
|
|
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
|
|
pub enum SensorType {
|
|
/// Light Detection and Ranging
|
|
LiDAR,
|
|
/// Camera (RGB)
|
|
Camera,
|
|
/// Radar
|
|
Radar,
|
|
/// Inertial Measurement Unit
|
|
IMU,
|
|
/// Global Navigation Satellite System
|
|
GNSS,
|
|
/// Wheel odometry
|
|
Odometry,
|
|
}
|
|
|
|
/// Sensor data timestamp
|
|
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
|
|
pub struct Timestamp {
|
|
pub seconds: u64,
|
|
pub nanoseconds: u32,
|
|
}
|
|
|
|
impl Timestamp {
|
|
pub fn now() -> Self {
|
|
let duration = std::time::SystemTime::now()
|
|
.duration_since(std::time::UNIX_EPOCH)
|
|
.unwrap();
|
|
|
|
Self {
|
|
seconds: duration.as_secs(),
|
|
nanoseconds: duration.subsec_nanos(),
|
|
}
|
|
}
|
|
|
|
pub fn to_seconds(&self) -> f64 {
|
|
self.seconds as f64 + self.nanoseconds as f64 * 1e-9
|
|
}
|
|
|
|
pub fn duration_since(&self, other: &Self) -> f64 {
|
|
self.to_seconds() - other.to_seconds()
|
|
}
|
|
}
|
|
|
|
/// Multi-sensor data frame
|
|
#[derive(Debug, Clone)]
|
|
pub struct SensorFrame {
|
|
/// Frame timestamp
|
|
pub timestamp: Timestamp,
|
|
/// LiDAR point cloud data
|
|
pub lidar_data: Option<LidarData>,
|
|
/// Camera image data
|
|
pub camera_data: Option<CameraData>,
|
|
/// Radar detection data
|
|
pub radar_data: Option<RadarData>,
|
|
/// IMU measurement
|
|
pub imu_data: Option<ImuData>,
|
|
/// GNSS position
|
|
pub gnss_data: Option<GnssData>,
|
|
/// Odometry data
|
|
pub odometry_data: Option<OdometryData>,
|
|
}
|
|
|
|
/// LiDAR sensor data
|
|
#[derive(Debug, Clone)]
|
|
pub struct LidarData {
|
|
/// Point cloud
|
|
pub point_cloud: PointCloud,
|
|
/// Sensor pose relative to vehicle
|
|
pub sensor_pose: Pose3D,
|
|
/// Sensor configuration
|
|
pub config: LidarConfig,
|
|
}
|
|
|
|
/// LiDAR configuration
|
|
#[derive(Debug, Clone)]
|
|
pub struct LidarConfig {
|
|
/// Sensor model name
|
|
pub model: String,
|
|
/// Vertical field of view (degrees)
|
|
pub vertical_fov: (f32, f32), // (min, max)
|
|
/// Horizontal field of view (degrees)
|
|
pub horizontal_fov: f32, // typically 360
|
|
/// Vertical resolution (number of beams)
|
|
pub vertical_resolution: usize,
|
|
/// Angular resolution (degrees per step)
|
|
pub angular_resolution: f32,
|
|
/// Maximum range (meters)
|
|
pub max_range: f32,
|
|
/// Minimum range (meters)
|
|
pub min_range: f32,
|
|
}
|
|
|
|
impl Default for LidarConfig {
|
|
fn default() -> Self {
|
|
// Default configuration similar to Velodyne VLP-16
|
|
Self {
|
|
model: "VLP-16".to_string(),
|
|
vertical_fov: (-15.0, 15.0),
|
|
horizontal_fov: 360.0,
|
|
vertical_resolution: 16,
|
|
angular_resolution: 0.2, // 0.2 degrees
|
|
max_range: 100.0,
|
|
min_range: 0.3,
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Camera sensor data
|
|
#[derive(Debug, Clone)]
|
|
pub struct CameraData {
|
|
/// RGB image tensor (3, H, W)
|
|
pub image: Tensor,
|
|
/// Camera intrinsic parameters
|
|
pub intrinsics: CameraIntrinsics,
|
|
/// Camera pose relative to vehicle
|
|
pub sensor_pose: Pose3D,
|
|
/// Exposure and timing info
|
|
pub metadata: CameraMetadata,
|
|
}
|
|
|
|
/// Camera intrinsic parameters
|
|
#[derive(Debug, Clone)]
|
|
pub struct CameraIntrinsics {
|
|
/// Focal length (fx, fy)
|
|
pub focal_length: (f32, f32),
|
|
/// Principal point (cx, cy)
|
|
pub principal_point: (f32, f32),
|
|
/// Distortion coefficients [k1, k2, p1, p2, k3]
|
|
pub distortion: [f32; 5],
|
|
/// Image size (width, height)
|
|
pub image_size: (usize, usize),
|
|
}
|
|
|
|
/// Camera metadata
|
|
#[derive(Debug, Clone)]
|
|
pub struct CameraMetadata {
|
|
/// Exposure time (seconds)
|
|
pub exposure_time: f32,
|
|
/// ISO sensitivity
|
|
pub iso: u32,
|
|
/// Frame sequence number
|
|
pub frame_id: u64,
|
|
}
|
|
|
|
/// Radar sensor data
|
|
#[derive(Debug, Clone)]
|
|
pub struct RadarData {
|
|
/// Radar detections
|
|
pub detections: Vec<RadarDetection>,
|
|
/// Sensor pose relative to vehicle
|
|
pub sensor_pose: Pose3D,
|
|
/// Radar configuration
|
|
pub config: RadarConfig,
|
|
}
|
|
|
|
/// Individual radar detection
|
|
#[derive(Debug, Clone)]
|
|
pub struct RadarDetection {
|
|
/// Range (meters)
|
|
pub range: f32,
|
|
/// Azimuth angle (radians)
|
|
pub azimuth: f32,
|
|
/// Elevation angle (radians)
|
|
pub elevation: f32,
|
|
/// Radial velocity (m/s)
|
|
pub velocity: f32,
|
|
/// Radar cross section (dBsm)
|
|
pub rcs: f32,
|
|
/// Signal-to-noise ratio (dB)
|
|
pub snr: f32,
|
|
}
|
|
|
|
/// Radar configuration
|
|
#[derive(Debug, Clone)]
|
|
pub struct RadarConfig {
|
|
/// Sensor model
|
|
pub model: String,
|
|
/// Frequency (GHz)
|
|
pub frequency: f32,
|
|
/// Maximum range (meters)
|
|
pub max_range: f32,
|
|
/// Range resolution (meters)
|
|
pub range_resolution: f32,
|
|
/// Azimuth field of view (degrees)
|
|
pub azimuth_fov: f32,
|
|
/// Elevation field of view (degrees)
|
|
pub elevation_fov: f32,
|
|
}
|
|
|
|
/// IMU sensor data
|
|
#[derive(Debug, Clone)]
|
|
pub struct ImuData {
|
|
/// Linear acceleration (m/s²) in body frame
|
|
pub acceleration: [f32; 3],
|
|
/// Angular velocity (rad/s) in body frame
|
|
pub angular_velocity: [f32; 3],
|
|
/// Magnetic field (µT) in body frame
|
|
pub magnetic_field: Option<[f32; 3]>,
|
|
/// Measurement covariance
|
|
pub covariance: Option<[[f32; 6]; 6]>,
|
|
}
|
|
|
|
/// GNSS sensor data
|
|
#[derive(Debug, Clone)]
|
|
pub struct GnssData {
|
|
/// Latitude (degrees)
|
|
pub latitude: f64,
|
|
/// Longitude (degrees)
|
|
pub longitude: f64,
|
|
/// Altitude (meters above sea level)
|
|
pub altitude: f32,
|
|
/// Position accuracy (meters)
|
|
pub position_accuracy: f32,
|
|
/// Fix quality
|
|
pub fix_quality: GnssFixQuality,
|
|
/// Number of satellites
|
|
pub num_satellites: u8,
|
|
}
|
|
|
|
/// GNSS fix quality
|
|
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
|
pub enum GnssFixQuality {
|
|
NoFix,
|
|
GPS,
|
|
DGPS,
|
|
RTK,
|
|
FloatRTK,
|
|
}
|
|
|
|
/// Vehicle odometry data
|
|
#[derive(Debug, Clone)]
|
|
pub struct OdometryData {
|
|
/// Vehicle velocity (m/s) in body frame
|
|
pub velocity: [f32; 3],
|
|
/// Vehicle pose estimate
|
|
pub pose: Pose3D,
|
|
/// Pose covariance
|
|
pub covariance: [[f32; 6]; 6],
|
|
/// Wheel speeds (rad/s) [front_left, front_right, rear_left, rear_right]
|
|
pub wheel_speeds: Option<[f32; 4]>,
|
|
}
|
|
|
|
/// 3D pose representation
|
|
#[derive(Debug, Clone)]
|
|
pub struct Pose3D {
|
|
/// Position (x, y, z) in meters
|
|
pub position: [f32; 3],
|
|
/// Orientation quaternion (w, x, y, z)
|
|
pub orientation: [f32; 4],
|
|
}
|
|
|
|
impl Default for Pose3D {
|
|
fn default() -> Self {
|
|
Self {
|
|
position: [0.0, 0.0, 0.0],
|
|
orientation: [1.0, 0.0, 0.0, 0.0], // Identity quaternion
|
|
}
|
|
}
|
|
}
|
|
|
|
impl Pose3D {
|
|
/// Create identity pose
|
|
pub fn identity() -> Self {
|
|
Self::default()
|
|
}
|
|
|
|
/// Create pose from position and Euler angles
|
|
pub fn from_position_euler(position: [f32; 3], euler: [f32; 3]) -> Self {
|
|
let (roll, pitch, yaw) = (euler[0], euler[1], euler[2]);
|
|
|
|
// Convert Euler angles to quaternion
|
|
let cr = (roll * 0.5).cos();
|
|
let sr = (roll * 0.5).sin();
|
|
let cp = (pitch * 0.5).cos();
|
|
let sp = (pitch * 0.5).sin();
|
|
let cy = (yaw * 0.5).cos();
|
|
let sy = (yaw * 0.5).sin();
|
|
|
|
let w = cr * cp * cy + sr * sp * sy;
|
|
let x = sr * cp * cy - cr * sp * sy;
|
|
let y = cr * sp * cy + sr * cp * sy;
|
|
let z = cr * cp * sy - sr * sp * cy;
|
|
|
|
Self {
|
|
position,
|
|
orientation: [w, x, y, z],
|
|
}
|
|
}
|
|
|
|
/// Get transformation matrix (4x4)
|
|
pub fn to_matrix(&self) -> [[f32; 4]; 4] {
|
|
let [x, y, z] = self.position;
|
|
let [w, qx, qy, qz] = self.orientation;
|
|
|
|
// Convert quaternion to rotation matrix
|
|
let xx = qx * qx;
|
|
let yy = qy * qy;
|
|
let zz = qz * qz;
|
|
let xy = qx * qy;
|
|
let xz = qx * qz;
|
|
let yz = qy * qz;
|
|
let wx = w * qx;
|
|
let wy = w * qy;
|
|
let wz = w * qz;
|
|
|
|
[
|
|
[1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy), x],
|
|
[2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx), y],
|
|
[2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy), z],
|
|
[0.0, 0.0, 0.0, 1.0],
|
|
]
|
|
}
|
|
|
|
/// Transform point from this coordinate frame to world
|
|
pub fn transform_point(&self, point: [f32; 3]) -> [f32; 3] {
|
|
let matrix = self.to_matrix();
|
|
[
|
|
matrix[0][0] * point[0]
|
|
+ matrix[0][1] * point[1]
|
|
+ matrix[0][2] * point[2]
|
|
+ matrix[0][3],
|
|
matrix[1][0] * point[0]
|
|
+ matrix[1][1] * point[1]
|
|
+ matrix[1][2] * point[2]
|
|
+ matrix[1][3],
|
|
matrix[2][0] * point[0]
|
|
+ matrix[2][1] * point[1]
|
|
+ matrix[2][2] * point[2]
|
|
+ matrix[2][3],
|
|
]
|
|
}
|
|
}
|
|
|
|
/// Autonomous driving perception pipeline
|
|
pub struct AutonomousPipeline {
|
|
/// Object detection models
|
|
detectors: HashMap<SensorType, Box<dyn AutonomousDetector>>,
|
|
/// Sensor fusion module
|
|
// fusion: fusion::SensorFusion,
|
|
/// Object tracker
|
|
// tracker: tracking::MultiObjectTracker,
|
|
/// Path planner integration
|
|
path_planner: Option<PathPlanner>,
|
|
// Safety validator
|
|
// safety_validator: safety::SafetyValidator,
|
|
}
|
|
|
|
impl AutonomousPipeline {
|
|
/// Create new autonomous perception pipeline
|
|
pub fn new() -> VisionResult<Self> {
|
|
let detectors = HashMap::new();
|
|
|
|
// Initialize default detectors
|
|
// detectors.insert(SensorType::LiDAR, Box::new(lidar::LidarDetector::new()?));
|
|
|
|
Ok(Self {
|
|
detectors,
|
|
// fusion: fusion::SensorFusion::new()?,
|
|
// tracker: tracking::MultiObjectTracker::new()?,
|
|
path_planner: None,
|
|
// safety_validator: safety::SafetyValidator::new(),
|
|
})
|
|
}
|
|
|
|
/// Process multi-sensor frame
|
|
pub fn process_frame(
|
|
&mut self,
|
|
frame: &SensorFrame,
|
|
) -> VisionResult<AutonomousPerceptionResult> {
|
|
let start_time = std::time::Instant::now();
|
|
|
|
let mut detections = Vec::new();
|
|
let mut detection_results = HashMap::new();
|
|
|
|
// Process LiDAR data
|
|
if let Some(ref lidar_data) = frame.lidar_data
|
|
&& let Some(detector) = self.detectors.get_mut(&SensorType::LiDAR)
|
|
{
|
|
let result = detector.detect_lidar(lidar_data)?;
|
|
detections.extend(result.boxes_3d.clone());
|
|
detection_results.insert(
|
|
SensorType::LiDAR,
|
|
DetectionResultVariant::Detection3D(result),
|
|
);
|
|
}
|
|
|
|
// Process camera data
|
|
if let Some(ref camera_data) = frame.camera_data
|
|
&& let Some(detector) = self.detectors.get_mut(&SensorType::Camera)
|
|
{
|
|
let result = detector.detect_camera(camera_data)?;
|
|
// Convert 2D detections to 3D estimates (simplified)
|
|
detection_results.insert(
|
|
SensorType::Camera,
|
|
DetectionResultVariant::Detection2D(result),
|
|
);
|
|
}
|
|
|
|
// Process radar data
|
|
if let Some(ref radar_data) = frame.radar_data {
|
|
let radar_detections = self.process_radar_data(radar_data)?;
|
|
detections.extend(radar_detections);
|
|
}
|
|
|
|
// Sensor fusion
|
|
// let fused_detections = self.fusion.fuse_detections(&detections, frame)?;
|
|
let fused_detections = detections.clone(); // Temporary: use raw detections
|
|
|
|
// Object tracking
|
|
// let tracked_objects = self.tracker.update(&fused_detections, frame.timestamp)?;
|
|
let tracked_objects = vec![]; // Temporary: empty tracking
|
|
|
|
// Safety validation
|
|
// let safety_status = self.safety_validator.validate(&tracked_objects, frame)?;
|
|
let safety_status = SafetyStatus {
|
|
safety_level: SafetyLevel::Safe,
|
|
collision_risks: vec![],
|
|
recommended_actions: vec![],
|
|
time_to_collision: None,
|
|
}; // Temporary default
|
|
|
|
// Path planning (if available)
|
|
let path_planning_result = if let Some(ref mut planner) = self.path_planner {
|
|
Some(planner.plan(&tracked_objects, frame)?)
|
|
} else {
|
|
None
|
|
};
|
|
|
|
let processing_time = start_time.elapsed().as_millis() as f32;
|
|
|
|
Ok(AutonomousPerceptionResult {
|
|
timestamp: frame.timestamp,
|
|
raw_detections: detection_results,
|
|
fused_detections,
|
|
tracked_objects,
|
|
safety_status,
|
|
path_plan: path_planning_result,
|
|
processing_time_ms: processing_time,
|
|
})
|
|
}
|
|
|
|
/// Add detector for specific sensor type
|
|
pub fn add_detector(&mut self, sensor_type: SensorType, detector: Box<dyn AutonomousDetector>) {
|
|
self.detectors.insert(sensor_type, detector);
|
|
}
|
|
|
|
/// Set path planner
|
|
pub fn set_path_planner(&mut self, planner: PathPlanner) {
|
|
self.path_planner = Some(planner);
|
|
}
|
|
|
|
/// Process radar data into 3D detections
|
|
fn process_radar_data(&self, radar_data: &RadarData) -> VisionResult<Vec<BoundingBox3D>> {
|
|
let mut detections = Vec::new();
|
|
|
|
for detection in &radar_data.detections {
|
|
if detection.range > 0.5 && detection.snr > 10.0 {
|
|
// Basic filtering
|
|
// Convert spherical to cartesian coordinates
|
|
let x = detection.range * detection.azimuth.cos() * detection.elevation.cos();
|
|
let y = detection.range * detection.azimuth.sin() * detection.elevation.cos();
|
|
let z = detection.range * detection.elevation.sin();
|
|
|
|
// Transform to vehicle coordinates
|
|
let point_sensor = [x, y, z];
|
|
let point_vehicle = radar_data.sensor_pose.transform_point(point_sensor);
|
|
|
|
// Create 3D bounding box (with estimated size)
|
|
let bbox_3d = BoundingBox3D::new(
|
|
point_vehicle,
|
|
[2.0, 1.0, 1.5], // Estimated vehicle dimensions
|
|
[0.0, 0.0, 0.0], // No rotation estimate from radar
|
|
detection.snr / 50.0, // Convert SNR to confidence
|
|
0, // Unknown class from radar alone
|
|
);
|
|
|
|
detections.push(bbox_3d);
|
|
}
|
|
}
|
|
|
|
debug!(
|
|
"Processed {} radar detections into {} 3D boxes",
|
|
radar_data.detections.len(),
|
|
detections.len()
|
|
);
|
|
Ok(detections)
|
|
}
|
|
}
|
|
|
|
/// Autonomous detector trait
|
|
pub trait AutonomousDetector {
|
|
/// Detect objects from LiDAR data
|
|
fn detect_lidar(&mut self, _lidar_data: &LidarData) -> VisionResult<Detection3DResult> {
|
|
Err(VisionError::invalid_input("LiDAR detection not supported"))
|
|
}
|
|
|
|
/// Detect objects from camera data
|
|
fn detect_camera(&mut self, _camera_data: &CameraData) -> VisionResult<DetectionResult> {
|
|
Err(VisionError::invalid_input("Camera detection not supported"))
|
|
}
|
|
|
|
/// Get supported sensor types
|
|
fn supported_sensors(&self) -> Vec<SensorType>;
|
|
}
|
|
|
|
/// Detection result variants for different sensors
|
|
#[derive(Debug, Clone)]
|
|
pub enum DetectionResultVariant {
|
|
Detection2D(DetectionResult),
|
|
Detection3D(Detection3DResult),
|
|
}
|
|
|
|
/// Tracked object in 3D space
|
|
#[derive(Debug, Clone)]
|
|
pub struct TrackedObject {
|
|
/// Unique track ID
|
|
pub track_id: usize,
|
|
/// Current 3D bounding box
|
|
pub bbox_3d: BoundingBox3D,
|
|
/// Object velocity (m/s) in world coordinates
|
|
pub velocity: [f32; 3],
|
|
/// Track confidence
|
|
pub track_confidence: f32,
|
|
/// Object classification
|
|
pub object_class: ObjectClass,
|
|
/// Track age (number of frames)
|
|
pub age: usize,
|
|
/// Number of consecutive detections
|
|
pub hits: usize,
|
|
/// Time since last detection
|
|
pub time_since_update: f32,
|
|
/// Predicted trajectory
|
|
pub predicted_trajectory: Vec<TrajectoryPoint>,
|
|
}
|
|
|
|
/// Object classification for autonomous driving
|
|
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
|
pub enum ObjectClass {
|
|
Vehicle,
|
|
Pedestrian,
|
|
Cyclist,
|
|
Motorcycle,
|
|
Truck,
|
|
Bus,
|
|
TrafficSign,
|
|
TrafficLight,
|
|
Barrier,
|
|
Construction,
|
|
Animal,
|
|
Unknown,
|
|
}
|
|
|
|
/// Point in predicted trajectory
|
|
#[derive(Debug, Clone)]
|
|
pub struct TrajectoryPoint {
|
|
/// Position at time t
|
|
pub position: [f32; 3],
|
|
/// Time from now (seconds)
|
|
pub time_offset: f32,
|
|
/// Uncertainty covariance
|
|
pub covariance: [[f32; 3]; 3],
|
|
}
|
|
|
|
/// Safety assessment result
|
|
#[derive(Debug, Clone)]
|
|
pub struct SafetyStatus {
|
|
/// Overall safety level
|
|
pub safety_level: SafetyLevel,
|
|
/// Potential collision risks
|
|
pub collision_risks: Vec<CollisionRisk>,
|
|
/// Recommended actions
|
|
pub recommended_actions: Vec<SafetyAction>,
|
|
/// Time to collision estimates
|
|
pub time_to_collision: Option<f32>,
|
|
}
|
|
|
|
/// Safety levels
|
|
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
|
pub enum SafetyLevel {
|
|
Safe,
|
|
Caution,
|
|
Warning,
|
|
Critical,
|
|
Emergency,
|
|
}
|
|
|
|
/// Collision risk assessment
|
|
#[derive(Debug, Clone)]
|
|
pub struct CollisionRisk {
|
|
/// Object involved in potential collision
|
|
pub object_id: usize,
|
|
/// Risk probability [0, 1]
|
|
pub probability: f32,
|
|
/// Time to collision (seconds)
|
|
pub time_to_collision: f32,
|
|
/// Collision severity estimate
|
|
pub severity: CollisionSeverity,
|
|
}
|
|
|
|
/// Collision severity levels
|
|
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
|
pub enum CollisionSeverity {
|
|
Minor,
|
|
Moderate,
|
|
Major,
|
|
Severe,
|
|
}
|
|
|
|
/// Safety actions
|
|
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
|
pub enum SafetyAction {
|
|
None,
|
|
SlowDown,
|
|
Stop,
|
|
SteerLeft,
|
|
SteerRight,
|
|
EmergencyBrake,
|
|
PullOver,
|
|
}
|
|
|
|
/// Complete perception result
|
|
#[derive(Debug, Clone)]
|
|
pub struct AutonomousPerceptionResult {
|
|
/// Frame timestamp
|
|
pub timestamp: Timestamp,
|
|
/// Raw detection results per sensor
|
|
pub raw_detections: HashMap<SensorType, DetectionResultVariant>,
|
|
/// Fused 3D detections
|
|
pub fused_detections: Vec<BoundingBox3D>,
|
|
/// Tracked objects with motion
|
|
pub tracked_objects: Vec<TrackedObject>,
|
|
/// Safety assessment
|
|
pub safety_status: SafetyStatus,
|
|
/// Path planning result
|
|
pub path_plan: Option<PathPlan>,
|
|
/// Total processing time
|
|
pub processing_time_ms: f32,
|
|
}
|
|
|
|
/// Vehicle state for motion planning
|
|
#[derive(Debug, Clone)]
|
|
pub struct VehicleState {
|
|
/// Current pose
|
|
pub pose: Pose3D,
|
|
/// Linear velocity (m/s)
|
|
pub velocity: [f32; 3],
|
|
/// Angular velocity (rad/s)
|
|
pub angular_velocity: [f32; 3],
|
|
/// Acceleration (m/s²)
|
|
pub acceleration: [f32; 3],
|
|
/// Steering angle (radians)
|
|
pub steering_angle: f32,
|
|
}
|
|
|
|
/// Coordinate system transformations
|
|
pub struct CoordinateTransforms;
|
|
|
|
impl CoordinateTransforms {
|
|
/// Convert from LiDAR coordinates to vehicle coordinates
|
|
pub fn lidar_to_vehicle(point: [f32; 3], sensor_pose: &Pose3D) -> [f32; 3] {
|
|
sensor_pose.transform_point(point)
|
|
}
|
|
|
|
/// Convert from vehicle coordinates to world coordinates
|
|
pub fn vehicle_to_world(point: [f32; 3], vehicle_pose: &Pose3D) -> [f32; 3] {
|
|
vehicle_pose.transform_point(point)
|
|
}
|
|
|
|
/// Project 3D point to camera image
|
|
pub fn world_to_camera_image(
|
|
point: [f32; 3],
|
|
camera_pose: &Pose3D,
|
|
intrinsics: &CameraIntrinsics,
|
|
) -> Option<(f32, f32)> {
|
|
// Transform to camera coordinates
|
|
let matrix = camera_pose.to_matrix();
|
|
|
|
// Inverse transform (world to camera)
|
|
let cam_x = matrix[0][0] * (point[0] - matrix[0][3])
|
|
+ matrix[1][0] * (point[1] - matrix[1][3])
|
|
+ matrix[2][0] * (point[2] - matrix[2][3]);
|
|
let cam_y = matrix[0][1] * (point[0] - matrix[0][3])
|
|
+ matrix[1][1] * (point[1] - matrix[1][3])
|
|
+ matrix[2][1] * (point[2] - matrix[2][3]);
|
|
let cam_z = matrix[0][2] * (point[0] - matrix[0][3])
|
|
+ matrix[1][2] * (point[1] - matrix[1][3])
|
|
+ matrix[2][2] * (point[2] - matrix[2][3]);
|
|
|
|
// Check if point is in front of camera
|
|
if cam_z <= 0.0 {
|
|
return None;
|
|
}
|
|
|
|
// Project to image plane
|
|
let fx = intrinsics.focal_length.0;
|
|
let fy = intrinsics.focal_length.1;
|
|
let cx = intrinsics.principal_point.0;
|
|
let cy = intrinsics.principal_point.1;
|
|
|
|
let u = fx * cam_x / cam_z + cx;
|
|
let v = fy * cam_y / cam_z + cy;
|
|
|
|
// Check if projection is within image bounds
|
|
if u >= 0.0
|
|
&& u < intrinsics.image_size.0 as f32
|
|
&& v >= 0.0
|
|
&& v < intrinsics.image_size.1 as f32
|
|
{
|
|
Some((u, v))
|
|
} else {
|
|
None
|
|
}
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
#[test]
|
|
fn test_timestamp() {
|
|
let ts1 = Timestamp {
|
|
seconds: 100,
|
|
nanoseconds: 500_000_000,
|
|
};
|
|
let ts2 = Timestamp {
|
|
seconds: 101,
|
|
nanoseconds: 0,
|
|
};
|
|
|
|
assert_eq!(ts1.to_seconds(), 100.5);
|
|
assert_eq!(ts2.duration_since(&ts1), 0.5);
|
|
}
|
|
|
|
#[test]
|
|
fn test_pose_3d() {
|
|
let pose =
|
|
Pose3D::from_position_euler([1.0, 2.0, 3.0], [0.0, 0.0, std::f32::consts::PI / 2.0]);
|
|
let point = [1.0, 0.0, 0.0];
|
|
let transformed = pose.transform_point(point);
|
|
|
|
// After 90 degree yaw rotation, (1,0,0) should become (0,1,0), then add translation
|
|
assert!((transformed[0] - 1.0).abs() < 0.01); // x = 1 + 0
|
|
assert!((transformed[1] - 3.0).abs() < 0.01); // y = 2 + 1
|
|
assert!((transformed[2] - 3.0).abs() < 0.01); // z = 3 + 0
|
|
}
|
|
|
|
#[test]
|
|
fn test_sensor_frame_creation() {
|
|
let frame = SensorFrame {
|
|
timestamp: Timestamp::now(),
|
|
lidar_data: None,
|
|
camera_data: None,
|
|
radar_data: None,
|
|
imu_data: None,
|
|
gnss_data: None,
|
|
odometry_data: None,
|
|
};
|
|
|
|
assert!(frame.timestamp.seconds > 0);
|
|
}
|
|
}
|