Function obstacle_stop 

pub fn obstacle_stop(twist: Twist, range_m: f32, stop_distance_m: f32) -> Twist

Stops forward motion when an obstacle is within the stopping distance, leaving turning free.

This is the simplest reliable safety reflex for a robot with a forward range sensor: hold the requested rotation so the robot can still turn away, but cut vx and vy to zero once the nearest reading falls inside the stop distance, so it does not drive into what it sees.

Arguments

• twist - the requested body motion.
• range_m - the nearest measured range ahead, in meters.
• stop_distance_m - the range at or below which forward motion is cut; its magnitude is used.

Returns

The original twist when the way is clear, or one with no translation (rotation preserved) when an obstacle is within range. A range that is not a number, which a sensor that saw nothing it could measure can report, counts as an obstacle: the way is clear only when a reading says so.

Examples

use pamoja_kit::{obstacle_stop, Twist};

let driving = Twist::new(1.0, 0.0, 0.5);
// Clear ahead: unchanged.
assert_eq!(obstacle_stop(driving, 2.0, 0.5), driving);
// Obstacle at 0.3 m: forward cut, turn kept so it can escape.
assert_eq!(obstacle_stop(driving, 0.3, 0.5), Twist::new(0.0, 0.0, 0.5));