path-planning 0.1.0

/* Copyright (C) 2020 Dylan Staatz - All Rights Reserved. */

/// Base Obstacle parameters
mod params;

/// 2d 32-bit obstacle implementations
pub mod obstacles_2d_f32;
/// 2d 64-bit obstacle implementations
pub mod obstacles_2d_f64;
/// 3d 32-bit obstacle implementations
pub mod obstacles_3d_f32;
/// 3d 64-bit obstacle implementations
pub mod obstacles_3d_f64;

////////////////////////////////////////////////////////////////////////////////

use nalgebra::storage::Storage;
use nalgebra::{Const, Vector};
use serde::{Deserialize, Serialize};

use crate::cspace::CSpace;
use crate::params::FromParams;
use crate::trajectories::FullTrajectory;

/// A unique identifer for obstacles in the environment
pub type ObstacleId = usize;

/// An obstacle can either alway present(Static) or appear once within range (Appearing)
#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
pub enum Behavior {
  Static,
  Appearing,
}

impl Default for Behavior {
  fn default() -> Behavior {
    Behavior::Static
  }
}

/// A generic representation of an obstacle space in an environment
pub trait ObstacleSpace<X, CS, const N: usize>:
  FromParams + Clone + Obstacle<X, CS, N>
where
  CS: CSpace<X, N>,
{
  /// The type of obstacles in the space
  type Obs: AppearingObstacle<X, CS, N>;

  /// Build a new obstacle space with no obstacles in it
  fn empty() -> Self;

  /// Build a new obstacle space with the given obstacles
  fn new(obstacles: Vec<Self::Obs>) -> Self {
    let mut new_ = Self::empty();
    new_.add_obstacles(obstacles.into_iter().enumerate());
    new_
  }

  /// Add an obstacle to the environement
  ///
  /// Overwrites existing obstacle if `id` has already been added
  fn add_obstacle(&mut self, id: ObstacleId, obstacle: Self::Obs) {
    self.add_obstacles(vec![(id, obstacle)])
  }

  /// Add multiple obstacles to the environment
  ///
  /// Overwrites existing obstacle if `id` has already been added
  fn add_obstacles<I>(&mut self, obstacles: I)
  where
    I: IntoIterator<Item = (ObstacleId, Self::Obs)>;

  /// Optionally returns the specified obstacle id
  fn get_obstacle(&self, id: ObstacleId) -> Option<&Self::Obs> {
    self.get_obstacles(&[id; 1]).into_iter().next()
  }

  /// Gets references to the given obstacle id's
  ///
  /// Doesn't return obstacles that don't exist
  fn get_obstacles(&self, obstacles: &[ObstacleId]) -> Vec<&Self::Obs>;

  /// Gets a newly built obstacle_space only containing the given obstacle id's
  fn get_obstacles_as_obstacle_space(&self, obstacles: &[ObstacleId]) -> Self {
    Self::new(self.get_obstacles(obstacles).into_iter().cloned().collect())
  }

  /// Copies out all yet sensed obstacles in the environement
  fn get_sensed_obstacles(&self) -> Vec<(ObstacleId, &Self::Obs)>;

  /// Copies out all not yet sensed obstacles in the environement
  fn get_not_sensed_obstacles(&self) -> Vec<(ObstacleId, &Self::Obs)>;

  /// Get vector of all valid obstacle ids
  fn get_all_obstacle_ids(&self) -> Vec<ObstacleId>;

  /// Copies out all obstacles with their respective ids
  fn get_all_obstacles(&self) -> Vec<(ObstacleId, &Self::Obs)> {
    self
      .get_all_obstacle_ids()
      .into_iter()
      .filter_map(|obs_id| self.get_obstacle(obs_id).map(|obs| (obs_id, obs)))
      .collect()
  }

  /// Remove the specified obstacle from the environment
  fn remove_obstacle(&mut self, id: ObstacleId) {
    self.remove_obstacles(&[id; 1])
  }

  /// Removes the specified obstacle id's if they have been added previously
  fn remove_obstacles(&mut self, obstacles: &[ObstacleId]);

  /// The total number of obstacles, regardless of sensing
  fn count(&self) -> usize;

  /// Updates the internal obsacle to include obstacles that can be sensed at the given `pose` inside the `radius`
  ///
  /// Returns a list of any added obstacles
  fn check_sensors<S>(
    &mut self,
    pose: &Vector<X, Const<N>, S>,
    radius: X,
  ) -> Vec<ObstacleId>
  where
    S: Storage<X, Const<N>>;
}

/// An generic obstacle in an Rrt-like environment
pub trait Obstacle<X, CS, const N: usize>: FromParams + Clone
where
  CS: CSpace<X, N>,
{
  /// Checks that the obstacle does not intersect the given trajectory
  fn trajectory_free<FT, S1, S2>(&self, t: &FT) -> bool
  where
    FT: FullTrajectory<X, CS::Traj, S1, S2, N>,
    S1: Storage<X, Const<N>>,
    S2: Storage<X, Const<N>>;

  /// Checks that the coordinate is not in the obstacle
  fn is_free<S>(&self, x: &Vector<X, Const<N>, S>) -> bool
  where
    S: Storage<X, Const<N>>;
}

/// An obstacle type that can be sensed within a radius
pub trait AppearingObstacle<X, CS, const N: usize>: Obstacle<X, CS, N>
where
  CS: CSpace<X, N>,
{
  /// Checks if self is within radius of pose
  fn can_sense<S>(&self, pose: &Vector<X, Const<N>, S>, radius: X) -> bool
  where
    S: Storage<X, Const<N>>;
}