Skip to main content

rust_robotics_core/
traits.rs

1//! Common traits defining interfaces for robotics algorithms
2
3use crate::error::RoboticsError;
4use crate::types::*;
5
6/// Trait for path planning algorithms
7pub trait PathPlanner {
8    /// Plan a path from start to goal
9    fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError>;
10}
11
12/// Trait for state estimation algorithms (EKF, UKF, Particle Filter, etc.)
13pub trait StateEstimator {
14    /// State type used by this estimator
15    type State;
16    /// Measurement type used by this estimator
17    type Measurement;
18    /// Control input type
19    type Control;
20
21    /// Prediction step
22    fn predict(&mut self, control: &Self::Control, dt: f64);
23
24    /// Update step with measurement
25    fn update(&mut self, measurement: &Self::Measurement);
26
27    /// Get current state estimate
28    fn get_state(&self) -> &Self::State;
29
30    /// Get current covariance estimate (if applicable)
31    fn get_covariance(&self) -> Option<&nalgebra::DMatrix<f64>> {
32        None
33    }
34}
35
36/// Trait for path tracking/following algorithms
37pub trait PathTracker {
38    /// Compute control input to follow the path
39    fn compute_control(&mut self, current_state: &State2D, path: &Path2D) -> ControlInput;
40
41    /// Check if the goal has been reached
42    fn is_goal_reached(&self, current_state: &State2D, goal: Point2D) -> bool;
43}
44
45/// Trait for vehicle/robot motion models
46pub trait MotionModel {
47    /// State type
48    type State;
49    /// Control type
50    type Control;
51
52    /// Propagate state forward in time
53    fn propagate(&self, state: &Self::State, control: &Self::Control, dt: f64) -> Self::State;
54
55    /// Compute Jacobian with respect to state (for EKF)
56    fn jacobian_state(
57        &self,
58        state: &Self::State,
59        control: &Self::Control,
60        dt: f64,
61    ) -> nalgebra::DMatrix<f64>;
62}
63
64/// Trait for observation/measurement models
65pub trait ObservationModel {
66    /// State type
67    type State;
68    /// Measurement type
69    type Measurement;
70
71    /// Predict measurement from state
72    fn predict(&self, state: &Self::State) -> Self::Measurement;
73
74    /// Compute Jacobian with respect to state (for EKF)
75    fn jacobian(&self, state: &Self::State) -> nalgebra::DMatrix<f64>;
76}
77
78/// Trait for controllers (PID, LQR, MPC, etc.)
79pub trait Controller {
80    /// State type
81    type State;
82    /// Reference/target type
83    type Reference;
84    /// Output control type
85    type Output;
86
87    /// Compute control output
88    fn compute(&mut self, state: &Self::State, reference: &Self::Reference) -> Self::Output;
89
90    /// Reset controller state
91    fn reset(&mut self);
92}
93
94#[cfg(test)]
95mod tests {
96    use super::*;
97
98    // Test that traits compile correctly
99    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}