path-planning 0.1.0

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

use kdtree::KdTree;
use nalgebra::storage::Storage;
use nalgebra::{Const, SVector, VectorSlice};
use num_traits::Float;
use petgraph::stable_graph::NodeIndex;
use rayon::iter::{IntoParallelIterator, ParallelIterator};
use serde::{de::DeserializeOwned, Deserialize, Serialize};

use crate::cspace::CSpace;
use crate::error::{InvalidParamError, Result};
use crate::obstacles::{Obstacle, ObstacleSpace};
use crate::path_planner::{MoveGoal, PathPlanner, RobotSpecs};
use crate::scalar::Scalar;
use crate::trajectories::{
  FullTrajOwned, FullTrajRefOwned, FullTrajectory, Trajectory,
};
use crate::util::math::unit_d_ball_vol;

use super::{Node, RrtStarTree};

/// RRT* generic parameters
#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
#[serde(bound(
  serialize = "X: Serialize",
  deserialize = "X: DeserializeOwned"
))]
pub struct RrtStarParams<X> {
  pub min_cost: X,
  pub portion: X,
  pub delta: X,
  pub gamma: X,
}

/// RRT* Serializable State information
#[derive(Debug, Serialize, Deserialize)]
#[serde(bound(
  serialize = "X: Serialize, CS::Traj: Serialize, OS: Serialize",
  deserialize = "X: DeserializeOwned, CS::Traj: DeserializeOwned, OS: DeserializeOwned",
))]
pub struct RrtStarState<X, CS, OS, const N: usize>
where
  X: Scalar,
  CS: CSpace<X, N>,
  OS: ObstacleSpace<X, CS, N>,
  CS::Traj: Trajectory<X, N>,
{
  /// All the nodes with their associated costs and edges that connect them
  pub tree: RrtStarTree<X, CS::Traj, N>,

  /// The current path being travelled from some point to a node in the tree
  pub current_path: Option<(SVector<X, N>, CS::Traj, NodeIndex)>,

  /// The last know current pose of the system
  pub pose: SVector<X, N>,

  /// Tracks the obstacles in the space
  pub obs_space: OS,

  /// Shrinking ball radius
  pub radius: X,

  /// Robot specifications
  pub robot_specs: RobotSpecs<X>,

  // Algorithm Parameters
  pub params: RrtStarParams<X>,
}

impl<X, CS, OS, const N: usize> Clone for RrtStarState<X, CS, OS, N>
where
  X: Scalar,
  CS: CSpace<X, N>,
  OS: ObstacleSpace<X, CS, N>,
{
  fn clone(&self) -> Self {
    Self {
      tree: self.tree.clone(),
      current_path: self.current_path.clone(),
      pose: self.pose.clone(),
      obs_space: self.obs_space.clone(),
      radius: self.radius.clone(),
      robot_specs: self.robot_specs.clone(),
      params: self.params.clone(),
    }
  }
}

/// RRT* implementation
pub struct RrtStar<X, CS, OS, const N: usize>
where
  X: Scalar,
  CS: CSpace<X, N>,
  OS: ObstacleSpace<X, CS, N>,
{
  /// All the serializable state information.
  state: RrtStarState<X, CS, OS, N>,

  /// For fast nearest neighbor searching
  kdtree: KdTree<X, NodeIndex, [X; N]>,

  /// Configuration space
  cspace: CS,
}

impl<X, CS, OS, const N: usize> PathPlanner<X, CS, OS, N>
  for RrtStar<X, CS, OS, N>
