[package]
name = "emergency-stop-py"
version = "0.1.0"
description = "E-stop monitor with multi-condition safety checks"
language = "python"

#!/usr/bin/env python3
"""Emergency stop monitor — multi-condition safety with debounce and fail-safe."""

import horus
from horus import Node, CmdVel, EmergencyStop, LaserScan, BatteryState, us, ms

# ── Safety thresholds ────────────────────────────────────────

MIN_OBSTACLE_DISTANCE = 0.2    # meters — hard stop if anything closer
LOW_BATTERY_VOLTAGE = 10.5     # volts — shutdown to prevent brownout
CLEAR_THRESHOLD = 50           # ticks of clear signals before releasing
STALE_THRESHOLD = 20           # ticks without sensor data = fault

# ── State ────────────────────────────────────────────────────

estop_active = [False]
consecutive_clears = [0]
ticks_since_scan = [0]
ticks_since_battery = [0]
total_ticks = [0]
trigger_reason = [""]

# ── Safety checks ────────────────────────────────────────────

def check_obstacle(scan):
    """Check if any LaserScan range is below the safety minimum."""
    if scan is None:
        return False, ""
    for i, r in enumerate(scan.ranges):
        if 0.01 < r < MIN_OBSTACLE_DISTANCE:
            return True, f"obstacle at {r:.2f}m (ray {i})"
    return False, ""

def check_battery(battery):
    """Check if battery voltage is critically low."""
    if battery is None:
        return False, ""
    if battery.voltage > 0 and battery.voltage < LOW_BATTERY_VOLTAGE:
        return True, f"battery low: {battery.voltage:.1f}V"
    return False, ""

def check_stale_sensors():
    """Check if sensor data has stopped arriving (publisher crash)."""
    if ticks_since_scan[0] > STALE_THRESHOLD:
        return True, f"lidar stale ({ticks_since_scan[0]} ticks)"
    if ticks_since_battery[0] > STALE_THRESHOLD:
        return True, f"battery stale ({ticks_since_battery[0]} ticks)"
    return False, ""

# ── Node callbacks ───────────────────────────────────────────

def estop_tick(node):
    total_ticks[0] += 1
    ticks_since_scan[0] += 1
    ticks_since_battery[0] += 1

    # IMPORTANT: always recv() ALL topics every tick to drain buffers
    scan = node.recv("lidar.scan")
    battery = node.recv("battery.state")

    if scan is not None:
        ticks_since_scan[0] = 0
    if battery is not None:
        ticks_since_battery[0] = 0

    # Evaluate all safety conditions
    triggered = False
    reason = ""

    obstacle_bad, obstacle_reason = check_obstacle(scan)
    if obstacle_bad:
        triggered = True
        reason = obstacle_reason

    battery_bad, battery_reason = check_battery(battery)
    if battery_bad:
        triggered = True
        reason = battery_reason

    stale_bad, stale_reason = check_stale_sensors()
    if stale_bad:
        triggered = True
        reason = stale_reason

    if triggered:
        # SAFETY: immediately activate E-stop
        estop_active[0] = True
        consecutive_clears[0] = 0
        trigger_reason[0] = reason
        print(f"E-STOP TRIGGERED: {reason}")
    else:
        consecutive_clears[0] += 1

    # Require N consecutive clear signals before releasing
    if estop_active[0] and consecutive_clears[0] >= CLEAR_THRESHOLD:
        estop_active[0] = False
        print("E-STOP RELEASED after clear period")

    if estop_active[0]:
        # SAFETY: override cmd_vel with zero — stops all motion
        node.send("cmd_vel", CmdVel(linear=0.0, angular=0.0))

    # Publish E-stop status for monitoring
    node.send("safety.estop", EmergencyStop(
        engaged=estop_active[0],
        reason=trigger_reason[0] if estop_active[0] else "",
    ))

def estop_shutdown(node):
    # SAFETY: zero velocity on shutdown
    node.send("cmd_vel", CmdVel(linear=0.0, angular=0.0))
    node.send("safety.estop", EmergencyStop(engaged=True, reason="shutdown"))
    print(f"EStop: shutdown after {total_ticks[0]} ticks")

# ── Main ─────────────────────────────────────────────────────

estop_node = Node(
    name="EStop",
    tick=estop_tick,
    shutdown=estop_shutdown,
    rate=100,                      # 100 Hz safety monitoring
    order=100,                     # IMPORTANT: runs LAST — overrides cmd_vel from other nodes
    subs=["lidar.scan", "battery.state"],
    pubs=["cmd_vel", "safety.estop"],
    budget=200 * us,               # tight budget for safety node
    deadline=500 * us,             # tight deadline
    on_miss="safe_mode",           # force safe state on deadline miss
)

if __name__ == "__main__":
    horus.run(
        estop_node,
        watchdog_ms=500,
    )

[HORUS] Scheduler running — tick_rate: 1000 Hz
[HORUS] Node "EStop" started (100 Hz)
E-STOP TRIGGERED: obstacle at 0.15m (ray 42)
E-STOP RELEASED after clear period
^C
EStop: shutdown after 1500 ticks
[HORUS] Shutting down...
[HORUS] Node "EStop" shutdown complete