pamoja_sim/
robot.rs

1//! A hardware-free mobile robot you can drive and watch move.
2
3use pamoja_core::{Actuator, Result, Sensor};
4use pamoja_kit::{Odometry, Pose, Twist};
5
6/// A simulated differential-drive robot: drive it with a [`Twist`], read back its [`Pose`].
7///
8/// This stands in for a real rover in a hardware-free test or demo. It is both an
9/// [`Actuator`] whose command is a body twist and a [`Sensor`] whose reading is the pose:
10/// each command advances the robot one time step at the commanded velocity, integrating the
11/// motion with the same exact-arc odometry a real robot would use, and a read returns where it
12/// has reached. A control loop can therefore be developed and tested end to end with no robot.
13///
14/// # Examples
15///
16/// ```
17/// use pamoja_core::{Actuator, Sensor};
18/// use pamoja_kit::Twist;
19/// use pamoja_sim::SimRobot;
20///
21/// # async fn demo() -> pamoja_core::Result<()> {
22/// let mut robot = SimRobot::new(0.1); // 0.1 s per command
23/// // Drive straight at 1 m/s for ten steps: about one metre forward.
24/// for _ in 0..10 {
25///     robot.apply(Twist::planar(1.0, 0.0)).await?;
26/// }
27/// let pose = robot.read().await?;
28/// assert!((pose.x - 1.0).abs() < 1e-5 && pose.y.abs() < 1e-5);
29/// # Ok(())
30/// # }
31/// ```
32#[derive(Clone, Copy, Debug)]
33pub struct SimRobot {
34    odometry: Odometry,
35    dt: f32,
36}
37
38impl SimRobot {
39    /// Creates a robot at the origin that advances `dt` seconds per command.
40    ///
41    /// # Arguments
42    ///
43    /// * `dt` - the time each [`apply`](SimRobot::apply) advances the robot; its magnitude is used.
44    ///
45    /// # Returns
46    ///
47    /// The simulated robot.
48    pub fn new(dt: f32) -> Self {
49        Self {
50            odometry: Odometry::at_origin(),
51            dt: dt.abs(),
52        }
53    }
54
55    /// Creates a robot starting from a known pose.
56    ///
57    /// # Arguments
58    ///
59    /// * `pose` - the starting pose.
60    /// * `dt` - the time each command advances the robot; its magnitude is used.
61    ///
62    /// # Returns
63    ///
64    /// The simulated robot.
65    pub fn starting_at(pose: Pose, dt: f32) -> Self {
66        Self {
67            odometry: Odometry::new(pose),
68            dt: dt.abs(),
69        }
70    }
71
72    /// Returns the robot's current pose.
73    ///
74    /// # Returns
75    ///
76    /// The pose reached so far.
77    pub fn pose(&self) -> Pose {
78        self.odometry.pose()
79    }
80}
81
82impl Actuator for SimRobot {
83    type Command = Twist;
84
85    async fn apply(&mut self, command: Twist) -> Result<()> {
86        self.odometry.integrate(command.vx, command.omega, self.dt);
87        Ok(())
88    }
89}
90
91impl Sensor for SimRobot {
92    type Reading = Pose;
93
94    async fn read(&mut self) -> Result<Pose> {
95        Ok(self.odometry.pose())
96    }
97}
98
99#[cfg(test)]
100mod tests {
101    use super::*;
102    use core::f32::consts::FRAC_PI_2;
103
104    #[tokio::test]
105    async fn driving_straight_advances_along_x() {
106        let mut robot = SimRobot::new(0.1);
107        for _ in 0..10 {
108            robot.apply(Twist::planar(1.0, 0.0)).await.unwrap();
109        }
110        let pose = robot.read().await.unwrap();
111        assert!((pose.x - 1.0).abs() < 1e-5);
112        assert!(pose.y.abs() < 1e-5);
113    }
114
115    #[tokio::test]
116    async fn turning_in_place_changes_only_the_heading() {
117        let mut robot = SimRobot::new(0.5);
118        // Spin at 1 rad/s for two half-second steps: about one radian, no translation.
119        robot.apply(Twist::planar(0.0, 1.0)).await.unwrap();
120        robot.apply(Twist::planar(0.0, 1.0)).await.unwrap();
121        let pose = robot.read().await.unwrap();
122        assert!(pose.x.abs() < 1e-6 && pose.y.abs() < 1e-6);
123        assert!((pose.theta - 1.0).abs() < 1e-6);
124    }
125
126    #[tokio::test]
127    async fn a_quarter_circle_lands_at_the_arc_corner() {
128        let mut robot = SimRobot::new(FRAC_PI_2);
129        // One step of forward 1 m/s and 1 rad/s over pi/2 s traces a quarter circle of radius 1.
130        robot.apply(Twist::planar(1.0, 1.0)).await.unwrap();
131        let pose = robot.read().await.unwrap();
132        assert!((pose.x - 1.0).abs() < 1e-5 && (pose.y - 1.0).abs() < 1e-5);
133    }
134}
pamoja_sim/robot.rs

pamoja_sim/
robot.rs