where
  X: Scalar,
  CS: CSpace<X, N> + Send + Sync,
  CS::Traj: Send + Sync,
  OS: ObstacleSpace<X, CS, N> + Send + Sync,
  OS::Obs: Send + Sync,
{
  type Params = RrtStarParams<X>;
  type State = RrtStarState<X, CS, OS, N>;

  fn new(
    init: SVector<X, N>,
    goal: SVector<X, N>,
    robot_specs: RobotSpecs<X>,
    cspace: CS,
    obs_space: OS,
    params: Self::Params,
  ) -> Result<Self> {
    let tree = RrtStarTree::new(goal.clone());
    let current_path = None;
    let pose = init.clone();

    let mut kdtree: KdTree<X, NodeIndex, [X; N]> = KdTree::new(N.into());
    kdtree
      .add(goal.into(), tree.get_goal_idx())
      .expect("kdtree error");

    let radius = params.delta;

    // Validate robot_radius are greater than 0
    if !(X::zero() < robot_specs.robot_radius) {
      Err(InvalidParamError {
        parameter_name: "robot_specs.robot_radius",
        parameter_value: format!("{:?}", robot_specs.robot_radius),
      })?;
    }

    // Validate sensor_radius are greater than 0
    if !(X::zero() < robot_specs.sensor_radius) {
      Err(InvalidParamError {
        parameter_name: "robot_specs.sensor_radius",
        parameter_value: format!("{:?}", robot_specs.sensor_radius),
      })?;
    }

    // Validate min_cost is in [0, delta)
    if !(X::zero() <= params.min_cost && params.min_cost < params.delta) {
      Err(InvalidParamError {
        parameter_name: "params.min_cost",
        parameter_value: format!("{:?}", params.min_cost),
      })?;
    }

    // Validate portion is between 0 and 1
    if !(X::zero() < params.portion && params.portion < X::one()) {
      Err(InvalidParamError {
        parameter_name: "params.portion",
        parameter_value: format!("{:?}", params.portion),
      })?;
    }

    let state = RrtStarState {
      tree,
      current_path,
      pose,
      obs_space,
      radius,
      robot_specs,
      params,
    };

    let mut new = Self {
      state,
      kdtree,
      cspace,
    };

    new.state.radius = new.shrinking_ball_radius();
    new.check_gamma();

    // Check that the init and goal locations are in free space
    if !new.is_free(&init) {
      Err(InvalidParamError {
        parameter_name: "init",
        parameter_value: format!("{:?}", init),
      })?;
    }
    if !new.is_free(&goal) {
      Err(InvalidParamError {
        parameter_name: "goal",
        parameter_value: format!("{:?}", goal),
      })?;
    }

    Ok(new)
  }

  fn create_node(&mut self) -> &Self::State {
    // Continue to sample points until one is found
    loop {
      if let Some(()) = self.try_create_node() {
        break;
      }
    }

    // Update the new radius now that we changed self.count()
    self.state.radius = self.shrinking_ball_radius();
    self.get_state()
  }

  fn sample_node(&mut self) -> Option<&Self::State> {
    self.try_create_node()?;

    // Update the new radius now if we changed self.count()
    self.state.radius = self.shrinking_ball_radius();
    Some(self.get_state())
  }

  fn check_sensors(&mut self) {
    let added = self
      .state
      .obs_space
      .check_sensors(&self.state.pose, self.state.robot_specs.sensor_radius);

    if !added.is_empty() {
      // Copy out the added obstacles into a temporary obstacle space
      let obs = self
        .state
        .obs_space
        .get_obstacles(&added[..])
        .into_iter()
        .cloned()
        .collect();
      let added_obs_space = OS::new(obs);

      // Add to environment possibly creating orphans
      self.add_obstacle_to_environment(added_obs_space);

      // Cleanup
      // if the current target has been orphaned, remove it
      if let Some((_, _, move_goal_idx)) = self.state.current_path {
        if self.state.tree.is_orphan(move_goal_idx) {
          self.state.current_path = None;
        }
      }

      // // TODO: BUG:
      // // removing from tree without removing from kdtree will cause problems
      // self.state.tree.clear_orphans();
    }
  }

  fn get_obs_space(&self) -> &OS {
    &self.state.obs_space
  }

  fn get_obs_space_mut(&mut self) -> &mut OS {
    &mut self.state.obs_space
  }

  fn get_cspace(&self) -> &CS {
    &self.cspace
  }

  fn get_cspace_mut(&mut self) -> &mut CS {
    &mut self.cspace
  }

  fn check_nodes(&mut self, use_obs_space: bool, use_cspace: bool) {
    if !(use_obs_space || use_cspace) {
      // Nothing to do
      return;
    }

    let all_nodes = self.state.tree.all_nodes().collect::<Vec<_>>();
    let tree = &self.state.tree;
    let cspace = &self.cspace;
    let obs_space = &self.state.obs_space;

    let iter = all_nodes.into_par_iter().filter_map(|u_idx| {
      let u = tree.get_node(u_idx).state();

      // Check cspace and/or obs_space if required
      if (use_cspace && !cspace.is_free(u))
        || (use_obs_space && !obs_space.is_free(u))
      {
        Some(u_idx)
      } else {
        None
      }
    });
    let vec = iter.collect::<Vec<_>>();

    for u_idx in vec {
      self.state.tree.add_orphan(u_idx);
    }
    // // TODO: BUG:
    // // removing from tree without removing from kdtree will cause problems
    // self.state.tree.clear_orphans();
  }

  fn update_pose(
    &mut self,
    pose: SVector<X, N>,
    nearest: bool,
  ) -> Option<MoveGoal<X, N>> {
    self.state.pose = pose;
    log::info!("Updating pose to {:?}", <[X; N]>::from(pose));

    // Check sensors from this new pose
    self.check_sensors();

    if !nearest {
      // Check that the next move goal is still valid for this new pose,
      // if so, return it
      // otherwise, determine a new move goal
      if let Some((start_pose, _, move_goal_idx)) = self.state.current_path {
        let move_goal = self.state.tree.get_node(move_goal_idx).state();
        let pose_ref = pose.index((.., 0));
        let mg_ref = move_goal.index((.., 0));

        if let Some(new_trajectory) = self.cspace.trajectory(pose_ref, mg_ref) {
          if self.trajectory_free(&new_trajectory) {
            // Trajectory is valid
            let init_dist_err = self.cspace.cost(&start_pose, move_goal);
            let rel_dist_err = self.cspace.cost(&pose, move_goal);

            if init_dist_err > rel_dist_err {
              // We are closer to the move goal than initially

              if init_dist_err - rel_dist_err
                > init_dist_err * self.state.params.portion
                || rel_dist_err < self.state.params.min_cost
              {
                // We have reached the goal we were aiming for

                // Check if we have reach the global goal node
                if self.state.tree.get_goal_idx() == move_goal_idx {
                  log::info!("Reached the finish!");
                  return Some(MoveGoal::Finished);
                }

                // Find the first viable parent in the optimal path that is more
                // than min_cost away
                log::info!("Reached move goal, looking for next along path");
                let res = self.find_move_goal_along_path(&pose, move_goal_idx);

                // Update if found
                if let Some((new_trajectory, new_move_goal_idx)) = res {
                  self.state.current_path =
                    Some((pose, new_trajectory, new_move_goal_idx));
                  log::info!("New move goal found");
                  return Some(MoveGoal::New(*self.get_current_path()?.end()));
                } else {
                  log::info!("No valid move goal along path");
                }
              } else {
                // Keep the same move goal
                log::info!("Keeping same move goal");
                return Some(MoveGoal::Old(*self.get_current_path()?.end()));
              }
            } else {
              // We are farther from the move goal than initial

              if rel_dist_err - init_dist_err
                > init_dist_err * self.state.params.portion
              {
                // We are out of range of the goal we were aiming for
                // Falling through to look for a new path
                log::info!("Out of range of move goal");
              } else {
                // Keep the same move goal
                log::info!("Keeping same move goal");
                return Some(MoveGoal::Old(*self.get_current_path()?.end()));
              }
            }
          } else {
            // Trajectory is blocked by an obstacle
            log::info!("Current move goal blocked by obstacle");
          }
        } else {
          // The trajectory is invalid
          log::info!("Trajectory to current move goal infesible");
        }
      }
    }

    // The current move goal is invalid, find a new one
    log::info!("Move goal invalid, looking for a new one");
    self.state.current_path = self.find_new_path(pose);
    Some(MoveGoal::New(*self.get_current_path()?.end()))
  }

  fn get_tree(&self) -> Vec<FullTrajRefOwned<X, CS::Traj, N>> {
    unimplemented!()
  }

  fn get_current_path(&self) -> Option<FullTrajOwned<X, CS::Traj, N>> {
    let path = self.state.current_path.as_ref()?.clone();
    let start = path.0;
    let end = self.state.tree.get_node(path.2).state().clone();
    let traj = path.1;

    Some(FullTrajOwned::new(start, end, traj))
  }

  fn get_path_to_goal(&self) -> Option<Vec<SVector<X, N>>> {
    Some(
      self
        .state
        .tree
        .get_optimal_path(self.state.current_path.as_ref()?.2)?
        .map(|node_idx| self.state.tree.get_node(node_idx).state().clone())
        .collect(),
    )
  }

  fn get_last_pose(&self) -> &SVector<X, N> {
    &self.state.pose
  }

  fn get_state(&self) -> &Self::State {
    &self.state
  }

  fn get_goal(&self) -> &SVector<X, N> {
    self.state.tree.get_goal().state()
  }

  fn count(&self) -> usize {
    assert_eq!(self.kdtree.size(), self.state.tree.node_count());
    self.state.tree.node_count() + 1
  }
}

