diff --git a/crates/robot-kit/Cargo.toml b/crates/robot-kit/Cargo.toml index 10e43e1..76b2863 100644 --- a/crates/robot-kit/Cargo.toml +++ b/crates/robot-kit/Cargo.toml @@ -5,7 +5,7 @@ edition = "2021" authors = ["theonlyhennygod"] license = "MIT" description = "Robot control toolkit for ZeroClaw - drive, vision, speech, sensors, safety" -repository = "https://github.com/theonlyhennygod/zeroclaw" +repository = "https://github.com/zeroclaw-labs/zeroclaw" readme = "README.md" keywords = ["robotics", "raspberry-pi", "ai", "agent", "ros2"] categories = ["science::robotics", "embedded", "hardware-support"] @@ -54,6 +54,8 @@ chrono = { version = "0.4", features = ["clock", "std"] } # User directories directories = "5.0" + +[target.'cfg(target_os = "linux")'.dependencies] # GPIO (Raspberry Pi only, optional) rppal = { version = "0.19", optional = true } diff --git a/crates/robot-kit/README.md b/crates/robot-kit/README.md index ce61cbb..c1b24b6 100644 --- a/crates/robot-kit/README.md +++ b/crates/robot-kit/README.md @@ -88,13 +88,9 @@ pip install piper-tts ```bash # Clone and build -git clone https://github.com/your/zeroclaw +git clone https://github.com/zeroclaw-labs/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) +cargo build -p zeroclaw-robot-kit --release ``` ### 2. Configure @@ -102,8 +98,8 @@ cp -r examples/robot_kit src/tools/ ```bash # Copy config mkdir -p ~/.zeroclaw -cp examples/robot_kit/robot.toml ~/.zeroclaw/ -cp examples/robot_kit/SOUL.md ~/.zeroclaw/workspace/ +cp crates/robot-kit/robot.toml ~/.zeroclaw/ +cp crates/robot-kit/SOUL.md ~/.zeroclaw/workspace/ # Edit for your hardware nano ~/.zeroclaw/robot.toml @@ -125,23 +121,24 @@ ollama serve & ## Integration -Add to `src/tools/mod.rs`: +This crate is currently added as a standalone workspace member. +It is not auto-registered in the core runtime by default. + +Use it directly from Rust: ```rust -mod robot_kit; +use zeroclaw_robot_kit::{create_tools, RobotConfig}; -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())), - ] +fn build_robot_tools() { + let config = RobotConfig::default(); + let tools = create_tools(&config); + assert_eq!(tools.len(), 6); } ``` +If you want runtime registration in `zeroclaw`, add a thin adapter that maps this +crate's tools to the project's `src/tools::Tool` and register it in the factory. + ## Usage Examples ### Play Hide and Seek diff --git a/crates/robot-kit/src/config.rs b/crates/robot-kit/src/config.rs index e0f289c..718897f 100644 --- a/crates/robot-kit/src/config.rs +++ b/crates/robot-kit/src/config.rs @@ -185,17 +185,17 @@ impl Default for RobotConfig { 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 + 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 }, } } diff --git a/crates/robot-kit/src/drive.rs b/crates/robot-kit/src/drive.rs index 30e8ae1..e848f79 100644 --- a/crates/robot-kit/src/drive.rs +++ b/crates/robot-kit/src/drive.rs @@ -18,7 +18,13 @@ 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 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 @@ -29,10 +35,19 @@ 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<()> { + 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 + linear_x, + linear_y, + angular_z, + duration_ms ); tokio::time::sleep(Duration::from_millis(duration_ms.min(100))).await; Ok(()) @@ -55,7 +70,13 @@ struct Ros2Drive { #[async_trait] impl DriveBackend for Ros2Drive { - async fn move_robot(&self, linear_x: f64, linear_y: f64, angular_z: f64, duration_ms: u64) -> Result<()> { + 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!( @@ -64,12 +85,22 @@ impl DriveBackend for Ros2Drive { ); let output = tokio::process::Command::new("ros2") - .args(["topic", "pub", "--once", &self.topic, "geometry_msgs/msg/Twist", &msg]) + .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)); + anyhow::bail!( + "ROS2 publish failed: {}", + String::from_utf8_lossy(&output.stderr) + ); } // Hold for duration then stop @@ -82,7 +113,14 @@ impl DriveBackend for Ros2Drive { 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]) + .args([ + "topic", + "pub", + "--once", + &self.topic, + "geometry_msgs/msg/Twist", + msg, + ]) .output() .await?; Ok(()) @@ -101,22 +139,30 @@ struct SerialDrive { #[async_trait] impl DriveBackend for SerialDrive { - async fn move_robot(&self, linear_x: f64, linear_y: f64, angular_z: f64, duration_ms: u64) -> Result<()> { + 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); + 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)?; + let mut serial = std::fs::OpenOptions::new().write(true).open(&port)?; serial.write_all(cmd.as_bytes())?; serial.flush()?; Ok::<_, anyhow::Error>(()) - }).await??; + }) + .await??; tokio::time::sleep(Duration::from_millis(duration_ms)).await; Ok(()) @@ -141,8 +187,12 @@ pub struct DriveTool { 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() }), + "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), }; @@ -213,7 +263,9 @@ impl Tool for DriveTool { return Ok(ToolResult { success: false, output: String::new(), - error: Some("Rate limited: wait 1 second between drive commands".to_string()), + error: Some( + "Rate limited: wait 1 second between drive commands".to_string(), + ), }); } } @@ -236,41 +288,76 @@ impl Tool for DriveTool { "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)) + ( + 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)) + ( + -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)) + ( + 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)) + ( + 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)) + ( + 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)) + ( + 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)) + ( + lx, + ly, + az, + duration.min(self.config.safety.max_drive_duration * 1000), + ) } _ => { return Ok(ToolResult { @@ -281,7 +368,9 @@ impl Tool for DriveTool { } }; - self.backend.move_robot(linear_x, linear_y, angular_z, duration_ms).await?; + self.backend + .move_robot(linear_x, linear_y, angular_z, duration_ms) + .await?; Ok(ToolResult { success: true, @@ -314,7 +403,10 @@ mod tests { #[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(); + let result = tool + .execute(json!({"action": "forward", "distance": 1.0})) + .await + .unwrap(); assert!(result.success); assert!(result.output.contains("forward")); } diff --git a/crates/robot-kit/src/emote.rs b/crates/robot-kit/src/emote.rs index 568cc01..19b0ba6 100644 --- a/crates/robot-kit/src/emote.rs +++ b/crates/robot-kit/src/emote.rs @@ -13,16 +13,16 @@ 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, // ;) + Happy, // :) + Sad, // :( + Surprised, // :O + Thinking, // :? + Sleepy, // -_- + Excited, // ^_^ + Love, // <3 <3 + Angry, // >:( + Confused, // @_@ + Wink, // ;) } impl Expression { @@ -57,62 +57,53 @@ impl Expression { 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, + 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, + 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, + 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, + 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, + 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, ] } _ => { @@ -205,7 +196,12 @@ impl EmoteTool { } "dance" => { // Cycle through expressions - for expr in [Expression::Happy, Expression::Excited, Expression::Love, Expression::Happy] { + 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; } @@ -318,10 +314,13 @@ mod tests { #[tokio::test] async fn emote_happy() { let tool = EmoteTool::new(RobotConfig::default()); - let result = tool.execute(json!({ - "expression": "happy", - "duration": 0 - })).await.unwrap(); + let result = tool + .execute(json!({ + "expression": "happy", + "duration": 0 + })) + .await + .unwrap(); assert!(result.success); } } diff --git a/crates/robot-kit/src/listen.rs b/crates/robot-kit/src/listen.rs index b04e7d3..9f99fe2 100644 --- a/crates/robot-kit/src/listen.rs +++ b/crates/robot-kit/src/listen.rs @@ -23,24 +23,34 @@ impl ListenTool { let _ = std::fs::create_dir_all(&recordings_dir); - Self { config, 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 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(), + "-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() @@ -63,16 +73,23 @@ impl ListenTool { // 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))); + .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(), + "-m", + model_path.to_str().unwrap(), + "-f", + audio_path.to_str().unwrap(), "--no-timestamps", - "-otxt", // Output as text + "-otxt", // Output as text ]) .output() .await?; @@ -127,10 +144,7 @@ impl Tool for ListenTool { } async fn execute(&self, args: Value) -> Result { - let duration = args["duration"] - .as_u64() - .unwrap_or(5) - .clamp(1, 30); + let duration = args["duration"].as_u64().unwrap_or(5).clamp(1, 30); // Record audio tracing::info!("Recording audio for {} seconds...", duration); diff --git a/crates/robot-kit/src/look.rs b/crates/robot-kit/src/look.rs index 5f95d14..17dad91 100644 --- a/crates/robot-kit/src/look.rs +++ b/crates/robot-kit/src/look.rs @@ -24,7 +24,10 @@ impl LookTool { // Ensure capture directory exists let _ = std::fs::create_dir_all(&capture_dir); - Self { config, capture_dir } + Self { + config, + capture_dir, + } } /// Capture image using ffmpeg (works with most cameras) @@ -39,10 +42,14 @@ impl LookTool { // 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", + "-f", + "v4l2", + "-video_size", + &format!("{}x{}", width, height), + "-i", + device, + "-frames:v", + "1", "-y", // Overwrite filename.to_str().unwrap(), ]) @@ -53,9 +60,11 @@ impl LookTool { // Fallback: try fswebcam (simpler, often works on Pi) let fallback = tokio::process::Command::new("fswebcam") .args([ - "-r", &format!("{}x{}", width, height), + "-r", + &format!("{}x{}", width, height), "--no-banner", - "-d", device, + "-d", + device, filename.to_str().unwrap(), ]) .output() @@ -84,10 +93,8 @@ impl LookTool { // 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, - ); + let base64_image = + base64::Engine::encode(&base64::engine::general_purpose::STANDARD, &image_bytes); // Call Ollama with image let client = reqwest::Client::new(); @@ -182,15 +189,18 @@ impl Tool for LookTool { }), Err(e) => Ok(ToolResult { success: false, - output: format!("Image captured at {} but description failed", image_path.display()), + 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 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? \ diff --git a/crates/robot-kit/src/safety.rs b/crates/robot-kit/src/safety.rs index 397fee1..3a5f6ce 100644 --- a/crates/robot-kit/src/safety.rs +++ b/crates/robot-kit/src/safety.rs @@ -123,7 +123,9 @@ impl SafetyMonitor { // 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())); + return Err(reason + .clone() + .unwrap_or_else(|| "Movement blocked".to_string())); } // Check obstacle distance in movement direction @@ -133,7 +135,9 @@ impl SafetyMonitor { "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() }); + let _ = self.event_tx.send(SafetyEvent::MovementDenied { + reason: msg.clone(), + }); return Err(msg); } @@ -141,12 +145,17 @@ impl SafetyMonitor { 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)); + 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 + direction, + distance, + safe_distance ); } @@ -219,12 +228,12 @@ impl SafetyMonitor { // 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 - )); + *self.state.block_reason.write().await = + Some(format!("Obstacle at {:.2}m ({}°)", distance, angle)); - let _ = self.event_tx.send(SafetyEvent::ObstacleDetected { 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); @@ -336,10 +345,7 @@ pub struct SafeDrive { } impl SafeDrive { - pub fn new( - drive: Arc, - safety: Arc, - ) -> Self { + pub fn new(drive: Arc, safety: Arc) -> Self { Self { inner_drive: drive, safety, @@ -361,10 +367,7 @@ impl crate::traits::Tool for SafeDrive { self.inner_drive.parameters_schema() } - async fn execute( - &self, - args: serde_json::Value, - ) -> Result { + async fn execute(&self, args: serde_json::Value) -> Result { // ToolResult imported at top of file let action = args["action"].as_str().unwrap_or("unknown"); @@ -392,13 +395,11 @@ impl crate::traits::Tool for SafeDrive { self.inner_drive.execute(modified_args).await } - Err(reason) => { - Ok(ToolResult { - success: false, - output: String::new(), - error: Some(format!("Safety blocked movement: {}", reason)), - }) - } + Err(reason) => Ok(ToolResult { + success: false, + output: String::new(), + error: Some(format!("Safety blocked movement: {}", reason)), + }), } } } @@ -421,7 +422,10 @@ pub async fn preflight_check(config: &RobotConfig) -> Result> { } if config.safety.estop_pin.is_none() { - warnings.push("WARNING: No E-stop pin configured. Recommend wiring a hardware stop button.".to_string()); + warnings.push( + "WARNING: No E-stop pin configured. Recommend wiring a hardware stop button." + .to_string(), + ); } // Check for sensor availability diff --git a/crates/robot-kit/src/sense.rs b/crates/robot-kit/src/sense.rs index b838b1b..9ed39c3 100644 --- a/crates/robot-kit/src/sense.rs +++ b/crates/robot-kit/src/sense.rs @@ -76,7 +76,9 @@ impl SenseTool { .map(|(i, &d)| (d, i as u16)) .unwrap_or((999.0, 0)); - let forward_clear = ranges[0..30].iter().chain(ranges[330..360].iter()) + let forward_clear = ranges[0..30] + .iter() + .chain(ranges[330..360].iter()) .all(|&d| d > self.config.safety.min_obstacle_distance); Ok(LidarScan { @@ -118,10 +120,16 @@ impl SenseTool { .map(|(i, &d)| (d, i as u16)) .unwrap_or((999.0, 0)); - let forward_clear = ranges[0..30].iter().chain(ranges[330..360].iter()) + 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 }) + Ok(LidarScan { + ranges, + nearest, + forward_clear, + }) } _ => { // Fallback to mock if hardware unavailable @@ -159,10 +167,16 @@ impl SenseTool { .map(|(i, &d)| (d, i as u16)) .unwrap_or((999.0, 0)); - let forward_clear = ranges[0..30].iter().chain(ranges[330..360].iter()) + 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 }) + Ok(LidarScan { + ranges, + nearest, + forward_clear, + }) } /// Check PIR motion sensors @@ -199,8 +213,10 @@ impl SenseTool { // 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(), + "--trigger", + &trigger.to_string(), + "--echo", + &echo.to_string(), ]) .output() .await; @@ -265,7 +281,11 @@ impl Tool for SenseTool { format!( "Forward: {:.2}m {}. Nearest obstacle: {:.2}m at {}°", fwd_dist, - if scan.forward_clear { "(clear)" } else { "(BLOCKED)" }, + if scan.forward_clear { + "(clear)" + } else { + "(BLOCKED)" + }, scan.nearest.0, scan.nearest.1 ) @@ -291,9 +311,17 @@ impl Tool for SenseTool { - 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" } + 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(), @@ -344,7 +372,10 @@ impl Tool for SenseTool { Ok(ToolResult { success: true, output: if scan.forward_clear { - format!("Path ahead is CLEAR (nearest obstacle: {:.2}m)", scan.nearest.0) + 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]) }, @@ -362,9 +393,18 @@ impl Tool for SenseTool { 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() }, + 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 ); @@ -397,7 +437,10 @@ mod tests { #[tokio::test] async fn sense_scan_mock() { let tool = SenseTool::new(RobotConfig::default()); - let result = tool.execute(json!({"action": "scan", "direction": "all"})).await.unwrap(); + let result = tool + .execute(json!({"action": "scan", "direction": "all"})) + .await + .unwrap(); assert!(result.success); assert!(result.output.contains("Forward")); } @@ -405,7 +448,10 @@ mod tests { #[tokio::test] async fn sense_clear_ahead() { let tool = SenseTool::new(RobotConfig::default()); - let result = tool.execute(json!({"action": "clear_ahead"})).await.unwrap(); + 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 index fa6dde3..6f793e7 100644 --- a/crates/robot-kit/src/speak.rs +++ b/crates/robot-kit/src/speak.rs @@ -34,7 +34,10 @@ impl SpeakTool { // Model path let model_path = directories::UserDirs::new() - .map(|d| d.home_dir().join(format!(".zeroclaw/models/piper/{}.onnx", voice))) + .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) @@ -51,8 +54,10 @@ impl SpeakTool { // 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(), + "--model", + model_path.to_str().unwrap(), + "--output_file", + output_path.to_str().unwrap(), ]) .stdin(std::process::Stdio::piped()) .spawn()?; @@ -70,10 +75,7 @@ impl SpeakTool { // Play audio using aplay let play_result = tokio::process::Command::new("aplay") - .args([ - "-D", speaker_device, - output_path.to_str().unwrap(), - ]) + .args(["-D", speaker_device, output_path.to_str().unwrap()]) .output() .await?; @@ -171,9 +173,9 @@ impl Tool for SpeakTool { } // Speak text - let text = args["text"] - .as_str() - .ok_or_else(|| anyhow::anyhow!("Missing 'text' parameter (or use 'sound' for effects)"))?; + 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 { diff --git a/crates/robot-kit/src/tests.rs b/crates/robot-kit/src/tests.rs index 2e7fac7..9c10565 100644 --- a/crates/robot-kit/src/tests.rs +++ b/crates/robot-kit/src/tests.rs @@ -8,7 +8,7 @@ #[cfg(test)] mod unit_tests { use crate::config::RobotConfig; - use crate::traits::{Tool, ToolResult}; + use crate::traits::Tool; use crate::{DriveTool, EmoteTool, ListenTool, LookTool, SenseTool, SpeakTool}; use serde_json::json; @@ -208,9 +208,7 @@ mod unit_tests { assert!(result.success); // Mock should report clear or blocked - assert!( - result.output.contains("CLEAR") || result.output.contains("BLOCKED") - ); + assert!(result.output.contains("CLEAR") || result.output.contains("BLOCKED")); } #[tokio::test] @@ -246,8 +244,16 @@ mod unit_tests { let tool = EmoteTool::new(config); let expressions = [ - "happy", "sad", "surprised", "thinking", "sleepy", "excited", "love", "angry", - "confused", "wink", + "happy", + "sad", + "surprised", + "thinking", + "sleepy", + "excited", + "love", + "angry", + "confused", + "wink", ]; for expr in expressions { @@ -265,9 +271,7 @@ mod unit_tests { let config = RobotConfig::default(); let tool = EmoteTool::new(config); - let result = tool - .execute(json!({"expression": "nonexistent"})) - .await; + let result = tool.execute(json!({"expression": "nonexistent"})).await; assert!(result.is_err()); } @@ -313,7 +317,7 @@ mod unit_tests { #[cfg(feature = "safety")] mod safety_tests { use crate::config::SafetyConfig; - use crate::safety::{SafetyEvent, SafetyMonitor, SensorReading}; + use crate::safety::{SafetyEvent, SafetyMonitor}; use std::sync::atomic::Ordering; fn test_safety_config() -> SafetyConfig {