428 lines
13 KiB
Rust
428 lines
13 KiB
Rust
//! Drive Tool - Motor control for omni-directional movement
|
|
//!
|
|
//! Supports multiple backends:
|
|
//! - ROS2: Publishes geometry_msgs/Twist to cmd_vel topic
|
|
//! - GPIO: Direct PWM control via rppal
|
|
//! - Serial: Arduino/motor controller via serial commands
|
|
//! - Mock: Logs commands for testing
|
|
|
|
use crate::config::RobotConfig;
|
|
use crate::traits::{Tool, ToolResult};
|
|
use anyhow::Result;
|
|
use async_trait::async_trait;
|
|
use serde_json::{json, Value};
|
|
use std::sync::Arc;
|
|
use std::time::Duration;
|
|
use tokio::sync::Mutex;
|
|
|
|
/// Drive backend abstraction
|
|
#[async_trait]
|
|
trait DriveBackend: Send + Sync {
|
|
async fn move_robot(
|
|
&self,
|
|
linear_x: f64,
|
|
linear_y: f64,
|
|
angular_z: f64,
|
|
duration_ms: u64,
|
|
) -> Result<()>;
|
|
async fn stop(&self) -> Result<()>;
|
|
#[allow(dead_code)]
|
|
async fn get_odometry(&self) -> Result<(f64, f64, f64)>; // x, y, theta - reserved for future odometry integration
|
|
}
|
|
|
|
/// Mock backend for testing
|
|
struct MockDrive;
|
|
|
|
#[async_trait]
|
|
impl DriveBackend for MockDrive {
|
|
async fn move_robot(
|
|
&self,
|
|
linear_x: f64,
|
|
linear_y: f64,
|
|
angular_z: f64,
|
|
duration_ms: u64,
|
|
) -> Result<()> {
|
|
tracing::info!(
|
|
"MOCK DRIVE: linear=({:.2}, {:.2}), angular={:.2}, duration={}ms",
|
|
linear_x,
|
|
linear_y,
|
|
angular_z,
|
|
duration_ms
|
|
);
|
|
tokio::time::sleep(Duration::from_millis(duration_ms.min(100))).await;
|
|
Ok(())
|
|
}
|
|
|
|
async fn stop(&self) -> Result<()> {
|
|
tracing::info!("MOCK DRIVE: STOP");
|
|
Ok(())
|
|
}
|
|
|
|
async fn get_odometry(&self) -> Result<(f64, f64, f64)> {
|
|
Ok((0.0, 0.0, 0.0))
|
|
}
|
|
}
|
|
|
|
/// ROS2 backend - shells out to ros2 topic pub
|
|
struct Ros2Drive {
|
|
topic: String,
|
|
}
|
|
|
|
#[async_trait]
|
|
impl DriveBackend for Ros2Drive {
|
|
async fn move_robot(
|
|
&self,
|
|
linear_x: f64,
|
|
linear_y: f64,
|
|
angular_z: f64,
|
|
duration_ms: u64,
|
|
) -> Result<()> {
|
|
// Publish Twist message via ros2 CLI
|
|
// In production, use rclrs (Rust ROS2 bindings) instead
|
|
let msg = format!(
|
|
"{{linear: {{x: {:.2}, y: {:.2}, z: 0.0}}, angular: {{x: 0.0, y: 0.0, z: {:.2}}}}}",
|
|
linear_x, linear_y, angular_z
|
|
);
|
|
|
|
let output = tokio::process::Command::new("ros2")
|
|
.args([
|
|
"topic",
|
|
"pub",
|
|
"--once",
|
|
&self.topic,
|
|
"geometry_msgs/msg/Twist",
|
|
&msg,
|
|
])
|
|
.output()
|
|
.await?;
|
|
|
|
if !output.status.success() {
|
|
anyhow::bail!(
|
|
"ROS2 publish failed: {}",
|
|
String::from_utf8_lossy(&output.stderr)
|
|
);
|
|
}
|
|
|
|
// Hold for duration then stop
|
|
tokio::time::sleep(Duration::from_millis(duration_ms)).await;
|
|
self.stop().await?;
|
|
|
|
Ok(())
|
|
}
|
|
|
|
async fn stop(&self) -> Result<()> {
|
|
let msg = "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}";
|
|
tokio::process::Command::new("ros2")
|
|
.args([
|
|
"topic",
|
|
"pub",
|
|
"--once",
|
|
&self.topic,
|
|
"geometry_msgs/msg/Twist",
|
|
msg,
|
|
])
|
|
.output()
|
|
.await?;
|
|
Ok(())
|
|
}
|
|
|
|
async fn get_odometry(&self) -> Result<(f64, f64, f64)> {
|
|
// Would subscribe to /odom topic in production
|
|
Ok((0.0, 0.0, 0.0))
|
|
}
|
|
}
|
|
|
|
/// Serial backend - sends commands to Arduino/motor controller
|
|
struct SerialDrive {
|
|
port: String,
|
|
}
|
|
|
|
#[async_trait]
|
|
impl DriveBackend for SerialDrive {
|
|
async fn move_robot(
|
|
&self,
|
|
linear_x: f64,
|
|
linear_y: f64,
|
|
angular_z: f64,
|
|
duration_ms: u64,
|
|
) -> Result<()> {
|
|
// Protocol: "M <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
|
|
);
|
|
|
|
// Use blocking serial in spawn_blocking
|
|
let port = self.port.clone();
|
|
tokio::task::spawn_blocking(move || {
|
|
use std::io::Write;
|
|
let mut serial = std::fs::OpenOptions::new().write(true).open(&port)?;
|
|
serial.write_all(cmd.as_bytes())?;
|
|
serial.flush()?;
|
|
Ok::<_, anyhow::Error>(())
|
|
})
|
|
.await??;
|
|
|
|
tokio::time::sleep(Duration::from_millis(duration_ms)).await;
|
|
Ok(())
|
|
}
|
|
|
|
async fn stop(&self) -> Result<()> {
|
|
self.move_robot(0.0, 0.0, 0.0, 0).await
|
|
}
|
|
|
|
async fn get_odometry(&self) -> Result<(f64, f64, f64)> {
|
|
Ok((0.0, 0.0, 0.0))
|
|
}
|
|
}
|
|
|
|
/// Main Drive Tool
|
|
pub struct DriveTool {
|
|
config: RobotConfig,
|
|
backend: Arc<dyn DriveBackend>,
|
|
last_command: Arc<Mutex<Option<std::time::Instant>>>,
|
|
}
|
|
|
|
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(),
|
|
}),
|
|
// "gpio" => Arc::new(GpioDrive::new(&config)), // Would use rppal
|
|
_ => Arc::new(MockDrive),
|
|
};
|
|
|
|
Self {
|
|
config,
|
|
backend,
|
|
last_command: Arc::new(Mutex::new(None)),
|
|
}
|
|
}
|
|
}
|
|
|
|
#[async_trait]
|
|
impl Tool for DriveTool {
|
|
fn name(&self) -> &str {
|
|
"drive"
|
|
}
|
|
|
|
fn description(&self) -> &str {
|
|
"Move the robot. Supports omni-directional movement (forward, backward, strafe left/right, rotate). \
|
|
Use 'stop' action to halt immediately. Distance is in meters, rotation in degrees."
|
|
}
|
|
|
|
fn parameters_schema(&self) -> Value {
|
|
json!({
|
|
"type": "object",
|
|
"properties": {
|
|
"action": {
|
|
"type": "string",
|
|
"enum": ["forward", "backward", "left", "right", "rotate_left", "rotate_right", "stop", "custom"],
|
|
"description": "Movement action. 'left'/'right' are strafe (omni wheels). 'rotate_*' spins in place."
|
|
},
|
|
"distance": {
|
|
"type": "number",
|
|
"description": "Distance in meters (for linear moves) or degrees (for rotation). Default 0.5m or 90deg."
|
|
},
|
|
"speed": {
|
|
"type": "number",
|
|
"description": "Speed multiplier 0.0-1.0. Default 0.5 (half speed for safety)."
|
|
},
|
|
"linear_x": {
|
|
"type": "number",
|
|
"description": "Custom: forward/backward velocity (-1.0 to 1.0)"
|
|
},
|
|
"linear_y": {
|
|
"type": "number",
|
|
"description": "Custom: left/right strafe velocity (-1.0 to 1.0)"
|
|
},
|
|
"angular_z": {
|
|
"type": "number",
|
|
"description": "Custom: rotation velocity (-1.0 to 1.0)"
|
|
}
|
|
},
|
|
"required": ["action"]
|
|
})
|
|
}
|
|
|
|
async fn execute(&self, args: Value) -> Result<ToolResult> {
|
|
let action = args["action"]
|
|
.as_str()
|
|
.ok_or_else(|| anyhow::anyhow!("Missing 'action' parameter"))?;
|
|
|
|
// Safety: check max drive duration
|
|
{
|
|
let mut last = self.last_command.lock().await;
|
|
if let Some(instant) = *last {
|
|
if instant.elapsed() < Duration::from_secs(1) {
|
|
return Ok(ToolResult {
|
|
success: false,
|
|
output: String::new(),
|
|
error: Some(
|
|
"Rate limited: wait 1 second between drive commands".to_string(),
|
|
),
|
|
});
|
|
}
|
|
}
|
|
*last = Some(std::time::Instant::now());
|
|
}
|
|
|
|
let speed = args["speed"].as_f64().unwrap_or(0.5).clamp(0.0, 1.0);
|
|
let max_speed = self.config.drive.max_speed * speed;
|
|
let max_rotation = self.config.drive.max_rotation * speed;
|
|
|
|
let (linear_x, linear_y, angular_z, duration_ms) = match action {
|
|
"stop" => {
|
|
self.backend.stop().await?;
|
|
return Ok(ToolResult {
|
|
success: true,
|
|
output: "Robot stopped".to_string(),
|
|
error: None,
|
|
});
|
|
}
|
|
"forward" => {
|
|
let dist = args["distance"].as_f64().unwrap_or(0.5);
|
|
let duration = (dist / max_speed * 1000.0) as u64;
|
|
(
|
|
max_speed,
|
|
0.0,
|
|
0.0,
|
|
duration.min(self.config.safety.max_drive_duration * 1000),
|
|
)
|
|
}
|
|
"backward" => {
|
|
let dist = args["distance"].as_f64().unwrap_or(0.5);
|
|
let duration = (dist / max_speed * 1000.0) as u64;
|
|
(
|
|
-max_speed,
|
|
0.0,
|
|
0.0,
|
|
duration.min(self.config.safety.max_drive_duration * 1000),
|
|
)
|
|
}
|
|
"left" => {
|
|
let dist = args["distance"].as_f64().unwrap_or(0.5);
|
|
let duration = (dist / max_speed * 1000.0) as u64;
|
|
(
|
|
0.0,
|
|
max_speed,
|
|
0.0,
|
|
duration.min(self.config.safety.max_drive_duration * 1000),
|
|
)
|
|
}
|
|
"right" => {
|
|
let dist = args["distance"].as_f64().unwrap_or(0.5);
|
|
let duration = (dist / max_speed * 1000.0) as u64;
|
|
(
|
|
0.0,
|
|
-max_speed,
|
|
0.0,
|
|
duration.min(self.config.safety.max_drive_duration * 1000),
|
|
)
|
|
}
|
|
"rotate_left" => {
|
|
let degrees = args["distance"].as_f64().unwrap_or(90.0);
|
|
let radians = degrees.to_radians();
|
|
let duration = (radians / max_rotation * 1000.0) as u64;
|
|
(
|
|
0.0,
|
|
0.0,
|
|
max_rotation,
|
|
duration.min(self.config.safety.max_drive_duration * 1000),
|
|
)
|
|
}
|
|
"rotate_right" => {
|
|
let degrees = args["distance"].as_f64().unwrap_or(90.0);
|
|
let radians = degrees.to_radians();
|
|
let duration = (radians / max_rotation * 1000.0) as u64;
|
|
(
|
|
0.0,
|
|
0.0,
|
|
-max_rotation,
|
|
duration.min(self.config.safety.max_drive_duration * 1000),
|
|
)
|
|
}
|
|
"custom" => {
|
|
let lx = args["linear_x"].as_f64().unwrap_or(0.0).clamp(-1.0, 1.0) * max_speed;
|
|
let ly = args["linear_y"].as_f64().unwrap_or(0.0).clamp(-1.0, 1.0) * max_speed;
|
|
let az = args["angular_z"].as_f64().unwrap_or(0.0).clamp(-1.0, 1.0) * max_rotation;
|
|
let duration = args["duration_ms"].as_u64().unwrap_or(1000);
|
|
(
|
|
lx,
|
|
ly,
|
|
az,
|
|
duration.min(self.config.safety.max_drive_duration * 1000),
|
|
)
|
|
}
|
|
_ => {
|
|
return Ok(ToolResult {
|
|
success: false,
|
|
output: String::new(),
|
|
error: Some(format!("Unknown action: {action}")),
|
|
});
|
|
}
|
|
};
|
|
|
|
self.backend
|
|
.move_robot(linear_x, linear_y, angular_z, duration_ms)
|
|
.await?;
|
|
|
|
Ok(ToolResult {
|
|
success: true,
|
|
output: format!(
|
|
"Moved: action={}, linear=({:.2}, {:.2}), angular={:.2}, duration={}ms",
|
|
action, linear_x, linear_y, angular_z, duration_ms
|
|
),
|
|
error: None,
|
|
})
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
#[test]
|
|
fn drive_tool_name() {
|
|
let tool = DriveTool::new(RobotConfig::default());
|
|
assert_eq!(tool.name(), "drive");
|
|
}
|
|
|
|
#[test]
|
|
fn drive_tool_schema_has_action() {
|
|
let tool = DriveTool::new(RobotConfig::default());
|
|
let schema = tool.parameters_schema();
|
|
assert!(schema["properties"]["action"].is_object());
|
|
}
|
|
|
|
#[tokio::test]
|
|
async fn drive_forward_mock() {
|
|
let tool = DriveTool::new(RobotConfig::default());
|
|
let result = tool
|
|
.execute(json!({"action": "forward", "distance": 1.0}))
|
|
.await
|
|
.unwrap();
|
|
assert!(result.success);
|
|
assert!(result.output.contains("forward"));
|
|
}
|
|
|
|
#[tokio::test]
|
|
async fn drive_stop() {
|
|
let tool = DriveTool::new(RobotConfig::default());
|
|
let result = tool.execute(json!({"action": "stop"})).await.unwrap();
|
|
assert!(result.success);
|
|
assert!(result.output.contains("stopped"));
|
|
}
|
|
|
|
#[tokio::test]
|
|
async fn drive_unknown_action() {
|
|
let tool = DriveTool::new(RobotConfig::default());
|
|
let result = tool.execute(json!({"action": "fly"})).await.unwrap();
|
|
assert!(!result.success);
|
|
}
|
|
}
|