mecha10_behavior_runtime/actions/

estop.rs

//! Leaf condition node reporting whether the teleop emergency-stop signal is currently active -
//! the real-time backing for the highest-priority (`priority: 255`) subsumption interrupt layer
//! that preempts every behavior tree, including interrupt layers below it like
//! `instance-track-interrupt`.
//!
//! This reuses the EXISTING teleop safety signal rather than inventing a new one:
//! `mecha10_core::teleop::TeleopInput::emergency_stop(source, timestamp)` builds a
//! `priority: 255`, `emergency_stop: true` command, and the CLI TUI's e-stop key
//! (`packages/cli/src/ui/teleop.rs`'s `send_emergency_stop`) publishes it to the `/teleop/cmd`
//! topic (that file's local `TELEOP_CMD: Topic<TeleopInput>` constant). This node subscribes to
//! that same topic and only inspects the `emergency_stop` field of whatever arrives there,
//! ignoring `source`/`priority`/`command`/`timestamp` - mirroring
//! `CapabilityTaskNode::poll_target`'s minimal `TargetRequest` struct, which likewise only reads
//! the one field it cares about out of a larger message.

use crate::{BehaviorNode, Context, NodeStatus, TreeStatusSnapshot};
use async_trait::async_trait;
use mecha10_core::Receiver;
use serde::{Deserialize, Serialize};
use serde_json::Value;

/// The topic this node watches by default - the same `/teleop/cmd` topic the CLI TUI's e-stop
/// key publishes `TeleopInput` (including emergency-stop commands) to; see
/// `packages/cli/src/ui/teleop.rs`'s `TELEOP_CMD` constant. Configurable via `topic` so a
/// project that republishes teleop commands elsewhere can point this node there instead.
fn default_topic() -> String {
    "/teleop/cmd".to_string()
}

/// The subset of `mecha10_core::teleop::TeleopInput` this node actually inspects. Every other
/// field is ignored (and, because this doesn't `deny_unknown_fields`, never even rejected).
#[derive(Debug, Clone, Serialize, Deserialize)]
struct EmergencyStopSignal {
    #[serde(default)]
    emergency_stop: bool,
}

#[derive(Debug, Serialize, Deserialize)]
struct EstopActiveConfig {
    #[serde(default = "default_topic")]
    topic: String,
}

pub struct EstopActiveNode {
    topic: String,
    receiver: Option<Receiver<Value>>,
    /// Whether the latest message seen on `topic` carried `emergency_stop: true`. Defaults to
    /// `false` before any message has arrived - the absence of a signal is not itself treated as
    /// an emergency, the same Failure-until-proven-active convention
    /// `CapabilityTaskNode::has_target` uses for its target-tracking mode.
    active: bool,
    last_status: Option<NodeStatus>,
}

impl std::fmt::Debug for EstopActiveNode {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        f.debug_struct("EstopActiveNode")
            .field("topic", &self.topic)
            .field("subscribed", &self.receiver.is_some())
            .field("active", &self.active)
            .finish()
    }
}

impl EstopActiveNode {
    /// Create a new EstopActiveNode from JSON configuration. `topic` is optional and defaults to
    /// [`default_topic`]. A bare `null`/absent config (e.g. `{"type": "node", "node":
    /// "estop_active"}` with no `config` key at all) is treated the same as `{}` - the loader
    /// passes `Value::Null` through for a leaf node with no `config` entry, which
    /// `serde_json::from_value` can't deserialize directly into a struct.
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let cfg: EstopActiveConfig = if config.is_null() {
            EstopActiveConfig { topic: default_topic() }
        } else {
            serde_json::from_value(config)?
        };
        Ok(Self {
            topic: cfg.topic,
            receiver: None,
            active: false,
            last_status: None,
        })
    }

    /// Construct a node with an already-connected receiver, bypassing live subscription.
    ///
    /// Used by unit tests to exercise `poll()`/`tick()` convergence without requiring a real
    /// `Context`/Redis.
    #[cfg(test)]
    fn with_receiver(receiver: Receiver<Value>) -> Self {
        Self {
            topic: "test/topic".to_string(),
            receiver: Some(receiver),
            active: false,
            last_status: None,
        }
    }

    /// Drain every pending message, updating `active` from the latest one seen (if any) - only
    /// the most recent message matters, mirroring `SensorCheckNode::evaluate`/
    /// `CapabilityTaskNode::poll_target`.
    fn poll(&mut self) {
        let Some(receiver) = self.receiver.as_mut() else {
            return;
        };

        let mut latest = None;
        while let Ok(value) = receiver.try_recv() {
            match serde_json::from_value::<EmergencyStopSignal>(value) {
                Ok(signal) => latest = Some(signal.emergency_stop),
                Err(e) => tracing::warn!("EstopActiveNode: dropping malformed teleop message: {}", e),
            }
        }

        if let Some(active) = latest {
            self.active = active;
        }
    }
}

