fix(robot-kit): format crate and harden cross-platform feature gating
This commit is contained in:
Chummy
parent
0dfc707c49
commit
d70324f4f7
11 changed files with 374 additions and 204 deletions
|
|
@ -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
|
||||
},
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 <lx> <ly> <az> <ms>\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<dyn DriveBackend> = 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"));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<PathBuf> {
|
||||
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<ToolResult> {
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -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? \
|
||||
|
|
|
|||
|
|
@ -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<dyn crate::traits::Tool>,
|
||||
safety: Arc<SafetyMonitor>,
|
||||
) -> Self {
|
||||
pub fn new(drive: Arc<dyn crate::traits::Tool>, safety: Arc<SafetyMonitor>) -> 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<ToolResult> {
|
||||
async fn execute(&self, args: serde_json::Value) -> Result<ToolResult> {
|
||||
// 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<Vec<String>> {
|
|||
}
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue