fix(robot-kit): format crate and harden cross-platform feature gating
This commit is contained in:
parent
0dfc707c49
commit
d70324f4f7
11 changed files with 374 additions and 204 deletions
|
|
@ -5,7 +5,7 @@ edition = "2021"
|
||||||
authors = ["theonlyhennygod"]
|
authors = ["theonlyhennygod"]
|
||||||
license = "MIT"
|
license = "MIT"
|
||||||
description = "Robot control toolkit for ZeroClaw - drive, vision, speech, sensors, safety"
|
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"
|
readme = "README.md"
|
||||||
keywords = ["robotics", "raspberry-pi", "ai", "agent", "ros2"]
|
keywords = ["robotics", "raspberry-pi", "ai", "agent", "ros2"]
|
||||||
categories = ["science::robotics", "embedded", "hardware-support"]
|
categories = ["science::robotics", "embedded", "hardware-support"]
|
||||||
|
|
@ -54,6 +54,8 @@ chrono = { version = "0.4", features = ["clock", "std"] }
|
||||||
# User directories
|
# User directories
|
||||||
directories = "5.0"
|
directories = "5.0"
|
||||||
|
|
||||||
|
|
||||||
|
[target.'cfg(target_os = "linux")'.dependencies]
|
||||||
# GPIO (Raspberry Pi only, optional)
|
# GPIO (Raspberry Pi only, optional)
|
||||||
rppal = { version = "0.19", optional = true }
|
rppal = { version = "0.19", optional = true }
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -88,13 +88,9 @@ pip install piper-tts
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
# Clone and build
|
# Clone and build
|
||||||
git clone https://github.com/your/zeroclaw
|
git clone https://github.com/zeroclaw-labs/zeroclaw
|
||||||
cd zeroclaw
|
cd zeroclaw
|
||||||
cargo build --release
|
cargo build -p zeroclaw-robot-kit --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
|
### 2. Configure
|
||||||
|
|
@ -102,8 +98,8 @@ cp -r examples/robot_kit src/tools/
|
||||||
```bash
|
```bash
|
||||||
# Copy config
|
# Copy config
|
||||||
mkdir -p ~/.zeroclaw
|
mkdir -p ~/.zeroclaw
|
||||||
cp examples/robot_kit/robot.toml ~/.zeroclaw/
|
cp crates/robot-kit/robot.toml ~/.zeroclaw/
|
||||||
cp examples/robot_kit/SOUL.md ~/.zeroclaw/workspace/
|
cp crates/robot-kit/SOUL.md ~/.zeroclaw/workspace/
|
||||||
|
|
||||||
# Edit for your hardware
|
# Edit for your hardware
|
||||||
nano ~/.zeroclaw/robot.toml
|
nano ~/.zeroclaw/robot.toml
|
||||||
|
|
@ -125,23 +121,24 @@ ollama serve &
|
||||||
|
|
||||||
## Integration
|
## 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
|
```rust
|
||||||
mod robot_kit;
|
use zeroclaw_robot_kit::{create_tools, RobotConfig};
|
||||||
|
|
||||||
pub fn robot_tools(config: &RobotConfig) -> Vec<Arc<dyn Tool>> {
|
fn build_robot_tools() {
|
||||||
vec![
|
let config = RobotConfig::default();
|
||||||
Arc::new(robot_kit::DriveTool::new(config.clone())),
|
let tools = create_tools(&config);
|
||||||
Arc::new(robot_kit::LookTool::new(config.clone())),
|
assert_eq!(tools.len(), 6);
|
||||||
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())),
|
|
||||||
]
|
|
||||||
}
|
}
|
||||||
```
|
```
|
||||||
|
|
||||||
|
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
|
## Usage Examples
|
||||||
|
|
||||||
### Play Hide and Seek
|
### Play Hide and Seek
|
||||||
|
|
|
||||||
|
|
@ -185,17 +185,17 @@ impl Default for RobotConfig {
|
||||||
ultrasonic_pins: Some((23, 24)),
|
ultrasonic_pins: Some((23, 24)),
|
||||||
},
|
},
|
||||||
safety: SafetyConfig {
|
safety: SafetyConfig {
|
||||||
min_obstacle_distance: 0.3, // 30cm - absolute minimum
|
min_obstacle_distance: 0.3, // 30cm - absolute minimum
|
||||||
slow_zone_multiplier: 3.0, // Start slowing at 90cm
|
slow_zone_multiplier: 3.0, // Start slowing at 90cm
|
||||||
approach_speed_limit: 0.3, // 30% max speed near obstacles
|
approach_speed_limit: 0.3, // 30% max speed near obstacles
|
||||||
max_drive_duration: 30, // Auto-stop after 30s
|
max_drive_duration: 30, // Auto-stop after 30s
|
||||||
estop_pin: Some(4), // GPIO 4 for big red button
|
estop_pin: Some(4), // GPIO 4 for big red button
|
||||||
bump_sensor_pins: vec![5, 6], // Front bump sensors
|
bump_sensor_pins: vec![5, 6], // Front bump sensors
|
||||||
bump_reverse_distance: 0.15, // Back up 15cm after bump
|
bump_reverse_distance: 0.15, // Back up 15cm after bump
|
||||||
confirm_movement: false, // Don't require verbal confirm
|
confirm_movement: false, // Don't require verbal confirm
|
||||||
predict_collisions: true, // Use LIDAR prediction
|
predict_collisions: true, // Use LIDAR prediction
|
||||||
sensor_timeout_secs: 5, // Block if sensors stale 5s
|
sensor_timeout_secs: 5, // Block if sensors stale 5s
|
||||||
blind_mode_speed_limit: 0.2, // 20% speed without sensors
|
blind_mode_speed_limit: 0.2, // 20% speed without sensors
|
||||||
},
|
},
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -18,7 +18,13 @@ use tokio::sync::Mutex;
|
||||||
/// Drive backend abstraction
|
/// Drive backend abstraction
|
||||||
#[async_trait]
|
#[async_trait]
|
||||||
trait DriveBackend: Send + Sync {
|
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<()>;
|
async fn stop(&self) -> Result<()>;
|
||||||
#[allow(dead_code)]
|
#[allow(dead_code)]
|
||||||
async fn get_odometry(&self) -> Result<(f64, f64, f64)>; // x, y, theta - reserved for future odometry integration
|
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]
|
#[async_trait]
|
||||||
impl DriveBackend for MockDrive {
|
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!(
|
tracing::info!(
|
||||||
"MOCK DRIVE: linear=({:.2}, {:.2}), angular={:.2}, duration={}ms",
|
"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;
|
tokio::time::sleep(Duration::from_millis(duration_ms.min(100))).await;
|
||||||
Ok(())
|
Ok(())
|
||||||
|
|
@ -55,7 +70,13 @@ struct Ros2Drive {
|
||||||
|
|
||||||
#[async_trait]
|
#[async_trait]
|
||||||
impl DriveBackend for Ros2Drive {
|
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
|
// Publish Twist message via ros2 CLI
|
||||||
// In production, use rclrs (Rust ROS2 bindings) instead
|
// In production, use rclrs (Rust ROS2 bindings) instead
|
||||||
let msg = format!(
|
let msg = format!(
|
||||||
|
|
@ -64,12 +85,22 @@ impl DriveBackend for Ros2Drive {
|
||||||
);
|
);
|
||||||
|
|
||||||
let output = tokio::process::Command::new("ros2")
|
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()
|
.output()
|
||||||
.await?;
|
.await?;
|
||||||
|
|
||||||
if !output.status.success() {
|
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
|
// Hold for duration then stop
|
||||||
|
|
@ -82,7 +113,14 @@ impl DriveBackend for Ros2Drive {
|
||||||
async fn stop(&self) -> Result<()> {
|
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}}";
|
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")
|
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()
|
.output()
|
||||||
.await?;
|
.await?;
|
||||||
Ok(())
|
Ok(())
|
||||||
|
|
@ -101,22 +139,30 @@ struct SerialDrive {
|
||||||
|
|
||||||
#[async_trait]
|
#[async_trait]
|
||||||
impl DriveBackend for SerialDrive {
|
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 <lx> <ly> <az> <ms>\n"
|
// Protocol: "M <lx> <ly> <az> <ms>\n"
|
||||||
// The motor controller interprets this and drives motors
|
// 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
|
// Use blocking serial in spawn_blocking
|
||||||
let port = self.port.clone();
|
let port = self.port.clone();
|
||||||
tokio::task::spawn_blocking(move || {
|
tokio::task::spawn_blocking(move || {
|
||||||
use std::io::Write;
|
use std::io::Write;
|
||||||
let mut serial = std::fs::OpenOptions::new()
|
let mut serial = std::fs::OpenOptions::new().write(true).open(&port)?;
|
||||||
.write(true)
|
|
||||||
.open(&port)?;
|
|
||||||
serial.write_all(cmd.as_bytes())?;
|
serial.write_all(cmd.as_bytes())?;
|
||||||
serial.flush()?;
|
serial.flush()?;
|
||||||
Ok::<_, anyhow::Error>(())
|
Ok::<_, anyhow::Error>(())
|
||||||
}).await??;
|
})
|
||||||
|
.await??;
|
||||||
|
|
||||||
tokio::time::sleep(Duration::from_millis(duration_ms)).await;
|
tokio::time::sleep(Duration::from_millis(duration_ms)).await;
|
||||||
Ok(())
|
Ok(())
|
||||||
|
|
@ -141,8 +187,12 @@ pub struct DriveTool {
|
||||||
impl DriveTool {
|
impl DriveTool {
|
||||||
pub fn new(config: RobotConfig) -> Self {
|
pub fn new(config: RobotConfig) -> Self {
|
||||||
let backend: Arc<dyn DriveBackend> = match config.drive.backend.as_str() {
|
let backend: Arc<dyn DriveBackend> = match config.drive.backend.as_str() {
|
||||||
"ros2" => Arc::new(Ros2Drive { topic: config.drive.ros2_topic.clone() }),
|
"ros2" => Arc::new(Ros2Drive {
|
||||||
"serial" => Arc::new(SerialDrive { port: config.drive.serial_port.clone() }),
|
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
|
// "gpio" => Arc::new(GpioDrive::new(&config)), // Would use rppal
|
||||||
_ => Arc::new(MockDrive),
|
_ => Arc::new(MockDrive),
|
||||||
};
|
};
|
||||||
|
|
@ -213,7 +263,9 @@ impl Tool for DriveTool {
|
||||||
return Ok(ToolResult {
|
return Ok(ToolResult {
|
||||||
success: false,
|
success: false,
|
||||||
output: String::new(),
|
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" => {
|
"forward" => {
|
||||||
let dist = args["distance"].as_f64().unwrap_or(0.5);
|
let dist = args["distance"].as_f64().unwrap_or(0.5);
|
||||||
let duration = (dist / max_speed * 1000.0) as u64;
|
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" => {
|
"backward" => {
|
||||||
let dist = args["distance"].as_f64().unwrap_or(0.5);
|
let dist = args["distance"].as_f64().unwrap_or(0.5);
|
||||||
let duration = (dist / max_speed * 1000.0) as u64;
|
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" => {
|
"left" => {
|
||||||
let dist = args["distance"].as_f64().unwrap_or(0.5);
|
let dist = args["distance"].as_f64().unwrap_or(0.5);
|
||||||
let duration = (dist / max_speed * 1000.0) as u64;
|
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" => {
|
"right" => {
|
||||||
let dist = args["distance"].as_f64().unwrap_or(0.5);
|
let dist = args["distance"].as_f64().unwrap_or(0.5);
|
||||||
let duration = (dist / max_speed * 1000.0) as u64;
|
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" => {
|
"rotate_left" => {
|
||||||
let degrees = args["distance"].as_f64().unwrap_or(90.0);
|
let degrees = args["distance"].as_f64().unwrap_or(90.0);
|
||||||
let radians = degrees.to_radians();
|
let radians = degrees.to_radians();
|
||||||
let duration = (radians / max_rotation * 1000.0) as u64;
|
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" => {
|
"rotate_right" => {
|
||||||
let degrees = args["distance"].as_f64().unwrap_or(90.0);
|
let degrees = args["distance"].as_f64().unwrap_or(90.0);
|
||||||
let radians = degrees.to_radians();
|
let radians = degrees.to_radians();
|
||||||
let duration = (radians / max_rotation * 1000.0) as u64;
|
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" => {
|
"custom" => {
|
||||||
let lx = args["linear_x"].as_f64().unwrap_or(0.0).clamp(-1.0, 1.0) * max_speed;
|
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 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 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);
|
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 {
|
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 {
|
Ok(ToolResult {
|
||||||
success: true,
|
success: true,
|
||||||
|
|
@ -314,7 +403,10 @@ mod tests {
|
||||||
#[tokio::test]
|
#[tokio::test]
|
||||||
async fn drive_forward_mock() {
|
async fn drive_forward_mock() {
|
||||||
let tool = DriveTool::new(RobotConfig::default());
|
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.success);
|
||||||
assert!(result.output.contains("forward"));
|
assert!(result.output.contains("forward"));
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -13,16 +13,16 @@ use std::path::PathBuf;
|
||||||
/// Predefined LED expressions
|
/// Predefined LED expressions
|
||||||
#[derive(Debug, Clone, Copy)]
|
#[derive(Debug, Clone, Copy)]
|
||||||
pub enum Expression {
|
pub enum Expression {
|
||||||
Happy, // :)
|
Happy, // :)
|
||||||
Sad, // :(
|
Sad, // :(
|
||||||
Surprised, // :O
|
Surprised, // :O
|
||||||
Thinking, // :?
|
Thinking, // :?
|
||||||
Sleepy, // -_-
|
Sleepy, // -_-
|
||||||
Excited, // ^_^
|
Excited, // ^_^
|
||||||
Love, // <3 <3
|
Love, // <3 <3
|
||||||
Angry, // >:(
|
Angry, // >:(
|
||||||
Confused, // @_@
|
Confused, // @_@
|
||||||
Wink, // ;)
|
Wink, // ;)
|
||||||
}
|
}
|
||||||
|
|
||||||
impl Expression {
|
impl Expression {
|
||||||
|
|
@ -57,62 +57,53 @@ impl Expression {
|
||||||
Self::Happy => {
|
Self::Happy => {
|
||||||
// Simple smiley
|
// Simple smiley
|
||||||
vec![
|
vec![
|
||||||
black, black, yellow, yellow, yellow, yellow, black, black,
|
black, black, yellow, yellow, yellow, yellow, black, black, black, yellow,
|
||||||
black, yellow, black, black, black, black, yellow, black,
|
black, black, black, black, yellow, black, yellow, black, white, black, black,
|
||||||
yellow, black, white, black, black, white, black, yellow,
|
white, black, yellow, yellow, black, black, black, black, black, black, yellow,
|
||||||
yellow, black, black, black, black, black, black, yellow,
|
yellow, black, white, black, black, white, black, yellow, yellow, black, black,
|
||||||
yellow, black, white, black, black, white, black, yellow,
|
white, white, black, black, yellow, black, yellow, black, black, black, black,
|
||||||
yellow, black, black, white, white, black, black, yellow,
|
yellow, black, black, black, yellow, yellow, yellow, yellow, black, black,
|
||||||
black, yellow, black, black, black, black, yellow, black,
|
|
||||||
black, black, yellow, yellow, yellow, yellow, black, black,
|
|
||||||
]
|
]
|
||||||
}
|
}
|
||||||
Self::Sad => {
|
Self::Sad => {
|
||||||
vec![
|
vec![
|
||||||
black, black, blue, blue, blue, blue, black, black,
|
black, black, blue, blue, blue, blue, black, black, black, blue, black, black,
|
||||||
black, blue, black, black, black, black, blue, black,
|
black, black, blue, black, blue, black, white, black, black, white, black,
|
||||||
blue, black, white, black, black, white, black, blue,
|
blue, blue, black, black, black, black, black, black, blue, blue, black, black,
|
||||||
blue, black, black, black, black, black, black, blue,
|
white, white, black, black, blue, blue, black, white, black, black, white,
|
||||||
blue, black, black, white, white, black, black, blue,
|
black, blue, black, blue, black, black, black, black, blue, black, black,
|
||||||
blue, black, white, black, black, white, black, blue,
|
black, blue, blue, blue, blue, black, black,
|
||||||
black, blue, black, black, black, black, blue, black,
|
|
||||||
black, black, blue, blue, blue, blue, black, black,
|
|
||||||
]
|
]
|
||||||
}
|
}
|
||||||
Self::Excited => {
|
Self::Excited => {
|
||||||
vec![
|
vec![
|
||||||
yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow,
|
yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, black,
|
||||||
yellow, black, black, yellow, yellow, black, black, yellow,
|
black, yellow, yellow, black, black, yellow, yellow, black, white, yellow,
|
||||||
yellow, black, white, yellow, yellow, white, black, yellow,
|
yellow, white, black, yellow, yellow, yellow, yellow, yellow, yellow, yellow,
|
||||||
yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow,
|
yellow, yellow, yellow, black, black, black, black, black, black, yellow,
|
||||||
yellow, black, black, black, black, black, black, yellow,
|
yellow, black, white, white, white, white, black, yellow, yellow, black, black,
|
||||||
yellow, black, white, white, white, white, black, yellow,
|
black, black, black, black, yellow, yellow, yellow, yellow, yellow, yellow,
|
||||||
yellow, black, black, black, black, black, black, yellow,
|
yellow, yellow, yellow,
|
||||||
yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow,
|
|
||||||
]
|
]
|
||||||
}
|
}
|
||||||
Self::Love => {
|
Self::Love => {
|
||||||
vec![
|
vec![
|
||||||
black, pink, pink, black, black, pink, pink, 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, pink,
|
||||||
pink, pink, pink, pink, pink, pink, pink, pink,
|
pink, pink, pink, pink, pink, pink, pink, black, pink, pink, pink, pink, pink,
|
||||||
pink, pink, pink, pink, pink, pink, pink, pink,
|
pink, black, black, black, pink, pink, pink, pink, black, black, black, black,
|
||||||
black, pink, pink, pink, pink, pink, pink, black,
|
black, pink, pink, black, black, black, black, black, black, black, black,
|
||||||
black, black, pink, pink, pink, pink, black, black,
|
black, black, black,
|
||||||
black, black, black, pink, pink, black, black, black,
|
|
||||||
black, black, black, black, black, black, black, black,
|
|
||||||
]
|
]
|
||||||
}
|
}
|
||||||
Self::Angry => {
|
Self::Angry => {
|
||||||
vec![
|
vec![
|
||||||
red, red, black, black, black, black, red, red,
|
red, red, black, black, black, black, red, red, black, red, red, black, black,
|
||||||
black, red, red, black, black, red, red, black,
|
red, red, black, black, black, red, black, black, red, black, black, black,
|
||||||
black, black, red, black, black, red, black, black,
|
black, white, black, black, white, black, black, black, black, black, black,
|
||||||
black, black, white, black, black, white, black, black,
|
black, black, black, black, black, black, white, white, white, white, black,
|
||||||
black, black, black, black, black, black, black, black,
|
black, black, white, black, black, black, black, white, black, black, black,
|
||||||
black, black, white, white, white, white, black, black,
|
black, black, black, black, 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" => {
|
"dance" => {
|
||||||
// Cycle through expressions
|
// 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?;
|
self.set_expression(expr).await?;
|
||||||
tokio::time::sleep(std::time::Duration::from_millis(500)).await;
|
tokio::time::sleep(std::time::Duration::from_millis(500)).await;
|
||||||
}
|
}
|
||||||
|
|
@ -318,10 +314,13 @@ mod tests {
|
||||||
#[tokio::test]
|
#[tokio::test]
|
||||||
async fn emote_happy() {
|
async fn emote_happy() {
|
||||||
let tool = EmoteTool::new(RobotConfig::default());
|
let tool = EmoteTool::new(RobotConfig::default());
|
||||||
let result = tool.execute(json!({
|
let result = tool
|
||||||
"expression": "happy",
|
.execute(json!({
|
||||||
"duration": 0
|
"expression": "happy",
|
||||||
})).await.unwrap();
|
"duration": 0
|
||||||
|
}))
|
||||||
|
.await
|
||||||
|
.unwrap();
|
||||||
assert!(result.success);
|
assert!(result.success);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -23,24 +23,34 @@ impl ListenTool {
|
||||||
|
|
||||||
let _ = std::fs::create_dir_all(&recordings_dir);
|
let _ = std::fs::create_dir_all(&recordings_dir);
|
||||||
|
|
||||||
Self { config, recordings_dir }
|
Self {
|
||||||
|
config,
|
||||||
|
recordings_dir,
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Record audio using arecord (ALSA)
|
/// Record audio using arecord (ALSA)
|
||||||
async fn record_audio(&self, duration_secs: u64) -> Result<PathBuf> {
|
async fn record_audio(&self, duration_secs: u64) -> Result<PathBuf> {
|
||||||
let timestamp = chrono::Utc::now().format("%Y%m%d_%H%M%S");
|
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;
|
let device = &self.config.audio.mic_device;
|
||||||
|
|
||||||
// Record using arecord (standard on Linux/Pi)
|
// Record using arecord (standard on Linux/Pi)
|
||||||
let output = tokio::process::Command::new("arecord")
|
let output = tokio::process::Command::new("arecord")
|
||||||
.args([
|
.args([
|
||||||
"-D", device,
|
"-D",
|
||||||
"-f", "S16_LE", // 16-bit signed little-endian
|
device,
|
||||||
"-r", "16000", // 16kHz (Whisper expects this)
|
"-f",
|
||||||
"-c", "1", // Mono
|
"S16_LE", // 16-bit signed little-endian
|
||||||
"-d", &duration_secs.to_string(),
|
"-r",
|
||||||
|
"16000", // 16kHz (Whisper expects this)
|
||||||
|
"-c",
|
||||||
|
"1", // Mono
|
||||||
|
"-d",
|
||||||
|
&duration_secs.to_string(),
|
||||||
filename.to_str().unwrap(),
|
filename.to_str().unwrap(),
|
||||||
])
|
])
|
||||||
.output()
|
.output()
|
||||||
|
|
@ -63,16 +73,23 @@ impl ListenTool {
|
||||||
|
|
||||||
// whisper.cpp model path (typically in ~/.zeroclaw/models/)
|
// whisper.cpp model path (typically in ~/.zeroclaw/models/)
|
||||||
let model_path = directories::UserDirs::new()
|
let model_path = directories::UserDirs::new()
|
||||||
.map(|d| d.home_dir().join(format!(".zeroclaw/models/ggml-{}.bin", model)))
|
.map(|d| {
|
||||||
.unwrap_or_else(|| PathBuf::from(format!("/usr/local/share/whisper/ggml-{}.bin", model)));
|
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
|
// Run whisper.cpp
|
||||||
let output = tokio::process::Command::new(whisper_path)
|
let output = tokio::process::Command::new(whisper_path)
|
||||||
.args([
|
.args([
|
||||||
"-m", model_path.to_str().unwrap(),
|
"-m",
|
||||||
"-f", audio_path.to_str().unwrap(),
|
model_path.to_str().unwrap(),
|
||||||
|
"-f",
|
||||||
|
audio_path.to_str().unwrap(),
|
||||||
"--no-timestamps",
|
"--no-timestamps",
|
||||||
"-otxt", // Output as text
|
"-otxt", // Output as text
|
||||||
])
|
])
|
||||||
.output()
|
.output()
|
||||||
.await?;
|
.await?;
|
||||||
|
|
@ -127,10 +144,7 @@ impl Tool for ListenTool {
|
||||||
}
|
}
|
||||||
|
|
||||||
async fn execute(&self, args: Value) -> Result<ToolResult> {
|
async fn execute(&self, args: Value) -> Result<ToolResult> {
|
||||||
let duration = args["duration"]
|
let duration = args["duration"].as_u64().unwrap_or(5).clamp(1, 30);
|
||||||
.as_u64()
|
|
||||||
.unwrap_or(5)
|
|
||||||
.clamp(1, 30);
|
|
||||||
|
|
||||||
// Record audio
|
// Record audio
|
||||||
tracing::info!("Recording audio for {} seconds...", duration);
|
tracing::info!("Recording audio for {} seconds...", duration);
|
||||||
|
|
|
||||||
|
|
@ -24,7 +24,10 @@ impl LookTool {
|
||||||
// Ensure capture directory exists
|
// Ensure capture directory exists
|
||||||
let _ = std::fs::create_dir_all(&capture_dir);
|
let _ = std::fs::create_dir_all(&capture_dir);
|
||||||
|
|
||||||
Self { config, capture_dir }
|
Self {
|
||||||
|
config,
|
||||||
|
capture_dir,
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Capture image using ffmpeg (works with most cameras)
|
/// Capture image using ffmpeg (works with most cameras)
|
||||||
|
|
@ -39,10 +42,14 @@ impl LookTool {
|
||||||
// Use ffmpeg for broad camera compatibility
|
// Use ffmpeg for broad camera compatibility
|
||||||
let output = tokio::process::Command::new("ffmpeg")
|
let output = tokio::process::Command::new("ffmpeg")
|
||||||
.args([
|
.args([
|
||||||
"-f", "v4l2",
|
"-f",
|
||||||
"-video_size", &format!("{}x{}", width, height),
|
"v4l2",
|
||||||
"-i", device,
|
"-video_size",
|
||||||
"-frames:v", "1",
|
&format!("{}x{}", width, height),
|
||||||
|
"-i",
|
||||||
|
device,
|
||||||
|
"-frames:v",
|
||||||
|
"1",
|
||||||
"-y", // Overwrite
|
"-y", // Overwrite
|
||||||
filename.to_str().unwrap(),
|
filename.to_str().unwrap(),
|
||||||
])
|
])
|
||||||
|
|
@ -53,9 +60,11 @@ impl LookTool {
|
||||||
// Fallback: try fswebcam (simpler, often works on Pi)
|
// Fallback: try fswebcam (simpler, often works on Pi)
|
||||||
let fallback = tokio::process::Command::new("fswebcam")
|
let fallback = tokio::process::Command::new("fswebcam")
|
||||||
.args([
|
.args([
|
||||||
"-r", &format!("{}x{}", width, height),
|
"-r",
|
||||||
|
&format!("{}x{}", width, height),
|
||||||
"--no-banner",
|
"--no-banner",
|
||||||
"-d", device,
|
"-d",
|
||||||
|
device,
|
||||||
filename.to_str().unwrap(),
|
filename.to_str().unwrap(),
|
||||||
])
|
])
|
||||||
.output()
|
.output()
|
||||||
|
|
@ -84,10 +93,8 @@ impl LookTool {
|
||||||
|
|
||||||
// Read image as base64
|
// Read image as base64
|
||||||
let image_bytes = tokio::fs::read(image_path).await?;
|
let image_bytes = tokio::fs::read(image_path).await?;
|
||||||
let base64_image = base64::Engine::encode(
|
let base64_image =
|
||||||
&base64::engine::general_purpose::STANDARD,
|
base64::Engine::encode(&base64::engine::general_purpose::STANDARD, &image_bytes);
|
||||||
&image_bytes,
|
|
||||||
);
|
|
||||||
|
|
||||||
// Call Ollama with image
|
// Call Ollama with image
|
||||||
let client = reqwest::Client::new();
|
let client = reqwest::Client::new();
|
||||||
|
|
@ -182,15 +189,18 @@ impl Tool for LookTool {
|
||||||
}),
|
}),
|
||||||
Err(e) => Ok(ToolResult {
|
Err(e) => Ok(ToolResult {
|
||||||
success: false,
|
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()),
|
error: Some(e.to_string()),
|
||||||
}),
|
}),
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
"find" => {
|
"find" => {
|
||||||
let target = args["prompt"]
|
let target = args["prompt"].as_str().ok_or_else(|| {
|
||||||
.as_str()
|
anyhow::anyhow!("'find' action requires 'prompt' specifying what to find")
|
||||||
.ok_or_else(|| anyhow::anyhow!("'find' action requires 'prompt' specifying what to find"))?;
|
})?;
|
||||||
|
|
||||||
let prompt = format!(
|
let prompt = format!(
|
||||||
"Look at this image and determine: Is there a {} visible? \
|
"Look at this image and determine: Is there a {} visible? \
|
||||||
|
|
|
||||||
|
|
@ -123,7 +123,9 @@ impl SafetyMonitor {
|
||||||
// Check general movement permission
|
// Check general movement permission
|
||||||
if !self.state.can_move.load(Ordering::SeqCst) {
|
if !self.state.can_move.load(Ordering::SeqCst) {
|
||||||
let reason = self.state.block_reason.read().await;
|
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
|
// Check obstacle distance in movement direction
|
||||||
|
|
@ -133,7 +135,9 @@ impl SafetyMonitor {
|
||||||
"Obstacle too close: {:.2}m (min: {:.2}m)",
|
"Obstacle too close: {:.2}m (min: {:.2}m)",
|
||||||
min_dist, self.config.min_obstacle_distance
|
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);
|
return Err(msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -141,12 +145,17 @@ impl SafetyMonitor {
|
||||||
if distance > min_dist - self.config.min_obstacle_distance {
|
if distance > min_dist - self.config.min_obstacle_distance {
|
||||||
let safe_distance = (min_dist - self.config.min_obstacle_distance).max(0.0);
|
let safe_distance = (min_dist - self.config.min_obstacle_distance).max(0.0);
|
||||||
if safe_distance < 0.1 {
|
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
|
// Allow reduced distance
|
||||||
tracing::warn!(
|
tracing::warn!(
|
||||||
"Reducing {} distance from {:.2}m to {:.2}m due to obstacle",
|
"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
|
// Check if too close
|
||||||
if distance < self.config.min_obstacle_distance {
|
if distance < self.config.min_obstacle_distance {
|
||||||
self.state.can_move.store(false, Ordering::SeqCst);
|
self.state.can_move.store(false, Ordering::SeqCst);
|
||||||
*self.state.block_reason.write().await = Some(format!(
|
*self.state.block_reason.write().await =
|
||||||
"Obstacle at {:.2}m ({}°)",
|
Some(format!("Obstacle at {:.2}m ({}°)", distance, angle));
|
||||||
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) {
|
} else if !self.state.estop_active.load(Ordering::SeqCst) {
|
||||||
// Clear block if obstacle moved away and no E-stop
|
// Clear block if obstacle moved away and no E-stop
|
||||||
self.state.can_move.store(true, Ordering::SeqCst);
|
self.state.can_move.store(true, Ordering::SeqCst);
|
||||||
|
|
@ -336,10 +345,7 @@ pub struct SafeDrive {
|
||||||
}
|
}
|
||||||
|
|
||||||
impl SafeDrive {
|
impl SafeDrive {
|
||||||
pub fn new(
|
pub fn new(drive: Arc<dyn crate::traits::Tool>, safety: Arc<SafetyMonitor>) -> Self {
|
||||||
drive: Arc<dyn crate::traits::Tool>,
|
|
||||||
safety: Arc<SafetyMonitor>,
|
|
||||||
) -> Self {
|
|
||||||
Self {
|
Self {
|
||||||
inner_drive: drive,
|
inner_drive: drive,
|
||||||
safety,
|
safety,
|
||||||
|
|
@ -361,10 +367,7 @@ impl crate::traits::Tool for SafeDrive {
|
||||||
self.inner_drive.parameters_schema()
|
self.inner_drive.parameters_schema()
|
||||||
}
|
}
|
||||||
|
|
||||||
async fn execute(
|
async fn execute(&self, args: serde_json::Value) -> Result<ToolResult> {
|
||||||
&self,
|
|
||||||
args: serde_json::Value,
|
|
||||||
) -> Result<ToolResult> {
|
|
||||||
// ToolResult imported at top of file
|
// ToolResult imported at top of file
|
||||||
|
|
||||||
let action = args["action"].as_str().unwrap_or("unknown");
|
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
|
self.inner_drive.execute(modified_args).await
|
||||||
}
|
}
|
||||||
Err(reason) => {
|
Err(reason) => Ok(ToolResult {
|
||||||
Ok(ToolResult {
|
success: false,
|
||||||
success: false,
|
output: String::new(),
|
||||||
output: String::new(),
|
error: Some(format!("Safety blocked movement: {}", reason)),
|
||||||
error: Some(format!("Safety blocked movement: {}", reason)),
|
}),
|
||||||
})
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -421,7 +422,10 @@ pub async fn preflight_check(config: &RobotConfig) -> Result<Vec<String>> {
|
||||||
}
|
}
|
||||||
|
|
||||||
if config.safety.estop_pin.is_none() {
|
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
|
// Check for sensor availability
|
||||||
|
|
|
||||||
|
|
@ -76,7 +76,9 @@ impl SenseTool {
|
||||||
.map(|(i, &d)| (d, i as u16))
|
.map(|(i, &d)| (d, i as u16))
|
||||||
.unwrap_or((999.0, 0));
|
.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);
|
.all(|&d| d > self.config.safety.min_obstacle_distance);
|
||||||
|
|
||||||
Ok(LidarScan {
|
Ok(LidarScan {
|
||||||
|
|
@ -118,10 +120,16 @@ impl SenseTool {
|
||||||
.map(|(i, &d)| (d, i as u16))
|
.map(|(i, &d)| (d, i as u16))
|
||||||
.unwrap_or((999.0, 0));
|
.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);
|
.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
|
// Fallback to mock if hardware unavailable
|
||||||
|
|
@ -159,10 +167,16 @@ impl SenseTool {
|
||||||
.map(|(i, &d)| (d, i as u16))
|
.map(|(i, &d)| (d, i as u16))
|
||||||
.unwrap_or((999.0, 0));
|
.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);
|
.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
|
/// Check PIR motion sensors
|
||||||
|
|
@ -199,8 +213,10 @@ impl SenseTool {
|
||||||
// Ultrasonic requires µs-level timing, so shell out to helper
|
// Ultrasonic requires µs-level timing, so shell out to helper
|
||||||
let output = tokio::process::Command::new("hc-sr04")
|
let output = tokio::process::Command::new("hc-sr04")
|
||||||
.args([
|
.args([
|
||||||
"--trigger", &trigger.to_string(),
|
"--trigger",
|
||||||
"--echo", &echo.to_string(),
|
&trigger.to_string(),
|
||||||
|
"--echo",
|
||||||
|
&echo.to_string(),
|
||||||
])
|
])
|
||||||
.output()
|
.output()
|
||||||
.await;
|
.await;
|
||||||
|
|
@ -265,7 +281,11 @@ impl Tool for SenseTool {
|
||||||
format!(
|
format!(
|
||||||
"Forward: {:.2}m {}. Nearest obstacle: {:.2}m at {}°",
|
"Forward: {:.2}m {}. Nearest obstacle: {:.2}m at {}°",
|
||||||
fwd_dist,
|
fwd_dist,
|
||||||
if scan.forward_clear { "(clear)" } else { "(BLOCKED)" },
|
if scan.forward_clear {
|
||||||
|
"(clear)"
|
||||||
|
} else {
|
||||||
|
"(BLOCKED)"
|
||||||
|
},
|
||||||
scan.nearest.0,
|
scan.nearest.0,
|
||||||
scan.nearest.1
|
scan.nearest.1
|
||||||
)
|
)
|
||||||
|
|
@ -291,9 +311,17 @@ impl Tool for SenseTool {
|
||||||
- Right (270°): {:.2}m\n\
|
- Right (270°): {:.2}m\n\
|
||||||
- Nearest: {:.2}m at {}°\n\
|
- Nearest: {:.2}m at {}°\n\
|
||||||
- Forward path: {}",
|
- Forward path: {}",
|
||||||
scan.ranges[0], scan.ranges[90], scan.ranges[180], scan.ranges[270],
|
scan.ranges[0],
|
||||||
scan.nearest.0, scan.nearest.1,
|
scan.ranges[90],
|
||||||
if scan.forward_clear { "CLEAR" } else { "BLOCKED" }
|
scan.ranges[180],
|
||||||
|
scan.ranges[270],
|
||||||
|
scan.nearest.0,
|
||||||
|
scan.nearest.1,
|
||||||
|
if scan.forward_clear {
|
||||||
|
"CLEAR"
|
||||||
|
} else {
|
||||||
|
"BLOCKED"
|
||||||
|
}
|
||||||
)
|
)
|
||||||
}
|
}
|
||||||
_ => "Unknown direction".to_string(),
|
_ => "Unknown direction".to_string(),
|
||||||
|
|
@ -344,7 +372,10 @@ impl Tool for SenseTool {
|
||||||
Ok(ToolResult {
|
Ok(ToolResult {
|
||||||
success: true,
|
success: true,
|
||||||
output: if scan.forward_clear {
|
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 {
|
} else {
|
||||||
format!("Path ahead is BLOCKED (obstacle at {:.2}m)", scan.ranges[0])
|
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\
|
LIDAR: nearest {:.2}m at {}°, forward {}\n\
|
||||||
Motion: {}\n\
|
Motion: {}\n\
|
||||||
Ultrasonic: {:.2}m",
|
Ultrasonic: {:.2}m",
|
||||||
scan.nearest.0, scan.nearest.1,
|
scan.nearest.0,
|
||||||
if scan.forward_clear { "CLEAR" } else { "BLOCKED" },
|
scan.nearest.1,
|
||||||
if motion.detected { format!("DETECTED ({:?})", motion.sensors_triggered) } else { "none".to_string() },
|
if scan.forward_clear {
|
||||||
|
"CLEAR"
|
||||||
|
} else {
|
||||||
|
"BLOCKED"
|
||||||
|
},
|
||||||
|
if motion.detected {
|
||||||
|
format!("DETECTED ({:?})", motion.sensors_triggered)
|
||||||
|
} else {
|
||||||
|
"none".to_string()
|
||||||
|
},
|
||||||
distance
|
distance
|
||||||
);
|
);
|
||||||
|
|
||||||
|
|
@ -397,7 +437,10 @@ mod tests {
|
||||||
#[tokio::test]
|
#[tokio::test]
|
||||||
async fn sense_scan_mock() {
|
async fn sense_scan_mock() {
|
||||||
let tool = SenseTool::new(RobotConfig::default());
|
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.success);
|
||||||
assert!(result.output.contains("Forward"));
|
assert!(result.output.contains("Forward"));
|
||||||
}
|
}
|
||||||
|
|
@ -405,7 +448,10 @@ mod tests {
|
||||||
#[tokio::test]
|
#[tokio::test]
|
||||||
async fn sense_clear_ahead() {
|
async fn sense_clear_ahead() {
|
||||||
let tool = SenseTool::new(RobotConfig::default());
|
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);
|
assert!(result.success);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -34,7 +34,10 @@ impl SpeakTool {
|
||||||
|
|
||||||
// Model path
|
// Model path
|
||||||
let model_path = directories::UserDirs::new()
|
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)));
|
.unwrap_or_else(|| PathBuf::from(format!("/usr/local/share/piper/{}.onnx", voice)));
|
||||||
|
|
||||||
// Adjust text based on emotion (simple SSML-like modifications)
|
// Adjust text based on emotion (simple SSML-like modifications)
|
||||||
|
|
@ -51,8 +54,10 @@ impl SpeakTool {
|
||||||
// Pipe text to piper, output to WAV
|
// Pipe text to piper, output to WAV
|
||||||
let mut piper = tokio::process::Command::new(piper_path)
|
let mut piper = tokio::process::Command::new(piper_path)
|
||||||
.args([
|
.args([
|
||||||
"--model", model_path.to_str().unwrap(),
|
"--model",
|
||||||
"--output_file", output_path.to_str().unwrap(),
|
model_path.to_str().unwrap(),
|
||||||
|
"--output_file",
|
||||||
|
output_path.to_str().unwrap(),
|
||||||
])
|
])
|
||||||
.stdin(std::process::Stdio::piped())
|
.stdin(std::process::Stdio::piped())
|
||||||
.spawn()?;
|
.spawn()?;
|
||||||
|
|
@ -70,10 +75,7 @@ impl SpeakTool {
|
||||||
|
|
||||||
// Play audio using aplay
|
// Play audio using aplay
|
||||||
let play_result = tokio::process::Command::new("aplay")
|
let play_result = tokio::process::Command::new("aplay")
|
||||||
.args([
|
.args(["-D", speaker_device, output_path.to_str().unwrap()])
|
||||||
"-D", speaker_device,
|
|
||||||
output_path.to_str().unwrap(),
|
|
||||||
])
|
|
||||||
.output()
|
.output()
|
||||||
.await?;
|
.await?;
|
||||||
|
|
||||||
|
|
@ -171,9 +173,9 @@ impl Tool for SpeakTool {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Speak text
|
// Speak text
|
||||||
let text = args["text"]
|
let text = args["text"].as_str().ok_or_else(|| {
|
||||||
.as_str()
|
anyhow::anyhow!("Missing 'text' parameter (or use 'sound' for effects)")
|
||||||
.ok_or_else(|| anyhow::anyhow!("Missing 'text' parameter (or use 'sound' for effects)"))?;
|
})?;
|
||||||
|
|
||||||
if text.is_empty() {
|
if text.is_empty() {
|
||||||
return Ok(ToolResult {
|
return Ok(ToolResult {
|
||||||
|
|
|
||||||
|
|
@ -8,7 +8,7 @@
|
||||||
#[cfg(test)]
|
#[cfg(test)]
|
||||||
mod unit_tests {
|
mod unit_tests {
|
||||||
use crate::config::RobotConfig;
|
use crate::config::RobotConfig;
|
||||||
use crate::traits::{Tool, ToolResult};
|
use crate::traits::Tool;
|
||||||
use crate::{DriveTool, EmoteTool, ListenTool, LookTool, SenseTool, SpeakTool};
|
use crate::{DriveTool, EmoteTool, ListenTool, LookTool, SenseTool, SpeakTool};
|
||||||
use serde_json::json;
|
use serde_json::json;
|
||||||
|
|
||||||
|
|
@ -208,9 +208,7 @@ mod unit_tests {
|
||||||
|
|
||||||
assert!(result.success);
|
assert!(result.success);
|
||||||
// Mock should report clear or blocked
|
// Mock should report clear or blocked
|
||||||
assert!(
|
assert!(result.output.contains("CLEAR") || result.output.contains("BLOCKED"));
|
||||||
result.output.contains("CLEAR") || result.output.contains("BLOCKED")
|
|
||||||
);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#[tokio::test]
|
#[tokio::test]
|
||||||
|
|
@ -246,8 +244,16 @@ mod unit_tests {
|
||||||
let tool = EmoteTool::new(config);
|
let tool = EmoteTool::new(config);
|
||||||
|
|
||||||
let expressions = [
|
let expressions = [
|
||||||
"happy", "sad", "surprised", "thinking", "sleepy", "excited", "love", "angry",
|
"happy",
|
||||||
"confused", "wink",
|
"sad",
|
||||||
|
"surprised",
|
||||||
|
"thinking",
|
||||||
|
"sleepy",
|
||||||
|
"excited",
|
||||||
|
"love",
|
||||||
|
"angry",
|
||||||
|
"confused",
|
||||||
|
"wink",
|
||||||
];
|
];
|
||||||
|
|
||||||
for expr in expressions {
|
for expr in expressions {
|
||||||
|
|
@ -265,9 +271,7 @@ mod unit_tests {
|
||||||
let config = RobotConfig::default();
|
let config = RobotConfig::default();
|
||||||
let tool = EmoteTool::new(config);
|
let tool = EmoteTool::new(config);
|
||||||
|
|
||||||
let result = tool
|
let result = tool.execute(json!({"expression": "nonexistent"})).await;
|
||||||
.execute(json!({"expression": "nonexistent"}))
|
|
||||||
.await;
|
|
||||||
|
|
||||||
assert!(result.is_err());
|
assert!(result.is_err());
|
||||||
}
|
}
|
||||||
|
|
@ -313,7 +317,7 @@ mod unit_tests {
|
||||||
#[cfg(feature = "safety")]
|
#[cfg(feature = "safety")]
|
||||||
mod safety_tests {
|
mod safety_tests {
|
||||||
use crate::config::SafetyConfig;
|
use crate::config::SafetyConfig;
|
||||||
use crate::safety::{SafetyEvent, SafetyMonitor, SensorReading};
|
use crate::safety::{SafetyEvent, SafetyMonitor};
|
||||||
use std::sync::atomic::Ordering;
|
use std::sync::atomic::Ordering;
|
||||||
|
|
||||||
fn test_safety_config() -> SafetyConfig {
|
fn test_safety_config() -> SafetyConfig {
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue