rust_robotics_core/
traits.rs1use crate::error::RoboticsError;
4use crate::types::*;
5
6pub trait PathPlanner {
8 fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError>;
10}
11
12pub trait StateEstimator {
14 type State;
16 type Measurement;
18 type Control;
20
21 fn predict(&mut self, control: &Self::Control, dt: f64);
23
24 fn update(&mut self, measurement: &Self::Measurement);
26
27 fn get_state(&self) -> &Self::State;
29
30 fn get_covariance(&self) -> Option<&nalgebra::DMatrix<f64>> {
32 None
33 }
34}
35
36pub trait PathTracker {
38 fn compute_control(&mut self, current_state: &State2D, path: &Path2D) -> ControlInput;
40
41 fn is_goal_reached(&self, current_state: &State2D, goal: Point2D) -> bool;
43}
44
45pub trait MotionModel {
47 type State;
49 type Control;
51
52 fn propagate(&self, state: &Self::State, control: &Self::Control, dt: f64) -> Self::State;
54
55 fn jacobian_state(
57 &self,
58 state: &Self::State,
59 control: &Self::Control,
60 dt: f64,
61 ) -> nalgebra::DMatrix<f64>;
62}
63
64pub trait ObservationModel {
66 type State;
68 type Measurement;
70
71 fn predict(&self, state: &Self::State) -> Self::Measurement;
73
74 fn jacobian(&self, state: &Self::State) -> nalgebra::DMatrix<f64>;
76}
77
78pub trait Controller {
80 type State;
82 type Reference;
84 type Output;
86
87 fn compute(&mut self, state: &Self::State, reference: &Self::Reference) -> Self::Output;
89
90 fn reset(&mut self);
92}
93
94#[cfg(test)]
95mod tests {
96 use super::*;
97
98 struct DummyPlanner;
100
101 impl PathPlanner for DummyPlanner {
102 fn plan(&self, _start: Point2D, _goal: Point2D) -> Result<Path2D, RoboticsError> {
103 Ok(Path2D::new())
104 }
105 }
106
107 #[test]
108 fn test_path_planner_trait() {
109 let planner = DummyPlanner;
110 let result = planner.plan(Point2D::origin(), Point2D::new(1.0, 1.0));
111 assert!(result.is_ok());
112 }
113}