impl<X, CS, OS, const N: usize> RrtStar<X, CS, OS, N>
where
  X: Scalar,
  CS: CSpace<X, N> + Send + Sync,
  CS::Traj: Send + Sync,
  OS: ObstacleSpace<X, CS, N> + Send + Sync,
  OS::Obs: Send + Sync,
{
  /// Try to place a new node in the tree, returns None when unsuccessful
  fn try_create_node(&mut self) -> Option<()> {
    let mut v = self.cspace.sample();
    let (cost, v_nearest) = self.nearest(&v);

    if cost > self.state.params.delta {
      self
        .cspace
        .saturate(&mut v, v_nearest, self.state.params.delta);
    }

    match self.is_free(&v) {
      true => {
        let (v, v_near) = self.extend(v)?;
        self.rewire_neighbors(v, v_near);
        Some(())
      }
      false => None,
    }
  }

  /// Returns the updated shrinking ball radius
  fn shrinking_ball_radius(&self) -> X {
    let dims = X::from(N).unwrap();
    let num_vertices = X::from(self.count()).unwrap();

    let temp = <X as Float>::ln(num_vertices) / num_vertices;
    let temp = <X as Float>::powf(temp, <X as Float>::recip(dims));

    <X as Float>::min(self.state.params.gamma * temp, self.state.params.delta)
  }

  /// Warn if gamma is too low for a connected graph
  fn check_gamma(&self) {
    let d = X::from(N).unwrap();
    let one = X::one();
    let two = one + one;
    let one_over_d = <X as Float>::recip(d);
    let volume = self.cspace.volume();

    let temp1 = <X as Float>::powf(two * (one + one_over_d), one_over_d);
    let temp2 = <X as Float>::powf(volume / unit_d_ball_vol(N), one_over_d);

    let min_gamma = temp1 * temp2;

    log::info!(
      "Gamma: {:?}, Minimum Gamma: {:?}, volume: {:?}",
      self.state.params.gamma,
      min_gamma,
      volume
    );

    // Assert
    if !(self.state.params.gamma > min_gamma) {
      log::warn!("Gamma is too small to gaurantee a conected graph");
    }
  }

  /// Find the nearest node to v and the cost to get to it
  fn nearest(&self, v: &SVector<X, N>) -> (X, &SVector<X, N>) {
    let vec = self
      .kdtree
      .nearest(v.into(), 1, &|a, b| {
        let a = VectorSlice::<X, Const<N>>::from_slice(a);
        let b = VectorSlice::<X, Const<N>>::from_slice(b);
        self.cspace.cost(&a, &b)
      })
      .unwrap();
    let (cost, &v_nearest_idx) = vec.first().unwrap();
    let v_nearest = self.state.tree.get_node(v_nearest_idx).state();
    (*cost, v_nearest)
  }

  /// Try to extend the tree to include the given node
  fn extend(
    &mut self,
    v: SVector<X, N>,
  ) -> Option<(NodeIndex, Vec<NodeIndex>)> {
    let v_near = self.near(&v);

    let mut v = Node::new(v);
    let (trajectory, parent_idx) = self.find_parent(&mut v, &v_near)?;

    let (v_idx, _) = self.state.tree.add_node(v, parent_idx, trajectory);
    self
      .kdtree
      .add(
        self.state.tree.get_node(v_idx).state().clone().into(),
        v_idx,
      )
      .expect("kdtree error");

    Some((v_idx, v_near))
  }

  /// Return vector of nodes that are within self.state.radius
  fn near(&self, v: &SVector<X, N>) -> Vec<NodeIndex> {
    let result = self.kdtree.within(v.into(), self.state.radius, &|a, b| {
      let a = VectorSlice::<X, Const<N>>::from_slice(a);
      let b = VectorSlice::<X, Const<N>>::from_slice(b);
      self.cspace.cost(&a, &b)
    });

    match result {
      Ok(vec) => vec.iter().map(|&(_, &id)| id).collect(),
      Err(kdtree::ErrorKind::ZeroCapacity) => vec![],
      _ => panic!("kdtree error"),
    }
  }

  /// Searchs u_near for the best selection of a parent for v that minimzes the look-ahead cost estimate
  /// Returns Some(trajectory_to_parent, parent_idx) if a fesiable parent was found
  fn find_parent(
    &self,
    v: &mut Node<X, N>,
    u_near: &Vec<NodeIndex>,
  ) -> Option<(CS::Traj, NodeIndex)> {
    let mut parent = None;

    for &u_idx in u_near {
      // Check each node for a valid trajectory, searching keeping track of minimum viable path
      let u = self.state.tree.get_node(u_idx);
      let v_ref = v.state().index((.., 0));
      let u_ref = u.state().index((.., 0));

      // Check System dynamics
      if let Some(trajectory) = self.cspace.trajectory(v_ref, u_ref) {
        // Check cost actually makes the minimum cost go down
        let cost = trajectory.cost();
        if v.cost > cost + u.cost {
          // Check trajectory for intersections with obstacles
          if self.trajectory_free(&trajectory) {
            parent = Some((trajectory.to_trajectory(), u_idx));
            v.cost = cost + u.cost;
          }
        }
      }
    }
    parent
  }

  /// Rewires incoming neighbors to v if doing so lowers the cost of u
  fn rewire_neighbors(&mut self, v_idx: NodeIndex, v_near: Vec<NodeIndex>) {
    for u_idx in v_near {
      if self.state.tree.is_parent(v_idx, u_idx) {
        continue;
      }

      // Gather data
      let u = self.state.tree.get_node(u_idx);
      let v = self.state.tree.get_node(v_idx);
      let u_ref = u.state().index((.., 0));
      let v_ref = v.state().index((.., 0));

      // Check trajectory u -> v
      if let Some(trajectory) = self.cspace.trajectory(u_ref, v_ref) {
        let v_cost = v.cost;
        let trajectory_cost = trajectory.cost();

        // Check if cost is lower through v
        if u.cost > trajectory_cost + v_cost {
          // Check trajectory for intersections with obstacles
          if self.trajectory_free(&trajectory) {
            let trajectory = trajectory.to_trajectory();
            let u = self.state.tree.get_node_mut(u_idx);
            u.cost = trajectory_cost + v_cost;
            self.state.tree.update_edge(u_idx, v_idx, trajectory);
          }
        }
      }
    }
  }

  /// Updates the tree nodes and edges that become blocked by this obstacle
  fn add_obstacle_to_environment<O>(&mut self, obstacle: O)
  where
    O: Obstacle<X, CS, N> + Send + Sync,
  {
    // Find all the edges that intersect with the obstacle
    let tree_ref = &self.state.tree;
    let iter = tree_ref.all_edges().collect::<Vec<_>>();
    let iter = iter.into_par_iter().filter_map(|edge_idx| {
      let trajectory = self.state.tree.get_trajectory(edge_idx);
      match obstacle.trajectory_free(&trajectory) {
        true => None,
        false => {
          let (v_idx, _u_idx) = self.state.tree.get_endpoints(edge_idx);
          Some(v_idx)
        }
      }
    });

    // Compute this iterator
    let vec: Vec<_> = iter.collect();

    for v_idx in vec {
      self.state.tree.add_orphan(v_idx);
    }
  }

  /// Find the first viable parent in the optimal path that is more than min_cost away
  ///
  /// panics if move_goal_idx is an orphan
  fn find_move_goal_along_path(
    &self,
    pose: &SVector<X, N>,
    move_goal_idx: NodeIndex,
  ) -> Option<(CS::Traj, NodeIndex)> {
    // This is an invariant, if the move_goal exists then the optimal path exists
    let mut optimal_path_iter =
      self.state.tree.get_optimal_path(move_goal_idx).unwrap();

    optimal_path_iter.next(); // Pop off the current goal (move_goal_idx)

    // Seach the path for the first viable node that is more than min_cost away
    for node_idx in optimal_path_iter {
      let node = self.state.tree.get_node(node_idx).state();

      if let Some(trajectory) =
        self.cspace.trajectory(pose.clone(), node.clone())
      {
        if self.trajectory_free(&trajectory) {
          // Valid trajectory, see if it is longer than min_cost
          let cost = trajectory.cost();
          if self.state.params.min_cost < cost {
            return Some((trajectory.to_trajectory(), node_idx));
          }
          continue;
        }
      }
      // Invalid trajectory, cut off search if farther than delta
      let cost = self.cspace.cost(pose, node);
      if self.state.params.delta < cost {
        log::info!("Cutting search along path short");
        return None;
      }
    }
    None
  }

  /// Check that the given coordinate does not intersect with any obstacles
  fn is_free(&self, v: &SVector<X, N>) -> bool {
    self.cspace.is_free(v) && self.state.obs_space.is_free(v)
  }

  /// Check that the given trajectory does not intersect with any obstacles
  fn trajectory_free<TF, S1, S2>(&self, t: &TF) -> bool
  where
    TF: FullTrajectory<X, CS::Traj, S1, S2, N>,
    S1: Storage<X, Const<N>>,
    S2: Storage<X, Const<N>>,
  {
    self.state.obs_space.trajectory_free(t)
  }

  /// The current move goal is unreachable, this looks for a new one
  /// Returns Some new move goal if one is viable
  fn find_new_path(
    &self,
    pose: SVector<X, N>,
  ) -> Option<(SVector<X, N>, CS::Traj, NodeIndex)> {
    // Create temporary Node as the given pose
    let mut pose_node = Node::new(pose);

    // Create Iterator over all points in cost order
    let cost_func = |a: &[X], b: &[X]| {
      let a = VectorSlice::<X, Const<N>>::from_slice(a);
      let b = VectorSlice::<X, Const<N>>::from_slice(b);
      self.cspace.cost(&a, &b)
    };
    let mut iter = self
      .kdtree
      .iter_nearest(pose.as_slice(), &cost_func)
      .unwrap();

    // Create first batch to determine the initial batch size
    let mut batch = Vec::new();
    loop {
      match iter.next() {
        Some((cost, &idx)) => {
          batch.push(idx);
          if cost >= self.state.radius {
            break;
          }
        }
        None => break,
      }
    }

    // Search the first batch
    log::info!("Searching {:?} nearest nodes", batch.len());
    if let Some((traj, idx)) = self.find_parent(&mut pose_node, &batch) {
      log::info!("Found new move goal");
      return Some((pose, traj, idx));
    }

    // Search for a viable target in batches that double in size each loop
    // up until the cost is greater than delta
    loop {
      // Reset and create new batch of double size
      let batch_size = 2 * batch.len();
      batch.clear();
      while let Some((cost, &idx)) = iter.next() {
        if self.state.params.delta < cost {
          log::info!("Cutting search short");
          break;
        }

        batch.push(idx);
        if batch.len() >= batch_size {
          break;
        }
      }

      // Iterator stopped giving elements, we have searched every node
      if batch.is_empty() {
        log::info!("End of search, no new move goal found");
        return None;
      }

      // Search the batch
      log::info!("Searching next {:?} nearest nodes", batch.len());
      if let Some((traj, idx)) = self.find_parent(&mut pose_node, &batch) {
        log::info!("Found new move goal");
        return Some((pose, traj, idx));
      }
    }
  }
}

