mecha10_nodes_motor/

lib.rs

//! Motor Node
//!
//! This node manages motor control with configurable controller backends.
//! It provides velocity control through the standard `/motor/cmd_vel` topic
//! and publishes odometry data.
//!
//! # Controller Selection
//!
//! The node supports multiple motor controllers, selected via configuration:
//!
//! - **mock**: Simulated motors for testing/development (no hardware required)
//! - **l298n**: L298N dual H-bridge driver for DC motors
//!
//! # Topics
//!
//! - Subscribes: `/motor/cmd_vel` (Twist) - Velocity commands
//! - Publishes: `/odom` (Odometry) - Odometry data
//! - Publishes: `/motor/status` (MotorStatus) - Motor status
//!
//! # Configuration
//!
//! ```json
//! {
//!   "controller": {
//!     "type": "mock",  // or "l298n"
//!     "wheel_base": 0.3,
//!     "max_velocity": 1.0
//!   },
//!   "odom_rate_hz": 50.0,
//!   "max_linear_vel": 0.5,
//!   "max_angular_vel": 1.0
//! }
//! ```
//!
//! See [config.rs](config.rs) for full configuration options.

mod config;
mod factory;

pub use config::{ControllerConfig, L298nControllerConfig, L298nMotorPins, MockControllerConfig, MotorConfig};
pub use factory::{create_motor_controller, BoxedMotorController, DynMotorController};

use mecha10_core::actuator::{MotorStatus, Twist};
use mecha10_core::prelude::*;
use mecha10_core::sensor::Odometry;
use mecha10_core::topics::Topic;
use std::time::Duration;

/// Motor node topics
pub mod topics {
    use super::*;

    // Input topics
    pub const CMD_VEL: Topic<Twist> = Topic::new("/motor/cmd_vel");

    // Output topics
    pub const ODOM: Topic<Odometry> = Topic::new("/odom");
    pub const MOTOR_STATUS: Topic<MotorStatus> = Topic::new("/motor/status");
}

/// Motor node
///
/// Manages motor control using a configurable controller backend.
pub struct MotorNode {
    config: MotorConfig,
    controller: BoxedMotorController,
    // Odometry state (dead reckoning from commanded velocities)
    x: f32,
    y: f32,
    theta: f32,
    // Current velocity command
    cmd_linear: f32,
    cmd_angular: f32,
    // Command timeout tracking
    last_cmd_time: u64,
    timed_out: bool,
    // Status tracking
    update_counter: u64,
}

impl std::fmt::Debug for MotorNode {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        f.debug_struct("MotorNode")
            .field("config", &self.config)
            .field("controller", &self.controller.name())
            .field("update_counter", &self.update_counter)
            .finish()
    }
}

#[async_trait]
impl NodeImpl for MotorNode {
    type Config = MotorConfig;

    async fn init(config: Self::Config) -> Result<Self> {
        info!("Initializing motor node");

        // Create controller based on config
        let controller = factory::create_motor_controller(config.controller.clone())
            .await
            .map_err(|e| Mecha10Error::NodeInit(format!("Failed to create controller: {}", e)))?;

        info!("Motor controller created: {}", controller.name());
        info!(
            "Odometry rate: {} Hz, Max linear: {} m/s, Max angular: {} rad/s",
            config.odom_rate_hz, config.max_linear_vel, config.max_angular_vel
        );

        Ok(Self {
            config,
            controller,
            x: 0.0,
            y: 0.0,
            theta: 0.0,
            cmd_linear: 0.0,
            cmd_angular: 0.0,
            last_cmd_time: 0,
            timed_out: false,
            update_counter: 0,
        })
    }

