mecha10_nodes_imu/
lib.rs

1//! IMU Node
2//!
3//! This node manages IMU sensors and publishes orientation, angular velocity,
4//! and linear acceleration data.
5//!
6//! # Controller Selection
7//!
8//! The node uses a factory pattern to create the appropriate controller based
9//! on configuration:
10//!
11//! - `mock`: Simulated IMU for testing/development (default)
12//! - `mpu6050`: MPU6050 6-DOF IMU (requires `mpu6050` feature)
13//!
14//! # Example Configuration
15//!
16//! ```json
17//! {
18//!   "controller": {
19//!     "type": "mock",
20//!     "has_magnetometer": false
21//!   },
22//!   "update_rate_hz": 100.0
23//! }
24//! ```
25//!
26//! # Topics
27//!
28//! - Publishes: `/imu/data` (ImuData) - IMU sensor data
29
30mod config;
31mod factory;
32
33pub use config::{ControllerConfig, ImuConfig, MockControllerConfig, Mpu6050ControllerConfig};
34use factory::{create_imu_controller, BoxedImuController};
35use mecha10_core::prelude::*;
36use mecha10_core::sensor::ImuData;
37use mecha10_core::topics::Topic;
38use std::time::Duration;
39
40/// IMU node topics
41pub mod topics {
42    use super::*;
43
44    pub const IMU_DATA: Topic<ImuData> = Topic::new("/imu/data");
45}
46
47/// IMU node
48///
49/// Manages an IMU controller and publishes sensor data at a configured rate.
50pub struct ImuNode {
51    config: ImuConfig,
52    controller: BoxedImuController,
53    update_counter: u64,
54}
55
56impl std::fmt::Debug for ImuNode {
57    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
58        f.debug_struct("ImuNode")
59            .field("config", &self.config)
60            .field("controller", &self.controller.name())
61            .field("update_counter", &self.update_counter)
62            .finish()
63    }
64}
65
66#[async_trait]
67impl NodeImpl for ImuNode {
68    type Config = ImuConfig;
69
70    async fn init(config: Self::Config) -> Result<Self> {
71        info!("Initializing IMU node");
72        info!("Update rate: {} Hz", config.update_rate_hz);
73
74        // Create controller using factory
75        let controller = create_imu_controller(config.controller.clone())
76            .await
77            .map_err(|e| anyhow::anyhow!("Failed to create IMU controller: {}", e))?;
78
79        info!("IMU controller created: {}", controller.name());
80
81        Ok(Self {
82            config,
83            controller,
84            update_counter: 0,
85        })
86    }
87
88    async fn run(&mut self, ctx: &Context) -> Result<()> {
89        let update_interval = Duration::from_secs_f32(1.0 / self.config.update_rate_hz);
90        let mut interval = tokio::time::interval(update_interval);
91
92        // Start controller
93        self.controller
94            .start()
95            .await
96            .map_err(|e| anyhow::anyhow!("Failed to start IMU controller: {}", e))?;
97
98        info!("IMU node running at {} Hz", self.config.update_rate_hz);
99        info!("Controller: {}", self.controller.name());
100        info!("Publishing to: /imu/data");
101
102        loop {
103            interval.tick().await;
104
105            // Read IMU data from controller
106            let imu_data = self
107                .controller
108                .read_imu()
109                .await
110                .map_err(|e| anyhow::anyhow!("Failed to read IMU: {}", e))?;
111
112            // Publish IMU data
113            ctx.publish_to(topics::IMU_DATA, &imu_data).await?;
114
115            self.update_counter += 1;
116
117            if self.update_counter % 1000 == 0 {
118                debug!("Published {} IMU updates", self.update_counter);
119            }
120        }
121    }
122}
123
124/// Run the IMU node
125pub async fn run() -> Result<()> {
126    // Create context first (needed for config loading)
127    let ctx = Context::new("imu").await?;
128
129    // Load config using V2 system with environment-aware file-level fallback
130    let config: ImuConfig = ctx.load_node_config("imu").await?;
131
132    // Run the node with Normal priority (sensor data)
133    run_node::<ImuNode>(config, ctx, HealthReportingConfig::normal()).await
134}
mecha10_nodes_imu/lib.rs

mecha10_nodes_imu/
lib.rs
