path_planning/obstacles/
mod.rs

1/* Copyright (C) 2020 Dylan Staatz - All Rights Reserved. */
2
3/// Base Obstacle parameters
4mod params;
5
6/// 2d 32-bit obstacle implementations
7pub mod obstacles_2d_f32;
8/// 2d 64-bit obstacle implementations
9pub mod obstacles_2d_f64;
10/// 3d 32-bit obstacle implementations
11pub mod obstacles_3d_f32;
12/// 3d 64-bit obstacle implementations
13pub mod obstacles_3d_f64;
14
15////////////////////////////////////////////////////////////////////////////////
16
17use nalgebra::storage::Storage;
18use nalgebra::{Const, Vector};
19use serde::{Deserialize, Serialize};
20
21use crate::cspace::CSpace;
22use crate::params::FromParams;
23use crate::trajectories::FullTrajectory;
24
25/// A unique identifer for obstacles in the environment
26pub type ObstacleId = usize;
27
28/// An obstacle can either alway present(Static) or appear once within range (Appearing)
29#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
30pub enum Behavior {
31  Static,
32  Appearing,
33}
34
35impl Default for Behavior {
36  fn default() -> Behavior {
37    Behavior::Static
38  }
39}
40
41/// A generic representation of an obstacle space in an environment
42pub trait ObstacleSpace<X, CS, const N: usize>:
43  FromParams + Clone + Obstacle<X, CS, N>
44where
45  CS: CSpace<X, N>,
46{
47  /// The type of obstacles in the space
48  type Obs: AppearingObstacle<X, CS, N>;
49
50  /// Build a new obstacle space with no obstacles in it
51  fn empty() -> Self;
52
53  /// Build a new obstacle space with the given obstacles
54  fn new(obstacles: Vec<Self::Obs>) -> Self {
55    let mut new_ = Self::empty();
56    new_.add_obstacles(obstacles.into_iter().enumerate());
57    new_
58  }
59
60  /// Add an obstacle to the environement
61  ///
62  /// Overwrites existing obstacle if `id` has already been added
63  fn add_obstacle(&mut self, id: ObstacleId, obstacle: Self::Obs) {
64    self.add_obstacles(vec![(id, obstacle)])
65  }
66
67  /// Add multiple obstacles to the environment
68  ///
69  /// Overwrites existing obstacle if `id` has already been added
70  fn add_obstacles<I>(&mut self, obstacles: I)
71  where
72    I: IntoIterator<Item = (ObstacleId, Self::Obs)>;
73
74  /// Optionally returns the specified obstacle id
75  fn get_obstacle(&self, id: ObstacleId) -> Option<&Self::Obs> {
76    self.get_obstacles(&[id; 1]).into_iter().next()
77  }
78
79  /// Gets references to the given obstacle id's
80  ///
81  /// Doesn't return obstacles that don't exist
82  fn get_obstacles(&self, obstacles: &[ObstacleId]) -> Vec<&Self::Obs>;
83
84  /// Gets a newly built obstacle_space only containing the given obstacle id's
85  fn get_obstacles_as_obstacle_space(&self, obstacles: &[ObstacleId]) -> Self {
86    Self::new(self.get_obstacles(obstacles).into_iter().cloned().collect())
87  }
88
89  /// Copies out all yet sensed obstacles in the environement
90  fn get_sensed_obstacles(&self) -> Vec<(ObstacleId, &Self::Obs)>;
91
92  /// Copies out all not yet sensed obstacles in the environement
93  fn get_not_sensed_obstacles(&self) -> Vec<(ObstacleId, &Self::Obs)>;
94
95  /// Get vector of all valid obstacle ids
96  fn get_all_obstacle_ids(&self) -> Vec<ObstacleId>;
97
98  /// Copies out all obstacles with their respective ids
99  fn get_all_obstacles(&self) -> Vec<(ObstacleId, &Self::Obs)> {
100    self
101      .get_all_obstacle_ids()
102      .into_iter()
103      .filter_map(|obs_id| self.get_obstacle(obs_id).map(|obs| (obs_id, obs)))
104      .collect()
105  }
106
107  /// Remove the specified obstacle from the environment
108  fn remove_obstacle(&mut self, id: ObstacleId) {
109    self.remove_obstacles(&[id; 1])
110  }
111
112  /// Removes the specified obstacle id's if they have been added previously
113  fn remove_obstacles(&mut self, obstacles: &[ObstacleId]);
114
115  /// The total number of obstacles, regardless of sensing
116  fn count(&self) -> usize;
117
118  /// Updates the internal obsacle to include obstacles that can be sensed at the given `pose` inside the `radius`
119  ///
120  /// Returns a list of any added obstacles
121  fn check_sensors<S>(
122    &mut self,
123    pose: &Vector<X, Const<N>, S>,
124    radius: X,
125  ) -> Vec<ObstacleId>
126  where
127    S: Storage<X, Const<N>>;
128}
129
130/// An generic obstacle in an Rrt-like environment
131pub trait Obstacle<X, CS, const N: usize>: FromParams + Clone
132where
133  CS: CSpace<X, N>,
134{
135  /// Checks that the obstacle does not intersect the given trajectory
136  fn trajectory_free<FT, S1, S2>(&self, t: &FT) -> bool
137  where
138    FT: FullTrajectory<X, CS::Traj, S1, S2, N>,
139    S1: Storage<X, Const<N>>,
140    S2: Storage<X, Const<N>>;
141
142  /// Checks that the coordinate is not in the obstacle
143  fn is_free<S>(&self, x: &Vector<X, Const<N>, S>) -> bool
144  where
145    S: Storage<X, Const<N>>;
146}
147
148/// An obstacle type that can be sensed within a radius
149pub trait AppearingObstacle<X, CS, const N: usize>: Obstacle<X, CS, N>
150where
151  CS: CSpace<X, N>,
152{
153  /// Checks if self is within radius of pose
154  fn can_sense<S>(&self, pose: &Vector<X, Const<N>, S>, radius: X) -> bool
155  where
156    S: Storage<X, Const<N>>;
157}
path_planning/obstacles/mod.rs

path_planning/obstacles/
mod.rs