    async fn run(&mut self, ctx: &Context) -> Result<()> {
        // Start the controller
        self.controller
            .start()
            .await
            .map_err(|e| Mecha10Error::NodeInit(format!("Failed to start controller: {}", e)))?;

        // Subscribe to velocity commands
        let mut cmd_vel_rx = ctx.subscribe(topics::CMD_VEL).await?;

        // Setup timers
        let odom_interval = Duration::from_secs_f32(1.0 / self.config.odom_rate_hz);
        let mut odom_timer = tokio::time::interval(odom_interval);

        let status_interval = Duration::from_secs_f32(1.0 / self.config.status_rate_hz);
        let mut status_timer = tokio::time::interval(status_interval);

        // Command timeout check interval (100ms)
        let timeout_interval = Duration::from_millis(100);
        let mut timeout_timer = tokio::time::interval(timeout_interval);

        info!("Motor node running");
        info!("Controller: {}", self.controller.name());
        info!("Odometry rate: {} Hz", self.config.odom_rate_hz);
        info!("Status rate: {} Hz", self.config.status_rate_hz);
        if self.config.cmd_timeout_sec > 0.0 {
            info!("Command timeout: {} sec", self.config.cmd_timeout_sec);
        } else {
            info!("Command timeout: disabled");
        }
        info!("Listening on: /motor/cmd_vel");
        info!("Publishing to: /odom, /motor/status");

        loop {
            tokio::select! {
                // Handle velocity commands
                Some(cmd) = cmd_vel_rx.recv() => {
                    self.handle_cmd_vel(cmd).await?;
                }

                // Periodic odometry updates
                _ = odom_timer.tick() => {
                    self.update_odometry();
                    self.publish_odometry(ctx).await?;
                    self.update_counter += 1;

                    if self.update_counter % 1000 == 0 {
                        debug!("Published {} odometry updates", self.update_counter);
                    }
                }

                // Periodic status updates
                _ = status_timer.tick() => {
                    self.publish_status(ctx).await?;
                }

                // Check command timeout
                _ = timeout_timer.tick() => {
                    self.check_command_timeout().await?;
                }
            }
        }
    }

    async fn shutdown(&mut self) -> Result<()> {
        info!("Shutting down motor node");

        // Emergency stop first
        if let Err(e) = self.controller.emergency_stop().await {
            warn!("Emergency stop failed: {}", e);
        }

        // Stop the controller
        if let Err(e) = self.controller.stop().await {
            warn!("Controller stop failed: {}", e);
        }

        info!("Motor node shutdown complete");
        Ok(())
    }
}

impl MotorNode {
    /// Handle incoming velocity command
    async fn handle_cmd_vel(&mut self, cmd: Twist) -> Result<()> {
        // Update command timestamp
        self.last_cmd_time = now_micros();
        self.timed_out = false;

        // Clamp velocities to configured limits
        self.cmd_linear = cmd
            .linear
            .clamp(-self.config.max_linear_vel, self.config.max_linear_vel);
        self.cmd_angular = cmd
            .angular
            .clamp(-self.config.max_angular_vel, self.config.max_angular_vel);

        debug!(
            "Velocity command: linear={:.2} m/s, angular={:.2} rad/s",
            self.cmd_linear, self.cmd_angular
        );

        // Convert twist to wheel velocities using differential drive kinematics
        let wheel_base = self.get_wheel_base();
        let left_vel = self.cmd_linear - (self.cmd_angular * wheel_base / 2.0);
        let right_vel = self.cmd_linear + (self.cmd_angular * wheel_base / 2.0);

        // Send to controller
        self.controller
            .set_velocity(left_vel, right_vel)
            .await
            .map_err(|e| Mecha10Error::NodeInit(format!("Failed to set velocity: {}", e)))?;

        Ok(())
    }

    /// Check for command timeout and stop motors if exceeded
    async fn check_command_timeout(&mut self) -> Result<()> {
        // Skip if timeout disabled
        if self.config.cmd_timeout_sec <= 0.0 {
            return Ok(());
        }

        // Skip if no command received yet
        if self.last_cmd_time == 0 {
            return Ok(());
        }

        let now = now_micros();
        let timeout_micros = (self.config.cmd_timeout_sec * 1_000_000.0) as u64;
        let elapsed = now.saturating_sub(self.last_cmd_time);

        if elapsed > timeout_micros && !self.timed_out {
            // Timeout exceeded, stop motors
            warn!(
                "Command timeout exceeded ({:.1}s > {:.1}s), stopping motors",
                elapsed as f32 / 1_000_000.0,
                self.config.cmd_timeout_sec
            );

            self.cmd_linear = 0.0;
            self.cmd_angular = 0.0;
            self.timed_out = true;

            // Send stop command to controller
            self.controller
                .set_velocity(0.0, 0.0)
                .await
                .map_err(|e| Mecha10Error::NodeInit(format!("Failed to stop motors: {}", e)))?;
        }

        Ok(())
    }