#[async_trait]
impl BehaviorNode for EstopActiveNode {
    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
        if self.receiver.is_none() {
            self.receiver = Some(ctx.subscribe_raw::<Value>(&self.topic).await?);
        }
        self.poll();
        let status = if self.active {
            NodeStatus::Running
        } else {
            NodeStatus::Failure
        };
        self.last_status = Some(status);
        Ok(status)
    }

    fn name(&self) -> &str {
        "EstopActiveNode"
    }

    // Deliberately does NOT override `active_capability()` (default `None`): unlike
    // `instance-track`'s `capability_task` leaf, this node never hands control to an external
    // capability node - it only reports a safety condition.

    fn status_snapshot(&self) -> TreeStatusSnapshot {
        TreeStatusSnapshot {
            name: self.name().to_string(),
            status: self.last_status,
            active_capability: self.active_capability().map(String::from),
            children: Vec::new(),
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use mecha10_core::teleop::{TeleopCommand, TeleopInput};
    use tokio::sync::mpsc;

    fn node_with_channel() -> (EstopActiveNode, mpsc::UnboundedSender<Value>) {
        let (tx, rx) = mpsc::unbounded_channel();
        (EstopActiveNode::with_receiver(Receiver::from(rx)), tx)
    }

    #[test]
    fn no_message_yet_defaults_to_inactive() {
        let (mut node, _tx) = node_with_channel();
        node.poll();
        assert!(!node.active);
    }

    #[test]
    fn emergency_stop_true_marks_active() {
        let (mut node, tx) = node_with_channel();
        let input = TeleopInput::emergency_stop("safety", 123);
        tx.send(serde_json::to_value(&input).unwrap()).unwrap();

        node.poll();

        assert!(node.active);
    }

    #[test]
    fn emergency_stop_false_marks_inactive() {
        let (mut node, tx) = node_with_channel();
        let input = TeleopInput::new("gamepad", 1.0, 0.0, 123);
        tx.send(serde_json::to_value(&input).unwrap()).unwrap();

        node.poll();

        assert!(!node.active);
    }

    #[test]
    fn later_message_overrides_earlier_one() {
        let (mut node, tx) = node_with_channel();
        tx.send(serde_json::to_value(TeleopInput::emergency_stop("safety", 1)).unwrap())
            .unwrap();
        tx.send(serde_json::to_value(TeleopInput::new("gamepad", 0.0, 0.0, 2)).unwrap())
            .unwrap();

        node.poll();

        assert!(!node.active);
    }

    #[test]
    fn ignores_malformed_messages_and_keeps_last_known_state() {
        let (mut node, tx) = node_with_channel();
        tx.send(serde_json::to_value(TeleopInput::emergency_stop("safety", 1)).unwrap())
            .unwrap();
        node.poll();
        assert!(node.active);

        tx.send(serde_json::json!("not an object")).unwrap();
        node.poll();

        assert!(
            node.active,
            "a malformed message must not reset a previously-known state"
        );
    }

    /// End-to-end `tick()` (real subscription via `Context`) convergence, mirroring
    /// `composition.rs`'s Redis-backed tests: `Failure` before any message, `Running` once an
    /// `emergency_stop: true` message is published.
    #[tokio::test]
    #[ignore = "Requires Redis infrastructure"]
    async fn tick_returns_failure_before_any_message_then_running_once_estop_fires() {
        let ctx = crate::test_helpers::create_test_context("estop_active_tick").await;
        let mut node = EstopActiveNode::from_json(serde_json::json!({ "topic": "/teleop/cmd" })).unwrap();

        assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Failure);

        let input = TeleopInput::emergency_stop("safety", 1);
        ctx.publish_to_path("/teleop/cmd", &input).await.unwrap();
        assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Running);
    }

    #[test]
    fn does_not_override_active_capability() {
        let (node, _tx) = node_with_channel();
        assert_eq!(node.active_capability(), None);
    }

    #[test]
    fn from_json_defaults_topic() {
        let node = EstopActiveNode::from_json(serde_json::json!({})).unwrap();
        assert_eq!(node.topic, "/teleop/cmd");
    }

    #[test]
    fn from_json_defaults_topic_for_a_bare_null_config() {
        // The loader passes `Value::Null` through for a leaf node with no `config` key at all
        // (e.g. `{"type": "node", "node": "estop_active"}`) - must not error out.
        let node = EstopActiveNode::from_json(Value::Null).unwrap();
        assert_eq!(node.topic, "/teleop/cmd");
    }

    #[test]
    fn from_json_reads_explicit_topic() {
        let node = EstopActiveNode::from_json(serde_json::json!({ "topic": "/custom/teleop" })).unwrap();
        assert_eq!(node.topic, "/custom/teleop");
    }

    #[test]
    fn teleop_command_variant_is_irrelevant_to_the_signal() {
        // Sanity: TeleopCommand::Drivetrain{0,0} with emergency_stop is what
        // TeleopInput::emergency_stop actually builds - confirm our struct only reads the one
        // field regardless of command shape.
        let input = TeleopInput::emergency_stop("safety", 1);
        assert!(matches!(input.command, TeleopCommand::Drivetrain { .. }));
        let signal: EmergencyStopSignal = serde_json::from_value(serde_json::to_value(&input).unwrap()).unwrap();
        assert!(signal.emergency_stop);
    }
}