Add encoder-based motion commands for scenario testing
Replace time-based movement with MoveDistance and Rotate commands that use encoder feedback for precise motion control. Commands complete when target wheel distances are achieved, not after a fixed duration.
This commit is contained in:
codetiger
@@ -27,10 +27,34 @@ use sangam_io::core::types::{
|
||||
use sangam_io::devices::mock::MockDriver;
|
||||
use sangam_io::devices::mock::config::SimulationConfig;
|
||||
|
||||
use vastu_slam::io::{Scenario, SvgConfig, SvgVisualizer, markers_by_distance};
|
||||
use vastu_slam::io::{Command, Scenario, SvgConfig, SvgVisualizer, markers_by_distance};
|
||||
use vastu_slam::matching::CorrelativeMatcherConfig;
|
||||
use vastu_slam::{LidarScan, MapConfig, OccupancyGridMap, Pose2D};
|
||||
|
||||
/// Command timeout in seconds
|
||||
const COMMAND_TIMEOUT_SECS: u64 = 30;
|
||||
|
||||
/// Encoder tolerance: ~1cm = 45 ticks (at 4464 ticks/m)
|
||||
const ENCODER_TOLERANCE_TICKS: i32 = 45;
|
||||
|
||||
/// Grace period after stopping before next command (ms)
|
||||
const COMMAND_GRACE_PERIOD_MS: u64 = 100;
|
||||
|
||||
/// Check if wheel has reached target (approaching from zero, never crossing)
|
||||
/// Returns true when delta enters the tolerance zone BEFORE the target
|
||||
fn wheel_reached_target(delta: i32, expected: i32, tolerance: i32) -> bool {
|
||||
if expected > 0 {
|
||||
// Moving forward: stop when approaching target from below
|
||||
delta >= expected - tolerance
|
||||
} else if expected < 0 {
|
||||
// Moving backward: stop when approaching target from above
|
||||
delta <= expected + tolerance
|
||||
} else {
|
||||
// No movement expected
|
||||
true
|
||||
}
|
||||
}
|
||||
|
||||
/// Convert SangamIO PointCloud2D to VastuSLAM LidarScan
|
||||
fn to_lidar_scan(points: &[(f32, f32, u8)]) -> LidarScan {
|
||||
let angles: Vec<f32> = points.iter().map(|(a, _, _)| *a).collect();
|
||||
@@ -113,6 +137,23 @@ fn compute_odometry(
|
||||
pose.theta += dtheta;
|
||||
}
|
||||
|
||||
/// Get current encoder values (left, right)
|
||||
fn get_encoder_values(
|
||||
sensor_data: &HashMap<String, Arc<Mutex<SensorGroupData>>>,
|
||||
) -> Option<(u16, u16)> {
|
||||
let sensor_status = sensor_data.get("sensor_status")?;
|
||||
let data = sensor_status.lock().ok()?;
|
||||
let left = match data.values.get("wheel_left") {
|
||||
Some(SensorValue::U16(v)) => *v,
|
||||
_ => return None,
|
||||
};
|
||||
let right = match data.values.get("wheel_right") {
|
||||
Some(SensorValue::U16(v)) => *v,
|
||||
_ => return None,
|
||||
};
|
||||
Some((left, right))
|
||||
}
|
||||
|
||||
/// Create a command to enable lidar
|
||||
fn lidar_enable() -> SangamCommand {
|
||||
SangamCommand::ComponentControl {
|
||||
@@ -298,53 +339,163 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
|
||||
|
||||
for (i, cmd) in scenario.commands.iter().enumerate() {
|
||||
let (linear, angular) = cmd.velocities();
|
||||
let duration_ms = cmd.duration_ms();
|
||||
let sim_duration_ms = (duration_ms as f32 / speed_factor) as u64;
|
||||
|
||||
log::debug!(
|
||||
"Command {}/{}: v={:.2} m/s, ω={:.2} rad/s, duration={}ms (sim: {}ms)",
|
||||
i + 1,
|
||||
scenario.commands.len(),
|
||||
linear,
|
||||
angular,
|
||||
duration_ms,
|
||||
sim_duration_ms
|
||||
// Determine if this command uses encoder-based or time-based execution
|
||||
let use_encoder_verification = matches!(
|
||||
cmd,
|
||||
Command::MoveDistance { .. } | Command::Rotate { .. }
|
||||
);
|
||||
|
||||
// Send velocity command
|
||||
driver.send_command(set_velocity(linear, angular))?;
|
||||
if use_encoder_verification {
|
||||
// Get target wheel distances for encoder verification
|
||||
let (target_left_m, target_right_m) = cmd.wheel_distances().unwrap_or((0.0, 0.0));
|
||||
let expected_left_ticks = (target_left_m * ticks_per_meter) as i32;
|
||||
let expected_right_ticks = (target_right_m * ticks_per_meter) as i32;
|
||||
|
||||
let cmd_start = Instant::now();
|
||||
while cmd_start.elapsed().as_millis() < sim_duration_ms as u128 {
|
||||
// Update odometry
|
||||
compute_odometry(
|
||||
&sensor_data,
|
||||
&mut prev_wheel_left,
|
||||
&mut prev_wheel_right,
|
||||
&mut current_pose,
|
||||
ticks_per_meter,
|
||||
wheel_base,
|
||||
log::debug!(
|
||||
"Command {}/{}: ENCODER-BASED - target L:{} R:{} ticks, v={:.2} m/s, ω={:.2} rad/s",
|
||||
i + 1,
|
||||
scenario.commands.len(),
|
||||
expected_left_ticks,
|
||||
expected_right_ticks,
|
||||
linear,
|
||||
angular
|
||||
);
|
||||
|
||||
// Process lidar scan with matching
|
||||
if let Some(scan) = get_lidar_scan(&sensor_data, &mut last_lidar_seq) {
|
||||
let (_result, corrected_pose, match_result) =
|
||||
map.observe_lidar_with_matching(&scan, current_pose, &matcher_config);
|
||||
// Record starting encoder values
|
||||
let (start_left, start_right) =
|
||||
get_encoder_values(&sensor_data).unwrap_or((prev_wheel_left, prev_wheel_right));
|
||||
|
||||
if match_result.converged && match_result.score >= 0.5 {
|
||||
current_pose = corrected_pose;
|
||||
log::trace!("Match accepted: score={:.3}", match_result.score);
|
||||
} else {
|
||||
log::debug!(
|
||||
"Match rejected: score={:.3}, converged={}",
|
||||
match_result.score,
|
||||
match_result.converged
|
||||
);
|
||||
// Send velocity command
|
||||
driver.send_command(set_velocity(linear, angular))?;
|
||||
|
||||
// Monitor until target achieved (with timeout)
|
||||
let timeout = Duration::from_secs(COMMAND_TIMEOUT_SECS);
|
||||
let cmd_start = Instant::now();
|
||||
|
||||
loop {
|
||||
// Update odometry
|
||||
compute_odometry(
|
||||
&sensor_data,
|
||||
&mut prev_wheel_left,
|
||||
&mut prev_wheel_right,
|
||||
&mut current_pose,
|
||||
ticks_per_meter,
|
||||
wheel_base,
|
||||
);
|
||||
|
||||
// Process lidar scan with matching
|
||||
if let Some(scan) = get_lidar_scan(&sensor_data, &mut last_lidar_seq) {
|
||||
let (_result, corrected_pose, match_result) =
|
||||
map.observe_lidar_with_matching(&scan, current_pose, &matcher_config);
|
||||
|
||||
if match_result.converged && match_result.score >= 0.5 {
|
||||
current_pose = corrected_pose;
|
||||
log::trace!("Match accepted: score={:.3}", match_result.score);
|
||||
}
|
||||
trajectory.push(current_pose);
|
||||
}
|
||||
trajectory.push(current_pose);
|
||||
|
||||
// Check encoder progress
|
||||
let (cur_left, cur_right) =
|
||||
get_encoder_values(&sensor_data).unwrap_or((start_left, start_right));
|
||||
|
||||
// Calculate accumulated ticks (handle wraparound with signed arithmetic)
|
||||
let delta_left = cur_left.wrapping_sub(start_left) as i16 as i32;
|
||||
let delta_right = cur_right.wrapping_sub(start_right) as i16 as i32;
|
||||
|
||||
// Check if target achieved (directional - stops before crossing)
|
||||
let left_done =
|
||||
wheel_reached_target(delta_left, expected_left_ticks, ENCODER_TOLERANCE_TICKS);
|
||||
let right_done =
|
||||
wheel_reached_target(delta_right, expected_right_ticks, ENCODER_TOLERANCE_TICKS);
|
||||
|
||||
if left_done && right_done {
|
||||
log::debug!(
|
||||
"Command {}/{}: TARGET ACHIEVED - L:{}/{} R:{}/{} ticks",
|
||||
i + 1,
|
||||
scenario.commands.len(),
|
||||
delta_left,
|
||||
expected_left_ticks,
|
||||
delta_right,
|
||||
expected_right_ticks
|
||||
);
|
||||
break;
|
||||
}
|
||||
|
||||
// Timeout safety
|
||||
if cmd_start.elapsed() > timeout {
|
||||
log::warn!(
|
||||
"Command {}/{}: TIMEOUT - expected L:{} R:{}, got L:{} R:{}",
|
||||
i + 1,
|
||||
scenario.commands.len(),
|
||||
expected_left_ticks,
|
||||
expected_right_ticks,
|
||||
delta_left,
|
||||
delta_right
|
||||
);
|
||||
break;
|
||||
}
|
||||
|
||||
// Short sleep for fast response to encoder changes
|
||||
std::thread::sleep(Duration::from_millis(2));
|
||||
}
|
||||
|
||||
std::thread::sleep(Duration::from_millis(20));
|
||||
// Stop motion after encoder-based command
|
||||
driver.send_command(set_velocity(0.0, 0.0))?;
|
||||
|
||||
// Grace period for robot to fully stop before next command
|
||||
std::thread::sleep(Duration::from_millis(COMMAND_GRACE_PERIOD_MS));
|
||||
} else {
|
||||
// Time-based execution for Move and Stay commands
|
||||
let duration_ms = cmd.duration_ms();
|
||||
let sim_duration_ms = (duration_ms as f32 / speed_factor) as u64;
|
||||
|
||||
log::debug!(
|
||||
"Command {}/{}: TIME-BASED - v={:.2} m/s, ω={:.2} rad/s, duration={}ms (sim: {}ms)",
|
||||
i + 1,
|
||||
scenario.commands.len(),
|
||||
linear,
|
||||
angular,
|
||||
duration_ms,
|
||||
sim_duration_ms
|
||||
);
|
||||
|
||||
// Send velocity command
|
||||
driver.send_command(set_velocity(linear, angular))?;
|
||||
|
||||
let cmd_start = Instant::now();
|
||||
while cmd_start.elapsed().as_millis() < sim_duration_ms as u128 {
|
||||
// Update odometry
|
||||
compute_odometry(
|
||||
&sensor_data,
|
||||
&mut prev_wheel_left,
|
||||
&mut prev_wheel_right,
|
||||
&mut current_pose,
|
||||
ticks_per_meter,
|
||||
wheel_base,
|
||||
);
|
||||
|
||||
// Process lidar scan with matching
|
||||
if let Some(scan) = get_lidar_scan(&sensor_data, &mut last_lidar_seq) {
|
||||
let (_result, corrected_pose, match_result) =
|
||||
map.observe_lidar_with_matching(&scan, current_pose, &matcher_config);
|
||||
|
||||
if match_result.converged && match_result.score >= 0.5 {
|
||||
current_pose = corrected_pose;
|
||||
log::trace!("Match accepted: score={:.3}", match_result.score);
|
||||
} else {
|
||||
log::debug!(
|
||||
"Match rejected: score={:.3}, converged={}",
|
||||
match_result.score,
|
||||
match_result.converged
|
||||
);
|
||||
}
|
||||
trajectory.push(current_pose);
|
||||
}
|
||||
|
||||
std::thread::sleep(Duration::from_millis(20));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
# Tests scan matching during pure rotation
|
||||
|
||||
name: "360 Rotation Test"
|
||||
description: "Rotate in place at 0.5 rad/s for full 360 degrees"
|
||||
description: "Rotate in place exactly 360 degrees CCW"
|
||||
|
||||
map: "medium_room"
|
||||
|
||||
@@ -11,11 +11,9 @@ seed:
|
||||
rotation: true
|
||||
|
||||
commands:
|
||||
# Full 360 rotation: 2*PI / 0.5 rad/s = ~12.57 seconds
|
||||
- type: move
|
||||
linear_velocity: 0.0
|
||||
angular_velocity: 0.5
|
||||
duration_ms: 12566
|
||||
# Full 360° CCW rotation in place
|
||||
- type: rotate
|
||||
degrees: 360
|
||||
|
||||
svg_output:
|
||||
scale: 50.0
|
||||
|
||||
@@ -2,13 +2,13 @@
|
||||
# Tests scan matching during combined linear and angular motion
|
||||
|
||||
name: "Square Path"
|
||||
description: "Navigate a 1m x 1m square path with 90-degree turns"
|
||||
description: "Navigate a 1m x 1m square path with exact 90-degree turns"
|
||||
|
||||
map: "medium_room"
|
||||
|
||||
start_pose:
|
||||
x: 4.0
|
||||
y: 4.0
|
||||
x: 3.5
|
||||
y: 3.5
|
||||
theta: 0.0
|
||||
|
||||
seed:
|
||||
@@ -16,77 +16,41 @@ seed:
|
||||
rotation: true
|
||||
|
||||
commands:
|
||||
# Side 1: Move forward 1m
|
||||
- type: move
|
||||
linear_velocity: 0.2
|
||||
angular_velocity: 0.0
|
||||
duration_ms: 5000
|
||||
# Side 1: Move forward exactly 1m
|
||||
- type: move_distance
|
||||
left_distance_m: 1.0
|
||||
right_distance_m: 1.0
|
||||
|
||||
# Pause
|
||||
- type: stay
|
||||
duration_ms: 500
|
||||
# Turn 90° CW in place
|
||||
- type: rotate
|
||||
degrees: -90
|
||||
|
||||
# Turn 90 degrees CW (PI/2 at 0.5 rad/s = ~3.14s)
|
||||
- type: move
|
||||
linear_velocity: 0.0
|
||||
angular_velocity: -0.5
|
||||
duration_ms: 3142
|
||||
# Side 2: Move forward exactly 1m
|
||||
- type: move_distance
|
||||
left_distance_m: 1.0
|
||||
right_distance_m: 1.0
|
||||
|
||||
# Pause
|
||||
- type: stay
|
||||
duration_ms: 500
|
||||
# Turn 90° CW in place
|
||||
- type: rotate
|
||||
degrees: -90
|
||||
|
||||
# Side 2: Move forward 1m
|
||||
- type: move
|
||||
linear_velocity: 0.2
|
||||
angular_velocity: 0.0
|
||||
duration_ms: 5000
|
||||
# Side 3: Move forward exactly 1m
|
||||
- type: move_distance
|
||||
left_distance_m: 1.0
|
||||
right_distance_m: 1.0
|
||||
|
||||
# Pause
|
||||
- type: stay
|
||||
duration_ms: 500
|
||||
# Turn 90° CW in place
|
||||
- type: rotate
|
||||
degrees: -90
|
||||
|
||||
# Turn 90 degrees CW
|
||||
- type: move
|
||||
linear_velocity: 0.0
|
||||
angular_velocity: -0.5
|
||||
duration_ms: 3142
|
||||
|
||||
# Pause
|
||||
- type: stay
|
||||
duration_ms: 500
|
||||
|
||||
# Side 3: Move forward 1m
|
||||
- type: move
|
||||
linear_velocity: 0.2
|
||||
angular_velocity: 0.0
|
||||
duration_ms: 5000
|
||||
|
||||
# Pause
|
||||
- type: stay
|
||||
duration_ms: 500
|
||||
|
||||
# Turn 90 degrees CW
|
||||
- type: move
|
||||
linear_velocity: 0.0
|
||||
angular_velocity: -0.5
|
||||
duration_ms: 3142
|
||||
|
||||
# Pause
|
||||
- type: stay
|
||||
duration_ms: 500
|
||||
|
||||
# Side 4: Move forward 1m (back to start)
|
||||
- type: move
|
||||
linear_velocity: 0.2
|
||||
angular_velocity: 0.0
|
||||
duration_ms: 5000
|
||||
# Side 4: Move forward exactly 1m (back to start)
|
||||
- type: move_distance
|
||||
left_distance_m: 1.0
|
||||
right_distance_m: 1.0
|
||||
|
||||
# Final turn to original heading
|
||||
- type: move
|
||||
linear_velocity: 0.0
|
||||
angular_velocity: -0.5
|
||||
duration_ms: 3142
|
||||
- type: rotate
|
||||
degrees: -90
|
||||
|
||||
svg_output:
|
||||
scale: 75.0
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
# Tests scan matching during forward motion
|
||||
|
||||
name: "Straight Line Test"
|
||||
description: "Move forward at 0.2 m/s for 10 seconds (2m total)"
|
||||
description: "Move forward exactly 2 meters using wheel distances"
|
||||
|
||||
map: "medium_room"
|
||||
|
||||
@@ -11,10 +11,10 @@ seed:
|
||||
rotation: true
|
||||
|
||||
commands:
|
||||
- type: move
|
||||
linear_velocity: 0.2
|
||||
angular_velocity: 0.0
|
||||
duration_ms: 10000
|
||||
# Forward 2m: both wheels travel equal distance
|
||||
- type: move_distance
|
||||
left_distance_m: 2.0
|
||||
right_distance_m: 2.0
|
||||
|
||||
svg_output:
|
||||
scale: 50.0
|
||||
|
||||
@@ -71,6 +71,15 @@ impl Default for SeedConfig {
|
||||
}
|
||||
}
|
||||
|
||||
/// Default maximum wheel velocity for auto-calculating duration (m/s)
|
||||
const DEFAULT_MAX_WHEEL_VELOCITY: f32 = 0.3;
|
||||
|
||||
/// Default wheel base for differential drive kinematics (meters)
|
||||
const DEFAULT_WHEEL_BASE: f32 = 0.233;
|
||||
|
||||
/// Default angular velocity for rotation commands (rad/s)
|
||||
const DEFAULT_ANGULAR_VELOCITY: f32 = 0.5;
|
||||
|
||||
/// A movement command
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
#[serde(tag = "type", rename_all = "snake_case")]
|
||||
@@ -84,6 +93,24 @@ pub enum Command {
|
||||
/// Duration in milliseconds
|
||||
duration_ms: u64,
|
||||
},
|
||||
/// Move by specifying wheel distances directly (more accurate for testing)
|
||||
MoveDistance {
|
||||
/// Left wheel distance in meters
|
||||
left_distance_m: f32,
|
||||
/// Right wheel distance in meters
|
||||
right_distance_m: f32,
|
||||
/// Optional duration in ms (auto-calculated if omitted based on max velocity)
|
||||
#[serde(default)]
|
||||
duration_ms: Option<u64>,
|
||||
},
|
||||
/// Rotate in place by a specified angle in degrees
|
||||
Rotate {
|
||||
/// Rotation angle in degrees (positive = CCW, negative = CW)
|
||||
degrees: f32,
|
||||
/// Optional duration in ms (auto-calculated if omitted based on angular velocity)
|
||||
#[serde(default)]
|
||||
duration_ms: Option<u64>,
|
||||
},
|
||||
/// Stay still for a duration
|
||||
Stay {
|
||||
/// Duration in milliseconds
|
||||
@@ -96,6 +123,26 @@ impl Command {
|
||||
pub fn duration_ms(&self) -> u64 {
|
||||
match self {
|
||||
Command::Move { duration_ms, .. } => *duration_ms,
|
||||
Command::MoveDistance {
|
||||
left_distance_m,
|
||||
right_distance_m,
|
||||
duration_ms,
|
||||
} => {
|
||||
// Use provided duration or auto-calculate from max wheel velocity
|
||||
duration_ms.unwrap_or_else(|| {
|
||||
let max_distance = left_distance_m.abs().max(right_distance_m.abs());
|
||||
let duration_secs = max_distance / DEFAULT_MAX_WHEEL_VELOCITY;
|
||||
(duration_secs * 1000.0) as u64
|
||||
})
|
||||
}
|
||||
Command::Rotate { degrees, duration_ms } => {
|
||||
// Use provided duration or auto-calculate from angular velocity
|
||||
duration_ms.unwrap_or_else(|| {
|
||||
let radians = degrees.to_radians();
|
||||
let duration_secs = radians.abs() / DEFAULT_ANGULAR_VELOCITY;
|
||||
(duration_secs * 1000.0) as u64
|
||||
})
|
||||
}
|
||||
Command::Stay { duration_ms } => *duration_ms,
|
||||
}
|
||||
}
|
||||
@@ -113,9 +160,76 @@ impl Command {
|
||||
angular_velocity,
|
||||
..
|
||||
} => (*linear_velocity, *angular_velocity),
|
||||
Command::MoveDistance {
|
||||
left_distance_m,
|
||||
right_distance_m,
|
||||
..
|
||||
} => {
|
||||
// Calculate velocities from wheel distances and duration
|
||||
let duration_secs = self.duration_secs();
|
||||
if duration_secs <= 0.0 {
|
||||
return (0.0, 0.0);
|
||||
}
|
||||
|
||||
// Wheel velocities
|
||||
let vel_left = left_distance_m / duration_secs;
|
||||
let vel_right = right_distance_m / duration_secs;
|
||||
|
||||
// Differential drive kinematics:
|
||||
// v = (v_r + v_l) / 2
|
||||
// ω = (v_r - v_l) / wheel_base
|
||||
let linear_velocity = (vel_left + vel_right) / 2.0;
|
||||
let angular_velocity = (vel_right - vel_left) / DEFAULT_WHEEL_BASE;
|
||||
|
||||
(linear_velocity, angular_velocity)
|
||||
}
|
||||
Command::Rotate { degrees, .. } => {
|
||||
// Pure rotation: no linear velocity
|
||||
let duration_secs = self.duration_secs();
|
||||
if duration_secs <= 0.0 {
|
||||
return (0.0, 0.0);
|
||||
}
|
||||
|
||||
let radians = degrees.to_radians();
|
||||
let angular_velocity = radians / duration_secs;
|
||||
|
||||
(0.0, angular_velocity)
|
||||
}
|
||||
Command::Stay { .. } => (0.0, 0.0),
|
||||
}
|
||||
}
|
||||
|
||||
/// Get wheel distances (left, right) in meters, if applicable
|
||||
pub fn wheel_distances(&self) -> Option<(f32, f32)> {
|
||||
match self {
|
||||
Command::MoveDistance {
|
||||
left_distance_m,
|
||||
right_distance_m,
|
||||
..
|
||||
} => Some((*left_distance_m, *right_distance_m)),
|
||||
Command::Rotate { degrees, .. } => {
|
||||
// Calculate wheel arc lengths for rotation
|
||||
// Arc length = angle × (wheel_base / 2)
|
||||
// For CW rotation (negative degrees): left forward (+), right backward (-)
|
||||
// For CCW rotation (positive degrees): left backward (-), right forward (+)
|
||||
let radians = degrees.to_radians();
|
||||
let arc_length = radians * (DEFAULT_WHEEL_BASE / 2.0);
|
||||
// arc_length is negative for CW (negative degrees)
|
||||
// -arc_length flips to positive for left wheel (forward)
|
||||
// arc_length stays negative for right wheel (backward)
|
||||
Some((-arc_length, arc_length))
|
||||
}
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
/// Get rotation angle in degrees, if applicable
|
||||
pub fn rotation_degrees(&self) -> Option<f32> {
|
||||
match self {
|
||||
Command::Rotate { degrees, .. } => Some(*degrees),
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// SVG output configuration
|
||||
@@ -310,4 +424,194 @@ commands: []
|
||||
let scenario = Scenario::from_yaml(yaml).unwrap();
|
||||
assert_eq!(scenario.svg_filename(), "my_test_scenario.svg");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_move_distance_parsing() {
|
||||
let yaml = r#"
|
||||
name: "Distance Test"
|
||||
map: "room"
|
||||
commands:
|
||||
- type: move_distance
|
||||
left_distance_m: 0.5
|
||||
right_distance_m: 0.5
|
||||
- type: move_distance
|
||||
left_distance_m: 1.0
|
||||
right_distance_m: 1.0
|
||||
duration_ms: 5000
|
||||
"#;
|
||||
let scenario = Scenario::from_yaml(yaml).unwrap();
|
||||
assert_eq!(scenario.commands.len(), 2);
|
||||
|
||||
// First command: auto-calculated duration (0.5m / 0.3m/s = 1.667s)
|
||||
let cmd1 = &scenario.commands[0];
|
||||
assert_eq!(cmd1.wheel_distances(), Some((0.5, 0.5)));
|
||||
let duration_ms = cmd1.duration_ms();
|
||||
assert!(duration_ms > 1600 && duration_ms < 1700, "Expected ~1667ms, got {}", duration_ms);
|
||||
|
||||
// Second command: explicit duration
|
||||
let cmd2 = &scenario.commands[1];
|
||||
assert_eq!(cmd2.duration_ms(), 5000);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_move_distance_velocities_forward() {
|
||||
// Straight forward: equal wheel distances
|
||||
let cmd = Command::MoveDistance {
|
||||
left_distance_m: 0.6,
|
||||
right_distance_m: 0.6,
|
||||
duration_ms: Some(2000), // 2 seconds
|
||||
};
|
||||
|
||||
let (linear, angular) = cmd.velocities();
|
||||
// vel_left = vel_right = 0.6m / 2s = 0.3 m/s
|
||||
// linear = (0.3 + 0.3) / 2 = 0.3 m/s
|
||||
// angular = (0.3 - 0.3) / 0.233 = 0 rad/s
|
||||
assert!((linear - 0.3).abs() < 1e-6, "Expected linear=0.3, got {}", linear);
|
||||
assert!(angular.abs() < 1e-6, "Expected angular=0, got {}", angular);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_move_distance_velocities_rotation() {
|
||||
// Rotation in place: opposite wheel distances
|
||||
let cmd = Command::MoveDistance {
|
||||
left_distance_m: -0.1165, // half wheel base
|
||||
right_distance_m: 0.1165,
|
||||
duration_ms: Some(1000), // 1 second
|
||||
};
|
||||
|
||||
let (linear, angular) = cmd.velocities();
|
||||
// vel_left = -0.1165 m/s, vel_right = 0.1165 m/s
|
||||
// linear = (-0.1165 + 0.1165) / 2 = 0 m/s
|
||||
// angular = (0.1165 - (-0.1165)) / 0.233 = 0.233 / 0.233 = 1 rad/s
|
||||
assert!(linear.abs() < 1e-6, "Expected linear=0, got {}", linear);
|
||||
assert!((angular - 1.0).abs() < 0.01, "Expected angular=1.0, got {}", angular);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_move_distance_auto_duration() {
|
||||
// Auto-calculated duration based on max wheel velocity (0.3 m/s)
|
||||
let cmd = Command::MoveDistance {
|
||||
left_distance_m: 0.9,
|
||||
right_distance_m: 0.6,
|
||||
duration_ms: None,
|
||||
};
|
||||
|
||||
// Duration based on max distance: 0.9m / 0.3m/s = 3s = 3000ms
|
||||
// Allow for floating point rounding (2999 or 3000)
|
||||
let duration = cmd.duration_ms();
|
||||
assert!(
|
||||
(2999..=3001).contains(&duration),
|
||||
"Expected ~3000ms, got {}",
|
||||
duration
|
||||
);
|
||||
|
||||
// Velocities with ~3s duration
|
||||
let (linear, angular) = cmd.velocities();
|
||||
// vel_left = 0.9/3 = 0.3, vel_right = 0.6/3 = 0.2
|
||||
// linear = (0.3 + 0.2) / 2 = 0.25
|
||||
// angular = (0.2 - 0.3) / 0.233 ≈ -0.429
|
||||
assert!((linear - 0.25).abs() < 0.01);
|
||||
assert!((angular + 0.429).abs() < 0.02);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_rotate_parsing() {
|
||||
let yaml = r#"
|
||||
name: "Rotate Test"
|
||||
map: "room"
|
||||
commands:
|
||||
- type: rotate
|
||||
degrees: 90
|
||||
- type: rotate
|
||||
degrees: -180
|
||||
duration_ms: 3000
|
||||
"#;
|
||||
let scenario = Scenario::from_yaml(yaml).unwrap();
|
||||
assert_eq!(scenario.commands.len(), 2);
|
||||
|
||||
// First command: 90° CCW, auto-calculated duration
|
||||
let cmd1 = &scenario.commands[0];
|
||||
assert_eq!(cmd1.rotation_degrees(), Some(90.0));
|
||||
|
||||
// Second command: 180° CW with explicit duration
|
||||
let cmd2 = &scenario.commands[1];
|
||||
assert_eq!(cmd2.rotation_degrees(), Some(-180.0));
|
||||
assert_eq!(cmd2.duration_ms(), 3000);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_rotate_velocities_ccw() {
|
||||
// 90° CCW rotation with explicit duration
|
||||
let cmd = Command::Rotate {
|
||||
degrees: 90.0,
|
||||
duration_ms: Some(1000), // 1 second
|
||||
};
|
||||
|
||||
let (linear, angular) = cmd.velocities();
|
||||
// linear = 0 (pure rotation)
|
||||
// angular = π/2 / 1s ≈ 1.571 rad/s
|
||||
assert!(linear.abs() < 1e-6, "Expected linear=0, got {}", linear);
|
||||
assert!(
|
||||
(angular - std::f32::consts::FRAC_PI_2).abs() < 0.01,
|
||||
"Expected angular≈1.571, got {}",
|
||||
angular
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_rotate_velocities_cw() {
|
||||
// 90° CW rotation (negative degrees)
|
||||
let cmd = Command::Rotate {
|
||||
degrees: -90.0,
|
||||
duration_ms: Some(1000),
|
||||
};
|
||||
|
||||
let (linear, angular) = cmd.velocities();
|
||||
assert!(linear.abs() < 1e-6);
|
||||
assert!(
|
||||
(angular + std::f32::consts::FRAC_PI_2).abs() < 0.01,
|
||||
"Expected angular≈-1.571, got {}",
|
||||
angular
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_rotate_auto_duration() {
|
||||
// 360° rotation with default angular velocity (0.5 rad/s)
|
||||
let cmd = Command::Rotate {
|
||||
degrees: 360.0,
|
||||
duration_ms: None,
|
||||
};
|
||||
|
||||
// Duration = 2π / 0.5 rad/s ≈ 12.566s ≈ 12566ms
|
||||
let duration = cmd.duration_ms();
|
||||
assert!(
|
||||
(12500..=12600).contains(&duration),
|
||||
"Expected ~12566ms, got {}",
|
||||
duration
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_rotate_wheel_distances() {
|
||||
// 360° CCW rotation
|
||||
let cmd = Command::Rotate {
|
||||
degrees: 360.0,
|
||||
duration_ms: None,
|
||||
};
|
||||
|
||||
let (left, right) = cmd.wheel_distances().unwrap();
|
||||
// Arc length = 2π × (0.233/2) ≈ 0.732m
|
||||
// CCW: left backward (-), right forward (+)
|
||||
assert!(
|
||||
(left + 0.732).abs() < 0.01,
|
||||
"Expected left≈-0.732, got {}",
|
||||
left
|
||||
);
|
||||
assert!(
|
||||
(right - 0.732).abs() < 0.01,
|
||||
"Expected right≈0.732, got {}",
|
||||
right
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user