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 grid-based path planning algorithms
13pub trait GridPathPlanner {
14    /// Plan a path on a grid from start to goal
15    fn plan(&self, start: GridNode, goal: GridNode) -> Result<Path2D, RoboticsError>;
16
17    /// Set obstacles for the planner
18    fn set_obstacles(&mut self, obstacles: &Obstacles);
19}
20
21/// Trait for sampling-based path planning algorithms (RRT, PRM, etc.)
22pub trait SamplingBasedPlanner: PathPlanner {
23    /// Get the tree/graph built during planning
24    fn get_tree(&self) -> &[(Point2D, Option<usize>)];
25
26    /// Set maximum iterations for planning
27    fn set_max_iterations(&mut self, max_iter: usize);
28}
29
30/// Trait for state estimation algorithms (EKF, UKF, Particle Filter, etc.)
31pub trait StateEstimator {
32    /// State type used by this estimator
33    type State;
34    /// Measurement type used by this estimator
35    type Measurement;
36    /// Control input type
37    type Control;
38
39    /// Prediction step
40    fn predict(&mut self, control: &Self::Control, dt: f64);
41
42    /// Update step with measurement
43    fn update(&mut self, measurement: &Self::Measurement);
44
45    /// Get current state estimate
46    fn get_state(&self) -> &Self::State;
47
48    /// Get current covariance estimate (if applicable)
49    fn get_covariance(&self) -> Option<&nalgebra::DMatrix<f64>> {
50        None
51    }
52}
53
54/// Simplified state estimator for 2D localization
55pub trait Estimator2D {
56    /// Prediction step
57    fn predict(&mut self, control: &ControlInput, dt: f64);
58
59    /// Update step with position measurement
60    fn update(&mut self, measurement: Point2D);
61
62    /// Get current state estimate
63    fn get_state(&self) -> State2D;
64}
65
66/// Trait for path tracking/following algorithms
67pub trait PathTracker {
68    /// Compute control input to follow the path
69    fn compute_control(&mut self, current_state: &State2D, path: &Path2D) -> ControlInput;
70
71    /// Check if the goal has been reached
72    fn is_goal_reached(&self, current_state: &State2D, goal: Point2D) -> bool;
73}
74
75/// Trait for trajectory tracking with time-parameterized paths
76pub trait TrajectoryTracker {
77    /// Trajectory point with time
78    type TrajectoryPoint;
79
80    /// Compute control input to follow the trajectory at given time
81    fn compute_control(&mut self, current_state: &State2D, time: f64) -> ControlInput;
82}
83
84/// Trait for vehicle/robot motion models
85pub trait MotionModel {
86    /// State type
87    type State;
88    /// Control type
89    type Control;
90
91    /// Propagate state forward in time
92    fn propagate(&self, state: &Self::State, control: &Self::Control, dt: f64) -> Self::State;
93
94    /// Compute Jacobian with respect to state (for EKF)
95    fn jacobian_state(
96        &self,
97        state: &Self::State,
98        control: &Self::Control,
99        dt: f64,
100    ) -> nalgebra::DMatrix<f64>;
101}
102
103/// Trait for observation/measurement models
104pub trait ObservationModel {
105    /// State type
106    type State;
107    /// Measurement type
108    type Measurement;
109
110    /// Predict measurement from state
111    fn predict(&self, state: &Self::State) -> Self::Measurement;
112
113    /// Compute Jacobian with respect to state (for EKF)
114    fn jacobian(&self, state: &Self::State) -> nalgebra::DMatrix<f64>;
115}
116
117/// Trait for controllers (PID, LQR, MPC, etc.)
118pub trait Controller {
119    /// State type
120    type State;
121    /// Reference/target type
122    type Reference;
123    /// Output control type
124    type Output;
125
126    /// Compute control output
127    fn compute(&mut self, state: &Self::State, reference: &Self::Reference) -> Self::Output;
128
129    /// Reset controller state
130    fn reset(&mut self);
131}
132
133#[cfg(test)]
134mod tests {
135    use super::*;
136
137    // Test that traits compile correctly
138    struct DummyPlanner;
139
140    impl PathPlanner for DummyPlanner {
141        fn plan(&self, _start: Point2D, _goal: Point2D) -> Result<Path2D, RoboticsError> {
142            Ok(Path2D::new())
143        }
144    }
145
146    #[test]
147    fn test_path_planner_trait() {
148        let planner = DummyPlanner;
149        let result = planner.plan(Point2D::origin(), Point2D::new(1.0, 1.0));
150        assert!(result.is_ok());
151    }
152}
rust_robotics_core/traits.rs

rust_robotics_core/
traits.rs