    /// Get wheel base from controller config
    fn get_wheel_base(&self) -> f32 {
        match &self.config.controller {
            ControllerConfig::Mock(cfg) => cfg.wheel_base,
            ControllerConfig::L298n(cfg) => cfg.wheel_base,
        }
    }

    /// Get wheel radius from controller config
    fn get_wheel_radius(&self) -> f32 {
        match &self.config.controller {
            ControllerConfig::Mock(cfg) => cfg.wheel_radius,
            ControllerConfig::L298n(cfg) => cfg.wheel_radius,
        }
    }

    /// Update odometry using dead reckoning
    fn update_odometry(&mut self) {
        let dt = 1.0 / self.config.odom_rate_hz;

        // Simple differential drive kinematics
        let delta_theta = self.cmd_angular * dt;
        let delta_x = self.cmd_linear * dt * self.theta.cos();
        let delta_y = self.cmd_linear * dt * self.theta.sin();

        self.x += delta_x;
        self.y += delta_y;
        self.theta += delta_theta;

        // Normalize theta to [-PI, PI]
        while self.theta > std::f32::consts::PI {
            self.theta -= 2.0 * std::f32::consts::PI;
        }
        while self.theta < -std::f32::consts::PI {
            self.theta += 2.0 * std::f32::consts::PI;
        }
    }

    /// Publish odometry data
    async fn publish_odometry(&self, ctx: &Context) -> Result<()> {
        let odom = Odometry {
            timestamp: now_micros(),
            x: self.x,
            y: self.y,
            theta: self.theta,
            linear_vel: self.cmd_linear,
            angular_vel: self.cmd_angular,
        };

        ctx.publish_to(topics::ODOM, &odom).await?;
        Ok(())
    }

    /// Publish motor status
    async fn publish_status(&mut self, ctx: &Context) -> Result<()> {
        // Try to get status from controller
        let status = match self.controller.get_status().await {
            Ok(s) => s,
            Err(e) => {
                debug!("Could not get controller status: {}", e);
                // Fall back to calculated values
                self.calculate_status()
            }
        };

        ctx.publish_to(topics::MOTOR_STATUS, &status).await?;
        Ok(())
    }

    /// Calculate motor status from commanded velocities
    fn calculate_status(&self) -> MotorStatus {
        // Calculate motor RPMs based on wheel kinematics
        let wheel_base = self.get_wheel_base();
        let wheel_radius = self.get_wheel_radius();
        let linear_to_rpm = 60.0 / (2.0 * std::f32::consts::PI * wheel_radius);

        let left_vel = self.cmd_linear - (self.cmd_angular * wheel_base / 2.0);
        let right_vel = self.cmd_linear + (self.cmd_angular * wheel_base / 2.0);

        MotorStatus {
            timestamp: now_micros(),
            left_motor_rpm: left_vel * linear_to_rpm,
            right_motor_rpm: right_vel * linear_to_rpm,
            battery_voltage: 0.0, // Unknown without sensing
            temperature_c: 0.0,   // Unknown without sensing
        }
    }
}

/// Run the motor node with V2 configuration system
pub async fn run() -> Result<()> {
    // Create context first (needed for config loading)
    let ctx = Context::new("motor").await?;

    // Load config using V2 system with environment-aware file-level fallback
    let config: MotorConfig = ctx.load_node_config("motor").await?;

    // Run the node with Critical priority (safety-critical actuator control)
    run_node::<MotorNode>(config, ctx, HealthReportingConfig::critical()).await
}

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

    #[test]
    fn test_wheel_base_extraction() {
        let mock_config = MotorConfig {
            controller: ControllerConfig::Mock(MockControllerConfig {
                wheel_base: 0.4,
                ..Default::default()
            }),
            ..Default::default()
        };

        let l298n_config = MotorConfig {
            controller: ControllerConfig::L298n(L298nControllerConfig {
                wheel_base: 0.5,
                ..Default::default()
            }),
            ..Default::default()
        };

        // We can't easily test get_wheel_base without creating the full node,
        // but we can at least test that the configs are correctly structured
        assert!(matches!(mock_config.controller, ControllerConfig::Mock(_)));
        assert!(matches!(l298n_config.controller, ControllerConfig::L298n(_)));
    }
}