mecha10_nodes_teleop/

lib.rs

//! Teleoperation Coordination Node
//!
//! Arbitrates between multiple teleoperation input sources and publishes
//! velocity commands to the motor controller. Provides safety features like
//! command timeout, priority arbitration, and emergency stop.
//!
//! # Architecture
//!
//! This node is a **pure coordination layer** with no UI. Multiple control
//! interfaces (CLI, dashboard, game controller) publish to `/teleop/cmd`,
//! and this node decides which commands to forward to `/motor/cmd_vel`.
//!
//! # Topics
//!
//! - **Subscribes**: `/teleop/cmd` (TeleopInput) - Commands from control sources
//! - **Publishes**: `/motor/cmd_vel` (Twist) - Velocity commands to motor
//! - **Publishes**: `/teleop/status` (TeleopStatus) - Current state for UIs

mod config;

pub use config::TeleopConfig;
use mecha10_core::actuator::Twist;
use mecha10_core::prelude::*;
use mecha10_core::teleop::{TeleopInput, TeleopStatus};
use mecha10_core::topics::Topic;
use std::time::Duration;

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

    // Input topics
    pub const TELEOP_CMD: Topic<TeleopInput> = Topic::new("/teleop/cmd");

    // Output topics
    pub const CMD_VEL: Topic<Twist> = Topic::new("/motor/cmd_vel");
    pub const TELEOP_STATUS: Topic<TeleopStatus> = Topic::new("/teleop/status");
}

/// Teleoperation coordination node state
pub struct TeleopNode {
    config: TeleopConfig,
    status: TeleopStatus,
    current_linear: f32,
    current_angular: f32,
    target_linear: f32,
    target_angular: f32,
    last_input: Option<TeleopInput>,
}

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

#[async_trait]
impl NodeImpl for TeleopNode {
    type Config = TeleopConfig;

    async fn init(config: Self::Config) -> Result<Self> {
        info!("Initializing teleop coordination node");
        info!(
            "Max linear: {} m/s, Max angular: {} rad/s, Timeout: {} ms",
            config.max_linear_vel, config.max_angular_vel, config.command_timeout_ms
        );

        Ok(Self {
            config,
            status: TeleopStatus::new(),
            current_linear: 0.0,
            current_angular: 0.0,
            target_linear: 0.0,
            target_angular: 0.0,
            last_input: None,
        })
    }

    async fn run(&mut self, ctx: &Context) -> Result<()> {
        info!("Teleop coordination node running");
        info!("Subscribing to: /teleop/cmd");
        info!("Publishing to: /motor/cmd_vel, /teleop/status");
        info!(
            "Output rate: {} Hz, Smoothing: {} (factor: {})",
            self.config.output_rate_hz, self.config.enable_smoothing, self.config.smoothing_factor
        );

        // Subscribe to teleop commands
        let mut commands = ctx.subscribe(topics::TELEOP_CMD).await?;

        // Status publish interval
        let status_interval = Duration::from_secs_f32(1.0 / self.config.status_publish_rate);
        let mut status_ticker = tokio::time::interval(status_interval);
        status_ticker.set_missed_tick_behavior(tokio::time::MissedTickBehavior::Skip);

        // Timeout check interval (check more frequently than timeout)
        let timeout_check_interval = Duration::from_millis(self.config.command_timeout_ms / 2);
        let mut timeout_ticker = tokio::time::interval(timeout_check_interval);
        timeout_ticker.set_missed_tick_behavior(tokio::time::MissedTickBehavior::Skip);

        // Continuous output loop - publishes at fixed rate regardless of input timing
        // This decouples network jitter from motor output for smooth control
        let output_interval = Duration::from_secs_f32(1.0 / self.config.output_rate_hz);
        let mut output_ticker = tokio::time::interval(output_interval);
        output_ticker.set_missed_tick_behavior(tokio::time::MissedTickBehavior::Skip);

        loop {
            tokio::select! {
                // New teleop command received - updates target velocity
                Some(input) = commands.recv() => {
                    self.handle_input(input).await?;
                }

                // Continuous output - applies smoothing and publishes at fixed rate
                _ = output_ticker.tick() => {
                    self.update_and_publish(ctx).await?;
                }

                // Publish status periodically
                _ = status_ticker.tick() => {
                    self.publish_status(ctx).await?;
                }

                // Check for command timeout
                _ = timeout_ticker.tick() => {
                    self.check_timeout(ctx).await?;
                }
            }
        }
    }
}

