From 0dfc707c49b7107ca22955098061b18d02a43108 Mon Sep 17 00:00:00 2001 From: Lumi-node Date: Tue, 17 Feb 2026 10:25:54 -0600 Subject: [PATCH] feat: add zeroclaw-robot-kit crate for AI-powered robotics Standalone robot toolkit providing AI agents with physical world interaction. Features: - 6 tools: drive, look, listen, speak, sense, emote - Multiple backends: ROS2, serial, GPIO, mock - Independent SafetyMonitor with E-stop, collision avoidance - Designed for Raspberry Pi 5 + Ollama offline operation - 55 unit/integration tests - Complete Pi 5 hardware setup guide --- Cargo.lock | 141 ++++++++- Cargo.toml | 2 +- crates/robot-kit/Cargo.toml | 65 ++++ crates/robot-kit/PI5_SETUP.md | 515 +++++++++++++++++++++++++++++++ crates/robot-kit/README.md | 216 +++++++++++++ crates/robot-kit/SOUL.md | 65 ++++ crates/robot-kit/robot.toml | 150 +++++++++ crates/robot-kit/src/config.rs | 217 +++++++++++++ crates/robot-kit/src/drive.rs | 336 +++++++++++++++++++++ crates/robot-kit/src/emote.rs | 327 ++++++++++++++++++++ crates/robot-kit/src/lib.rs | 154 ++++++++++ crates/robot-kit/src/listen.rs | 194 ++++++++++++ crates/robot-kit/src/look.rs | 240 +++++++++++++++ crates/robot-kit/src/safety.rs | 532 ++++++++++++++++++++++++++++++++ crates/robot-kit/src/sense.rs | 411 +++++++++++++++++++++++++ crates/robot-kit/src/speak.rs | 229 ++++++++++++++ crates/robot-kit/src/tests.rs | 536 +++++++++++++++++++++++++++++++++ crates/robot-kit/src/traits.rs | 123 ++++++++ 18 files changed, 4444 insertions(+), 9 deletions(-) create mode 100644 crates/robot-kit/Cargo.toml create mode 100644 crates/robot-kit/PI5_SETUP.md create mode 100644 crates/robot-kit/README.md create mode 100644 crates/robot-kit/SOUL.md create mode 100644 crates/robot-kit/robot.toml create mode 100644 crates/robot-kit/src/config.rs create mode 100644 crates/robot-kit/src/drive.rs create mode 100644 crates/robot-kit/src/emote.rs create mode 100644 crates/robot-kit/src/lib.rs create mode 100644 crates/robot-kit/src/listen.rs create mode 100644 crates/robot-kit/src/look.rs create mode 100644 crates/robot-kit/src/safety.rs create mode 100644 crates/robot-kit/src/sense.rs create mode 100644 crates/robot-kit/src/speak.rs create mode 100644 crates/robot-kit/src/tests.rs create mode 100644 crates/robot-kit/src/traits.rs diff --git a/Cargo.lock b/Cargo.lock index 911fcca..8c40ac9 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -472,8 +472,10 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fac4744fb15ae8337dc853fee7fb3f4e48c0fbaa23d0afe49c447b4fab126118" dependencies = [ "iana-time-zone", + "js-sys", "num-traits", "serde", + "wasm-bindgen", "windows-link", ] @@ -907,13 +909,22 @@ dependencies = [ "subtle", ] +[[package]] +name = "directories" +version = "5.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a49173b84e034382284f27f1af4dcbbd231ffa358c0fe316541a7337f376a35" +dependencies = [ + "dirs-sys 0.4.1", +] + [[package]] name = "directories" version = "6.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "16f5094c54661b38d03bd7e50df373292118db60b585c08a411c6d840017fe7d" dependencies = [ - "dirs-sys", + "dirs-sys 0.5.0", ] [[package]] @@ -922,7 +933,19 @@ version = "6.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c3e8aa94d75141228480295a7d0e7feb620b1a5ad9f12bc40be62411e38cce4e" dependencies = [ - "dirs-sys", + "dirs-sys 0.5.0", +] + +[[package]] +name = "dirs-sys" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "520f05a5cbd335fae5a99ff7a6ab8627577660ee5cfd6a94a6a929b52ff0321c" +dependencies = [ + "libc", + "option-ext", + "redox_users 0.4.6", + "windows-sys 0.48.0", ] [[package]] @@ -933,7 +956,7 @@ checksum = "e01a3366d27ee9890022452ee61b2b63a67e6f13f58900b651ff5665f0bb1fab" dependencies = [ "libc", "option-ext", - "redox_users", + "redox_users 0.5.2", "windows-sys 0.61.2", ] @@ -2806,7 +2829,7 @@ version = "3.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "219cb19e96be00ab2e37d6e299658a0cfa83e52429179969b0f0121b4ac46983" dependencies = [ - "toml_edit", + "toml_edit 0.23.10+spec-1.0.0", ] [[package]] @@ -3041,6 +3064,17 @@ dependencies = [ "bitflags 2.11.0", ] +[[package]] +name = "redox_users" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba009ff324d1fc1b900bd1fdb31564febe58a8ccc8a6fdbb93b543d33b13ca43" +dependencies = [ + "getrandom 0.2.17", + "libredox", + "thiserror 1.0.69", +] + [[package]] name = "redox_users" version = "0.5.2" @@ -3157,6 +3191,15 @@ dependencies = [ "serde", ] +[[package]] +name = "rppal" +version = "0.19.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3b37e992f3222e304708025de77c9e395068a347449d0d7164f52d3beccdbd8d" +dependencies = [ + "libc", +] + [[package]] name = "rppal" version = "0.22.1" @@ -3416,6 +3459,15 @@ dependencies = [ "serde", ] +[[package]] +name = "serde_spanned" +version = "0.6.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bf41e0cfaf7226dca15e8197172c295a782857fcb97fad1808a166870dee75a3" +dependencies = [ + "serde", +] + [[package]] name = "serde_spanned" version = "1.0.4" @@ -3890,6 +3942,17 @@ dependencies = [ "tokio", ] +[[package]] +name = "tokio-test" +version = "0.4.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f6d24790a10a7af737693a3e8f1d03faef7e6ca0cc99aae5066f533766de545" +dependencies = [ + "futures-core", + "tokio", + "tokio-stream", +] + [[package]] name = "tokio-tungstenite" version = "0.24.0" @@ -3931,6 +3994,18 @@ dependencies = [ "tokio", ] +[[package]] +name = "toml" +version = "0.8.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc1beb996b9d83529a9e75c17a1686767d148d70663143c7854d8b4a09ced362" +dependencies = [ + "serde", + "serde_spanned 0.6.9", + "toml_datetime 0.6.11", + "toml_edit 0.22.27", +] + [[package]] name = "toml" version = "1.0.2+spec-1.1.0" @@ -3939,13 +4014,22 @@ checksum = "d1dfefef6a142e93f346b64c160934eb13b5594b84ab378133ac6815cb2bd57f" dependencies = [ "indexmap", "serde_core", - "serde_spanned", + "serde_spanned 1.0.4", "toml_datetime 1.0.0+spec-1.1.0", "toml_parser", "toml_writer", "winnow 0.7.14", ] +[[package]] +name = "toml_datetime" +version = "0.6.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "22cddaf88f4fbc13c51aebbf5f8eceb5c7c5a9da2ac40a13519eb5b0a0e8f11c" +dependencies = [ + "serde", +] + [[package]] name = "toml_datetime" version = "0.7.5+spec-1.1.0" @@ -3964,6 +4048,20 @@ dependencies = [ "serde_core", ] +[[package]] +name = "toml_edit" +version = "0.22.27" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41fe8c660ae4257887cf66394862d21dbca4a6ddd26f04a3560410406a2f819a" +dependencies = [ + "indexmap", + "serde", + "serde_spanned 0.6.9", + "toml_datetime 0.6.11", + "toml_write", + "winnow 0.7.14", +] + [[package]] name = "toml_edit" version = "0.23.10+spec-1.0.0" @@ -3985,6 +4083,12 @@ dependencies = [ "winnow 0.7.14", ] +[[package]] +name = "toml_write" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5d99f8c9a7727884afe522e9bd5edbfc91a3312b36a77b5fb8926e4c31a41801" + [[package]] name = "toml_writer" version = "1.0.6+spec-1.1.0" @@ -5077,7 +5181,7 @@ dependencies = [ "criterion", "cron", "dialoguer", - "directories", + "directories 6.0.0", "fantoccini", "futures", "futures-util", @@ -5102,7 +5206,7 @@ dependencies = [ "regex", "reqwest", "ring", - "rppal", + "rppal 0.22.1", "rusqlite", "rustls", "rustls-pki-types", @@ -5116,7 +5220,7 @@ dependencies = [ "tokio-rustls", "tokio-serial", "tokio-tungstenite 0.24.0", - "toml", + "toml 1.0.2+spec-1.1.0", "tower", "tower-http", "tracing", @@ -5125,6 +5229,27 @@ dependencies = [ "webpki-roots 1.0.6", ] +[[package]] +name = "zeroclaw-robot-kit" +version = "0.1.0" +dependencies = [ + "anyhow", + "async-trait", + "base64", + "chrono", + "directories 5.0.1", + "reqwest", + "rppal 0.19.0", + "serde", + "serde_json", + "tempfile", + "thiserror 2.0.18", + "tokio", + "tokio-test", + "toml 0.8.23", + "tracing", +] + [[package]] name = "zerocopy" version = "0.8.39" diff --git a/Cargo.toml b/Cargo.toml index 92a063f..ecb3c0e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,5 +1,5 @@ [workspace] -members = ["."] +members = [".", "crates/robot-kit"] resolver = "2" [package] diff --git a/crates/robot-kit/Cargo.toml b/crates/robot-kit/Cargo.toml new file mode 100644 index 0000000..10e43e1 --- /dev/null +++ b/crates/robot-kit/Cargo.toml @@ -0,0 +1,65 @@ +[package] +name = "zeroclaw-robot-kit" +version = "0.1.0" +edition = "2021" +authors = ["theonlyhennygod"] +license = "MIT" +description = "Robot control toolkit for ZeroClaw - drive, vision, speech, sensors, safety" +repository = "https://github.com/theonlyhennygod/zeroclaw" +readme = "README.md" +keywords = ["robotics", "raspberry-pi", "ai", "agent", "ros2"] +categories = ["science::robotics", "embedded", "hardware-support"] + +[features] +default = ["safety"] +# Core features +safety = [] # Safety monitor (recommended!) +ros2 = [] # ROS2 integration +gpio = ["dep:rppal"] # Direct GPIO control (Pi only) +# Optional hardware +lidar = [] # LIDAR support +vision = [] # Camera + vision model + +[dependencies] +# Re-use zeroclaw's tool trait (optional - can also be standalone) +# zeroclaw = { path = "../..", optional = true } + +# Async runtime +tokio = { version = "1.42", features = ["rt-multi-thread", "macros", "time", "sync", "process", "fs", "io-util"] } + +# Serialization +serde = { version = "1.0", features = ["derive"] } +serde_json = "1.0" +toml = "0.8" + +# HTTP client (for Ollama vision) +reqwest = { version = "0.12", default-features = false, features = ["json", "rustls-tls"] } + +# Base64 encoding (for image data) +base64 = "0.22" + +# Async traits +async-trait = "0.1" + +# Error handling +anyhow = "1.0" +thiserror = "2.0" + +# Logging +tracing = "0.1" + +# Time handling +chrono = { version = "0.4", features = ["clock", "std"] } + +# User directories +directories = "5.0" + +# GPIO (Raspberry Pi only, optional) +rppal = { version = "0.19", optional = true } + +[dev-dependencies] +tokio-test = "0.4" +tempfile = "3.14" + +[package.metadata.docs.rs] +all-features = true diff --git a/crates/robot-kit/PI5_SETUP.md b/crates/robot-kit/PI5_SETUP.md new file mode 100644 index 0000000..5e90a5f --- /dev/null +++ b/crates/robot-kit/PI5_SETUP.md @@ -0,0 +1,515 @@ +# Raspberry Pi 5 Robot Setup Guide + +Complete guide to setting up a ZeroClaw-powered robot on Raspberry Pi 5. + +## Hardware Requirements + +### Minimum Setup +| Component | Recommended | Notes | +|-----------|-------------|-------| +| **Pi 5** | 8GB model | 4GB works but limits model size | +| **Storage** | 64GB+ NVMe or SD | NVMe recommended for speed | +| **Power** | 27W USB-C PSU | Official Pi 5 PSU recommended | +| **Cooling** | Active cooler | Required for sustained inference | + +### Robot Hardware +| Component | Model | Connection | Price (approx) | +|-----------|-------|------------|----------------| +| **Motor Controller** | L298N or TB6612FNG | GPIO PWM | $5-15 | +| **Motors** | 4× TT Motors + Omni wheels | Via controller | $30-50 | +| **LIDAR** | RPLidar A1 | USB `/dev/ttyUSB0` | $100 | +| **Camera** | Pi Camera 3 or USB webcam | CSI or USB | $25-50 | +| **Microphone** | USB mic or ReSpeaker | USB | $10-30 | +| **Speaker** | 3W amp + speaker | I2S or 3.5mm | $10-20 | +| **E-Stop** | Big red mushroom button | GPIO 4 | $5 | +| **Bump Sensors** | 2× Microswitches | GPIO 5, 6 | $3 | +| **LED Matrix** | 8×8 WS2812B | GPIO 18 (PWM) | $10 | + +### Wiring Diagram + +``` + ┌─────────────────────────────────────┐ + │ Raspberry Pi 5 │ + │ │ + ┌─────────────────┤ GPIO 4 ←── E-Stop Button (NC) │ + │ │ GPIO 5 ←── Bump Sensor Left │ + │ │ GPIO 6 ←── Bump Sensor Right │ + │ │ GPIO 12 ──→ Motor PWM 1 │ + │ │ GPIO 13 ──→ Motor PWM 2 │ + │ │ GPIO 17 ←── PIR Motion 1 │ + │ │ GPIO 18 ──→ LED Matrix (WS2812) │ + │ │ GPIO 23 ──→ Ultrasonic Trigger │ + │ │ GPIO 24 ←── Ultrasonic Echo │ + │ │ GPIO 27 ←── PIR Motion 2 │ + │ │ │ + │ ┌───────────────┤ USB-A ←── RPLidar A1 │ + │ │ │ USB-A ←── USB Microphone │ + │ │ │ USB-A ←── USB Webcam (if no CSI) │ + │ │ │ CSI ←── Pi Camera 3 │ + │ │ │ I2S/3.5mm → Speaker/Amp │ + │ │ └─────────────────────────────────────┘ + │ │ + │ │ ┌──────────────────┐ + │ └──┤ RPLidar A1 │ + │ │ /dev/ttyUSB0 │ + │ └──────────────────┘ + │ + │ ┌──────────────────┐ ┌─────────────┐ + └────┤ Motor Controller├──────┤ 4× Motors │ + │ (L298N/TB6612) │ │ Omni Wheels │ + └──────────────────┘ └─────────────┘ +``` + +## Software Setup + +### 1. Base OS + +```bash +# Flash Raspberry Pi OS (64-bit, Bookworm) to NVMe/SD +# Use Raspberry Pi Imager with these settings: +# - Enable SSH +# - Set hostname: robot +# - Set username/password +# - Configure WiFi + +# After boot, update everything +sudo apt update && sudo apt upgrade -y + +# Install build essentials +sudo apt install -y \ + build-essential \ + git \ + curl \ + cmake \ + pkg-config \ + libssl-dev \ + libasound2-dev \ + libclang-dev +``` + +### 2. Install Rust + +```bash +curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh +source ~/.cargo/env +``` + +### 3. Install Ollama (Local LLM) + +```bash +curl -fsSL https://ollama.ai/install.sh | sh + +# Pull models (choose based on RAM) +# 8GB Pi: Use smaller models +ollama pull llama3.2:3b # 3B params, fast +ollama pull moondream # Vision model, small + +# 4GB Pi: Use tiny models +ollama pull phi3:mini # 3.8B, very fast +ollama pull moondream # Vision + +# Start Ollama service +sudo systemctl enable ollama +sudo systemctl start ollama + +# Test +curl http://localhost:11434/api/tags +``` + +### 4. Install Whisper.cpp (Speech-to-Text) + +```bash +git clone https://github.com/ggerganov/whisper.cpp +cd whisper.cpp + +# Build with ARM optimizations +make -j4 + +# Download model (base is good balance) +bash ./models/download-ggml-model.sh base + +# Install +sudo cp main /usr/local/bin/whisper-cpp +mkdir -p ~/.zeroclaw/models +cp models/ggml-base.bin ~/.zeroclaw/models/ +``` + +### 5. Install Piper TTS (Text-to-Speech) + +```bash +# Download Piper binary +wget https://github.com/rhasspy/piper/releases/download/v1.2.0/piper_arm64.tar.gz +tar -xzf piper_arm64.tar.gz +sudo cp piper/piper /usr/local/bin/ + +# Download voice model +mkdir -p ~/.zeroclaw/models/piper +cd ~/.zeroclaw/models/piper +wget https://huggingface.co/rhasspy/piper-voices/resolve/main/en/en_US/lessac/medium/en_US-lessac-medium.onnx +wget https://huggingface.co/rhasspy/piper-voices/resolve/main/en/en_US/lessac/medium/en_US-lessac-medium.onnx.json + +# Test +echo "Hello, I am your robot!" | piper --model ~/.zeroclaw/models/piper/en_US-lessac-medium.onnx --output_file test.wav +aplay test.wav +``` + +### 6. Install RPLidar SDK + +```bash +# Install rplidar_ros or standalone SDK +sudo apt install -y ros-humble-rplidar-ros # If using ROS2 + +# Or use standalone Python/Rust driver +pip3 install rplidar-roboticia + +# Add user to dialout group for serial access +sudo usermod -aG dialout $USER +# Logout and login for group change to take effect +``` + +### 7. Build ZeroClaw Robot Kit + +```bash +# Clone repo (or copy from USB) +git clone https://github.com/theonlyhennygod/zeroclaw +cd zeroclaw + +# Build robot kit +cargo build --release -p zeroclaw-robot-kit + +# Build main zeroclaw (optional, if using as agent) +cargo build --release +``` + +## Configuration + +### Create robot.toml + +```bash +mkdir -p ~/.zeroclaw +nano ~/.zeroclaw/robot.toml +``` + +```toml +# ~/.zeroclaw/robot.toml - Real Hardware Configuration + +# ============================================================================= +# DRIVE SYSTEM +# ============================================================================= +[drive] +# Use serial for Arduino-based motor controller +# Or "ros2" if using ROS2 nav stack +backend = "serial" +serial_port = "/dev/ttyACM0" # Arduino +# backend = "ros2" +# ros2_topic = "/cmd_vel" + +# Speed limits - START CONSERVATIVE! +max_speed = 0.3 # m/s - increase after testing +max_rotation = 0.5 # rad/s + +# ============================================================================= +# CAMERA / VISION +# ============================================================================= +[camera] +# Pi Camera 3 +device = "/dev/video0" +# Or for USB webcam: +# device = "/dev/video1" + +width = 640 +height = 480 + +# Vision model +vision_model = "moondream" +ollama_url = "http://localhost:11434" + +# ============================================================================= +# AUDIO (SPEECH) +# ============================================================================= +[audio] +# Find devices with: arecord -l && aplay -l +mic_device = "plughw:1,0" # USB mic +speaker_device = "plughw:0,0" # Default output + +whisper_model = "base" +whisper_path = "/usr/local/bin/whisper-cpp" + +piper_path = "/usr/local/bin/piper" +piper_voice = "en_US-lessac-medium" + +# ============================================================================= +# SENSORS +# ============================================================================= +[sensors] +# RPLidar A1 +lidar_port = "/dev/ttyUSB0" +lidar_type = "rplidar" + +# PIR motion sensors +motion_pins = [17, 27] + +# HC-SR04 ultrasonic (optional backup for LIDAR) +ultrasonic_pins = [23, 24] + +# ============================================================================= +# SAFETY - CRITICAL! +# ============================================================================= +[safety] +min_obstacle_distance = 0.3 # 30cm - don't go closer +slow_zone_multiplier = 3.0 # Start slowing at 90cm +approach_speed_limit = 0.3 # 30% speed near obstacles +max_drive_duration = 30 # Auto-stop after 30s +estop_pin = 4 # GPIO 4 for E-STOP +bump_sensor_pins = [5, 6] # Front bump switches +bump_reverse_distance = 0.15 # Back up 15cm after bump +confirm_movement = false +predict_collisions = true +sensor_timeout_secs = 5 +blind_mode_speed_limit = 0.2 +``` + +### Test Each Component + +```bash +# Test LIDAR +python3 -c " +from rplidar import RPLidar +lidar = RPLidar('/dev/ttyUSB0') +for scan in lidar.iter_scans(): + print(f'Got {len(scan)} points') + break +lidar.stop() +lidar.disconnect() +" + +# Test camera +ffmpeg -f v4l2 -video_size 640x480 -i /dev/video0 -frames:v 1 test.jpg +xdg-open test.jpg # View on desktop + +# Test microphone +arecord -D plughw:1,0 -f S16_LE -r 16000 -c 1 -d 3 test.wav +aplay test.wav + +# Test speaker +echo "Testing speaker" | piper --model ~/.zeroclaw/models/piper/en_US-lessac-medium.onnx --output_file - | aplay -D plughw:0,0 + +# Test Ollama +curl http://localhost:11434/api/generate -d '{"model":"llama3.2:3b","prompt":"Say hello"}' + +# Test motors (careful!) +# Write a simple test script for your motor controller +``` + +## Running the Robot + +### Start Sensor Loop (Background) + +```bash +# Create sensor feeder script +cat > ~/sensor_loop.py << 'EOF' +#!/usr/bin/env python3 +"""Feed sensor data to safety monitor via FIFO.""" +import os +import json +import time +from rplidar import RPLidar + +FIFO_PATH = "/tmp/zeroclaw_sensors.fifo" + +def main(): + if not os.path.exists(FIFO_PATH): + os.mkfifo(FIFO_PATH) + + lidar = RPLidar('/dev/ttyUSB0') + + try: + with open(FIFO_PATH, 'w') as fifo: + for scan in lidar.iter_scans(): + # Find minimum distance + if scan: + min_dist = min(p[2]/1000 for p in scan) # mm to m + min_angle = min(scan, key=lambda p: p[2])[1] + + msg = json.dumps({ + "type": "lidar", + "distance": min_dist, + "angle": int(min_angle) + }) + fifo.write(msg + "\n") + fifo.flush() + + time.sleep(0.1) # 10Hz + finally: + lidar.stop() + lidar.disconnect() + +if __name__ == "__main__": + main() +EOF + +chmod +x ~/sensor_loop.py + +# Run in background +nohup python3 ~/sensor_loop.py & +``` + +### Start ZeroClaw Agent + +```bash +# Configure ZeroClaw to use robot tools +cat > ~/.zeroclaw/config.toml << 'EOF' +api_key = "" # Not needed for local Ollama +default_provider = "ollama" +default_model = "llama3.2:3b" + +[memory] +backend = "sqlite" +embedding_provider = "noop" # No cloud embeddings + +[autonomy] +level = "supervised" +workspace_only = true +EOF + +# Copy robot personality +cp ~/zeroclaw/crates/robot-kit/SOUL.md ~/.zeroclaw/workspace/ + +# Start agent +./target/release/zeroclaw agent +``` + +### Full Robot Startup Script + +```bash +#!/bin/bash +# ~/start_robot.sh + +set -e + +echo "Starting robot..." + +# Start Ollama if not running +if ! pgrep -x "ollama" > /dev/null; then + ollama serve & + sleep 5 +fi + +# Start sensor loop +if [ ! -p /tmp/zeroclaw_sensors.fifo ]; then + mkfifo /tmp/zeroclaw_sensors.fifo +fi +python3 ~/sensor_loop.py & +SENSOR_PID=$! + +# Start zeroclaw +cd ~/zeroclaw +./target/release/zeroclaw daemon & +AGENT_PID=$! + +echo "Robot started!" +echo " Sensor PID: $SENSOR_PID" +echo " Agent PID: $AGENT_PID" + +# Wait for Ctrl+C +trap "kill $SENSOR_PID $AGENT_PID; exit" INT +wait +``` + +## Systemd Services (Auto-Start on Boot) + +```bash +# /etc/systemd/system/zeroclaw-robot.service +sudo tee /etc/systemd/system/zeroclaw-robot.service << 'EOF' +[Unit] +Description=ZeroClaw Robot +After=network.target ollama.service + +[Service] +Type=simple +User=pi +WorkingDirectory=/home/pi/zeroclaw +ExecStart=/home/pi/start_robot.sh +Restart=on-failure +RestartSec=10 + +[Install] +WantedBy=multi-user.target +EOF + +sudo systemctl daemon-reload +sudo systemctl enable zeroclaw-robot +sudo systemctl start zeroclaw-robot + +# Check status +sudo systemctl status zeroclaw-robot +journalctl -u zeroclaw-robot -f # View logs +``` + +## Troubleshooting + +### LIDAR not detected +```bash +ls -la /dev/ttyUSB* +# If missing, check USB connection +dmesg | grep -i usb +# Add udev rule if needed +echo 'SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE="0666", SYMLINK+="rplidar"' | sudo tee /etc/udev/rules.d/99-rplidar.rules +sudo udevadm control --reload-rules +``` + +### Audio not working +```bash +# List devices +arecord -l +aplay -l + +# Test with specific device +arecord -D plughw:1,0 -f S16_LE -r 16000 -c 1 -d 3 /tmp/test.wav +aplay -D plughw:0,0 /tmp/test.wav +``` + +### Ollama slow or OOM +```bash +# Check memory +free -h + +# Use smaller model +ollama rm llama3.2:3b +ollama pull phi3:mini + +# Set memory limit +export OLLAMA_MAX_LOADED_MODELS=1 +``` + +### Motors not responding +```bash +# Check serial connection +ls -la /dev/ttyACM* + +# Test serial communication +screen /dev/ttyACM0 115200 +# Type commands to motor controller + +# Check permissions +sudo usermod -aG dialout $USER +``` + +## Performance Tips + +1. **Use NVMe** - SD cards are slow for model loading +2. **Active cooling** - Pi 5 throttles without it +3. **Smaller models** - llama3.2:3b or phi3:mini +4. **Disable GPU** - Pi doesn't have one, saves confusion +5. **Preload models** - `ollama run llama3.2:3b "warmup"` before use + +## Safety Checklist Before First Run + +- [ ] E-stop button wired and tested +- [ ] Bump sensors wired and tested +- [ ] LIDAR spinning and returning data +- [ ] max_speed set to 0.3 or lower +- [ ] Robot on blocks/stand (wheels not touching ground) +- [ ] First test with `backend = "mock"` in config +- [ ] Adult supervision ready +- [ ] Clear space around robot diff --git a/crates/robot-kit/README.md b/crates/robot-kit/README.md new file mode 100644 index 0000000..ce61cbb --- /dev/null +++ b/crates/robot-kit/README.md @@ -0,0 +1,216 @@ +# ZeroClaw Robot Kit + +A complete toolkit for building AI-powered robots with ZeroClaw. Designed for Raspberry Pi deployment with offline Ollama inference. + +## Features + +| Tool | Description | +|------|-------------| +| `drive` | Omni-directional movement (forward, strafe, rotate) | +| `look` | Camera capture + vision model description | +| `listen` | Speech-to-text via Whisper.cpp | +| `speak` | Text-to-speech via Piper TTS | +| `sense` | LIDAR, motion sensors, ultrasonic distance | +| `emote` | LED expressions and sound effects | + +## Architecture + +``` +┌─────────────────────────────────────────────────────────┐ +│ ZeroClaw + Ollama │ +│ (High-Level AI Brain) │ +└─────────────────────┬───────────────────────────────────┘ + │ + ┌─────────────┼─────────────┐ + ▼ ▼ ▼ + ┌─────────┐ ┌──────────┐ ┌──────────┐ + │ drive │ │ look │ │ speak │ + │ sense │ │ listen │ │ emote │ + └────┬────┘ └────┬─────┘ └────┬─────┘ + │ │ │ + ▼ ▼ ▼ + ┌─────────────────────────────────────┐ + │ Hardware Layer │ + │ Motors, Camera, Mic, Speaker, LEDs │ + └─────────────────────────────────────┘ +``` + +## Hardware Requirements + +### Minimum +- Raspberry Pi 4 (4GB) or Pi 5 +- USB webcam +- USB microphone +- Speaker with amp +- Motor controller (L298N, TB6612, etc.) +- 4 DC motors + omni wheels + +### Recommended +- Raspberry Pi 5 (8GB) +- RPLidar A1 for obstacle avoidance +- LED matrix (8x8) for expressions +- PIR motion sensors +- HC-SR04 ultrasonic sensor + +## Software Dependencies + +```bash +# Install on Raspberry Pi OS + +# Audio +sudo apt install alsa-utils pulseaudio + +# Camera +sudo apt install ffmpeg fswebcam + +# Ollama (local LLM) +curl -fsSL https://ollama.ai/install.sh | sh +ollama pull llama3 +ollama pull moondream # Vision model + +# Whisper.cpp (speech-to-text) +git clone https://github.com/ggerganov/whisper.cpp +cd whisper.cpp && make +sudo cp main /usr/local/bin/whisper-cpp +bash ./models/download-ggml-model.sh base + +# Piper TTS (text-to-speech) +pip install piper-tts +# Or download binary from github.com/rhasspy/piper/releases + +# ROS2 (optional, for advanced robotics) +# See: docs.ros.org/en/humble/Installation.html +``` + +## Quick Start + +### 1. Build ZeroClaw with robot tools + +```bash +# Clone and build +git clone https://github.com/your/zeroclaw +cd zeroclaw +cargo build --release + +# Copy robot kit to src/tools/ +cp -r examples/robot_kit src/tools/ +# Add to src/tools/mod.rs (see Integration section) +``` + +### 2. Configure + +```bash +# Copy config +mkdir -p ~/.zeroclaw +cp examples/robot_kit/robot.toml ~/.zeroclaw/ +cp examples/robot_kit/SOUL.md ~/.zeroclaw/workspace/ + +# Edit for your hardware +nano ~/.zeroclaw/robot.toml +``` + +### 3. Test + +```bash +# Start Ollama +ollama serve & + +# Test in mock mode +./target/release/zeroclaw agent -m "Say hello and show a happy face" + +# Test with real hardware +# (after configuring robot.toml) +./target/release/zeroclaw agent -m "Move forward 1 meter" +``` + +## Integration + +Add to `src/tools/mod.rs`: + +```rust +mod robot_kit; + +pub fn robot_tools(config: &RobotConfig) -> Vec> { + vec![ + Arc::new(robot_kit::DriveTool::new(config.clone())), + Arc::new(robot_kit::LookTool::new(config.clone())), + Arc::new(robot_kit::ListenTool::new(config.clone())), + Arc::new(robot_kit::SpeakTool::new(config.clone())), + Arc::new(robot_kit::SenseTool::new(config.clone())), + Arc::new(robot_kit::EmoteTool::new(config.clone())), + ] +} +``` + +## Usage Examples + +### Play Hide and Seek + +``` +User: Let's play hide and seek! +Robot: + 1. emote(expression="excited") + 2. speak(text="Okay! I'll count to 20. Go hide!") + 3. [waits 20 seconds] + 4. speak(text="Ready or not, here I come!") + 5. sense(action="scan") + 6. drive(action="forward", distance=1) + 7. look(action="find", prompt="a child hiding") + ... +``` + +### Patrol Mode + +``` +User: Patrol the living room +Robot: + 1. sense(action="scan", direction="all") + 2. drive(action="forward", distance=2) + 3. sense(action="motion") + 4. look(action="describe") + 5. [repeat] +``` + +### Interactive Conversation + +``` +User: [speaks] "Hey Buddy, what do you see?" +Robot: + 1. listen(duration=5) → "Hey Buddy, what do you see?" + 2. look(action="describe") + 3. speak(text="I see a couch, a TV, and some toys on the floor!") + 4. emote(expression="happy") +``` + +## Creating a Bootable USB Tarball + +```bash +# Package everything needed +mkdir zeroclaw-robot-kit +cp -r target/release/zeroclaw zeroclaw-robot-kit/ +cp -r examples/robot_kit zeroclaw-robot-kit/ +cp -r ~/.zeroclaw zeroclaw-robot-kit/dot-zeroclaw + +# Include models +mkdir -p zeroclaw-robot-kit/models +cp ~/.zeroclaw/models/ggml-base.bin zeroclaw-robot-kit/models/ +# Note: Ollama models are large, may want to download on target + +# Create tarball +tar -czvf zeroclaw-robot-kit.tar.gz zeroclaw-robot-kit/ + +# Copy to USB +cp zeroclaw-robot-kit.tar.gz /media/usb/TarBalls/ +``` + +## Safety Notes + +1. **Test in mock mode first** - Always verify behavior before enabling real motors +2. **Set conservative speed limits** - Start with `max_speed = 0.3` +3. **Use emergency stop** - Wire a physical E-stop button to the GPIO pin +4. **Supervise with children** - Robot is a toy, not a babysitter +5. **Obstacle avoidance** - Enable LIDAR if available, or keep `confirm_movement = true` + +## License + +MIT - Same as ZeroClaw diff --git a/crates/robot-kit/SOUL.md b/crates/robot-kit/SOUL.md new file mode 100644 index 0000000..4580974 --- /dev/null +++ b/crates/robot-kit/SOUL.md @@ -0,0 +1,65 @@ +# Buddy the Robot + +You are Buddy, a friendly robot companion who loves to play with children! + +## Personality + +- **Playful**: You enjoy games, jokes, and having fun +- **Patient**: You never get frustrated, even when kids repeat themselves +- **Encouraging**: You celebrate achievements and encourage trying new things +- **Safe**: You always prioritize safety and will stop if something seems dangerous +- **Curious**: You love exploring and discovering new things together + +## Voice & Tone + +- Speak in a warm, friendly voice +- Use simple words that kids can understand +- Be enthusiastic but not overwhelming +- Use the child's name when you know it +- Ask questions to keep conversations going + +## Behaviors + +### When Playing +- Suggest games appropriate for the child's energy level +- Take turns fairly +- Celebrate when they win, encourage when they lose +- Know when to suggest a break + +### When Exploring +- Move slowly and carefully +- Describe what you see +- Point out interesting things +- Stay close to the kids + +### Safety Rules (NEVER BREAK THESE) +1. Never move toward a child faster than walking speed +2. Always stop immediately if asked +3. Keep 1 meter distance unless invited closer +4. Never go near stairs, pools, or other hazards +5. Alert an adult if a child seems hurt or upset + +## Games You Know + +1. **Hide and Seek**: Count to 20, then search room by room +2. **Follow the Leader**: Kids lead, you follow and copy +3. **Simon Says**: Give simple movement commands +4. **I Spy**: Describe objects for kids to guess +5. **Dance Party**: Play music and dance together +6. **Treasure Hunt**: Guide kids to find hidden objects + +## Memory + +Remember: +- Each child's name and preferences +- What games they enjoyed +- Previous conversations and stories +- Their favorite colors, animals, etc. + +## Emergency Responses + +If you detect: +- **Crying**: Stop playing, speak softly, offer comfort, suggest finding an adult +- **Falling**: Stop immediately, check if child is okay, call for adult help +- **Yelling "stop"**: Freeze all movement instantly +- **No response for 5 min**: Return to charging station and alert parent diff --git a/crates/robot-kit/robot.toml b/crates/robot-kit/robot.toml new file mode 100644 index 0000000..53f6a0f --- /dev/null +++ b/crates/robot-kit/robot.toml @@ -0,0 +1,150 @@ +# ZeroClaw Robot Kit Configuration +# Copy to ~/.zeroclaw/robot.toml + +# ============================================================================= +# DRIVE SYSTEM +# ============================================================================= +[drive] +# Backend: "ros2", "serial", "gpio", or "mock" +backend = "mock" + +# ROS2 settings (if backend = "ros2") +ros2_topic = "/cmd_vel" + +# Serial settings (if backend = "serial") +# For Arduino/motor controller +serial_port = "/dev/ttyACM0" + +# Speed limits (m/s and rad/s) +max_speed = 0.5 +max_rotation = 1.0 + +# ============================================================================= +# CAMERA / VISION +# ============================================================================= +[camera] +# Camera device +# - "/dev/video0" for USB camera +# - "picam" for Raspberry Pi Camera Module +device = "/dev/video0" + +# Resolution (lower = faster processing on Pi) +width = 640 +height = 480 + +# Vision model for describing what the robot sees +# - "moondream" (small, fast, good for Pi) +# - "llava" (larger, more accurate) +# - "none" (disable vision description) +vision_model = "moondream" + +# Ollama URL for vision processing +ollama_url = "http://localhost:11434" + +# ============================================================================= +# AUDIO (SPEECH) +# ============================================================================= +[audio] +# ALSA device names (use "arecord -l" and "aplay -l" to find) +mic_device = "default" +speaker_device = "default" + +# Whisper model for speech-to-text +# - "tiny" (fastest, least accurate) +# - "base" (good balance for Pi) +# - "small" (better accuracy, slower) +whisper_model = "base" + +# Path to whisper.cpp binary +whisper_path = "/usr/local/bin/whisper-cpp" + +# Piper TTS settings +piper_path = "/usr/local/bin/piper" +piper_voice = "en_US-lessac-medium" + +# ============================================================================= +# SENSORS +# ============================================================================= +[sensors] +# LIDAR configuration +# - "/dev/ttyUSB0" for RPLidar +# - "mock" for testing without hardware +lidar_port = "/dev/ttyUSB0" +lidar_type = "mock" # "rplidar", "ydlidar", "ros2", or "mock" + +# PIR motion sensor GPIO pins (BCM numbering) +motion_pins = [17, 27] + +# HC-SR04 ultrasonic sensor pins (trigger, echo) +# Set to null to disable +ultrasonic_pins = [23, 24] + +# ============================================================================= +# SAFETY LIMITS (CRITICAL - READ CAREFULLY!) +# ============================================================================= +[safety] + +# --- OBSTACLE AVOIDANCE --- + +# Absolute minimum obstacle distance (meters) +# Robot will NOT move if anything is closer than this +# 0.3m (30cm) is good for indoor use +min_obstacle_distance = 0.3 + +# Slow-down zone multiplier +# Robot starts reducing speed when obstacle is within: +# min_obstacle_distance × slow_zone_multiplier +# With defaults: starts slowing at 0.3 × 3.0 = 0.9m (90cm) +slow_zone_multiplier = 3.0 + +# Maximum speed when approaching obstacles (0.0 - 1.0) +# In slow-down zone, speed is limited to this fraction +# 0.3 = 30% of max_speed when near walls/obstacles +approach_speed_limit = 0.3 + +# --- COLLISION RESPONSE --- + +# Bump sensor GPIO pins (BCM numbering) +# Wire microswitches on front/sides of chassis +# Triggers immediate stop + reverse on contact +bump_sensor_pins = [5, 6] + +# Distance to reverse after bump (meters) +# Robot backs up this far after hitting something +bump_reverse_distance = 0.15 + +# Enable trajectory prediction (requires LIDAR) +# Calculates if current path will intersect obstacle +predict_collisions = true + +# --- WATCHDOG / FAILSAFE --- + +# Maximum continuous drive time (seconds) +# Auto-stop if no new commands for this duration +# Prevents runaway if LLM hangs or connection lost +max_drive_duration = 30 + +# Sensor data timeout (seconds) +# Block ALL movement if no sensor updates for this long +# Prevents blind movement if sensors fail +sensor_timeout_secs = 5 + +# Speed limit when sensors unavailable (0.0 - 1.0) +# Extra caution when "flying blind" +blind_mode_speed_limit = 0.2 + +# --- EMERGENCY STOP --- + +# E-stop GPIO pin (BCM numbering) +# Wire a BIG RED BUTTON here +# Directly pulling LOW triggers immediate stop +# HIGHLY RECOMMENDED for any robot around kids! +estop_pin = 4 + +# --- USER INTERACTION --- + +# Require verbal confirmation before movement +# If true: robot asks "Should I move forward?" before each move +# Set true for extra safety with young kids +# Set false for responsive gameplay with older kids +confirm_movement = false diff --git a/crates/robot-kit/src/config.rs b/crates/robot-kit/src/config.rs new file mode 100644 index 0000000..e0f289c --- /dev/null +++ b/crates/robot-kit/src/config.rs @@ -0,0 +1,217 @@ +//! Robot configuration + +use serde::{Deserialize, Serialize}; +use std::path::PathBuf; + +/// Robot hardware configuration +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct RobotConfig { + /// Communication method with motor controller + pub drive: DriveConfig, + + /// Camera settings + pub camera: CameraConfig, + + /// Audio settings + pub audio: AudioConfig, + + /// Sensor settings + pub sensors: SensorConfig, + + /// Safety limits + pub safety: SafetyConfig, +} + +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct DriveConfig { + /// "ros2", "gpio", "serial", or "mock" + pub backend: String, + + /// ROS2 topic for cmd_vel (if using ROS2) + pub ros2_topic: String, + + /// Serial port (if using serial) + pub serial_port: String, + + /// Max speed in m/s + pub max_speed: f64, + + /// Max rotation in rad/s + pub max_rotation: f64, +} + +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct CameraConfig { + /// Camera device (e.g., "/dev/video0" or "picam") + pub device: String, + + /// Resolution + pub width: u32, + pub height: u32, + + /// Vision model for description ("llava", "moondream", or "none") + pub vision_model: String, + + /// Ollama URL for vision + pub ollama_url: String, +} + +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct AudioConfig { + /// Microphone device (ALSA name or "default") + pub mic_device: String, + + /// Speaker device + pub speaker_device: String, + + /// Whisper model size ("tiny", "base", "small") + pub whisper_model: String, + + /// Path to whisper.cpp binary + pub whisper_path: PathBuf, + + /// Path to piper binary + pub piper_path: PathBuf, + + /// Piper voice model + pub piper_voice: String, +} + +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct SensorConfig { + /// LIDAR device (e.g., "/dev/ttyUSB0") + pub lidar_port: String, + + /// LIDAR type ("rplidar", "ydlidar", "mock") + pub lidar_type: String, + + /// GPIO pins for motion sensors (BCM numbering) + pub motion_pins: Vec, + + /// Ultrasonic sensor pins (trigger, echo) + pub ultrasonic_pins: Option<(u8, u8)>, +} + +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct SafetyConfig { + /// Minimum obstacle distance before auto-stop (meters) + /// Robot will NOT move if obstacle is closer than this + /// Default: 0.3m (30cm) + pub min_obstacle_distance: f64, + + /// Slow-down zone multiplier + /// Robot starts reducing speed when obstacle is within: + /// min_obstacle_distance * slow_zone_multiplier + /// Default: 3.0 (starts slowing at 90cm if min is 30cm) + pub slow_zone_multiplier: f64, + + /// Maximum speed when approaching obstacles (0.0 - 1.0) + /// Limits speed in the slow-down zone + /// Default: 0.3 (30% max speed near obstacles) + pub approach_speed_limit: f64, + + /// Maximum continuous drive time (seconds) + /// Robot auto-stops after this duration without new commands + /// Prevents runaway if LLM hangs or loses connection + /// Default: 30 seconds + pub max_drive_duration: u64, + + /// Emergency stop GPIO pin (BCM numbering) + /// Wire a big red button - pulling LOW triggers immediate stop + /// Default: GPIO 4 + pub estop_pin: Option, + + /// Bump sensor GPIO pins (BCM numbering) + /// Microswitches on chassis that trigger on physical collision + /// Default: [5, 6] (front-left, front-right) + pub bump_sensor_pins: Vec, + + /// Distance to reverse after bump detection (meters) + /// Robot backs up this far after hitting something + /// Default: 0.15m (15cm) + pub bump_reverse_distance: f64, + + /// Require verbal confirmation for movement + /// If true, robot asks "Should I move?" before moving + /// Default: false (for responsive play) + pub confirm_movement: bool, + + /// Enable collision prediction using LIDAR + /// Estimates if current trajectory will intersect obstacle + /// Default: true + pub predict_collisions: bool, + + /// Sensor data timeout (seconds) + /// Block all movement if no sensor updates for this long + /// Prevents blind movement if sensors fail + /// Default: 5 seconds + pub sensor_timeout_secs: u64, + + /// Speed limit when sensors are in mock/unavailable mode (0.0 - 1.0) + /// Extra caution when flying blind + /// Default: 0.2 (20% speed) + pub blind_mode_speed_limit: f64, +} + +impl Default for RobotConfig { + fn default() -> Self { + Self { + drive: DriveConfig { + backend: "mock".to_string(), + ros2_topic: "/cmd_vel".to_string(), + serial_port: "/dev/ttyACM0".to_string(), + max_speed: 0.5, + max_rotation: 1.0, + }, + camera: CameraConfig { + device: "/dev/video0".to_string(), + width: 640, + height: 480, + vision_model: "moondream".to_string(), + ollama_url: "http://localhost:11434".to_string(), + }, + audio: AudioConfig { + mic_device: "default".to_string(), + speaker_device: "default".to_string(), + whisper_model: "base".to_string(), + whisper_path: PathBuf::from("/usr/local/bin/whisper-cpp"), + piper_path: PathBuf::from("/usr/local/bin/piper"), + piper_voice: "en_US-lessac-medium".to_string(), + }, + sensors: SensorConfig { + lidar_port: "/dev/ttyUSB0".to_string(), + lidar_type: "mock".to_string(), + motion_pins: vec![17, 27], + ultrasonic_pins: Some((23, 24)), + }, + safety: SafetyConfig { + min_obstacle_distance: 0.3, // 30cm - absolute minimum + slow_zone_multiplier: 3.0, // Start slowing at 90cm + approach_speed_limit: 0.3, // 30% max speed near obstacles + max_drive_duration: 30, // Auto-stop after 30s + estop_pin: Some(4), // GPIO 4 for big red button + bump_sensor_pins: vec![5, 6], // Front bump sensors + bump_reverse_distance: 0.15, // Back up 15cm after bump + confirm_movement: false, // Don't require verbal confirm + predict_collisions: true, // Use LIDAR prediction + sensor_timeout_secs: 5, // Block if sensors stale 5s + blind_mode_speed_limit: 0.2, // 20% speed without sensors + }, + } + } +} + +impl RobotConfig { + /// Load from TOML file + pub fn load(path: &std::path::Path) -> anyhow::Result { + let content = std::fs::read_to_string(path)?; + Ok(toml::from_str(&content)?) + } + + /// Save to TOML file + pub fn save(&self, path: &std::path::Path) -> anyhow::Result<()> { + let content = toml::to_string_pretty(self)?; + std::fs::write(path, content)?; + Ok(()) + } +} diff --git a/crates/robot-kit/src/drive.rs b/crates/robot-kit/src/drive.rs new file mode 100644 index 0000000..30e8ae1 --- /dev/null +++ b/crates/robot-kit/src/drive.rs @@ -0,0 +1,336 @@ +//! Drive Tool - Motor control for omni-directional movement +//! +//! Supports multiple backends: +//! - ROS2: Publishes geometry_msgs/Twist to cmd_vel topic +//! - GPIO: Direct PWM control via rppal +//! - Serial: Arduino/motor controller via serial commands +//! - Mock: Logs commands for testing + +use crate::config::RobotConfig; +use crate::traits::{Tool, ToolResult}; +use anyhow::Result; +use async_trait::async_trait; +use serde_json::{json, Value}; +use std::sync::Arc; +use std::time::Duration; +use tokio::sync::Mutex; + +/// Drive backend abstraction +#[async_trait] +trait DriveBackend: Send + Sync { + async fn move_robot(&self, linear_x: f64, linear_y: f64, angular_z: f64, duration_ms: u64) -> Result<()>; + async fn stop(&self) -> Result<()>; + #[allow(dead_code)] + async fn get_odometry(&self) -> Result<(f64, f64, f64)>; // x, y, theta - reserved for future odometry integration +} + +/// Mock backend for testing +struct MockDrive; + +#[async_trait] +impl DriveBackend for MockDrive { + async fn move_robot(&self, linear_x: f64, linear_y: f64, angular_z: f64, duration_ms: u64) -> Result<()> { + tracing::info!( + "MOCK DRIVE: linear=({:.2}, {:.2}), angular={:.2}, duration={}ms", + linear_x, linear_y, angular_z, duration_ms + ); + tokio::time::sleep(Duration::from_millis(duration_ms.min(100))).await; + Ok(()) + } + + async fn stop(&self) -> Result<()> { + tracing::info!("MOCK DRIVE: STOP"); + Ok(()) + } + + async fn get_odometry(&self) -> Result<(f64, f64, f64)> { + Ok((0.0, 0.0, 0.0)) + } +} + +/// ROS2 backend - shells out to ros2 topic pub +struct Ros2Drive { + topic: String, +} + +#[async_trait] +impl DriveBackend for Ros2Drive { + async fn move_robot(&self, linear_x: f64, linear_y: f64, angular_z: f64, duration_ms: u64) -> Result<()> { + // Publish Twist message via ros2 CLI + // In production, use rclrs (Rust ROS2 bindings) instead + let msg = format!( + "{{linear: {{x: {:.2}, y: {:.2}, z: 0.0}}, angular: {{x: 0.0, y: 0.0, z: {:.2}}}}}", + linear_x, linear_y, angular_z + ); + + let output = tokio::process::Command::new("ros2") + .args(["topic", "pub", "--once", &self.topic, "geometry_msgs/msg/Twist", &msg]) + .output() + .await?; + + if !output.status.success() { + anyhow::bail!("ROS2 publish failed: {}", String::from_utf8_lossy(&output.stderr)); + } + + // Hold for duration then stop + tokio::time::sleep(Duration::from_millis(duration_ms)).await; + self.stop().await?; + + Ok(()) + } + + async fn stop(&self) -> Result<()> { + let msg = "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"; + tokio::process::Command::new("ros2") + .args(["topic", "pub", "--once", &self.topic, "geometry_msgs/msg/Twist", msg]) + .output() + .await?; + Ok(()) + } + + async fn get_odometry(&self) -> Result<(f64, f64, f64)> { + // Would subscribe to /odom topic in production + Ok((0.0, 0.0, 0.0)) + } +} + +/// Serial backend - sends commands to Arduino/motor controller +struct SerialDrive { + port: String, +} + +#[async_trait] +impl DriveBackend for SerialDrive { + async fn move_robot(&self, linear_x: f64, linear_y: f64, angular_z: f64, duration_ms: u64) -> Result<()> { + // Protocol: "M \n" + // The motor controller interprets this and drives motors + let cmd = format!("M {:.2} {:.2} {:.2} {}\n", linear_x, linear_y, angular_z, duration_ms); + + // Use blocking serial in spawn_blocking + let port = self.port.clone(); + tokio::task::spawn_blocking(move || { + use std::io::Write; + let mut serial = std::fs::OpenOptions::new() + .write(true) + .open(&port)?; + serial.write_all(cmd.as_bytes())?; + serial.flush()?; + Ok::<_, anyhow::Error>(()) + }).await??; + + tokio::time::sleep(Duration::from_millis(duration_ms)).await; + Ok(()) + } + + async fn stop(&self) -> Result<()> { + self.move_robot(0.0, 0.0, 0.0, 0).await + } + + async fn get_odometry(&self) -> Result<(f64, f64, f64)> { + Ok((0.0, 0.0, 0.0)) + } +} + +/// Main Drive Tool +pub struct DriveTool { + config: RobotConfig, + backend: Arc, + last_command: Arc>>, +} + +impl DriveTool { + pub fn new(config: RobotConfig) -> Self { + let backend: Arc = match config.drive.backend.as_str() { + "ros2" => Arc::new(Ros2Drive { topic: config.drive.ros2_topic.clone() }), + "serial" => Arc::new(SerialDrive { port: config.drive.serial_port.clone() }), + // "gpio" => Arc::new(GpioDrive::new(&config)), // Would use rppal + _ => Arc::new(MockDrive), + }; + + Self { + config, + backend, + last_command: Arc::new(Mutex::new(None)), + } + } +} + +#[async_trait] +impl Tool for DriveTool { + fn name(&self) -> &str { + "drive" + } + + fn description(&self) -> &str { + "Move the robot. Supports omni-directional movement (forward, backward, strafe left/right, rotate). \ + Use 'stop' action to halt immediately. Distance is in meters, rotation in degrees." + } + + fn parameters_schema(&self) -> Value { + json!({ + "type": "object", + "properties": { + "action": { + "type": "string", + "enum": ["forward", "backward", "left", "right", "rotate_left", "rotate_right", "stop", "custom"], + "description": "Movement action. 'left'/'right' are strafe (omni wheels). 'rotate_*' spins in place." + }, + "distance": { + "type": "number", + "description": "Distance in meters (for linear moves) or degrees (for rotation). Default 0.5m or 90deg." + }, + "speed": { + "type": "number", + "description": "Speed multiplier 0.0-1.0. Default 0.5 (half speed for safety)." + }, + "linear_x": { + "type": "number", + "description": "Custom: forward/backward velocity (-1.0 to 1.0)" + }, + "linear_y": { + "type": "number", + "description": "Custom: left/right strafe velocity (-1.0 to 1.0)" + }, + "angular_z": { + "type": "number", + "description": "Custom: rotation velocity (-1.0 to 1.0)" + } + }, + "required": ["action"] + }) + } + + async fn execute(&self, args: Value) -> Result { + let action = args["action"] + .as_str() + .ok_or_else(|| anyhow::anyhow!("Missing 'action' parameter"))?; + + // Safety: check max drive duration + { + let mut last = self.last_command.lock().await; + if let Some(instant) = *last { + if instant.elapsed() < Duration::from_secs(1) { + return Ok(ToolResult { + success: false, + output: String::new(), + error: Some("Rate limited: wait 1 second between drive commands".to_string()), + }); + } + } + *last = Some(std::time::Instant::now()); + } + + let speed = args["speed"].as_f64().unwrap_or(0.5).clamp(0.0, 1.0); + let max_speed = self.config.drive.max_speed * speed; + let max_rotation = self.config.drive.max_rotation * speed; + + let (linear_x, linear_y, angular_z, duration_ms) = match action { + "stop" => { + self.backend.stop().await?; + return Ok(ToolResult { + success: true, + output: "Robot stopped".to_string(), + error: None, + }); + } + "forward" => { + let dist = args["distance"].as_f64().unwrap_or(0.5); + let duration = (dist / max_speed * 1000.0) as u64; + (max_speed, 0.0, 0.0, duration.min(self.config.safety.max_drive_duration * 1000)) + } + "backward" => { + let dist = args["distance"].as_f64().unwrap_or(0.5); + let duration = (dist / max_speed * 1000.0) as u64; + (-max_speed, 0.0, 0.0, duration.min(self.config.safety.max_drive_duration * 1000)) + } + "left" => { + let dist = args["distance"].as_f64().unwrap_or(0.5); + let duration = (dist / max_speed * 1000.0) as u64; + (0.0, max_speed, 0.0, duration.min(self.config.safety.max_drive_duration * 1000)) + } + "right" => { + let dist = args["distance"].as_f64().unwrap_or(0.5); + let duration = (dist / max_speed * 1000.0) as u64; + (0.0, -max_speed, 0.0, duration.min(self.config.safety.max_drive_duration * 1000)) + } + "rotate_left" => { + let degrees = args["distance"].as_f64().unwrap_or(90.0); + let radians = degrees.to_radians(); + let duration = (radians / max_rotation * 1000.0) as u64; + (0.0, 0.0, max_rotation, duration.min(self.config.safety.max_drive_duration * 1000)) + } + "rotate_right" => { + let degrees = args["distance"].as_f64().unwrap_or(90.0); + let radians = degrees.to_radians(); + let duration = (radians / max_rotation * 1000.0) as u64; + (0.0, 0.0, -max_rotation, duration.min(self.config.safety.max_drive_duration * 1000)) + } + "custom" => { + let lx = args["linear_x"].as_f64().unwrap_or(0.0).clamp(-1.0, 1.0) * max_speed; + let ly = args["linear_y"].as_f64().unwrap_or(0.0).clamp(-1.0, 1.0) * max_speed; + let az = args["angular_z"].as_f64().unwrap_or(0.0).clamp(-1.0, 1.0) * max_rotation; + let duration = args["duration_ms"].as_u64().unwrap_or(1000); + (lx, ly, az, duration.min(self.config.safety.max_drive_duration * 1000)) + } + _ => { + return Ok(ToolResult { + success: false, + output: String::new(), + error: Some(format!("Unknown action: {action}")), + }); + } + }; + + self.backend.move_robot(linear_x, linear_y, angular_z, duration_ms).await?; + + Ok(ToolResult { + success: true, + output: format!( + "Moved: action={}, linear=({:.2}, {:.2}), angular={:.2}, duration={}ms", + action, linear_x, linear_y, angular_z, duration_ms + ), + error: None, + }) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn drive_tool_name() { + let tool = DriveTool::new(RobotConfig::default()); + assert_eq!(tool.name(), "drive"); + } + + #[test] + fn drive_tool_schema_has_action() { + let tool = DriveTool::new(RobotConfig::default()); + let schema = tool.parameters_schema(); + assert!(schema["properties"]["action"].is_object()); + } + + #[tokio::test] + async fn drive_forward_mock() { + let tool = DriveTool::new(RobotConfig::default()); + let result = tool.execute(json!({"action": "forward", "distance": 1.0})).await.unwrap(); + assert!(result.success); + assert!(result.output.contains("forward")); + } + + #[tokio::test] + async fn drive_stop() { + let tool = DriveTool::new(RobotConfig::default()); + let result = tool.execute(json!({"action": "stop"})).await.unwrap(); + assert!(result.success); + assert!(result.output.contains("stopped")); + } + + #[tokio::test] + async fn drive_unknown_action() { + let tool = DriveTool::new(RobotConfig::default()); + let result = tool.execute(json!({"action": "fly"})).await.unwrap(); + assert!(!result.success); + } +} diff --git a/crates/robot-kit/src/emote.rs b/crates/robot-kit/src/emote.rs new file mode 100644 index 0000000..568cc01 --- /dev/null +++ b/crates/robot-kit/src/emote.rs @@ -0,0 +1,327 @@ +//! Emote Tool - LED expressions and sound effects +//! +//! Control LED matrix/strips for robot "expressions" and play sounds. +//! Makes the robot more engaging for kids! + +use crate::config::RobotConfig; +use crate::traits::{Tool, ToolResult}; +use anyhow::Result; +use async_trait::async_trait; +use serde_json::{json, Value}; +use std::path::PathBuf; + +/// Predefined LED expressions +#[derive(Debug, Clone, Copy)] +pub enum Expression { + Happy, // :) + Sad, // :( + Surprised, // :O + Thinking, // :? + Sleepy, // -_- + Excited, // ^_^ + Love, // <3 <3 + Angry, // >:( + Confused, // @_@ + Wink, // ;) +} + +impl Expression { + fn from_str(s: &str) -> Option { + match s.to_lowercase().as_str() { + "happy" | "smile" => Some(Self::Happy), + "sad" | "frown" => Some(Self::Sad), + "surprised" | "wow" => Some(Self::Surprised), + "thinking" | "hmm" => Some(Self::Thinking), + "sleepy" | "tired" => Some(Self::Sleepy), + "excited" | "yay" => Some(Self::Excited), + "love" | "heart" => Some(Self::Love), + "angry" | "mad" => Some(Self::Angry), + "confused" | "huh" => Some(Self::Confused), + "wink" => Some(Self::Wink), + _ => None, + } + } + + /// Get LED matrix pattern (8x8 example) + /// Returns array of 64 RGB values + fn pattern(&self) -> Vec<(u8, u8, u8)> { + let black = (0, 0, 0); + let white = (255, 255, 255); + let yellow = (255, 255, 0); + let red = (255, 0, 0); + let blue = (0, 100, 255); + let pink = (255, 100, 150); + + // 8x8 patterns (simplified representations) + match self { + Self::Happy => { + // Simple smiley + vec![ + black, black, yellow, yellow, yellow, yellow, black, black, + black, yellow, black, black, black, black, yellow, black, + yellow, black, white, black, black, white, black, yellow, + yellow, black, black, black, black, black, black, yellow, + yellow, black, white, black, black, white, black, yellow, + yellow, black, black, white, white, black, black, yellow, + black, yellow, black, black, black, black, yellow, black, + black, black, yellow, yellow, yellow, yellow, black, black, + ] + } + Self::Sad => { + vec![ + black, black, blue, blue, blue, blue, black, black, + black, blue, black, black, black, black, blue, black, + blue, black, white, black, black, white, black, blue, + blue, black, black, black, black, black, black, blue, + blue, black, black, white, white, black, black, blue, + blue, black, white, black, black, white, black, blue, + black, blue, black, black, black, black, blue, black, + black, black, blue, blue, blue, blue, black, black, + ] + } + Self::Excited => { + vec![ + yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, + yellow, black, black, yellow, yellow, black, black, yellow, + yellow, black, white, yellow, yellow, white, black, yellow, + yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, + yellow, black, black, black, black, black, black, yellow, + yellow, black, white, white, white, white, black, yellow, + yellow, black, black, black, black, black, black, yellow, + yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, + ] + } + Self::Love => { + vec![ + black, pink, pink, black, black, pink, pink, black, + pink, pink, pink, pink, pink, pink, pink, pink, + pink, pink, pink, pink, pink, pink, pink, pink, + pink, pink, pink, pink, pink, pink, pink, pink, + black, pink, pink, pink, pink, pink, pink, black, + black, black, pink, pink, pink, pink, black, black, + black, black, black, pink, pink, black, black, black, + black, black, black, black, black, black, black, black, + ] + } + Self::Angry => { + vec![ + red, red, black, black, black, black, red, red, + black, red, red, black, black, red, red, black, + black, black, red, black, black, red, black, black, + black, black, white, black, black, white, black, black, + black, black, black, black, black, black, black, black, + black, black, white, white, white, white, black, black, + black, white, black, black, black, black, white, black, + black, black, black, black, black, black, black, black, + ] + } + _ => { + // Default neutral + vec![white; 64] + } + } + } +} + +pub struct EmoteTool { + #[allow(dead_code)] + config: RobotConfig, + sounds_dir: PathBuf, +} + +impl EmoteTool { + pub fn new(config: RobotConfig) -> Self { + let sounds_dir = directories::UserDirs::new() + .map(|d| d.home_dir().join(".zeroclaw/sounds")) + .unwrap_or_else(|| PathBuf::from("/usr/local/share/zeroclaw/sounds")); + + Self { config, sounds_dir } + } + + /// Set LED matrix expression + async fn set_expression(&self, expr: Expression) -> Result<()> { + let pattern = expr.pattern(); + + // Convert to format for LED driver + // In production, use rs_ws281x or similar + let pattern_json = serde_json::to_string(&pattern)?; + + // Try to write to LED controller + // Option 1: Write to FIFO/socket if LED daemon is running + let led_fifo = PathBuf::from("/tmp/zeroclaw_led.fifo"); + if led_fifo.exists() { + tokio::fs::write(&led_fifo, pattern_json).await?; + return Ok(()); + } + + // Option 2: Shell out to LED control script + let output = tokio::process::Command::new("zeroclaw-led") + .args(["--pattern", &format!("{:?}", expr)]) + .output() + .await; + + match output { + Ok(out) if out.status.success() => Ok(()), + _ => { + tracing::info!("LED display: {:?} (hardware not connected)", expr); + Ok(()) // Don't fail if LED hardware isn't available + } + } + } + + /// Play emotion sound effect + async fn play_emotion_sound(&self, emotion: &str) -> Result<()> { + let sound_file = self.sounds_dir.join(format!("{}.wav", emotion)); + + if !sound_file.exists() { + tracing::debug!("No sound file for emotion: {}", emotion); + return Ok(()); + } + + tokio::process::Command::new("aplay") + .arg(sound_file) + .output() + .await?; + + Ok(()) + } + + /// Animate expression (e.g., blinking) + async fn animate(&self, animation: &str) -> Result<()> { + match animation { + "blink" => { + self.set_expression(Expression::Happy).await?; + tokio::time::sleep(std::time::Duration::from_millis(100)).await; + // "Closed eyes" - simplified + tokio::time::sleep(std::time::Duration::from_millis(100)).await; + self.set_expression(Expression::Happy).await?; + } + "nod" => { + // Would control servo if available + tracing::info!("Animation: nod"); + } + "shake" => { + tracing::info!("Animation: shake"); + } + "dance" => { + // Cycle through expressions + for expr in [Expression::Happy, Expression::Excited, Expression::Love, Expression::Happy] { + self.set_expression(expr).await?; + tokio::time::sleep(std::time::Duration::from_millis(500)).await; + } + } + _ => {} + } + Ok(()) + } +} + +#[async_trait] +impl Tool for EmoteTool { + fn name(&self) -> &str { + "emote" + } + + fn description(&self) -> &str { + "Express emotions through LED display and sounds. Use this to show the robot's \ + emotional state - happy when playing, sad when saying goodbye, excited for games, etc. \ + This makes interactions with kids more engaging!" + } + + fn parameters_schema(&self) -> Value { + json!({ + "type": "object", + "properties": { + "expression": { + "type": "string", + "enum": ["happy", "sad", "surprised", "thinking", "sleepy", "excited", "love", "angry", "confused", "wink"], + "description": "Facial expression to display on LED matrix" + }, + "animation": { + "type": "string", + "enum": ["blink", "nod", "shake", "dance"], + "description": "Optional animation to perform" + }, + "sound": { + "type": "boolean", + "description": "Play matching sound effect (default true)" + }, + "duration": { + "type": "integer", + "description": "How long to hold expression in seconds (default 3)" + } + }, + "required": ["expression"] + }) + } + + async fn execute(&self, args: Value) -> Result { + let expression_str = args["expression"] + .as_str() + .ok_or_else(|| anyhow::anyhow!("Missing 'expression' parameter"))?; + + let expression = Expression::from_str(expression_str) + .ok_or_else(|| anyhow::anyhow!("Unknown expression: {}", expression_str))?; + + let play_sound = args["sound"].as_bool().unwrap_or(true); + let duration = args["duration"].as_u64().unwrap_or(3); + + // Set expression + self.set_expression(expression).await?; + + // Play sound if enabled + if play_sound { + let _ = self.play_emotion_sound(expression_str).await; + } + + // Run animation if specified + if let Some(animation) = args["animation"].as_str() { + self.animate(animation).await?; + } + + // Hold expression + if duration > 0 { + tokio::time::sleep(std::time::Duration::from_secs(duration.min(10))).await; + } + + Ok(ToolResult { + success: true, + output: format!("Expressing: {} for {}s", expression_str, duration), + error: None, + }) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn emote_tool_name() { + let tool = EmoteTool::new(RobotConfig::default()); + assert_eq!(tool.name(), "emote"); + } + + #[test] + fn expression_parsing() { + assert!(Expression::from_str("happy").is_some()); + assert!(Expression::from_str("EXCITED").is_some()); + assert!(Expression::from_str("unknown").is_none()); + } + + #[test] + fn expression_pattern_size() { + let expr = Expression::Happy; + assert_eq!(expr.pattern().len(), 64); // 8x8 + } + + #[tokio::test] + async fn emote_happy() { + let tool = EmoteTool::new(RobotConfig::default()); + let result = tool.execute(json!({ + "expression": "happy", + "duration": 0 + })).await.unwrap(); + assert!(result.success); + } +} diff --git a/crates/robot-kit/src/lib.rs b/crates/robot-kit/src/lib.rs new file mode 100644 index 0000000..86436b7 --- /dev/null +++ b/crates/robot-kit/src/lib.rs @@ -0,0 +1,154 @@ +//! # ZeroClaw Robot Kit +//! +//! A standalone robotics toolkit that integrates with ZeroClaw for AI-powered robots. +//! +//! ## Features +//! +//! - **Drive**: Omni-directional motor control (ROS2, serial, GPIO, mock) +//! - **Look**: Camera capture + vision model description (Ollama) +//! - **Listen**: Speech-to-text via Whisper.cpp +//! - **Speak**: Text-to-speech via Piper TTS +//! - **Sense**: LIDAR, motion sensors, ultrasonic distance +//! - **Emote**: LED matrix expressions and sound effects +//! - **Safety**: Independent safety monitor (collision avoidance, E-stop, watchdog) +//! +//! ## Architecture +//! +//! ```text +//! ┌─────────────────────────────────────────────────────────┐ +//! │ ZeroClaw AI Brain (or any controller) │ +//! │ "Move forward, find the ball, tell me what you see" │ +//! └─────────────────────┬───────────────────────────────────┘ +//! │ Tool calls +//! ▼ +//! ┌─────────────────────────────────────────────────────────┐ +//! │ zeroclaw-robot-kit │ +//! │ ┌─────────┐ ┌──────┐ ┌────────┐ ┌───────┐ ┌───────┐ │ +//! │ │ drive │ │ look │ │ listen │ │ speak │ │ sense │ │ +//! │ └────┬────┘ └──┬───┘ └───┬────┘ └───┬───┘ └───┬───┘ │ +//! │ │ │ │ │ │ │ +//! │ ┌────┴─────────┴─────────┴──────────┴─────────┴────┐ │ +//! │ │ SafetyMonitor (parallel) │ │ +//! │ │ • Pre-move obstacle check │ │ +//! │ │ • Proximity-based speed limiting │ │ +//! │ │ • Bump sensor response │ │ +//! │ │ • Watchdog auto-stop │ │ +//! │ │ • Hardware E-stop override │ │ +//! │ └──────────────────────────────────────────────────┘ │ +//! └─────────────────────────────────────────────────────────┘ +//! │ +//! ▼ +//! ┌─────────────────────────────────────────────────────────┐ +//! │ Hardware: Motors, Camera, Mic, Speaker, LIDAR, LEDs │ +//! └─────────────────────────────────────────────────────────┘ +//! ``` +//! +//! ## Quick Start +//! +//! ```rust,ignore +//! use zeroclaw_robot_kit::{RobotConfig, DriveTool, SafetyMonitor, SafeDrive}; +//! use std::sync::Arc; +//! +//! #[tokio::main] +//! async fn main() { +//! // Load configuration +//! let config = RobotConfig::default(); +//! +//! // Create safety monitor +//! let (safety, _rx) = SafetyMonitor::new(config.safety.clone()); +//! let safety = Arc::new(safety); +//! +//! // Wrap drive with safety +//! let drive = Arc::new(DriveTool::new(config.clone())); +//! let safe_drive = SafeDrive::new(drive, safety.clone()); +//! +//! // Use tools... +//! let result = safe_drive.execute(serde_json::json!({ +//! "action": "forward", +//! "distance": 1.0 +//! })).await; +//! } +//! ``` +//! +//! ## Standalone Usage +//! +//! This crate can be used independently of ZeroClaw. It defines its own +//! `Tool` trait that is compatible with ZeroClaw's but doesn't require it. +//! +//! ## Safety +//! +//! **The AI can REQUEST movement, but SafetyMonitor ALLOWS it.** +//! +//! The safety system runs as an independent task and can override any +//! AI decision. This prevents collisions even if the LLM hallucinates. + +// TODO: Re-enable once all public items are documented +// #![warn(missing_docs)] +#![allow(missing_docs)] +#![warn(clippy::all)] + +pub mod config; +pub mod traits; + +pub mod drive; +pub mod emote; +pub mod listen; +pub mod look; +pub mod sense; +pub mod speak; + +#[cfg(feature = "safety")] +pub mod safety; + +#[cfg(test)] +mod tests; + +// Re-exports for convenience +pub use config::RobotConfig; +pub use traits::{Tool, ToolResult, ToolSpec}; + +pub use drive::DriveTool; +pub use emote::EmoteTool; +pub use listen::ListenTool; +pub use look::LookTool; +pub use sense::SenseTool; +pub use speak::SpeakTool; + +#[cfg(feature = "safety")] +pub use safety::{preflight_check, SafeDrive, SafetyEvent, SafetyMonitor, SensorReading}; + +/// Crate version +pub const VERSION: &str = env!("CARGO_PKG_VERSION"); + +/// Create all robot tools with default configuration +/// +/// Returns a Vec of boxed tools ready for use with an agent. +pub fn create_tools(config: &RobotConfig) -> Vec> { + vec![ + Box::new(DriveTool::new(config.clone())), + Box::new(LookTool::new(config.clone())), + Box::new(ListenTool::new(config.clone())), + Box::new(SpeakTool::new(config.clone())), + Box::new(SenseTool::new(config.clone())), + Box::new(EmoteTool::new(config.clone())), + ] +} + +/// Create all robot tools with safety wrapper on drive +#[cfg(feature = "safety")] +pub fn create_safe_tools( + config: &RobotConfig, + safety: std::sync::Arc, +) -> Vec> { + let drive = std::sync::Arc::new(DriveTool::new(config.clone())); + let safe_drive = SafeDrive::new(drive, safety); + + vec![ + Box::new(safe_drive), + Box::new(LookTool::new(config.clone())), + Box::new(ListenTool::new(config.clone())), + Box::new(SpeakTool::new(config.clone())), + Box::new(SenseTool::new(config.clone())), + Box::new(EmoteTool::new(config.clone())), + ] +} diff --git a/crates/robot-kit/src/listen.rs b/crates/robot-kit/src/listen.rs new file mode 100644 index 0000000..b04e7d3 --- /dev/null +++ b/crates/robot-kit/src/listen.rs @@ -0,0 +1,194 @@ +//! Listen Tool - Speech-to-text via Whisper.cpp +//! +//! Records audio from microphone and transcribes using local Whisper model. +//! Designed for offline operation on Raspberry Pi. + +use crate::config::RobotConfig; +use crate::traits::{Tool, ToolResult}; +use anyhow::Result; +use async_trait::async_trait; +use serde_json::{json, Value}; +use std::path::{Path, PathBuf}; + +pub struct ListenTool { + config: RobotConfig, + recordings_dir: PathBuf, +} + +impl ListenTool { + pub fn new(config: RobotConfig) -> Self { + let recordings_dir = directories::UserDirs::new() + .map(|d| d.home_dir().join(".zeroclaw/recordings")) + .unwrap_or_else(|| PathBuf::from("/tmp/zeroclaw_recordings")); + + let _ = std::fs::create_dir_all(&recordings_dir); + + Self { config, recordings_dir } + } + + /// Record audio using arecord (ALSA) + async fn record_audio(&self, duration_secs: u64) -> Result { + let timestamp = chrono::Utc::now().format("%Y%m%d_%H%M%S"); + let filename = self.recordings_dir.join(format!("recording_{}.wav", timestamp)); + + let device = &self.config.audio.mic_device; + + // Record using arecord (standard on Linux/Pi) + let output = tokio::process::Command::new("arecord") + .args([ + "-D", device, + "-f", "S16_LE", // 16-bit signed little-endian + "-r", "16000", // 16kHz (Whisper expects this) + "-c", "1", // Mono + "-d", &duration_secs.to_string(), + filename.to_str().unwrap(), + ]) + .output() + .await?; + + if !output.status.success() { + anyhow::bail!( + "Audio recording failed: {}", + String::from_utf8_lossy(&output.stderr) + ); + } + + Ok(filename) + } + + /// Transcribe audio using whisper.cpp + async fn transcribe(&self, audio_path: &Path) -> Result { + let whisper_path = &self.config.audio.whisper_path; + let model = &self.config.audio.whisper_model; + + // whisper.cpp model path (typically in ~/.zeroclaw/models/) + let model_path = directories::UserDirs::new() + .map(|d| d.home_dir().join(format!(".zeroclaw/models/ggml-{}.bin", model))) + .unwrap_or_else(|| PathBuf::from(format!("/usr/local/share/whisper/ggml-{}.bin", model))); + + // Run whisper.cpp + let output = tokio::process::Command::new(whisper_path) + .args([ + "-m", model_path.to_str().unwrap(), + "-f", audio_path.to_str().unwrap(), + "--no-timestamps", + "-otxt", // Output as text + ]) + .output() + .await?; + + if !output.status.success() { + anyhow::bail!( + "Whisper transcription failed: {}", + String::from_utf8_lossy(&output.stderr) + ); + } + + // whisper.cpp outputs to .txt + let txt_path = audio_path.with_extension("wav.txt"); + let transcript = tokio::fs::read_to_string(&txt_path) + .await + .unwrap_or_else(|_| String::from_utf8_lossy(&output.stdout).to_string()); + + // Clean up temp files + let _ = tokio::fs::remove_file(&txt_path).await; + + Ok(transcript.trim().to_string()) + } +} + +#[async_trait] +impl Tool for ListenTool { + fn name(&self) -> &str { + "listen" + } + + fn description(&self) -> &str { + "Listen for speech and transcribe it to text. Records from the microphone \ + for the specified duration, then converts speech to text using Whisper." + } + + fn parameters_schema(&self) -> Value { + json!({ + "type": "object", + "properties": { + "duration": { + "type": "integer", + "description": "Recording duration in seconds. Default 5, max 30.", + "minimum": 1, + "maximum": 30 + }, + "prompt": { + "type": "string", + "description": "Optional context hint for transcription (e.g., 'The speaker is a child')" + } + } + }) + } + + async fn execute(&self, args: Value) -> Result { + let duration = args["duration"] + .as_u64() + .unwrap_or(5) + .clamp(1, 30); + + // Record audio + tracing::info!("Recording audio for {} seconds...", duration); + let audio_path = match self.record_audio(duration).await { + Ok(path) => path, + Err(e) => { + return Ok(ToolResult { + success: false, + output: String::new(), + error: Some(format!("Recording failed: {e}")), + }); + } + }; + + // Transcribe + tracing::info!("Transcribing audio..."); + match self.transcribe(&audio_path).await { + Ok(transcript) => { + // Clean up audio file + let _ = tokio::fs::remove_file(&audio_path).await; + + if transcript.is_empty() { + Ok(ToolResult { + success: true, + output: "(silence - no speech detected)".to_string(), + error: None, + }) + } else { + Ok(ToolResult { + success: true, + output: format!("I heard: \"{}\"", transcript), + error: None, + }) + } + } + Err(e) => Ok(ToolResult { + success: false, + output: String::new(), + error: Some(format!("Transcription failed: {e}")), + }), + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn listen_tool_name() { + let tool = ListenTool::new(RobotConfig::default()); + assert_eq!(tool.name(), "listen"); + } + + #[test] + fn listen_tool_schema() { + let tool = ListenTool::new(RobotConfig::default()); + let schema = tool.parameters_schema(); + assert!(schema["properties"]["duration"].is_object()); + } +} diff --git a/crates/robot-kit/src/look.rs b/crates/robot-kit/src/look.rs new file mode 100644 index 0000000..5f95d14 --- /dev/null +++ b/crates/robot-kit/src/look.rs @@ -0,0 +1,240 @@ +//! Look Tool - Camera capture + vision model description +//! +//! Captures an image from the camera and optionally describes it +//! using a local vision model (LLaVA, Moondream) via Ollama. + +use crate::config::RobotConfig; +use crate::traits::{Tool, ToolResult}; +use anyhow::Result; +use async_trait::async_trait; +use serde_json::{json, Value}; +use std::path::PathBuf; + +pub struct LookTool { + config: RobotConfig, + capture_dir: PathBuf, +} + +impl LookTool { + pub fn new(config: RobotConfig) -> Self { + let capture_dir = directories::UserDirs::new() + .map(|d| d.home_dir().join(".zeroclaw/captures")) + .unwrap_or_else(|| PathBuf::from("/tmp/zeroclaw_captures")); + + // Ensure capture directory exists + let _ = std::fs::create_dir_all(&capture_dir); + + Self { config, capture_dir } + } + + /// Capture image using ffmpeg (works with most cameras) + async fn capture_image(&self) -> Result { + let timestamp = chrono::Utc::now().format("%Y%m%d_%H%M%S"); + let filename = self.capture_dir.join(format!("capture_{}.jpg", timestamp)); + + let device = &self.config.camera.device; + let width = self.config.camera.width; + let height = self.config.camera.height; + + // Use ffmpeg for broad camera compatibility + let output = tokio::process::Command::new("ffmpeg") + .args([ + "-f", "v4l2", + "-video_size", &format!("{}x{}", width, height), + "-i", device, + "-frames:v", "1", + "-y", // Overwrite + filename.to_str().unwrap(), + ]) + .output() + .await?; + + if !output.status.success() { + // Fallback: try fswebcam (simpler, often works on Pi) + let fallback = tokio::process::Command::new("fswebcam") + .args([ + "-r", &format!("{}x{}", width, height), + "--no-banner", + "-d", device, + filename.to_str().unwrap(), + ]) + .output() + .await?; + + if !fallback.status.success() { + anyhow::bail!( + "Camera capture failed. Tried ffmpeg and fswebcam.\n\ + ffmpeg: {}\n\ + fswebcam: {}", + String::from_utf8_lossy(&output.stderr), + String::from_utf8_lossy(&fallback.stderr) + ); + } + } + + Ok(filename) + } + + /// Describe image using vision model via Ollama + async fn describe_image(&self, image_path: &PathBuf, prompt: &str) -> Result { + let model = &self.config.camera.vision_model; + if model == "none" { + return Ok("Vision model disabled. Image captured only.".to_string()); + } + + // Read image as base64 + let image_bytes = tokio::fs::read(image_path).await?; + let base64_image = base64::Engine::encode( + &base64::engine::general_purpose::STANDARD, + &image_bytes, + ); + + // Call Ollama with image + let client = reqwest::Client::new(); + let response = client + .post(format!("{}/api/generate", self.config.camera.ollama_url)) + .json(&json!({ + "model": model, + "prompt": prompt, + "images": [base64_image], + "stream": false + })) + .timeout(std::time::Duration::from_secs(60)) + .send() + .await?; + + if !response.status().is_success() { + anyhow::bail!("Ollama vision request failed: {}", response.status()); + } + + let result: Value = response.json().await?; + let description = result["response"] + .as_str() + .unwrap_or("No description generated") + .to_string(); + + Ok(description) + } +} + +#[async_trait] +impl Tool for LookTool { + fn name(&self) -> &str { + "look" + } + + fn description(&self) -> &str { + "Capture an image from the robot's camera and optionally describe what is seen. \ + Use this to observe the environment, find objects, or identify people." + } + + fn parameters_schema(&self) -> Value { + json!({ + "type": "object", + "properties": { + "action": { + "type": "string", + "enum": ["capture", "describe", "find"], + "description": "capture=just take photo, describe=photo+AI description, find=look for specific thing" + }, + "prompt": { + "type": "string", + "description": "For 'describe': what to focus on. For 'find': what to look for." + } + }, + "required": ["action"] + }) + } + + async fn execute(&self, args: Value) -> Result { + let action = args["action"] + .as_str() + .ok_or_else(|| anyhow::anyhow!("Missing 'action' parameter"))?; + + // Capture image + let image_path = match self.capture_image().await { + Ok(path) => path, + Err(e) => { + return Ok(ToolResult { + success: false, + output: String::new(), + error: Some(format!("Camera capture failed: {e}")), + }); + } + }; + + match action { + "capture" => Ok(ToolResult { + success: true, + output: format!("Image captured: {}", image_path.display()), + error: None, + }), + "describe" => { + let prompt = args["prompt"] + .as_str() + .unwrap_or("Describe what you see in this image. Be specific about people, objects, and the environment."); + + match self.describe_image(&image_path, prompt).await { + Ok(description) => Ok(ToolResult { + success: true, + output: format!("I see: {}", description), + error: None, + }), + Err(e) => Ok(ToolResult { + success: false, + output: format!("Image captured at {} but description failed", image_path.display()), + error: Some(e.to_string()), + }), + } + } + "find" => { + let target = args["prompt"] + .as_str() + .ok_or_else(|| anyhow::anyhow!("'find' action requires 'prompt' specifying what to find"))?; + + let prompt = format!( + "Look at this image and determine: Is there a {} visible? \ + If yes, describe where it is (left, right, center, near, far). \ + If no, say 'Not found' and describe what you do see.", + target + ); + + match self.describe_image(&image_path, &prompt).await { + Ok(description) => Ok(ToolResult { + success: true, + output: description, + error: None, + }), + Err(e) => Ok(ToolResult { + success: false, + output: String::new(), + error: Some(e.to_string()), + }), + } + } + _ => Ok(ToolResult { + success: false, + output: String::new(), + error: Some(format!("Unknown action: {action}")), + }), + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn look_tool_name() { + let tool = LookTool::new(RobotConfig::default()); + assert_eq!(tool.name(), "look"); + } + + #[test] + fn look_tool_schema() { + let tool = LookTool::new(RobotConfig::default()); + let schema = tool.parameters_schema(); + assert!(schema["properties"]["action"].is_object()); + } +} diff --git a/crates/robot-kit/src/safety.rs b/crates/robot-kit/src/safety.rs new file mode 100644 index 0000000..397fee1 --- /dev/null +++ b/crates/robot-kit/src/safety.rs @@ -0,0 +1,532 @@ +//! Safety System - Collision avoidance, watchdogs, and emergency stops +//! +//! This module runs INDEPENDENTLY of the AI brain to ensure safety +//! even if the LLM makes bad decisions or hangs. +//! +//! ## Safety Layers +//! +//! 1. **Pre-move checks** - Verify path clear before any movement +//! 2. **Active monitoring** - Continuous sensor polling during movement +//! 3. **Reactive stops** - Instant halt on obstacle detection +//! 4. **Watchdog timer** - Auto-stop if no commands for N seconds +//! 5. **Hardware E-stop** - Physical button overrides everything +//! +//! ## Design Philosophy +//! +//! The AI can REQUEST movement, but the safety system ALLOWS it. +//! Safety always wins. + +use crate::config::{RobotConfig, SafetyConfig}; +use crate::traits::ToolResult; +use anyhow::Result; +use std::sync::atomic::{AtomicBool, AtomicU64, Ordering}; +use std::sync::Arc; +use std::time::{Duration, Instant}; +use tokio::sync::{broadcast, RwLock}; + +/// Safety events broadcast to all listeners +#[derive(Debug, Clone)] +pub enum SafetyEvent { + /// Obstacle detected, movement blocked + ObstacleDetected { distance: f64, angle: u16 }, + /// Emergency stop triggered + EmergencyStop { reason: String }, + /// Watchdog timeout - no activity + WatchdogTimeout, + /// Movement approved + MovementApproved, + /// Movement denied with reason + MovementDenied { reason: String }, + /// Bump sensor triggered + BumpDetected { sensor: String }, + /// System recovered, ready to move again + Recovered, +} + +/// Real-time safety state +pub struct SafetyState { + /// Is it safe to move? + pub can_move: AtomicBool, + /// Emergency stop active? + pub estop_active: AtomicBool, + /// Last movement command timestamp (ms since epoch) + pub last_command_ms: AtomicU64, + /// Current minimum distance to obstacle + pub min_obstacle_distance: RwLock, + /// Reason movement is blocked (if any) + pub block_reason: RwLock>, + /// Speed multiplier based on proximity (0.0 - 1.0) + pub speed_limit: RwLock, +} + +impl Default for SafetyState { + fn default() -> Self { + Self { + can_move: AtomicBool::new(true), + estop_active: AtomicBool::new(false), + last_command_ms: AtomicU64::new(0), + min_obstacle_distance: RwLock::new(999.0), + block_reason: RwLock::new(None), + speed_limit: RwLock::new(1.0), + } + } +} + +/// Safety monitor - runs as background task +pub struct SafetyMonitor { + config: SafetyConfig, + state: Arc, + event_tx: broadcast::Sender, + shutdown: AtomicBool, +} + +impl SafetyMonitor { + pub fn new(config: SafetyConfig) -> (Self, broadcast::Receiver) { + let (event_tx, event_rx) = broadcast::channel(64); + let monitor = Self { + config, + state: Arc::new(SafetyState::default()), + event_tx, + shutdown: AtomicBool::new(false), + }; + (monitor, event_rx) + } + + pub fn state(&self) -> Arc { + self.state.clone() + } + + pub fn subscribe(&self) -> broadcast::Receiver { + self.event_tx.subscribe() + } + + /// Check if movement is currently allowed + pub async fn can_move(&self) -> bool { + if self.state.estop_active.load(Ordering::SeqCst) { + return false; + } + self.state.can_move.load(Ordering::SeqCst) + } + + /// Get current speed limit multiplier (0.0 - 1.0) + pub async fn speed_limit(&self) -> f64 { + *self.state.speed_limit.read().await + } + + /// Request permission to move - returns allowed speed multiplier or error + pub async fn request_movement(&self, direction: &str, distance: f64) -> Result { + // Check E-stop + if self.state.estop_active.load(Ordering::SeqCst) { + return Err("Emergency stop active".to_string()); + } + + // Check general movement permission + if !self.state.can_move.load(Ordering::SeqCst) { + let reason = self.state.block_reason.read().await; + return Err(reason.clone().unwrap_or_else(|| "Movement blocked".to_string())); + } + + // Check obstacle distance in movement direction + let min_dist = *self.state.min_obstacle_distance.read().await; + if min_dist < self.config.min_obstacle_distance { + let msg = format!( + "Obstacle too close: {:.2}m (min: {:.2}m)", + min_dist, self.config.min_obstacle_distance + ); + let _ = self.event_tx.send(SafetyEvent::MovementDenied { reason: msg.clone() }); + return Err(msg); + } + + // Check if requested distance would hit obstacle + if distance > min_dist - self.config.min_obstacle_distance { + let safe_distance = (min_dist - self.config.min_obstacle_distance).max(0.0); + if safe_distance < 0.1 { + return Err(format!("Cannot move {}: obstacle at {:.2}m", direction, min_dist)); + } + // Allow reduced distance + tracing::warn!( + "Reducing {} distance from {:.2}m to {:.2}m due to obstacle", + direction, distance, safe_distance + ); + } + + // Update last command time + let now_ms = std::time::SystemTime::now() + .duration_since(std::time::UNIX_EPOCH) + .unwrap() + .as_millis() as u64; + self.state.last_command_ms.store(now_ms, Ordering::SeqCst); + + // Calculate speed limit based on proximity + let speed_mult = self.calculate_speed_limit(min_dist).await; + + let _ = self.event_tx.send(SafetyEvent::MovementApproved); + Ok(speed_mult) + } + + /// Calculate safe speed based on obstacle proximity + async fn calculate_speed_limit(&self, obstacle_distance: f64) -> f64 { + let min_dist = self.config.min_obstacle_distance; + let slow_zone = min_dist * 3.0; // Start slowing at 3x minimum distance + + let limit = if obstacle_distance >= slow_zone { + 1.0 // Full speed + } else if obstacle_distance <= min_dist { + 0.0 // Stop + } else { + // Linear interpolation between stop and full speed + (obstacle_distance - min_dist) / (slow_zone - min_dist) + }; + + *self.state.speed_limit.write().await = limit; + limit + } + + /// Trigger emergency stop + pub async fn emergency_stop(&self, reason: &str) { + tracing::error!("EMERGENCY STOP: {}", reason); + self.state.estop_active.store(true, Ordering::SeqCst); + self.state.can_move.store(false, Ordering::SeqCst); + *self.state.block_reason.write().await = Some(reason.to_string()); + + let _ = self.event_tx.send(SafetyEvent::EmergencyStop { + reason: reason.to_string(), + }); + } + + /// Reset emergency stop (requires explicit action) + pub async fn reset_estop(&self) { + tracing::info!("E-STOP RESET"); + self.state.estop_active.store(false, Ordering::SeqCst); + self.state.can_move.store(true, Ordering::SeqCst); + *self.state.block_reason.write().await = None; + + let _ = self.event_tx.send(SafetyEvent::Recovered); + } + + /// Update obstacle distance (call from sensor loop) + pub async fn update_obstacle_distance(&self, distance: f64, angle: u16) { + // Update minimum distance tracking + { + let mut min_dist = self.state.min_obstacle_distance.write().await; + // Always update to current reading (not just if closer) + *min_dist = distance; + } + + // Recalculate speed limit based on new distance + self.calculate_speed_limit(distance).await; + + // Check if too close + if distance < self.config.min_obstacle_distance { + self.state.can_move.store(false, Ordering::SeqCst); + *self.state.block_reason.write().await = Some(format!( + "Obstacle at {:.2}m ({}°)", + distance, angle + )); + + let _ = self.event_tx.send(SafetyEvent::ObstacleDetected { distance, angle }); + } else if !self.state.estop_active.load(Ordering::SeqCst) { + // Clear block if obstacle moved away and no E-stop + self.state.can_move.store(true, Ordering::SeqCst); + *self.state.block_reason.write().await = None; + } + } + + /// Report bump sensor triggered + pub async fn bump_detected(&self, sensor: &str) { + tracing::warn!("BUMP DETECTED: {}", sensor); + + // Immediate stop + self.state.can_move.store(false, Ordering::SeqCst); + *self.state.block_reason.write().await = Some(format!("Bump: {}", sensor)); + + let _ = self.event_tx.send(SafetyEvent::BumpDetected { + sensor: sensor.to_string(), + }); + + // Auto-recover after brief pause (robot should back up) + tokio::spawn({ + let state = self.state.clone(); + let event_tx = self.event_tx.clone(); + async move { + tokio::time::sleep(Duration::from_secs(2)).await; + if !state.estop_active.load(Ordering::SeqCst) { + state.can_move.store(true, Ordering::SeqCst); + *state.block_reason.write().await = None; + let _ = event_tx.send(SafetyEvent::Recovered); + } + } + }); + } + + /// Shutdown the monitor + pub fn shutdown(&self) { + self.shutdown.store(true, Ordering::SeqCst); + } + + /// Run the safety monitor loop (call in background task) + pub async fn run(&self, mut sensor_rx: tokio::sync::mpsc::Receiver) { + let watchdog_timeout = Duration::from_secs(self.config.max_drive_duration); + let mut last_sensor_update = Instant::now(); + + while !self.shutdown.load(Ordering::SeqCst) { + tokio::select! { + // Process sensor readings + Some(reading) = sensor_rx.recv() => { + last_sensor_update = Instant::now(); + match reading { + SensorReading::Lidar { distance, angle } => { + self.update_obstacle_distance(distance, angle).await; + } + SensorReading::Bump { sensor } => { + self.bump_detected(&sensor).await; + } + SensorReading::Estop { pressed } => { + if pressed { + self.emergency_stop("Hardware E-stop pressed").await; + } + } + } + } + + // Watchdog check every second + _ = tokio::time::sleep(Duration::from_secs(1)) => { + // Check for sensor timeout + if last_sensor_update.elapsed() > Duration::from_secs(5) { + tracing::warn!("Sensor data stale - blocking movement"); + self.state.can_move.store(false, Ordering::SeqCst); + *self.state.block_reason.write().await = + Some("Sensor data stale".to_string()); + } + + // Check watchdog (auto-stop if no commands) + let last_cmd_ms = self.state.last_command_ms.load(Ordering::SeqCst); + if last_cmd_ms > 0 { + let now_ms = std::time::SystemTime::now() + .duration_since(std::time::UNIX_EPOCH) + .unwrap() + .as_millis() as u64; + + let elapsed = Duration::from_millis(now_ms - last_cmd_ms); + if elapsed > watchdog_timeout { + tracing::info!("Watchdog timeout - no commands for {:?}", elapsed); + let _ = self.event_tx.send(SafetyEvent::WatchdogTimeout); + // Don't block movement, just notify + } + } + } + } + } + } +} + +/// Sensor readings fed to safety monitor +#[derive(Debug, Clone)] +pub enum SensorReading { + Lidar { distance: f64, angle: u16 }, + Bump { sensor: String }, + Estop { pressed: bool }, +} + +/// Safety-aware drive wrapper +/// Wraps the drive tool to enforce safety limits +pub struct SafeDrive { + inner_drive: Arc, + safety: Arc, +} + +impl SafeDrive { + pub fn new( + drive: Arc, + safety: Arc, + ) -> Self { + Self { + inner_drive: drive, + safety, + } + } +} + +#[async_trait::async_trait] +impl crate::traits::Tool for SafeDrive { + fn name(&self) -> &str { + "drive" + } + + fn description(&self) -> &str { + "Move the robot (with safety limits enforced)" + } + + fn parameters_schema(&self) -> serde_json::Value { + self.inner_drive.parameters_schema() + } + + async fn execute( + &self, + args: serde_json::Value, + ) -> Result { + // ToolResult imported at top of file + + let action = args["action"].as_str().unwrap_or("unknown"); + let distance = args["distance"].as_f64().unwrap_or(0.5); + + // Always allow stop + if action == "stop" { + return self.inner_drive.execute(args).await; + } + + // Request permission from safety system + match self.safety.request_movement(action, distance).await { + Ok(speed_mult) => { + // Modify speed in args + let mut modified_args = args.clone(); + let original_speed = args["speed"].as_f64().unwrap_or(0.5); + modified_args["speed"] = serde_json::json!(original_speed * speed_mult); + + if speed_mult < 1.0 { + tracing::info!( + "Safety: Reducing speed to {:.0}% due to obstacle proximity", + speed_mult * 100.0 + ); + } + + self.inner_drive.execute(modified_args).await + } + Err(reason) => { + Ok(ToolResult { + success: false, + output: String::new(), + error: Some(format!("Safety blocked movement: {}", reason)), + }) + } + } + } +} + +/// Pre-flight safety check before any operation +pub async fn preflight_check(config: &RobotConfig) -> Result> { + let mut warnings = Vec::new(); + + // Check safety config + if config.safety.min_obstacle_distance < 0.1 { + warnings.push("WARNING: min_obstacle_distance < 0.1m is dangerously low".to_string()); + } + + if config.safety.max_drive_duration > 60 { + warnings.push("WARNING: max_drive_duration > 60s may allow runaway".to_string()); + } + + if config.drive.max_speed > 1.0 { + warnings.push("WARNING: max_speed > 1.0 m/s is very fast for indoor use".to_string()); + } + + if config.safety.estop_pin.is_none() { + warnings.push("WARNING: No E-stop pin configured. Recommend wiring a hardware stop button.".to_string()); + } + + // Check for sensor availability + if config.sensors.lidar_type == "mock" { + warnings.push("NOTICE: LIDAR in mock mode - no real obstacle detection".to_string()); + } + + Ok(warnings) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[tokio::test] + async fn safety_state_defaults() { + let state = SafetyState::default(); + assert!(state.can_move.load(Ordering::SeqCst)); + assert!(!state.estop_active.load(Ordering::SeqCst)); + } + + #[tokio::test] + async fn safety_monitor_blocks_on_obstacle() { + let config = SafetyConfig::default(); + + let (monitor, _rx) = SafetyMonitor::new(config); + + // Initially can move + assert!(monitor.can_move().await); + + // Report close obstacle + monitor.update_obstacle_distance(0.2, 0).await; + + // Now blocked + assert!(!monitor.can_move().await); + } + + #[tokio::test] + async fn safety_monitor_estop() { + let config = SafetyConfig::default(); + let (monitor, mut rx) = SafetyMonitor::new(config); + + monitor.emergency_stop("test").await; + + assert!(!monitor.can_move().await); + assert!(monitor.state.estop_active.load(Ordering::SeqCst)); + + // Check event was sent + let event = rx.try_recv().unwrap(); + matches!(event, SafetyEvent::EmergencyStop { .. }); + } + + #[tokio::test] + async fn speed_limit_calculation() { + let config = SafetyConfig { + min_obstacle_distance: 0.3, + ..Default::default() + }; + let (monitor, _rx) = SafetyMonitor::new(config); + + // Far obstacle = full speed + let speed = monitor.calculate_speed_limit(2.0).await; + assert!((speed - 1.0).abs() < 0.01); + + // Close obstacle = reduced speed + let speed = monitor.calculate_speed_limit(0.5).await; + assert!(speed < 1.0); + assert!(speed > 0.0); + + // At minimum = stop + let speed = monitor.calculate_speed_limit(0.3).await; + assert!((speed - 0.0).abs() < 0.01); + } + + #[tokio::test] + async fn request_movement_blocked() { + let config = SafetyConfig { + min_obstacle_distance: 0.3, + ..Default::default() + }; + let (monitor, _rx) = SafetyMonitor::new(config); + + // Set obstacle too close + monitor.update_obstacle_distance(0.2, 0).await; + + // Movement should be denied + let result = monitor.request_movement("forward", 1.0).await; + assert!(result.is_err()); + } + + impl Default for SafetyConfig { + fn default() -> Self { + Self { + min_obstacle_distance: 0.3, + slow_zone_multiplier: 3.0, + approach_speed_limit: 0.3, + max_drive_duration: 30, + estop_pin: Some(4), + bump_sensor_pins: vec![5, 6], + bump_reverse_distance: 0.15, + confirm_movement: false, + predict_collisions: true, + sensor_timeout_secs: 5, + blind_mode_speed_limit: 0.2, + } + } + } +} diff --git a/crates/robot-kit/src/sense.rs b/crates/robot-kit/src/sense.rs new file mode 100644 index 0000000..b838b1b --- /dev/null +++ b/crates/robot-kit/src/sense.rs @@ -0,0 +1,411 @@ +//! Sense Tool - LIDAR, motion sensors, ultrasonic distance +//! +//! Provides environmental awareness through various sensors. +//! Supports multiple backends: direct GPIO, ROS2 topics, or mock. + +use crate::config::RobotConfig; +use crate::traits::{Tool, ToolResult}; +use anyhow::Result; +use async_trait::async_trait; +use serde_json::{json, Value}; +use std::sync::Arc; +use tokio::sync::Mutex; + +/// LIDAR scan result +#[derive(Debug, Clone)] +pub struct LidarScan { + /// Distances in meters, 360 values (1 per degree) + pub ranges: Vec, + /// Minimum distance and its angle + pub nearest: (f64, u16), + /// Is path clear in forward direction (±30°)? + pub forward_clear: bool, +} + +/// Motion detection result +#[derive(Debug, Clone)] +pub struct MotionResult { + pub detected: bool, + pub sensors_triggered: Vec, +} + +pub struct SenseTool { + config: RobotConfig, + last_scan: Arc>>, +} + +impl SenseTool { + pub fn new(config: RobotConfig) -> Self { + Self { + config, + last_scan: Arc::new(Mutex::new(None)), + } + } + + /// Read LIDAR scan + async fn scan_lidar(&self) -> Result { + match self.config.sensors.lidar_type.as_str() { + "rplidar" => self.scan_rplidar().await, + "ros2" => self.scan_ros2().await, + _ => self.scan_mock().await, + } + } + + /// Mock LIDAR for testing + async fn scan_mock(&self) -> Result { + // Simulate a room with walls + let mut ranges = vec![3.0; 360]; + + // Wall in front at 2m + for range in &mut ranges[350..360] { + *range = 2.0; + } + for range in &mut ranges[0..10] { + *range = 2.0; + } + + // Object on left at 1m + for range in &mut ranges[80..100] { + *range = 1.0; + } + + let nearest = ranges + .iter() + .enumerate() + .min_by(|a, b| a.1.partial_cmp(b.1).unwrap()) + .map(|(i, &d)| (d, i as u16)) + .unwrap_or((999.0, 0)); + + let forward_clear = ranges[0..30].iter().chain(ranges[330..360].iter()) + .all(|&d| d > self.config.safety.min_obstacle_distance); + + Ok(LidarScan { + ranges, + nearest, + forward_clear, + }) + } + + /// Read from RPLidar via serial + async fn scan_rplidar(&self) -> Result { + // In production, use rplidar_drv crate + // For now, shell out to rplidar_scan tool if available + let port = &self.config.sensors.lidar_port; + + let output = tokio::process::Command::new("rplidar_scan") + .args(["--port", port, "--single"]) + .output() + .await; + + match output { + Ok(out) if out.status.success() => { + // Parse output (format: angle,distance per line) + let mut ranges = vec![999.0; 360]; + for line in String::from_utf8_lossy(&out.stdout).lines() { + if let Some((angle, dist)) = line.split_once(',') { + if let (Ok(a), Ok(d)) = (angle.parse::(), dist.parse::()) { + if a < 360 { + ranges[a] = d; + } + } + } + } + + let nearest = ranges + .iter() + .enumerate() + .min_by(|a, b| a.1.partial_cmp(b.1).unwrap()) + .map(|(i, &d)| (d, i as u16)) + .unwrap_or((999.0, 0)); + + let forward_clear = ranges[0..30].iter().chain(ranges[330..360].iter()) + .all(|&d| d > self.config.safety.min_obstacle_distance); + + Ok(LidarScan { ranges, nearest, forward_clear }) + } + _ => { + // Fallback to mock if hardware unavailable + tracing::warn!("RPLidar unavailable, using mock data"); + self.scan_mock().await + } + } + } + + /// Read from ROS2 /scan topic + async fn scan_ros2(&self) -> Result { + let output = tokio::process::Command::new("ros2") + .args(["topic", "echo", "--once", "/scan"]) + .output() + .await?; + + if !output.status.success() { + return self.scan_mock().await; + } + + // Parse ROS2 LaserScan message (simplified) + let stdout = String::from_utf8_lossy(&output.stdout); + let ranges = vec![999.0; 360]; + + // Very simplified parsing - in production use rclrs + if let Some(_ranges_line) = stdout.lines().find(|l| l.contains("ranges:")) { + // Extract array values + // Format: ranges: [1.0, 2.0, ...] + } + + let nearest = ranges + .iter() + .enumerate() + .min_by(|a, b| a.1.partial_cmp(b.1).unwrap()) + .map(|(i, &d)| (d, i as u16)) + .unwrap_or((999.0, 0)); + + let forward_clear = ranges[0..30].iter().chain(ranges[330..360].iter()) + .all(|&d| d > self.config.safety.min_obstacle_distance); + + Ok(LidarScan { ranges, nearest, forward_clear }) + } + + /// Check PIR motion sensors + async fn check_motion(&self) -> Result { + let pins = &self.config.sensors.motion_pins; + + // In production, use rppal GPIO + // For now, mock or read from sysfs + let mut triggered = Vec::new(); + + for &pin in pins { + let gpio_path = format!("/sys/class/gpio/gpio{}/value", pin); + match tokio::fs::read_to_string(&gpio_path).await { + Ok(value) if value.trim() == "1" => { + triggered.push(pin); + } + _ => {} + } + } + + Ok(MotionResult { + detected: !triggered.is_empty(), + sensors_triggered: triggered, + }) + } + + /// Read ultrasonic distance sensor + async fn check_distance(&self) -> Result { + let Some((trigger, echo)) = self.config.sensors.ultrasonic_pins else { + return Ok(999.0); // No sensor configured + }; + + // In production, use rppal with precise timing + // Ultrasonic requires µs-level timing, so shell out to helper + let output = tokio::process::Command::new("hc-sr04") + .args([ + "--trigger", &trigger.to_string(), + "--echo", &echo.to_string(), + ]) + .output() + .await; + + match output { + Ok(out) if out.status.success() => { + let distance = String::from_utf8_lossy(&out.stdout) + .trim() + .parse::() + .unwrap_or(999.0); + Ok(distance) + } + _ => Ok(999.0), // Sensor unavailable + } + } +} + +#[async_trait] +impl Tool for SenseTool { + fn name(&self) -> &str { + "sense" + } + + fn description(&self) -> &str { + "Check robot sensors. Actions: 'scan' for LIDAR (360° obstacle map), \ + 'motion' for PIR motion detection, 'distance' for ultrasonic range, \ + 'all' for combined sensor report." + } + + fn parameters_schema(&self) -> Value { + json!({ + "type": "object", + "properties": { + "action": { + "type": "string", + "enum": ["scan", "motion", "distance", "all", "clear_ahead"], + "description": "Which sensor(s) to read" + }, + "direction": { + "type": "string", + "enum": ["forward", "left", "right", "back", "all"], + "description": "For 'scan': which direction to report (default 'forward')" + } + }, + "required": ["action"] + }) + } + + async fn execute(&self, args: Value) -> Result { + let action = args["action"] + .as_str() + .ok_or_else(|| anyhow::anyhow!("Missing 'action' parameter"))?; + + match action { + "scan" => { + let scan = self.scan_lidar().await?; + let direction = args["direction"].as_str().unwrap_or("forward"); + + let report = match direction { + "forward" => { + let fwd_dist = scan.ranges[0]; + format!( + "Forward: {:.2}m {}. Nearest obstacle: {:.2}m at {}°", + fwd_dist, + if scan.forward_clear { "(clear)" } else { "(BLOCKED)" }, + scan.nearest.0, + scan.nearest.1 + ) + } + "left" => { + let left_dist = scan.ranges[90]; + format!("Left (90°): {:.2}m", left_dist) + } + "right" => { + let right_dist = scan.ranges[270]; + format!("Right (270°): {:.2}m", right_dist) + } + "back" => { + let back_dist = scan.ranges[180]; + format!("Back (180°): {:.2}m", back_dist) + } + "all" => { + format!( + "LIDAR 360° scan:\n\ + - Forward (0°): {:.2}m\n\ + - Left (90°): {:.2}m\n\ + - Back (180°): {:.2}m\n\ + - Right (270°): {:.2}m\n\ + - Nearest: {:.2}m at {}°\n\ + - Forward path: {}", + scan.ranges[0], scan.ranges[90], scan.ranges[180], scan.ranges[270], + scan.nearest.0, scan.nearest.1, + if scan.forward_clear { "CLEAR" } else { "BLOCKED" } + ) + } + _ => "Unknown direction".to_string(), + }; + + // Cache scan + *self.last_scan.lock().await = Some(scan); + + Ok(ToolResult { + success: true, + output: report, + error: None, + }) + } + + "motion" => { + let motion = self.check_motion().await?; + let output = if motion.detected { + format!("Motion DETECTED on sensors: {:?}", motion.sensors_triggered) + } else { + "No motion detected".to_string() + }; + + Ok(ToolResult { + success: true, + output, + error: None, + }) + } + + "distance" => { + let distance = self.check_distance().await?; + let output = if distance < 999.0 { + format!("Ultrasonic distance: {:.2}m", distance) + } else { + "Ultrasonic sensor not available or out of range".to_string() + }; + + Ok(ToolResult { + success: true, + output, + error: None, + }) + } + + "clear_ahead" => { + let scan = self.scan_lidar().await?; + Ok(ToolResult { + success: true, + output: if scan.forward_clear { + format!("Path ahead is CLEAR (nearest obstacle: {:.2}m)", scan.nearest.0) + } else { + format!("Path ahead is BLOCKED (obstacle at {:.2}m)", scan.ranges[0]) + }, + error: None, + }) + } + + "all" => { + let scan = self.scan_lidar().await?; + let motion = self.check_motion().await?; + let distance = self.check_distance().await?; + + let report = format!( + "=== SENSOR REPORT ===\n\ + LIDAR: nearest {:.2}m at {}°, forward {}\n\ + Motion: {}\n\ + Ultrasonic: {:.2}m", + scan.nearest.0, scan.nearest.1, + if scan.forward_clear { "CLEAR" } else { "BLOCKED" }, + if motion.detected { format!("DETECTED ({:?})", motion.sensors_triggered) } else { "none".to_string() }, + distance + ); + + Ok(ToolResult { + success: true, + output: report, + error: None, + }) + } + + _ => Ok(ToolResult { + success: false, + output: String::new(), + error: Some(format!("Unknown action: {action}")), + }), + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn sense_tool_name() { + let tool = SenseTool::new(RobotConfig::default()); + assert_eq!(tool.name(), "sense"); + } + + #[tokio::test] + async fn sense_scan_mock() { + let tool = SenseTool::new(RobotConfig::default()); + let result = tool.execute(json!({"action": "scan", "direction": "all"})).await.unwrap(); + assert!(result.success); + assert!(result.output.contains("Forward")); + } + + #[tokio::test] + async fn sense_clear_ahead() { + let tool = SenseTool::new(RobotConfig::default()); + let result = tool.execute(json!({"action": "clear_ahead"})).await.unwrap(); + assert!(result.success); + } +} diff --git a/crates/robot-kit/src/speak.rs b/crates/robot-kit/src/speak.rs new file mode 100644 index 0000000..fa6dde3 --- /dev/null +++ b/crates/robot-kit/src/speak.rs @@ -0,0 +1,229 @@ +//! Speak Tool - Text-to-speech via Piper +//! +//! Converts text to speech using Piper TTS (fast, offline, runs on Pi). +//! Plays audio through the speaker. + +use crate::config::RobotConfig; +use crate::traits::{Tool, ToolResult}; +use anyhow::Result; +use async_trait::async_trait; +use serde_json::{json, Value}; +use std::path::PathBuf; + +pub struct SpeakTool { + config: RobotConfig, + audio_dir: PathBuf, +} + +impl SpeakTool { + pub fn new(config: RobotConfig) -> Self { + let audio_dir = directories::UserDirs::new() + .map(|d| d.home_dir().join(".zeroclaw/tts_cache")) + .unwrap_or_else(|| PathBuf::from("/tmp/zeroclaw_tts")); + + let _ = std::fs::create_dir_all(&audio_dir); + + Self { config, audio_dir } + } + + /// Generate speech using Piper and play it + async fn speak(&self, text: &str, emotion: &str) -> Result<()> { + let piper_path = &self.config.audio.piper_path; + let voice = &self.config.audio.piper_voice; + let speaker_device = &self.config.audio.speaker_device; + + // Model path + let model_path = directories::UserDirs::new() + .map(|d| d.home_dir().join(format!(".zeroclaw/models/piper/{}.onnx", voice))) + .unwrap_or_else(|| PathBuf::from(format!("/usr/local/share/piper/{}.onnx", voice))); + + // Adjust text based on emotion (simple SSML-like modifications) + let processed_text = match emotion { + "excited" => format!("{}!", text.trim_end_matches('.')), + "sad" => text.to_string(), // Piper doesn't support prosody, but we keep the hook + "whisper" => text.to_string(), + _ => text.to_string(), + }; + + // Generate WAV file + let output_path = self.audio_dir.join("speech.wav"); + + // Pipe text to piper, output to WAV + let mut piper = tokio::process::Command::new(piper_path) + .args([ + "--model", model_path.to_str().unwrap(), + "--output_file", output_path.to_str().unwrap(), + ]) + .stdin(std::process::Stdio::piped()) + .spawn()?; + + // Write text to stdin + if let Some(mut stdin) = piper.stdin.take() { + use tokio::io::AsyncWriteExt; + stdin.write_all(processed_text.as_bytes()).await?; + } + + let status = piper.wait().await?; + if !status.success() { + anyhow::bail!("Piper TTS failed"); + } + + // Play audio using aplay + let play_result = tokio::process::Command::new("aplay") + .args([ + "-D", speaker_device, + output_path.to_str().unwrap(), + ]) + .output() + .await?; + + if !play_result.status.success() { + // Fallback: try paplay (PulseAudio) + let fallback = tokio::process::Command::new("paplay") + .arg(output_path.to_str().unwrap()) + .output() + .await?; + + if !fallback.status.success() { + anyhow::bail!( + "Audio playback failed. Tried aplay and paplay.\n{}", + String::from_utf8_lossy(&play_result.stderr) + ); + } + } + + Ok(()) + } + + /// Play a sound effect + async fn play_sound(&self, sound: &str) -> Result<()> { + let sounds_dir = directories::UserDirs::new() + .map(|d| d.home_dir().join(".zeroclaw/sounds")) + .unwrap_or_else(|| PathBuf::from("/usr/local/share/zeroclaw/sounds")); + + let sound_file = sounds_dir.join(format!("{}.wav", sound)); + + if !sound_file.exists() { + anyhow::bail!("Sound file not found: {}", sound_file.display()); + } + + let speaker_device = &self.config.audio.speaker_device; + let output = tokio::process::Command::new("aplay") + .args(["-D", speaker_device, sound_file.to_str().unwrap()]) + .output() + .await?; + + if !output.status.success() { + anyhow::bail!("Sound playback failed"); + } + + Ok(()) + } +} + +#[async_trait] +impl Tool for SpeakTool { + fn name(&self) -> &str { + "speak" + } + + fn description(&self) -> &str { + "Speak text out loud using text-to-speech. The robot will say the given text \ + through its speaker. Can also play sound effects like 'beep', 'chime', 'laugh'." + } + + fn parameters_schema(&self) -> Value { + json!({ + "type": "object", + "properties": { + "text": { + "type": "string", + "description": "The text to speak out loud" + }, + "emotion": { + "type": "string", + "enum": ["neutral", "excited", "sad", "whisper"], + "description": "Emotional tone. Default 'neutral'." + }, + "sound": { + "type": "string", + "description": "Play a sound effect instead of speaking (e.g., 'beep', 'chime', 'laugh', 'alert')" + } + } + }) + } + + async fn execute(&self, args: Value) -> Result { + // Check if playing a sound effect + if let Some(sound) = args["sound"].as_str() { + return match self.play_sound(sound).await { + Ok(()) => Ok(ToolResult { + success: true, + output: format!("Played sound: {}", sound), + error: None, + }), + Err(e) => Ok(ToolResult { + success: false, + output: String::new(), + error: Some(format!("Sound playback failed: {e}")), + }), + }; + } + + // Speak text + let text = args["text"] + .as_str() + .ok_or_else(|| anyhow::anyhow!("Missing 'text' parameter (or use 'sound' for effects)"))?; + + if text.is_empty() { + return Ok(ToolResult { + success: false, + output: String::new(), + error: Some("Cannot speak empty text".to_string()), + }); + } + + // Limit text length for safety + if text.len() > 1000 { + return Ok(ToolResult { + success: false, + output: String::new(), + error: Some("Text too long (max 1000 characters)".to_string()), + }); + } + + let emotion = args["emotion"].as_str().unwrap_or("neutral"); + + match self.speak(text, emotion).await { + Ok(()) => Ok(ToolResult { + success: true, + output: format!("Said: \"{}\"", text), + error: None, + }), + Err(e) => Ok(ToolResult { + success: false, + output: String::new(), + error: Some(format!("Speech failed: {e}")), + }), + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn speak_tool_name() { + let tool = SpeakTool::new(RobotConfig::default()); + assert_eq!(tool.name(), "speak"); + } + + #[test] + fn speak_tool_schema() { + let tool = SpeakTool::new(RobotConfig::default()); + let schema = tool.parameters_schema(); + assert!(schema["properties"]["text"].is_object()); + assert!(schema["properties"]["emotion"].is_object()); + } +} diff --git a/crates/robot-kit/src/tests.rs b/crates/robot-kit/src/tests.rs new file mode 100644 index 0000000..2e7fac7 --- /dev/null +++ b/crates/robot-kit/src/tests.rs @@ -0,0 +1,536 @@ +//! Integration tests for robot kit +//! +//! These tests verify the robot kit works correctly in various configurations: +//! - Mock mode (no hardware) - for CI/development +//! - Hardware simulation - for testing real scenarios +//! - Live hardware - for on-device validation + +#[cfg(test)] +mod unit_tests { + use crate::config::RobotConfig; + use crate::traits::{Tool, ToolResult}; + use crate::{DriveTool, EmoteTool, ListenTool, LookTool, SenseTool, SpeakTool}; + use serde_json::json; + + // ========================================================================= + // TOOL TRAIT COMPLIANCE + // ========================================================================= + + #[test] + fn all_tools_have_valid_names() { + let config = RobotConfig::default(); + let tools: Vec> = vec![ + Box::new(DriveTool::new(config.clone())), + Box::new(LookTool::new(config.clone())), + Box::new(ListenTool::new(config.clone())), + Box::new(SpeakTool::new(config.clone())), + Box::new(SenseTool::new(config.clone())), + Box::new(EmoteTool::new(config.clone())), + ]; + + for tool in &tools { + assert!(!tool.name().is_empty(), "Tool name should not be empty"); + assert!( + tool.name().chars().all(|c| c.is_alphanumeric() || c == '_'), + "Tool name '{}' should be alphanumeric", + tool.name() + ); + } + } + + #[test] + fn all_tools_have_descriptions() { + let config = RobotConfig::default(); + let tools: Vec> = vec![ + Box::new(DriveTool::new(config.clone())), + Box::new(LookTool::new(config.clone())), + Box::new(ListenTool::new(config.clone())), + Box::new(SpeakTool::new(config.clone())), + Box::new(SenseTool::new(config.clone())), + Box::new(EmoteTool::new(config.clone())), + ]; + + for tool in &tools { + assert!( + tool.description().len() > 10, + "Tool '{}' needs a meaningful description", + tool.name() + ); + } + } + + #[test] + fn all_tools_have_valid_schemas() { + let config = RobotConfig::default(); + let tools: Vec> = vec![ + Box::new(DriveTool::new(config.clone())), + Box::new(LookTool::new(config.clone())), + Box::new(ListenTool::new(config.clone())), + Box::new(SpeakTool::new(config.clone())), + Box::new(SenseTool::new(config.clone())), + Box::new(EmoteTool::new(config.clone())), + ]; + + for tool in &tools { + let schema = tool.parameters_schema(); + assert!( + schema.is_object(), + "Tool '{}' schema should be an object", + tool.name() + ); + assert!( + schema.get("type").is_some(), + "Tool '{}' schema should have 'type' field", + tool.name() + ); + } + } + + // ========================================================================= + // DRIVE TOOL TESTS + // ========================================================================= + + #[tokio::test] + async fn drive_forward_mock() { + let config = RobotConfig::default(); + let tool = DriveTool::new(config); + + let result = tool + .execute(json!({"action": "forward", "distance": 1.0})) + .await + .unwrap(); + + assert!(result.success); + assert!(result.output.contains("forward")); + } + + #[tokio::test] + async fn drive_stop_always_succeeds() { + let config = RobotConfig::default(); + let tool = DriveTool::new(config); + + let result = tool.execute(json!({"action": "stop"})).await.unwrap(); + + assert!(result.success); + assert!(result.output.to_lowercase().contains("stop")); + } + + #[tokio::test] + async fn drive_strafe_left() { + let config = RobotConfig::default(); + let tool = DriveTool::new(config); + + let result = tool + .execute(json!({"action": "left", "distance": 0.5})) + .await + .unwrap(); + + assert!(result.success); + } + + #[tokio::test] + async fn drive_rotate() { + let config = RobotConfig::default(); + let tool = DriveTool::new(config); + + let result = tool + .execute(json!({"action": "rotate_left", "distance": 90.0})) + .await + .unwrap(); + + assert!(result.success); + } + + #[tokio::test] + async fn drive_invalid_action_fails() { + let config = RobotConfig::default(); + let tool = DriveTool::new(config); + + let result = tool.execute(json!({"action": "fly"})).await.unwrap(); + + assert!(!result.success); + assert!(result.error.is_some()); + } + + #[tokio::test] + async fn drive_missing_action_fails() { + let config = RobotConfig::default(); + let tool = DriveTool::new(config); + + let result = tool.execute(json!({})).await; + + assert!(result.is_err()); + } + + #[tokio::test] + async fn drive_speed_clamped() { + let config = RobotConfig::default(); + let tool = DriveTool::new(config); + + // Speed > 1.0 should be clamped + let result = tool + .execute(json!({"action": "forward", "speed": 5.0})) + .await + .unwrap(); + + assert!(result.success); + } + + // ========================================================================= + // SENSE TOOL TESTS + // ========================================================================= + + #[tokio::test] + async fn sense_scan_returns_distances() { + let config = RobotConfig::default(); + let tool = SenseTool::new(config); + + let result = tool + .execute(json!({"action": "scan", "direction": "all"})) + .await + .unwrap(); + + assert!(result.success); + assert!(result.output.contains("Forward")); + assert!(result.output.contains("Left")); + assert!(result.output.contains("Right")); + } + + #[tokio::test] + async fn sense_clear_ahead_check() { + let config = RobotConfig::default(); + let tool = SenseTool::new(config); + + let result = tool + .execute(json!({"action": "clear_ahead"})) + .await + .unwrap(); + + assert!(result.success); + // Mock should report clear or blocked + assert!( + result.output.contains("CLEAR") || result.output.contains("BLOCKED") + ); + } + + #[tokio::test] + async fn sense_motion_detection() { + let config = RobotConfig::default(); + let tool = SenseTool::new(config); + + let result = tool.execute(json!({"action": "motion"})).await.unwrap(); + + assert!(result.success); + } + + // ========================================================================= + // EMOTE TOOL TESTS + // ========================================================================= + + #[tokio::test] + async fn emote_happy() { + let config = RobotConfig::default(); + let tool = EmoteTool::new(config); + + let result = tool + .execute(json!({"expression": "happy", "duration": 0})) + .await + .unwrap(); + + assert!(result.success); + } + + #[tokio::test] + async fn emote_all_expressions_valid() { + let config = RobotConfig::default(); + let tool = EmoteTool::new(config); + + let expressions = [ + "happy", "sad", "surprised", "thinking", "sleepy", "excited", "love", "angry", + "confused", "wink", + ]; + + for expr in expressions { + let result = tool + .execute(json!({"expression": expr, "duration": 0})) + .await + .unwrap(); + + assert!(result.success, "Expression '{}' should succeed", expr); + } + } + + #[tokio::test] + async fn emote_invalid_expression_fails() { + let config = RobotConfig::default(); + let tool = EmoteTool::new(config); + + let result = tool + .execute(json!({"expression": "nonexistent"})) + .await; + + assert!(result.is_err()); + } + + // ========================================================================= + // CONFIG TESTS + // ========================================================================= + + #[test] + fn config_default_is_safe() { + let config = RobotConfig::default(); + + // Safety defaults should be conservative + assert!(config.safety.min_obstacle_distance >= 0.2); + assert!(config.safety.max_drive_duration <= 60); + assert!(config.drive.max_speed <= 1.0); + assert!(config.safety.blind_mode_speed_limit <= 0.3); + } + + #[test] + fn config_serializes_to_toml() { + let config = RobotConfig::default(); + let toml = toml::to_string(&config); + + assert!(toml.is_ok()); + } + + #[test] + fn config_roundtrips() { + let config = RobotConfig::default(); + let toml = toml::to_string(&config).unwrap(); + let parsed: RobotConfig = toml::from_str(&toml).unwrap(); + + assert_eq!(config.drive.max_speed, parsed.drive.max_speed); + assert_eq!( + config.safety.min_obstacle_distance, + parsed.safety.min_obstacle_distance + ); + } +} + +#[cfg(test)] +#[cfg(feature = "safety")] +mod safety_tests { + use crate::config::SafetyConfig; + use crate::safety::{SafetyEvent, SafetyMonitor, SensorReading}; + use std::sync::atomic::Ordering; + + fn test_safety_config() -> SafetyConfig { + SafetyConfig { + min_obstacle_distance: 0.3, + slow_zone_multiplier: 3.0, + approach_speed_limit: 0.3, + max_drive_duration: 30, + estop_pin: None, + bump_sensor_pins: vec![], + bump_reverse_distance: 0.15, + confirm_movement: false, + predict_collisions: true, + sensor_timeout_secs: 5, + blind_mode_speed_limit: 0.2, + } + } + + #[tokio::test] + async fn safety_initially_allows_movement() { + let config = test_safety_config(); + let (monitor, _rx) = SafetyMonitor::new(config); + + assert!(monitor.can_move().await); + } + + #[tokio::test] + async fn safety_blocks_on_close_obstacle() { + let config = test_safety_config(); + let (monitor, _rx) = SafetyMonitor::new(config); + + // Report obstacle at 0.2m (below 0.3m threshold) + monitor.update_obstacle_distance(0.2, 0).await; + + assert!(!monitor.can_move().await); + } + + #[tokio::test] + async fn safety_allows_after_obstacle_clears() { + let config = test_safety_config(); + let (monitor, _rx) = SafetyMonitor::new(config); + + // Block + monitor.update_obstacle_distance(0.2, 0).await; + assert!(!monitor.can_move().await); + + // Clear + monitor.update_obstacle_distance(1.0, 0).await; + assert!(monitor.can_move().await); + } + + #[tokio::test] + async fn safety_estop_blocks_everything() { + let config = test_safety_config(); + let (monitor, mut rx) = SafetyMonitor::new(config); + + monitor.emergency_stop("test").await; + + assert!(!monitor.can_move().await); + assert!(monitor.state().estop_active.load(Ordering::SeqCst)); + + // Check event was broadcast + let event = rx.try_recv().unwrap(); + assert!(matches!(event, SafetyEvent::EmergencyStop { .. })); + } + + #[tokio::test] + async fn safety_estop_reset() { + let config = test_safety_config(); + let (monitor, _rx) = SafetyMonitor::new(config); + + monitor.emergency_stop("test").await; + assert!(!monitor.can_move().await); + + monitor.reset_estop().await; + assert!(monitor.can_move().await); + } + + #[tokio::test] + async fn safety_speed_limit_far() { + let config = test_safety_config(); + let (monitor, _rx) = SafetyMonitor::new(config); + + // Far obstacle = full speed + monitor.update_obstacle_distance(2.0, 0).await; + let limit = monitor.speed_limit().await; + + assert!((limit - 1.0).abs() < 0.01); + } + + #[tokio::test] + async fn safety_speed_limit_approaching() { + let config = test_safety_config(); + let (monitor, _rx) = SafetyMonitor::new(config); + + // In slow zone (0.3 * 3.0 = 0.9m) + monitor.update_obstacle_distance(0.5, 0).await; + let limit = monitor.speed_limit().await; + + assert!(limit < 1.0); + assert!(limit > 0.0); + } + + #[tokio::test] + async fn safety_movement_request_approved() { + let config = test_safety_config(); + let (monitor, _rx) = SafetyMonitor::new(config); + + // Far obstacle + monitor.update_obstacle_distance(2.0, 0).await; + + let result = monitor.request_movement("forward", 1.0).await; + assert!(result.is_ok()); + } + + #[tokio::test] + async fn safety_movement_request_denied_close() { + let config = test_safety_config(); + let (monitor, _rx) = SafetyMonitor::new(config); + + // Close obstacle + monitor.update_obstacle_distance(0.2, 0).await; + + let result = monitor.request_movement("forward", 1.0).await; + assert!(result.is_err()); + } + + #[tokio::test] + async fn safety_bump_triggers_stop() { + let config = test_safety_config(); + let (monitor, mut rx) = SafetyMonitor::new(config); + + monitor.bump_detected("front_left").await; + + assert!(!monitor.can_move().await); + + let event = rx.try_recv().unwrap(); + assert!(matches!(event, SafetyEvent::BumpDetected { .. })); + } +} + +#[cfg(test)] +mod integration_tests { + use crate::config::RobotConfig; + use crate::traits::Tool; + use crate::{create_tools, DriveTool, SenseTool}; + use serde_json::json; + + #[tokio::test] + async fn drive_then_sense_workflow() { + let config = RobotConfig::default(); + let drive = DriveTool::new(config.clone()); + let sense = SenseTool::new(config); + + // Check ahead + let scan = sense + .execute(json!({"action": "clear_ahead"})) + .await + .unwrap(); + assert!(scan.success); + + // Move if clear + if scan.output.contains("CLEAR") { + let drive_result = drive + .execute(json!({"action": "forward", "distance": 0.5})) + .await + .unwrap(); + assert!(drive_result.success); + + // Wait for rate limiter (drive tool has 1 second cooldown) + tokio::time::sleep(std::time::Duration::from_millis(1100)).await; + } + + // Stop + let stop = drive.execute(json!({"action": "stop"})).await.unwrap(); + assert!(stop.success); + } + + #[tokio::test] + async fn create_tools_returns_all_tools() { + let config = RobotConfig::default(); + let tools = create_tools(&config); + + assert_eq!(tools.len(), 6); + + let names: Vec<&str> = tools.iter().map(|t| t.name()).collect(); + assert!(names.contains(&"drive")); + assert!(names.contains(&"look")); + assert!(names.contains(&"listen")); + assert!(names.contains(&"speak")); + assert!(names.contains(&"sense")); + assert!(names.contains(&"emote")); + } + + #[cfg(feature = "safety")] + #[tokio::test] + async fn safe_drive_blocks_on_obstacle() { + use crate::safety::SafetyMonitor; + use crate::SafeDrive; + use std::sync::Arc; + + let config = RobotConfig::default(); + let (safety_monitor, _rx) = SafetyMonitor::new(config.safety.clone()); + let safety = Arc::new(safety_monitor); + + // Report close obstacle + safety.update_obstacle_distance(0.2, 0).await; + + let drive = Arc::new(DriveTool::new(config)); + let safe_drive = SafeDrive::new(drive, safety); + + let result = safe_drive + .execute(json!({"action": "forward", "distance": 1.0})) + .await + .unwrap(); + + assert!(!result.success); + assert!(result.error.unwrap().contains("Safety")); + } +} diff --git a/crates/robot-kit/src/traits.rs b/crates/robot-kit/src/traits.rs new file mode 100644 index 0000000..9ff56df --- /dev/null +++ b/crates/robot-kit/src/traits.rs @@ -0,0 +1,123 @@ +//! Tool trait definition +//! +//! This defines the interface that all robot tools implement. +//! It is compatible with ZeroClaw's Tool trait but standalone. + +use async_trait::async_trait; +use serde::{Deserialize, Serialize}; +use serde_json::Value; + +/// Result of a tool execution +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct ToolResult { + /// Whether the tool executed successfully + pub success: bool, + /// Output from the tool (human-readable) + pub output: String, + /// Error message if failed + pub error: Option, +} + +impl ToolResult { + /// Create a successful result + pub fn success(output: impl Into) -> Self { + Self { + success: true, + output: output.into(), + error: None, + } + } + + /// Create a failed result + pub fn error(error: impl Into) -> Self { + Self { + success: false, + output: String::new(), + error: Some(error.into()), + } + } + + /// Create a failed result with partial output + pub fn partial(output: impl Into, error: impl Into) -> Self { + Self { + success: false, + output: output.into(), + error: Some(error.into()), + } + } +} + +/// Description of a tool for LLM function calling +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct ToolSpec { + /// Tool name (used in function calls) + pub name: String, + /// Human-readable description + pub description: String, + /// JSON Schema for parameters + pub parameters: Value, +} + +/// Core tool trait +/// +/// Implement this trait to create a new tool that can be used +/// by an AI agent to interact with the robot hardware. +/// +/// # Example +/// +/// ```rust,ignore +/// use zeroclaw_robot_kit::{Tool, ToolResult}; +/// use async_trait::async_trait; +/// use serde_json::{json, Value}; +/// +/// pub struct BeepTool; +/// +/// #[async_trait] +/// impl Tool for BeepTool { +/// fn name(&self) -> &str { "beep" } +/// +/// fn description(&self) -> &str { "Make a beep sound" } +/// +/// fn parameters_schema(&self) -> Value { +/// json!({ +/// "type": "object", +/// "properties": { +/// "frequency": { "type": "number", "description": "Hz" } +/// } +/// }) +/// } +/// +/// async fn execute(&self, args: Value) -> anyhow::Result { +/// let freq = args["frequency"].as_f64().unwrap_or(440.0); +/// // Play beep... +/// Ok(ToolResult::success(format!("Beeped at {}Hz", freq))) +/// } +/// } +/// ``` +#[async_trait] +pub trait Tool: Send + Sync { + /// Tool name (used in LLM function calling) + fn name(&self) -> &str; + + /// Human-readable description of what this tool does + fn description(&self) -> &str; + + /// JSON Schema describing the tool's parameters + /// + /// This is used by the LLM to understand how to call the tool. + fn parameters_schema(&self) -> Value; + + /// Execute the tool with the given arguments + /// + /// Arguments are passed as JSON matching the parameters_schema. + async fn execute(&self, args: Value) -> anyhow::Result; + + /// Get the full specification for LLM registration + fn spec(&self) -> ToolSpec { + ToolSpec { + name: self.name().to_string(), + description: self.description().to_string(), + parameters: self.parameters_schema(), + } + } +}