1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
//! Shared kinematics helpers for wheeled-robot odometry.
//!
//! `motor_fake` (the simulated motor driver) and `motor` (the real motor
//! node) both integrate a simple differential-drive pose update from a
//! commanded linear/angular velocity each odometry tick. This module factors
//! that shared math out so both crates call the same function instead of
//! carrying two copies of the same integration + angle-normalization logic.
use std::f32::consts::PI;
/// A 2D robot pose: position plus heading (radians).
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Pose2D {
pub x: f32,
pub y: f32,
pub theta: f32,
}
/// Integrate a differential-drive pose forward by `dt` seconds given a
/// commanded linear and angular velocity, normalizing the resulting heading
/// to `[-PI, PI]`.
pub fn integrate_differential_drive(pose: Pose2D, linear: f32, angular: f32, dt: f32) -> Pose2D {
let delta_theta = angular * dt;
let delta_x = linear * dt * pose.theta.cos();
let delta_y = linear * dt * pose.theta.sin();
let mut theta = pose.theta + delta_theta;
while theta > PI {
theta -= 2.0 * PI;
}
while theta < -PI {
theta += 2.0 * PI;
}
Pose2D {
x: pose.x + delta_x,
y: pose.y + delta_y,
theta,
}
}
/// Shared "tick the odometry forward" behavior for anything that tracks a 2D
/// differential-drive pose by dead-reckoning a commanded linear/angular
/// velocity each tick (`motor_fake` and `motor` both do exactly this).
///
/// Implementers only need to expose their pose/command/rate fields; the
/// default [`Self::update_odometry`] method does the actual integration via
/// [`integrate_differential_drive`], so the dead-reckoning logic itself
/// lives in exactly one place instead of being copied per node/driver.
pub trait DifferentialDriveOdometry {
/// Current tracked pose.
fn odom_pose(&self) -> Pose2D;
/// Store a newly-integrated pose.
fn set_odom_pose(&mut self, pose: Pose2D);
/// Currently commanded (linear, angular) velocity.
fn commanded_velocity(&self) -> (f32, f32);
/// Odometry tick rate in Hz (used to derive `dt`).
fn odom_rate_hz(&self) -> f32;
/// Integrate one odometry tick forward and store the result.
fn update_odometry(&mut self) {
let dt = 1.0 / self.odom_rate_hz();
let (linear, angular) = self.commanded_velocity();
let pose = integrate_differential_drive(self.odom_pose(), linear, angular, dt);
self.set_odom_pose(pose);
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn integrates_straight_line_motion() {
let pose = Pose2D {
x: 0.0,
y: 0.0,
theta: 0.0,
};
let next = integrate_differential_drive(pose, 1.0, 0.0, 1.0);
assert!((next.x - 1.0).abs() < 1e-6);
assert!((next.y - 0.0).abs() < 1e-6);
assert!((next.theta - 0.0).abs() < 1e-6);
}
#[test]
fn normalizes_theta_above_pi() {
let pose = Pose2D {
x: 0.0,
y: 0.0,
theta: PI - 0.1,
};
let next = integrate_differential_drive(pose, 0.0, 1.0, 1.0);
assert!(next.theta <= PI && next.theta >= -PI);
}
#[test]
fn normalizes_theta_below_neg_pi() {
let pose = Pose2D {
x: 0.0,
y: 0.0,
theta: -PI + 0.1,
};
let next = integrate_differential_drive(pose, 0.0, -1.0, 1.0);
assert!(next.theta <= PI && next.theta >= -PI);
}
struct FakeOdom {
pose: Pose2D,
linear: f32,
angular: f32,
rate_hz: f32,
}
impl DifferentialDriveOdometry for FakeOdom {
fn odom_pose(&self) -> Pose2D {
self.pose
}
fn set_odom_pose(&mut self, pose: Pose2D) {
self.pose = pose;
}
fn commanded_velocity(&self) -> (f32, f32) {
(self.linear, self.angular)
}
fn odom_rate_hz(&self) -> f32 {
self.rate_hz
}
}
#[test]
fn default_update_odometry_matches_manual_integration() {
let mut odom = FakeOdom {
pose: Pose2D {
x: 0.0,
y: 0.0,
theta: 0.0,
},
linear: 2.0,
angular: 0.0,
rate_hz: 10.0,
};
odom.update_odometry();
let expected = integrate_differential_drive(
Pose2D {
x: 0.0,
y: 0.0,
theta: 0.0,
},
2.0,
0.0,
0.1,
);
assert_eq!(odom.pose, expected);
}
}