impl TeleopNode {
    /// Handle incoming teleop input - updates target velocities only
    /// Actual publishing happens in the continuous output loop
    async fn handle_input(&mut self, input: TeleopInput) -> Result<()> {
        let current_time = now_micros();

        // Check if command is expired
        if input.is_expired(current_time, self.config.command_timeout_ms) {
            debug!(
                "Ignoring expired command from {} (age: {} ms)",
                input.source,
                input.age_ms(current_time)
            );
            return Ok(());
        }

        // Check if this source has priority over current source
        let should_accept = if let Some(last) = &self.last_input {
            input.priority >= last.priority
        } else {
            true
        };

        if !should_accept {
            debug!(
                "Rejecting lower priority command from {} (priority: {}, current: {})",
                input.source,
                input.priority,
                self.last_input.as_ref().unwrap().priority
            );
            return Ok(());
        }

        // Handle emergency stop - set targets to zero immediately
        if input.emergency_stop {
            warn!("Emergency stop from {}", input.source);
            self.target_linear = 0.0;
            self.target_angular = 0.0;
            self.current_linear = 0.0;
            self.current_angular = 0.0;
            self.status.safety_engaged = true;
            self.last_input = Some(input.clone());
            self.update_status(&input);
            return Ok(());
        }

        // Clamp velocities to configured limits
        let target_linear = input
            .linear
            .clamp(-self.config.max_linear_vel, self.config.max_linear_vel);
        let target_angular = input
            .angular
            .clamp(-self.config.max_angular_vel, self.config.max_angular_vel);

        // Apply smoothing once if enabled, otherwise use target directly
        if self.config.enable_smoothing {
            let alpha = self.config.smoothing_factor;
            self.current_linear += alpha * (target_linear - self.current_linear);
            self.current_angular += alpha * (target_angular - self.current_angular);
        } else {
            self.current_linear = target_linear;
            self.current_angular = target_angular;
        }

        // Store targets for reference
        self.target_linear = target_linear;
        self.target_angular = target_angular;

        // Reset safety engaged when new valid command arrives
        if self.status.safety_engaged {
            debug!("Releasing safety stop, resuming normal operation");
            self.status.safety_engaged = false;
        }

        // Update state
        self.last_input = Some(input.clone());
        self.update_status(&input);

        debug!(
            "Command from {} (priority: {}): target=({:.2}, {:.2}), current=({:.2}, {:.2})",
            input.source, input.priority, target_linear, target_angular, self.current_linear, self.current_angular
        );

        Ok(())
    }

    /// Continuous output loop - republishes current velocity at fixed rate
    /// This ensures consistent output to simulation-bridge regardless of input timing
    async fn update_and_publish(&mut self, ctx: &Context) -> Result<()> {
        // Don't publish if no input received yet
        if self.last_input.is_none() {
            return Ok(());
        }

        // Don't publish if safety is engaged
        if self.status.safety_engaged {
            return Ok(());
        }

        // Just republish the current velocity - no additional smoothing here
        // Smoothing is applied once when input arrives in handle_input
        self.publish_velocity(ctx).await?;

        Ok(())
    }

    /// Check for command timeout and stop if needed
    async fn check_timeout(&mut self, ctx: &Context) -> Result<()> {
        if let Some(last) = &self.last_input {
            let current_time = now_micros();
            let age_ms = last.age_ms(current_time);

            if age_ms > self.config.command_timeout_ms && !self.status.safety_engaged {
                warn!(
                    "Command timeout ({}ms > {}ms), engaging safety stop",
                    age_ms, self.config.command_timeout_ms
                );
                self.safety_stop(ctx).await?;
            }
        }

        Ok(())
    }

    /// Safety stop - timeout or fault
    async fn safety_stop(&mut self, ctx: &Context) -> Result<()> {
        // Reset all velocities to zero immediately
        self.target_linear = 0.0;
        self.target_angular = 0.0;
        self.current_linear = 0.0;
        self.current_angular = 0.0;
        self.status.safety_engaged = true;
        self.status.active_source = "none".to_string();

        // Publish zero velocity
        self.publish_velocity(ctx).await?;

        Ok(())
    }

    /// Publish velocity command to motor
    async fn publish_velocity(&self, ctx: &Context) -> Result<()> {
        let twist = Twist {
            linear: self.current_linear,
            angular: self.current_angular,
        };

        ctx.publish_to(topics::CMD_VEL, &twist).await?;

        Ok(())
    }

    /// Publish status for UIs
    async fn publish_status(&self, ctx: &Context) -> Result<()> {
        ctx.publish_to(topics::TELEOP_STATUS, &self.status).await?;
        Ok(())
    }

    /// Update status from input
    fn update_status(&mut self, input: &TeleopInput) {
        self.status.active_source = input.source.clone();
        self.status.active_priority = input.priority;
        self.status.last_command_age_ms = 0;
        self.status.safety_engaged = input.emergency_stop;
        self.status.current_linear = self.current_linear;
        self.status.current_angular = self.current_angular;
        self.status.total_commands += 1;
    }
}

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

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

    // Run the node with Normal priority (teleop coordination)
    run_node::<TeleopNode>(config, ctx, HealthReportingConfig::normal()).await
}