#[cfg(test)]
mod tests {

  use parry3d::math::Isometry;
  use parry3d::shape::{Ball, Cuboid, SharedShape};
  use rand::SeedableRng;

  use crate::cspace::EuclideanSpace;
  use crate::obstacles::obstacles_3d_f32::{Obstacle3df32, ObstacleSpace3df32};
  use crate::rng::RNG;
  use crate::util::bounds::Bounds;

  use super::*;

  const SEED: u64 = 0xe580e2e93fd6b040;

  #[test]
  fn test_rrt_star() {
    let init = [5.0, 0.5, 5.0].into();
    let goal = [-5.0, 0.5, -5.0].into();

    let robot_specs = RobotSpecs {
      robot_radius: 0.1,
      sensor_radius: 2.0,
    };

    let bounds = Bounds::new([-5.0, 0.0, -5.0].into(), [5.0, 1.0, 5.0].into());
    let cspace = EuclideanSpace::new(bounds, RNG::seed_from_u64(SEED)).unwrap();

    let ball = Obstacle3df32::with_offset(
      SharedShape::new(Ball::new(0.5)),
      Isometry::translation(0.0, 0.5, 0.0),
    );

    let cube = Obstacle3df32::with_offset(
      SharedShape::new(Cuboid::new([0.5, 0.5, 0.5].into())),
      Isometry::translation(2.5, 0.5, 2.5),
    );

    let obs_space = ObstacleSpace3df32::from(vec![ball, cube]);

    let params = RrtStarParams {
      min_cost: 0.0,
      portion: 0.1,
      delta: 1.0,
      gamma: 1067.0,
    };

    let mut rrt_star =
      RrtStar::new(init, goal, robot_specs, cspace, obs_space, params).unwrap();

    let path = loop {
      rrt_star.create_node();
      rrt_star.update_pose(init, false);
      if let Some(path) = rrt_star.get_path_to_goal() {
        break path;
      }
    };

    let mut cost = 0.0;
    for i in 0..path.len() - 1 {
      cost += path[i].metric_distance(&path[i + 1]);
    }
    println!("{:?}", rrt_star.count());
    println!("{:?}", rrt_star.state.radius);
    println!("{:?}", path);
    println!("{:?}", cost);
  }

  #[test]
  fn test_serialize_rrt_star_state() {
    let init = [5.0, 0.5, 5.0].into();
    let goal = [-5.0, 0.5, -5.0].into();

    let robot_specs = RobotSpecs {
      robot_radius: 0.1,
      sensor_radius: 2.0,
    };

    let bounds = Bounds::new([-5.0, 0.0, -5.0].into(), [5.0, 1.0, 5.0].into());
    let cspace = EuclideanSpace::new(bounds, RNG::seed_from_u64(SEED)).unwrap();

    let ball = Obstacle3df32::with_offset(
      SharedShape::new(Ball::new(0.5)),
      Isometry::translation(0.0, 0.5, 0.0),
    );

    let cube = Obstacle3df32::with_offset(
      SharedShape::new(Cuboid::new([0.5, 0.5, 0.5].into())),
      Isometry::translation(2.5, 0.5, 2.5),
    );

    let obs_space = ObstacleSpace3df32::from(vec![ball, cube]);

    let params = RrtStarParams {
      min_cost: 0.0,
      portion: 0.1,
      delta: 1.0,
      gamma: 1067.0,
    };

    let mut rrt_star =
      RrtStar::new(init, goal, robot_specs, cspace, obs_space, params).unwrap();

    loop {
      rrt_star.create_node();
      rrt_star.update_pose(init, false);
      if let Some(_) = rrt_star.get_path_to_goal() {
        break;
      }
    }

    let state = rrt_star.get_state();
    let v = bincode::serialize(&state).unwrap();
    let _: RrtStarState<f32, EuclideanSpace<f32, 3>, ObstacleSpace3df32, 3> =
      bincode::deserialize(&v).unwrap();
  }
}