path_planning/rrtx/
rrtx_graph.rs

1/* Copyright (C) 2020 Dylan Staatz - All Rights Reserved. */
2
3pub use petgraph::stable_graph::{EdgeIndex, NodeIndex};
4
5////////////////////////////////////////////////////////////////////////////////
6
7use std::collections::VecDeque;
8use std::marker::PhantomData;
9
10use std::cmp::Ordering;
11use std::collections::HashSet;
12use std::ops::{Add, Index, IndexMut};
13
14use nalgebra::{SVector, Scalar};
15use num_traits::{Float, Zero};
16use petgraph::stable_graph::{
17  DefaultIx, EdgeIndices, Edges, NodeIndices, StableDiGraph, WalkNeighbors,
18};
19use petgraph::visit::EdgeRef;
20use petgraph::{Directed, Direction};
21use serde::{de::DeserializeOwned, Deserialize, Serialize};
22
23use crate::trajectories::{FullTrajRefOwned, FullTrajectory, Trajectory};
24
25/// The cost at a node
26#[derive(Debug, Clone, Copy, Default, PartialEq, Serialize, Deserialize)]
27#[serde(bound(
28  serialize = "X: Serialize",
29  deserialize = "X: DeserializeOwned"
30))]
31pub struct Cost<X> {
32  /// The look-ahead estimate of cost-to-goal
33  pub lmc: X,
34  /// The (e-consistent) cost-to-goal of reaching v_goal from v through the optimal tree
35  pub g: X,
36}
37
38impl<X> Cost<X> {
39  pub fn new(lmc: X, g: X) -> Self {
40    Self { lmc, g }
41  }
42}
43
44impl<X: Float> Cost<X> {
45  pub fn infinity() -> Self {
46    Self {
47      lmc: X::infinity(),
48      g: X::infinity(),
49    }
50  }
51
52  pub fn priority(&self) -> Priority<X> {
53    Priority(self.g.min(self.lmc), self.g)
54  }
55}
56
57impl<X: Add<X, Output = X>> Add for Cost<X> {
58  type Output = Self;
59
60  fn add(self, other: Self) -> Self::Output {
61    Self {
62      lmc: self.lmc + other.lmc,
63      g: self.g + other.g,
64    }
65  }
66}
67
68impl<X: Zero + PartialEq> Zero for Cost<X> {
69  fn zero() -> Self {
70    Self::new(X::zero(), X::zero())
71  }
72
73  fn is_zero(&self) -> bool {
74    self == &Self::zero()
75  }
76}
77
78/// The priority of a node in the queue
79/// priority should give the opposite of min cost because the priority queue
80/// sorts the maximum value first
81#[derive(Debug, Clone, Copy, Default, Serialize, Deserialize)]
82#[serde(bound(
83  serialize = "X: Serialize",
84  deserialize = "X: DeserializeOwned"
85))]
86pub struct Priority<X>(X, X);
87
88impl<X: Float> Priority<X> {
89  fn is_nan(&self) -> bool {
90    self.0.is_nan() || self.1.is_nan()
91  }
92}
93
94impl<X: Float> PartialEq for Priority<X> {
95  fn eq(&self, other: &Self) -> bool {
96    debug_assert!(!(self.is_nan() || other.is_nan()));
97    self.0 == other.0 && self.1 == other.1
98  }
99}
100
101/// Because of assertion in PartialEq impl
102impl<X: Float> Eq for Priority<X> {}
103
104impl<X: Float> PartialOrd for Priority<X> {
105  fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
106    if self == other {
107      return Some(Ordering::Equal);
108    }
109
110    let (a, b) = (self.0, self.1);
111    let (c, d) = (other.0, other.1);
112
113    // Flip the order because PriorityQueue sorts the maximum items first
114    match a == c {
115      true => b.partial_cmp(&d).map(|ord| ord.reverse()),
116      false => a.partial_cmp(&c).map(|ord| ord.reverse()),
117    }
118  }
119}
120
121impl<X: Float> Ord for Priority<X> {
122  fn cmp(&self, other: &Self) -> Ordering {
123    self.partial_cmp(other).unwrap()
124  }
125}
126
127/// A node in the Rrtx graph. Stores information about a particular state in the state space.
128#[derive(PartialEq, Clone, Debug, Serialize, Deserialize)]
129#[serde(bound(
130  serialize = "X: Serialize",
131  deserialize = "X: DeserializeOwned"
132))]
133pub struct Node<X: Scalar, const N: usize> {
134  /// The point in the state space this node is at (immutable)
135  state: SVector<X, N>,
136  /// The cost data for this node (mutable)
137  pub cost: Cost<X>,
138  /// The index of the edge that leads to the parent
139  parent_edge: Option<EdgeIndex>,
140}
141
142impl<X: Scalar + Float, const N: usize> Node<X, N> {
143  pub fn new(state: SVector<X, N>) -> Self {
144    Self {
145      state,
146      cost: Cost::infinity(),
147      parent_edge: None,
148    }
149  }
150}
151
152impl<X: Scalar, const N: usize> Node<X, N> {
153  fn with_cost(state: SVector<X, N>, cost: Cost<X>) -> Self {
154    Self {
155      state,
156      cost,
157      parent_edge: None,
158    }
159  }
160
161  pub fn state(&self) -> &SVector<X, N> {
162    &self.state
163  }
164
165  fn clear_parent_edge(&mut self) {
166    self.parent_edge = None;
167  }
168
169  fn set_parent_edge(&mut self, edge_idx: EdgeIndex) {
170    self.parent_edge = Some(edge_idx);
171  }
172
173  pub fn parent_edge(&self) -> Option<EdgeIndex> {
174    self.parent_edge
175  }
176}
177
178/// In Rrtx, all outgoing edges are either classified as Original or Running
179#[derive(PartialEq, Eq, Clone, Copy, Debug, Serialize, Deserialize)]
180pub enum NeighborType {
181  Original,
182  Running,
183}
184
185/// An edge in the Rrtx graph.
186/// Stores information about what type of connection this is between two nodes (directed)
187/// All information is from the perspective of node a, assuming this edge represents the directed edge a -> b.
188/// An outgoing edge of a in the directed edge a -> b
189/// An incoming edge of a in the directed edge b -> a
190#[derive(PartialEq, Eq, Clone, Copy, Debug, Serialize, Deserialize)]
191pub struct Edge<X, T, const N: usize>
192where
193  T: Trajectory<X, N>,
194{
195  bits: u8,
196  trajectory: T,
197  phantom_data: PhantomData<X>,
198}
199
200impl<X, T, const N: usize> Edge<X, T, N>
201where
202  T: Trajectory<X, N>,
203{
204  fn new(
205    outgoing_neighbor_type: NeighborType,
206    incoming_neighbor_type: NeighborType,
207    parent: bool,
208    trajectory: T,
209  ) -> Self {
210    let mut new = Self {
211      bits: 0,
212      trajectory,
213      phantom_data: PhantomData,
214    };
215    new.set_outgoing_neighbor_type(Some(outgoing_neighbor_type));
216    new.set_incoming_neighbor_type(Some(incoming_neighbor_type));
217    new.set_is_parent(parent);
218    new.set_is_trajectory_valid_bit(true);
219    new
220  }
221
222  pub fn bits(&self) -> u8 {
223    self.bits
224  }
225
226  /// Check at least one of the directions is valid
227  /// if false, then the edge should be deleted
228  fn is_valid(&self) -> bool {
229    self.is_valid_dir(Direction::Outgoing)
230      || self.is_valid_dir(Direction::Incoming)
231    // || self.is_parent()
232  }
233
234  /// Check if the specified direction is still valid (i.e. not None)
235  fn is_valid_dir(&self, direction: Direction) -> bool {
236    match direction {
237      Direction::Outgoing => self.outgoing_neighbor_type().is_some(),
238      Direction::Incoming => self.incoming_neighbor_type().is_some(),
239    }
240  }
241
242  /// For the directed edge a -> b,
243  /// - outgoing: removes a's knowledge of b
244  /// - incoming: removes b's knowledge of a
245  /// Returns whether the edge is still valid,
246  /// if false, then the edge should be deleted
247  fn remove_connection(&mut self, to_remove: Direction) -> bool {
248    match to_remove {
249      Direction::Outgoing => {
250        self.set_outgoing_neighbor_type(None);
251        self.is_valid_dir(Direction::Incoming)
252      }
253      Direction::Incoming => {
254        self.set_incoming_neighbor_type(None);
255        self.is_valid_dir(Direction::Outgoing)
256      }
257    }
258  }
259
260  /// Returns true if along the directed edge a -> b,
261  /// - outgoing: if b is a original neighbor of a
262  /// - incoming: if a is a original neighbor of b
263  fn is_original(&self, direction: Direction) -> bool {
264    match direction {
265      Direction::Outgoing => match self.outgoing_neighbor_type() {
266        Some(t) => t == NeighborType::Original,
267        None => false,
268      },
269      Direction::Incoming => match self.incoming_neighbor_type() {
270        Some(t) => t == NeighborType::Original,
271        None => false,
272      },
273    }
274  }
275
276  /// Returns true if along the directed edge a -> b,
277  /// - outgoing: if b is a running neighbor of a
278  /// - incoming: if a is a running neighbor of b
279  fn is_running(&self, direction: Direction) -> bool {
280    match direction {
281      Direction::Outgoing => match self.outgoing_neighbor_type() {
282        Some(t) => t == NeighborType::Running,
283        None => false,
284      },
285      Direction::Incoming => match self.incoming_neighbor_type() {
286        Some(t) => t == NeighborType::Running,
287        None => false,
288      },
289    }
290  }
291
292  /* Getters */
293
294  pub fn outgoing_neighbor_type(&self) -> Option<NeighborType> {
295    match self.get_is_outgoing_valid_bit() {
296      true => match self.get_is_outgoing_original_bit() {
297        true => Some(NeighborType::Original),
298        false => Some(NeighborType::Running),
299      },
300      false => None,
301    }
302  }
303
304  pub fn incoming_neighbor_type(&self) -> Option<NeighborType> {
305    match self.get_is_incoming_valid_bit() {
306      true => match self.get_is_incoming_original_bit() {
307        true => Some(NeighborType::Original),
308        false => Some(NeighborType::Running),
309      },
310      false => None,
311    }
312  }
313
314  /// Returns true if along the directed edge a -> b, b is the parent of a
315  pub fn is_parent(&self) -> bool {
316    self.get_is_parent_bit()
317  }
318
319  /// The if the trajectory to get from a to b is valid
320  pub fn trajectory(&self) -> Option<&T> {
321    match self.get_is_trajectory_valid_bit() {
322      true => Some(&self.trajectory),
323      false => None,
324    }
325  }
326
327  /* Setters */
328
329  fn set_outgoing_neighbor_type(&mut self, outgoing: Option<NeighborType>) {
330    match outgoing {
331      Some(NeighborType::Original) => {
332        self.set_is_outgoing_valid_bit(true);
333        self.set_is_outgoing_original_bit(true);
334      }
335      Some(NeighborType::Running) => {
336        self.set_is_outgoing_valid_bit(true);
337        self.set_is_outgoing_original_bit(false);
338      }
339      None => self.set_is_outgoing_valid_bit(false),
340    }
341  }
342
343  fn set_incoming_neighbor_type(&mut self, incoming: Option<NeighborType>) {
344    match incoming {
345      Some(NeighborType::Original) => {
346        self.set_is_incoming_valid_bit(true);
347        self.set_is_incoming_original_bit(true);
348      }
349      Some(NeighborType::Running) => {
350        self.set_is_incoming_valid_bit(true);
351        self.set_is_incoming_original_bit(false);
352      }
353      None => self.set_is_incoming_valid_bit(false),
354    }
355  }
356
357  fn set_is_parent(&mut self, is_parent: bool) {
358    self.set_is_parent_bit(is_parent)
359  }
360
361  fn set_trajectory(&mut self, trajectory: Option<T>) {
362    match trajectory {
363      Some(trajectory) => {
364        self.set_is_trajectory_valid_bit(true);
365        self.trajectory = trajectory;
366      }
367      None => self.set_is_trajectory_valid_bit(false),
368    }
369  }
370
371  const IS_OUTGOING_VALID_BIT: u8 = 5;
372  const IS_OUTGOING_ORIGINAL_BIT: u8 = 4;
373  const IS_INCOMING_VALID_BIT: u8 = 3;
374  const IS_INCOMING_ORIGINAL_BIT: u8 = 2;
375  const IS_PARENT_BIT: u8 = 1;
376  const IS_TRAJECTORY_VALID_BIT: u8 = 0;
377
378  fn get_is_outgoing_valid_bit(&self) -> bool {
379    self.get_bit(Self::IS_OUTGOING_VALID_BIT)
380  }
381
382  fn get_is_outgoing_original_bit(&self) -> bool {
383    self.get_bit(Self::IS_OUTGOING_ORIGINAL_BIT)
384  }
385
386  fn get_is_incoming_valid_bit(&self) -> bool {
387    self.get_bit(Self::IS_INCOMING_VALID_BIT)
388  }
389
390  fn get_is_incoming_original_bit(&self) -> bool {
391    self.get_bit(Self::IS_INCOMING_ORIGINAL_BIT)
392  }
393
394  fn get_is_parent_bit(&self) -> bool {
395    self.get_bit(Self::IS_PARENT_BIT)
396  }
397
398  fn get_is_trajectory_valid_bit(&self) -> bool {
399    self.get_bit(Self::IS_TRAJECTORY_VALID_BIT)
400  }
401
402  fn get_bit(&self, bit_pos: u8) -> bool {
403    let mask = 1 << bit_pos;
404    (self.bits & mask) != 0
405  }
406
407  fn set_is_outgoing_valid_bit(&mut self, is_valid: bool) {
408    self.set_bit(Self::IS_OUTGOING_VALID_BIT, is_valid)
409  }
410
411  fn set_is_outgoing_original_bit(&mut self, is_original: bool) {
412    self.set_bit(Self::IS_OUTGOING_ORIGINAL_BIT, is_original)
413  }
414
415  fn set_is_incoming_valid_bit(&mut self, is_valid: bool) {
416    self.set_bit(Self::IS_INCOMING_VALID_BIT, is_valid)
417  }
418
419  fn set_is_incoming_original_bit(&mut self, is_original: bool) {
420    self.set_bit(Self::IS_INCOMING_ORIGINAL_BIT, is_original)
421  }
422
423  fn set_is_parent_bit(&mut self, is_parent: bool) {
424    self.set_bit(Self::IS_PARENT_BIT, is_parent)
425  }
426
427  fn set_is_trajectory_valid_bit(&mut self, is_valid: bool) {
428    self.set_bit(Self::IS_TRAJECTORY_VALID_BIT, is_valid)
429  }
430
431  fn set_bit(&mut self, bit_pos: u8, value: bool) {
432    let mask = 1 << bit_pos;
433    match value {
434      true => self.bits |= mask,
435      false => self.bits &= !mask,
436    }
437  }
438}
439
440/// Different subsets for neighbors relative to a node
441pub enum NeighborSet {
442  Outgoing,
443  Incoming,
444  OriginalOutgoing,
445  OriginalIncoming,
446  RunningOutgoing,
447  RunningIncoming,
448}
449
450/// An iterator over different Neighbor sets of a node
451pub struct NeighborSetIter<'a, X, T, const N: usize>
452where
453  T: Trajectory<X, N>,
454{
455  outgoing: Edges<'a, Edge<X, T, N>, Directed>,
456  incoming: Edges<'a, Edge<X, T, N>, Directed>,
457  set_type: NeighborSet,
458}
459
460impl<'a, X, T, const N: usize> NeighborSetIter<'a, X, T, N>
461where
462  T: Trajectory<X, N>,
463{
464  fn new<NT>(
465    graph: &'a StableDiGraph<NT, Edge<X, T, N>>,
466    node: NodeIndex,
467    set_type: NeighborSet,
468  ) -> Self {
469    let outgoing = graph.edges_directed(node, Direction::Outgoing);
470    let incoming = graph.edges_directed(node, Direction::Incoming);
471    Self {
472      outgoing,
473      incoming,
474      set_type,
475    }
476  }
477}
478
479impl<'a, X, T, const N: usize> NeighborSetIter<'a, X, T, N>
480where
481  T: Trajectory<X, N>,
482{
483  pub fn next_edge(&mut self) -> Option<EdgeIndex> {
484    Some(self.next()?.0)
485  }
486
487  pub fn next_node(&mut self) -> Option<NodeIndex> {
488    Some(self.next()?.1)
489  }
490}
491
492impl<'a, X, T, const N: usize> Iterator for NeighborSetIter<'a, X, T, N>
493where
494  T: Trajectory<X, N>,
495{
496  type Item = (EdgeIndex, NodeIndex);
497
498  fn next(&mut self) -> Option<Self::Item> {
499    match self.set_type {
500      NeighborSet::Outgoing => loop {
501        let e = self.outgoing.next()?;
502        match e.weight().is_valid_dir(Direction::Outgoing) {
503          true => break Some((e.id(), e.target())),
504          false => continue,
505        }
506      },
507      NeighborSet::Incoming => loop {
508        let e = self.incoming.next()?;
509        match e.weight().is_valid_dir(Direction::Incoming) {
510          true => break Some((e.id(), e.source())),
511          false => continue,
512        }
513      },
514      NeighborSet::OriginalOutgoing => loop {
515        let e = self.outgoing.next()?;
516        match e.weight().is_original(Direction::Outgoing) {
517          true => break Some((e.id(), e.target())),
518          false => continue,
519        }
520      },
521      NeighborSet::OriginalIncoming => loop {
522        let e = self.incoming.next()?;
523        match e.weight().is_original(Direction::Incoming) {
524          true => break Some((e.id(), e.source())),
525          false => continue,
526        }
527      },
528      NeighborSet::RunningOutgoing => loop {
529        let e = self.outgoing.next()?;
530        match e.weight().is_running(Direction::Outgoing) {
531          true => break Some((e.id(), e.target())),
532          false => continue,
533        }
534      },
535      NeighborSet::RunningIncoming => loop {
536        let e = self.incoming.next()?;
537        match e.weight().is_running(Direction::Incoming) {
538          true => break Some((e.id(), e.source())),
539          false => continue,
540        }
541      },
542    }
543  }
544}
545
546/// An walker over different Neighbor sets of a node
547pub struct NeighborSetWalker {
548  outgoing: WalkNeighbors<DefaultIx>,
549  incoming: WalkNeighbors<DefaultIx>,
550  set_type: NeighborSet,
551}
552
553impl NeighborSetWalker {
554  fn new<X, T, NT, const N: usize>(
555    graph: &StableDiGraph<NT, Edge<X, T, N>>,
556    node: NodeIndex,
557    set_type: NeighborSet,
558  ) -> Self
559  where
560    T: Trajectory<X, N>,
561  {
562    let outgoing = graph.neighbors_directed(node, Direction::Outgoing).detach();
563    let incoming = graph.neighbors_directed(node, Direction::Incoming).detach();
564    Self {
565      outgoing,
566      incoming,
567      set_type,
568    }
569  }
570
571  /// Returns the node in the set along with the associated edge that connects that node
572  pub fn next<X, T, const N: usize>(
573    &mut self,
574    g: &RrtxGraph<X, T, N>,
575  ) -> Option<(EdgeIndex, NodeIndex)>
576  where
577    X: Scalar,
578    T: Trajectory<X, N>,
579  {
580    match self.set_type {
581      NeighborSet::Outgoing => loop {
582        let item = self.outgoing.next(&g.graph)?;
583        match g.graph[item.0].is_valid_dir(Direction::Outgoing) {
584          true => break Some(item),
585          false => continue,
586        }
587      },
588      NeighborSet::Incoming => loop {
589        let item = self.incoming.next(&g.graph)?;
590        match g.graph[item.0].is_valid_dir(Direction::Incoming) {
591          true => break Some(item),
592          false => continue,
593        }
594      },
595      NeighborSet::OriginalOutgoing => loop {
596        let item = self.outgoing.next(&g.graph)?;
597        match g.graph[item.0].is_original(Direction::Outgoing) {
598          true => break Some(item),
599          false => continue,
600        }
601      },
602      NeighborSet::OriginalIncoming => loop {
603        let item = self.incoming.next(&g.graph)?;
604        match g.graph[item.0].is_original(Direction::Incoming) {
605          true => break Some(item),
606          false => continue,
607        }
608      },
609      NeighborSet::RunningOutgoing => loop {
610        let item = self.outgoing.next(&g.graph)?;
611        match g.graph[item.0].is_running(Direction::Outgoing) {
612          true => break Some(item),
613          false => continue,
614        }
615      },
616      NeighborSet::RunningIncoming => loop {
617        let item = self.incoming.next(&g.graph)?;
618        match g.graph[item.0].is_running(Direction::Incoming) {
619          true => break Some(item),
620          false => continue,
621        }
622      },
623    }
624  }
625
626  /// Returns the next neighbor edge in the set
627  /// Note: might return
628  pub fn next_edge<X, T, const N: usize>(
629    &mut self,
630    g: &RrtxGraph<X, T, N>,
631  ) -> Option<EdgeIndex>
632  where
633    X: Scalar,
634    T: Trajectory<X, N>,
635  {
636    Some(self.next(g)?.0)
637  }
638
639  /// Returns the next neighbor node in the set
640  /// Note: only returns each node a maximum of once
641  pub fn next_node<X, T, const N: usize>(
642    &mut self,
643    g: &RrtxGraph<X, T, N>,
644  ) -> Option<NodeIndex>
645  where
646    X: Scalar,
647    T: Trajectory<X, N>,
648  {
649    Some(self.next(g)?.1)
650  }
651}
652
653/// An iterator over the children in the optimal subtree
654pub struct Childern<'a, X, T, const N: usize>
655where
656  T: Trajectory<X, N>,
657{
658  edges: Edges<'a, Edge<X, T, N>, Directed>,
659}
660
661impl<'a, X, T, const N: usize> Childern<'a, X, T, N>
662where
663  T: Trajectory<X, N>,
664{
665  fn new<NT>(
666    graph: &'a StableDiGraph<NT, Edge<X, T, N>>,
667    node: NodeIndex,
668  ) -> Self {
669    let edges = graph.edges_directed(node, Direction::Incoming);
670    Self { edges }
671  }
672}
673
674impl<'a, X, T, const N: usize> Childern<'a, X, T, N>
675where
676  T: Trajectory<X, N>,
677{
678  pub fn next_edge(&mut self) -> Option<EdgeIndex> {
679    Some(self.next()?.0)
680  }
681
682  pub fn next_node(&mut self) -> Option<NodeIndex> {
683    Some(self.next()?.1)
684  }
685}
686
687impl<'a, X, T, const N: usize> Iterator for Childern<'a, X, T, N>
688where
689  T: Trajectory<X, N>,
690{
691  type Item = (EdgeIndex, NodeIndex);
692
693  fn next(&mut self) -> Option<Self::Item> {
694    loop {
695      let e = self.edges.next()?;
696      match e.weight().is_parent() {
697        true => return Some((e.id(), e.source())),
698        false => continue,
699      }
700    }
701  }
702}
703
704/// An walker object over the children in the optimal subtree
705pub struct ChildernWalker {
706  neighbors: WalkNeighbors<DefaultIx>,
707}
708
709impl ChildernWalker {
710  fn new<X, T, NT, const N: usize>(
711    graph: &StableDiGraph<NT, Edge<X, T, N>>,
712    node: NodeIndex,
713  ) -> Self
714  where
715    T: Trajectory<X, N>,
716  {
717    let neighbors =
718      graph.neighbors_directed(node, Direction::Incoming).detach();
719    Self { neighbors }
720  }
721
722  pub fn next<X, T, const N: usize>(
723    &mut self,
724    g: &RrtxGraph<X, T, N>,
725  ) -> Option<(EdgeIndex, NodeIndex)>
726  where
727    X: Scalar,
728    T: Trajectory<X, N>,
729  {
730    loop {
731      let item = self.neighbors.next(&g.graph)?;
732      match g.graph[item.0].is_parent() {
733        true => return Some(item),
734        false => continue,
735      }
736    }
737  }
738
739  pub fn next_edge<X, T, const N: usize>(
740    &mut self,
741    g: &RrtxGraph<X, T, N>,
742  ) -> Option<EdgeIndex>
743  where
744    X: Scalar,
745    T: Trajectory<X, N>,
746  {
747    Some(self.next(g)?.0)
748  }
749
750  pub fn next_node<X, T, const N: usize>(
751    &mut self,
752    g: &RrtxGraph<X, T, N>,
753  ) -> Option<NodeIndex>
754  where
755    X: Scalar,
756    T: Trajectory<X, N>,
757  {
758    Some(self.next(g)?.1)
759  }
760}
761
762/// Iterator over all the node indices of the graph
763pub struct NodeIter<'a, X: Scalar, const N: usize> {
764  nodes: NodeIndices<'a, Node<X, N>>,
765}
766
767impl<'a, X: Scalar, const N: usize> NodeIter<'a, X, N> {
768  fn new<ET>(graph: &'a StableDiGraph<Node<X, N>, ET>) -> Self
769where {
770    Self {
771      nodes: graph.node_indices(),
772    }
773  }
774}
775
776impl<'a, X: Scalar, const N: usize> Iterator for NodeIter<'a, X, N> {
777  type Item = NodeIndex;
778
779  fn next(&mut self) -> Option<Self::Item> {
780    self.nodes.next()
781  }
782}
783
784/// Iterator over all the edge indices of the graph
785pub struct EdgeIter<'a, X, T, const N: usize>
786where
787  T: Trajectory<X, N>,
788{
789  edges: EdgeIndices<'a, Edge<X, T, N>>,
790}
791
792impl<'a, X, T, const N: usize> EdgeIter<'a, X, T, N>
793where
794  X: Scalar,
795  T: Trajectory<X, N>,
796{
797  fn new(graph: &'a StableDiGraph<Node<X, N>, Edge<X, T, N>>) -> Self {
798    Self {
799      edges: graph.edge_indices(),
800    }
801  }
802}
803
804impl<'a, X, T, const N: usize> Iterator for EdgeIter<'a, X, T, N>
805where
806  T: Trajectory<X, N>,
807{
808  type Item = EdgeIndex;
809
810  fn next(&mut self) -> Option<Self::Item> {
811    self.edges.next()
812  }
813}
814
815/// Iterator over edge indices of the graph in the optimal subtree
816pub struct OptimalPathIter<'a, X, T, const N: usize>
817where
818  X: Scalar,
819  T: Trajectory<X, N>,
820{
821  graph: &'a RrtxGraph<X, T, N>,
822  next_node: Option<NodeIndex>,
823}
824
825impl<'a, X, T, const N: usize> OptimalPathIter<'a, X, T, N>
826where
827  X: Scalar,
828  T: Trajectory<X, N>,
829{
830  fn new(graph: &'a RrtxGraph<X, T, N>, node: NodeIndex) -> Self {
831    Self {
832      graph,
833      next_node: Some(node),
834    }
835  }
836
837  pub fn detach(self) -> OptimalPathWalker {
838    OptimalPathWalker::new(self.next_node)
839  }
840}
841
842impl<'a, X, T, const N: usize> Iterator for OptimalPathIter<'a, X, T, N>
843where
844  X: Scalar,
845  T: Trajectory<X, N>,
846{
847  type Item = NodeIndex;
848
849  fn next(&mut self) -> Option<Self::Item> {
850    let node = self.next_node?;
851    match self.graph.parent(node) {
852      Some(parent) => self.next_node = Some(parent),
853      None => self.next_node = None,
854    }
855
856    Some(node)
857  }
858}
859
860/// Iterator over edge indices of the graph in the optimal subtree
861pub struct OptimalPathWalker {
862  next_node: Option<NodeIndex>,
863}
864
865impl OptimalPathWalker {
866  fn new(node: Option<NodeIndex>) -> Self {
867    Self { next_node: node }
868  }
869
870  pub fn next<X, T, const N: usize>(
871    &mut self,
872    g: &RrtxGraph<X, T, N>,
873  ) -> Option<NodeIndex>
874  where
875    X: Scalar,
876    T: Trajectory<X, N>,
877  {
878    let node = self.next_node?;
879    match g.parent(node) {
880      Some(parent) => self.next_node = Some(parent),
881      None => self.next_node = None,
882    }
883
884    Some(node)
885  }
886}
887
888#[derive(Clone, Debug, Serialize, Deserialize)]
889#[serde(bound(
890  serialize = "X: Serialize, T: Serialize",
891  deserialize = "X: DeserializeOwned, T: DeserializeOwned",
892))]
893pub struct RrtxGraph<X: Scalar, T: Trajectory<X, N>, const N: usize> {
894  /// The index of the goal node in the graph
895  goal_idx: NodeIndex,
896
897  /// The actual graph structure
898  graph: StableDiGraph<Node<X, N>, Edge<X, T, N>>,
899
900  /// All the nodes in the graph, used to test for existance faster
901  deleted: HashSet<NodeIndex>,
902
903  /// A temporary location to store orphaned nodes
904  orphans: HashSet<NodeIndex>,
905}
906
907impl<X, T, const N: usize> RrtxGraph<X, T, N>
908where
909  X: Scalar + Zero,
910  T: Trajectory<X, N>,
911{
912  /// Creates new graph with goal as the root node with cost zero
913  pub fn new(goal: SVector<X, N>) -> Self {
914    // Graph
915    let mut graph = StableDiGraph::new();
916    let goal_node = Node::with_cost(goal, Cost::zero());
917    let goal_idx = graph.add_node(goal_node);
918
919    // Nodes
920    let deleted = HashSet::new();
921
922    // Orphans
923    let orphans = HashSet::new();
924
925    Self {
926      goal_idx,
927      graph,
928      deleted,
929      orphans,
930    }
931  }
932}
933
934impl<X, T, const N: usize> RrtxGraph<X, T, N>
935where
936  X: Scalar,
937  T: Trajectory<X, N>,
938{
939  /// Returns the number of nodes in the graph
940  pub fn node_count(&self) -> usize {
941    self.graph.node_count()
942  }
943
944  /// Returns the index of the goal node
945  pub fn get_goal_idx(&self) -> NodeIndex {
946    self.goal_idx
947  }
948
949  /// Returns reference to the goal node
950  pub fn get_goal(&self) -> &Node<X, N> {
951    self.get_node(self.goal_idx)
952  }
953
954  /// Returns iterator over all nodes in the graph
955  pub fn all_nodes(&self) -> NodeIter<X, N> {
956    NodeIter::new(&self.graph)
957  }
958
959  /// Returns iterator over all edges in the graph
960  pub fn all_edges(&self) -> EdgeIter<X, T, N> {
961    EdgeIter::new(&self.graph)
962  }
963
964  /// Returns an iterator over the neighbors to a in the described set
965  pub fn neighbor_set(
966    &self,
967    a: NodeIndex,
968    set: NeighborSet,
969  ) -> NeighborSetIter<X, T, N> {
970    NeighborSetIter::new(&self.graph, a, set)
971  }
972
973  /// Returns an walker over the neighbors to a in the described set
974  pub fn neighbor_set_walker(
975    &self,
976    node: NodeIndex,
977    set: NeighborSet,
978  ) -> NeighborSetWalker {
979    NeighborSetWalker::new(&self.graph, node, set)
980  }
981
982  /// Returns the NodeIndex of the parent of a in the optimal subtree if exists
983  /// Returns None for the root node
984  pub fn parent(&self, node: NodeIndex) -> Option<NodeIndex> {
985    let parent_edge_idx = self.parent_edge(node)?;
986    debug_assert!(self.get_edge(parent_edge_idx).is_parent());
987    let (child, parent) = self.get_endpoints(parent_edge_idx);
988    debug_assert_eq!(child, node);
989    Some(parent)
990  }
991
992  /// Returns the edge directed at the nodes parent in the optimal subtree if exists
993  fn parent_edge(&self, node: NodeIndex) -> Option<EdgeIndex> {
994    self.get_node(node).parent_edge()
995  }
996
997  /// Looks up edge from node -> parent to see if parent is the parent of node
998  pub fn is_parent(&self, node: NodeIndex, parent: NodeIndex) -> bool {
999    match self.parent(node) {
1000      Some(true_parent) => true_parent == parent,
1001      None => false,
1002    }
1003  }
1004
1005  /// Returns iterator over all children of a in the optimal subtree.
1006  /// Iterator will be empty for leaf nodes
1007  pub fn children(&self, node: NodeIndex) -> Childern<X, T, N> {
1008    Childern::new(&self.graph, node)
1009  }
1010
1011  /// Returns walker over all children of a in the optimal subtree.
1012  /// Iterator will be empty for leaf nodes
1013  pub fn children_walker(&self, node: NodeIndex) -> ChildernWalker {
1014    ChildernWalker::new(&self.graph, node)
1015  }
1016
1017  /// Looks up edge from child -> node to see if node is the parent of child
1018  /// Returns None if they aren't connected
1019  pub fn is_child(&self, node: NodeIndex, child: NodeIndex) -> bool {
1020    self.is_parent(child, node)
1021  }
1022
1023  /// Add node and children of node to the internal set of orphans
1024  ///
1025  /// This garentees that the children of any orphan node are also marked as orphans
1026  /// as soon as they are added
1027  pub fn add_orphan(&mut self, node: NodeIndex) {
1028    // Add all childern node as orphans
1029    let mut queue = VecDeque::new();
1030    queue.push_back(node);
1031
1032    // While there are still nodes to process
1033    while let Some(node) = queue.pop_front() {
1034      // Set node as orphan
1035      if self.orphans.insert(node) {
1036        // if newly inserted, add childern to queue
1037        let mut children = self.children_walker(node);
1038        while let Some(child_idx) = children.next_node(&self) {
1039          queue.push_back(child_idx);
1040        }
1041      }
1042    }
1043  }
1044
1045  /// Remove node from the internal list of orphans, doesn't modifiy the graph
1046  pub fn remove_orphan(&mut self, node: NodeIndex) {
1047    let result = self.orphans.remove(&node);
1048    assert!(result);
1049  }
1050
1051  /// Checks if node is in the internal set of orphans
1052  pub fn is_orphan(&self, node: NodeIndex) -> bool {
1053    self.orphans.contains(&node)
1054  }
1055
1056  /// Get iterator over all orphans
1057  pub fn orphans(&self) -> impl Iterator<Item = NodeIndex> + '_ {
1058    self.orphans.iter().map(|&x| x)
1059  }
1060
1061  /// Adds a node to the graph and returns it's index
1062  pub fn add_node(&mut self, node: Node<X, N>) -> NodeIndex {
1063    self.graph.add_node(node)
1064  }
1065
1066  /// Adds a edge to the graph from a -> b with the associated data
1067  pub fn add_edge(
1068    &mut self,
1069    a: NodeIndex,
1070    b: NodeIndex,
1071    outgoing_neighbor_type: NeighborType,
1072    incoming_neighbor_type: NeighborType,
1073    is_parent: bool,
1074    trajectory: T,
1075  ) -> EdgeIndex {
1076    let edge = Edge::new(
1077      outgoing_neighbor_type,
1078      incoming_neighbor_type,
1079      is_parent,
1080      trajectory,
1081    );
1082    let edge_idx = self.graph.add_edge(a, b, edge);
1083
1084    if is_parent {
1085      let node = self.get_node_mut(a);
1086      node.set_parent_edge(edge_idx);
1087    }
1088
1089    edge_idx
1090  }
1091
1092  /// Switches the unique parent edge from node to the new_parent
1093  /// Still keeps a connection to the old parent
1094  /// Returns Ok if the new_parent was set correctly, otherwise Err
1095  /// Either way, returns the old_parent if was found
1096  pub fn change_parent(
1097    &mut self,
1098    node: NodeIndex,
1099    new_parent: NodeIndex,
1100  ) -> Result<Option<NodeIndex>, Option<NodeIndex>> {
1101    match self.remove_parent(node) {
1102      Some(old_parent) => match self.set_parent(node, new_parent) {
1103        Some(_) => Ok(Some(old_parent)),
1104        None => Err(Some(old_parent)),
1105      },
1106      None => match self.set_parent(node, new_parent) {
1107        Some(_) => Ok(None),
1108        None => Err(None),
1109      },
1110    }
1111  }
1112
1113  /// Blindly set the edge from node -> parent as a parent connection
1114  /// Returns none if edge wasn't found
1115  fn set_parent(
1116    &mut self,
1117    node_idx: NodeIndex,
1118    parent_idx: NodeIndex,
1119  ) -> Option<EdgeIndex> {
1120    let edge_idx = self.graph.find_edge(node_idx, parent_idx)?;
1121    let edge = self.get_edge_mut(edge_idx);
1122    edge.set_is_parent(true);
1123
1124    let node = self.get_node_mut(node_idx);
1125    node.set_parent_edge(edge_idx);
1126
1127    if node_idx.index() == 9091 {
1128      log::debug!(
1129        "{} is now parent of {}, edge_idx: {}",
1130        parent_idx.index(),
1131        node_idx.index(),
1132        edge_idx.index(),
1133      );
1134      assert_eq!(self.parent(node_idx).unwrap(), parent_idx);
1135    }
1136
1137    if parent_idx.index() == 9091 {
1138      log::debug!(
1139        "{} is now parent of {}",
1140        parent_idx.index(),
1141        node_idx.index()
1142      );
1143      log::debug!(
1144        "Parent of {}: {:?}",
1145        parent_idx.index(),
1146        self.parent(parent_idx)
1147      );
1148      log::debug!("{}: {:?}", parent_idx.index(), self.get_node(parent_idx));
1149    }
1150    Some(edge_idx)
1151  }
1152
1153  /// Blindly sets edge connected to parent node as not a parent
1154  /// Return Some parent index if found
1155  pub fn remove_parent(&mut self, node_idx: NodeIndex) -> Option<NodeIndex> {
1156    let old_parent = self.parent(node_idx)?;
1157
1158    let edge_idx = self.parent_edge(node_idx)?;
1159    let edge = self.get_edge_mut(edge_idx);
1160    edge.set_is_parent(false);
1161
1162    let node = self.get_node_mut(node_idx);
1163    node.clear_parent_edge();
1164
1165    if node_idx.index() == 9091 {
1166      log::debug!(
1167        "Removing parent of {}, old_parent: {:?}",
1168        node_idx.index(),
1169        old_parent.index()
1170      );
1171    }
1172
1173    Some(old_parent)
1174  }
1175
1176  /// Removes any outgoing or incoming running neighbor connection on the edge from node -> neighbor
1177  /// Returns None if the edge wasn't found
1178  pub fn remove_running_neighbor(
1179    &mut self,
1180    node: NodeIndex,
1181    neighbor: NodeIndex,
1182  ) -> Option<()> {
1183    let edge_idx = self.graph.find_edge(node, neighbor)?;
1184    let edge = self.get_edge_mut(edge_idx);
1185
1186    if edge.is_running(Direction::Outgoing) {
1187      edge.remove_connection(Direction::Outgoing);
1188    }
1189
1190    if edge.is_running(Direction::Incoming) {
1191      edge.remove_connection(Direction::Incoming);
1192    }
1193
1194    if !edge.is_valid() {
1195      assert!(!edge.is_parent());
1196      self.graph.remove_edge(edge_idx);
1197    }
1198
1199    Some(())
1200  }
1201
1202  /// Returns an iterator of edges over the optimal path to the goal node
1203  /// Returns None if no such path exists
1204  pub fn get_optimal_path(
1205    &self,
1206    node: NodeIndex,
1207  ) -> Option<OptimalPathIter<X, T, N>> {
1208    match self.is_orphan(node) {
1209      true => None,
1210      false => Some(OptimalPathIter::new(self, node)),
1211    }
1212  }
1213
1214  /// Returns a refernce the specified node
1215  ///
1216  /// panics if `idx` is invalid
1217  pub fn get_node(&self, idx: NodeIndex) -> &Node<X, N> {
1218    self.graph.index(idx)
1219  }
1220
1221  /// Returns a mutable refernce the specified node
1222  ///
1223  /// panics if `idx` is invalid
1224  pub fn get_node_mut(&mut self, idx: NodeIndex) -> &mut Node<X, N> {
1225    self.graph.index_mut(idx)
1226  }
1227
1228  /// Checks if the specified node exists
1229  pub fn check_node(&self, idx: NodeIndex) -> bool {
1230    !self.deleted.contains(&idx)
1231  }
1232
1233  /// Irreversibly delete the specified node
1234  ///
1235  /// panics if `idx` is invalid
1236  pub fn delete_node(&mut self, idx: NodeIndex) {
1237    // if the node has any children, remove the parent connection stored in the node,
1238    // the edge will be removed by petgraph
1239    let mut children = self.children_walker(idx);
1240    while let Some(child_idx) = children.next_node(&self) {
1241      self.get_node_mut(child_idx).clear_parent_edge();
1242    }
1243
1244    self.graph.remove_node(idx);
1245    self.deleted.insert(idx);
1246    self.orphans.remove(&idx);
1247  }
1248
1249  /// Returns a refernce the specified edge
1250  ///
1251  /// panics if `idx` is invalid
1252  pub fn get_edge(&self, idx: EdgeIndex) -> &Edge<X, T, N> {
1253    self.graph.index(idx)
1254  }
1255
1256  /// Returns a mutable refernce the specified edge
1257  ///
1258  /// panics if `idx` is invalid
1259  fn get_edge_mut(&mut self, idx: EdgeIndex) -> &mut Edge<X, T, N> {
1260    self.graph.index_mut(idx)
1261  }
1262
1263  /// Returns source and target endpoints of an edge
1264  ///
1265  /// panics if `idx` is invalid
1266  pub fn get_endpoints(&self, idx: EdgeIndex) -> (NodeIndex, NodeIndex) {
1267    self.graph.edge_endpoints(idx).unwrap()
1268  }
1269
1270  /// Returns the trajectory stored at the specified edge if exists
1271  ///
1272  /// panics if `idx` is invalid
1273  pub fn get_trajectory(
1274    &self,
1275    idx: EdgeIndex,
1276  ) -> Option<FullTrajRefOwned<X, T, N>> {
1277    let (start_idx, end_idx) = self.get_endpoints(idx);
1278    let start = self.get_node(start_idx).state();
1279    let end = self.get_node(end_idx).state();
1280    let traj_data = self.get_edge(idx).trajectory()?;
1281    Some(FullTrajRefOwned::new(start, end, traj_data))
1282  }
1283
1284  /// Updates the trajectory along the specified edge
1285  ///
1286  /// panics if `idx` is invalid
1287  pub fn update_trajectory(&mut self, idx: EdgeIndex, new_trajectory: T) {
1288    let edge = self.graph.index_mut(idx);
1289    edge.set_trajectory(Some(new_trajectory));
1290  }
1291
1292  /// Sets the cost of the given edge to infinity
1293  ///
1294  /// panics if `idx` is invalid
1295  pub fn set_infinite_edge_cost(&mut self, idx: EdgeIndex) {
1296    let edge = self.graph.index_mut(idx);
1297    edge.set_trajectory(None);
1298  }
1299}
1300
1301impl<X, T, const N: usize> RrtxGraph<X, T, N>
1302where
1303  X: Scalar + Float,
1304  T: Trajectory<X, N>,
1305{
1306  pub fn get_trajectory_cost(&self, idx: EdgeIndex) -> X {
1307    match self.get_trajectory(idx) {
1308      Some(trajectory) => trajectory.cost(),
1309      None => X::infinity(),
1310    }
1311  }
1312}
1313
1314#[cfg(test)]
1315mod tests {
1316
1317  use crate::trajectories::EuclideanTrajectory;
1318
1319  use super::*;
1320
1321  #[test]
1322  fn test_rrtx_graph_neighbor_set_iter() {
1323    let goal_coord = [1.5, 1.5].into();
1324
1325    let mut g = RrtxGraph::new(goal_coord);
1326    let goal = g.get_goal_idx();
1327
1328    let n1_coord = [2.0, 2.0].into();
1329    let n1 = Node::with_cost(n1_coord, Cost::new(0.1, 0.2));
1330    let n1 = g.graph.add_node(n1);
1331
1332    g.add_edge(
1333      n1,
1334      goal,
1335      NeighborType::Original,
1336      NeighborType::Running,
1337      true,
1338      EuclideanTrajectory::new(),
1339    );
1340    g.add_edge(
1341      goal,
1342      n1,
1343      NeighborType::Running,
1344      NeighborType::Original,
1345      false,
1346      EuclideanTrajectory::new(),
1347    );
1348
1349    let n2_coord = [-2.0, -2.0].into();
1350    let n2 = Node::with_cost(n2_coord, Cost::new(0.5, 0.6));
1351    let n2 = g.graph.add_node(n2);
1352
1353    g.add_edge(
1354      n2,
1355      goal,
1356      NeighborType::Original,
1357      NeighborType::Running,
1358      true,
1359      EuclideanTrajectory::new(),
1360    );
1361    g.add_edge(
1362      goal,
1363      n2,
1364      NeighborType::Running,
1365      NeighborType::Original,
1366      false,
1367      EuclideanTrajectory::new(),
1368    );
1369
1370    g.add_edge(
1371      n2,
1372      n1,
1373      NeighborType::Original,
1374      NeighborType::Running,
1375      false,
1376      EuclideanTrajectory::new(),
1377    );
1378    // No edge possible from n1 -> n2
1379
1380    // All Outgoing neighbors
1381    let mut iter = g.neighbor_set(goal, NeighborSet::Outgoing);
1382    assert_eq!(iter.next_node(), Some(n2));
1383    assert_eq!(iter.next_node(), Some(n1));
1384    assert_eq!(iter.next_node(), None);
1385    assert_eq!(iter.next_node(), None);
1386
1387    let mut iter = g.neighbor_set(n1, NeighborSet::Outgoing);
1388    assert_eq!(iter.next_node(), Some(goal));
1389    assert_eq!(iter.next_node(), None);
1390    assert_eq!(iter.next_node(), None);
1391
1392    let mut iter = g.neighbor_set(n2, NeighborSet::Outgoing);
1393    assert_eq!(iter.next_node(), Some(n1));
1394    assert_eq!(iter.next_node(), Some(goal));
1395    assert_eq!(iter.next_node(), None);
1396    assert_eq!(iter.next_node(), None);
1397
1398    // All Incoming neighbors
1399    let mut iter = g.neighbor_set(goal, NeighborSet::Incoming);
1400    assert_eq!(iter.next_node(), Some(n2));
1401    assert_eq!(iter.next_node(), Some(n1));
1402    assert_eq!(iter.next_node(), None);
1403    assert_eq!(iter.next_node(), None);
1404
1405    let mut iter = g.neighbor_set(n1, NeighborSet::Incoming);
1406    assert_eq!(iter.next_node(), Some(n2));
1407    assert_eq!(iter.next_node(), Some(goal));
1408    assert_eq!(iter.next_node(), None);
1409    assert_eq!(iter.next_node(), None);
1410
1411    let mut iter = g.neighbor_set(n2, NeighborSet::Incoming);
1412    assert_eq!(iter.next_node(), Some(goal));
1413    assert_eq!(iter.next_node(), None);
1414    assert_eq!(iter.next_node(), None);
1415
1416    // Original Outgoing neighbors
1417    let mut iter = g.neighbor_set(goal, NeighborSet::OriginalOutgoing);
1418    assert_eq!(iter.next_node(), None);
1419    assert_eq!(iter.next_node(), None);
1420
1421    let mut iter = g.neighbor_set(n1, NeighborSet::OriginalOutgoing);
1422    assert_eq!(iter.next_node(), Some(goal));
1423    assert_eq!(iter.next_node(), None);
1424    assert_eq!(iter.next_node(), None);
1425
1426    let mut iter = g.neighbor_set(n2, NeighborSet::OriginalOutgoing);
1427    assert_eq!(iter.next_node(), Some(n1));
1428    assert_eq!(iter.next_node(), Some(goal));
1429    assert_eq!(iter.next_node(), None);
1430    assert_eq!(iter.next_node(), None);
1431
1432    // Running Outgoing neighbors
1433    let mut iter = g.neighbor_set(goal, NeighborSet::RunningOutgoing);
1434    assert_eq!(iter.next_node(), Some(n2));
1435    assert_eq!(iter.next_node(), Some(n1));
1436    assert_eq!(iter.next_node(), None);
1437    assert_eq!(iter.next_node(), None);
1438
1439    let mut iter = g.neighbor_set(n1, NeighborSet::RunningOutgoing);
1440    assert_eq!(iter.next_node(), None);
1441    assert_eq!(iter.next_node(), None);
1442
1443    let mut iter = g.neighbor_set(n2, NeighborSet::RunningOutgoing);
1444    assert_eq!(iter.next_node(), None);
1445    assert_eq!(iter.next_node(), None);
1446
1447    // Original Incoming neighbors
1448    let mut iter = g.neighbor_set(goal, NeighborSet::OriginalIncoming);
1449    assert_eq!(iter.next_node(), None);
1450    assert_eq!(iter.next_node(), None);
1451
1452    let mut iter = g.neighbor_set(n1, NeighborSet::OriginalIncoming);
1453    assert_eq!(iter.next_node(), Some(goal));
1454    assert_eq!(iter.next_node(), None);
1455    assert_eq!(iter.next_node(), None);
1456
1457    let mut iter = g.neighbor_set(n2, NeighborSet::OriginalIncoming);
1458    assert_eq!(iter.next_node(), Some(goal));
1459    assert_eq!(iter.next_node(), None);
1460    assert_eq!(iter.next_node(), None);
1461
1462    // Running Incoming neighbors
1463    let mut iter = g.neighbor_set(goal, NeighborSet::RunningIncoming);
1464    assert_eq!(iter.next_node(), Some(n2));
1465    assert_eq!(iter.next_node(), Some(n1));
1466    assert_eq!(iter.next_node(), None);
1467    assert_eq!(iter.next_node(), None);
1468
1469    let mut iter = g.neighbor_set(n1, NeighborSet::RunningIncoming);
1470    assert_eq!(iter.next_node(), Some(n2));
1471    assert_eq!(iter.next_node(), None);
1472    assert_eq!(iter.next_node(), None);
1473
1474    let mut iter = g.neighbor_set(n2, NeighborSet::RunningIncoming);
1475    assert_eq!(iter.next_node(), None);
1476    assert_eq!(iter.next_node(), None);
1477  }
1478
1479  #[test]
1480  fn test_rrtx_graph_neighbor_set_walker() {
1481    let goal_coord = [1.5, 1.5].into();
1482
1483    let mut g = RrtxGraph::new(goal_coord);
1484    let goal = g.get_goal_idx();
1485
1486    let n1_coord = [2.0, 2.0].into();
1487    let n1 = Node::with_cost(n1_coord, Cost::new(0.1, 0.2));
1488    let n1 = g.graph.add_node(n1);
1489
1490    g.add_edge(
1491      n1,
1492      goal,
1493      NeighborType::Original,
1494      NeighborType::Running,
1495      true,
1496      EuclideanTrajectory::new(),
1497    );
1498    g.add_edge(
1499      goal,
1500      n1,
1501      NeighborType::Running,
1502      NeighborType::Original,
1503      false,
1504      EuclideanTrajectory::new(),
1505    );
1506
1507    let n2_coord = [-2.0, -2.0].into();
1508    let n2 = Node::with_cost(n2_coord, Cost::new(0.5, 0.6));
1509    let n2 = g.graph.add_node(n2);
1510
1511    g.add_edge(
1512      n2,
1513      goal,
1514      NeighborType::Original,
1515      NeighborType::Running,
1516      true,
1517      EuclideanTrajectory::new(),
1518    );
1519    g.add_edge(
1520      goal,
1521      n2,
1522      NeighborType::Running,
1523      NeighborType::Original,
1524      false,
1525      EuclideanTrajectory::new(),
1526    );
1527
1528    g.add_edge(
1529      n2,
1530      n1,
1531      NeighborType::Original,
1532      NeighborType::Running,
1533      false,
1534      EuclideanTrajectory::new(),
1535    );
1536    // No edge possible from n1 -> n2
1537
1538    // All Outgoing neighbors
1539    let mut iter = g.neighbor_set_walker(goal, NeighborSet::Outgoing);
1540    assert_eq!(iter.next_node(&g), Some(n2));
1541    assert_eq!(iter.next_node(&g), Some(n1));
1542    assert_eq!(iter.next_node(&g), None);
1543    assert_eq!(iter.next_node(&g), None);
1544
1545    let mut iter = g.neighbor_set_walker(n1, NeighborSet::Outgoing);
1546    assert_eq!(iter.next_node(&g), Some(goal));
1547    assert_eq!(iter.next_node(&g), None);
1548    assert_eq!(iter.next_node(&g), None);
1549
1550    let mut iter = g.neighbor_set_walker(n2, NeighborSet::Outgoing);
1551    assert_eq!(iter.next_node(&g), Some(n1));
1552    assert_eq!(iter.next_node(&g), Some(goal));
1553    assert_eq!(iter.next_node(&g), None);
1554    assert_eq!(iter.next_node(&g), None);
1555
1556    // All Incoming neighbors
1557    let mut iter = g.neighbor_set_walker(goal, NeighborSet::Incoming);
1558    assert_eq!(iter.next_node(&g), Some(n2));
1559    assert_eq!(iter.next_node(&g), Some(n1));
1560    assert_eq!(iter.next_node(&g), None);
1561    assert_eq!(iter.next_node(&g), None);
1562
1563    let mut iter = g.neighbor_set_walker(n1, NeighborSet::Incoming);
1564    assert_eq!(iter.next_node(&g), Some(n2));
1565    assert_eq!(iter.next_node(&g), Some(goal));
1566    assert_eq!(iter.next_node(&g), None);
1567    assert_eq!(iter.next_node(&g), None);
1568
1569    let mut iter = g.neighbor_set_walker(n2, NeighborSet::Incoming);
1570    assert_eq!(iter.next_node(&g), Some(goal));
1571    assert_eq!(iter.next_node(&g), None);
1572    assert_eq!(iter.next_node(&g), None);
1573
1574    // Original Outgoing neighbors
1575    let mut iter = g.neighbor_set_walker(goal, NeighborSet::OriginalOutgoing);
1576    assert_eq!(iter.next_node(&g), None);
1577    assert_eq!(iter.next_node(&g), None);
1578
1579    let mut iter = g.neighbor_set_walker(n1, NeighborSet::OriginalOutgoing);
1580    assert_eq!(iter.next_node(&g), Some(goal));
1581    assert_eq!(iter.next_node(&g), None);
1582    assert_eq!(iter.next_node(&g), None);
1583
1584    let mut iter = g.neighbor_set_walker(n2, NeighborSet::OriginalOutgoing);
1585    assert_eq!(iter.next_node(&g), Some(n1));
1586    assert_eq!(iter.next_node(&g), Some(goal));
1587    assert_eq!(iter.next_node(&g), None);
1588    assert_eq!(iter.next_node(&g), None);
1589
1590    // Running Outgoing neighbors
1591    let mut iter = g.neighbor_set_walker(goal, NeighborSet::RunningOutgoing);
1592    assert_eq!(iter.next_node(&g), Some(n2));
1593    assert_eq!(iter.next_node(&g), Some(n1));
1594    assert_eq!(iter.next_node(&g), None);
1595    assert_eq!(iter.next_node(&g), None);
1596
1597    let mut iter = g.neighbor_set_walker(n1, NeighborSet::RunningOutgoing);
1598    assert_eq!(iter.next_node(&g), None);
1599    assert_eq!(iter.next_node(&g), None);
1600
1601    let mut iter = g.neighbor_set_walker(n2, NeighborSet::RunningOutgoing);
1602    assert_eq!(iter.next_node(&g), None);
1603    assert_eq!(iter.next_node(&g), None);
1604
1605    // Original Incoming neighbors
1606    let mut iter = g.neighbor_set_walker(goal, NeighborSet::OriginalIncoming);
1607    assert_eq!(iter.next_node(&g), None);
1608    assert_eq!(iter.next_node(&g), None);
1609
1610    let mut iter = g.neighbor_set_walker(n1, NeighborSet::OriginalIncoming);
1611    assert_eq!(iter.next_node(&g), Some(goal));
1612    assert_eq!(iter.next_node(&g), None);
1613    assert_eq!(iter.next_node(&g), None);
1614
1615    let mut iter = g.neighbor_set_walker(n2, NeighborSet::OriginalIncoming);
1616    assert_eq!(iter.next_node(&g), Some(goal));
1617    assert_eq!(iter.next_node(&g), None);
1618    assert_eq!(iter.next_node(&g), None);
1619
1620    // Running Incoming neighbors
1621    let mut iter = g.neighbor_set_walker(goal, NeighborSet::RunningIncoming);
1622    assert_eq!(iter.next_node(&g), Some(n2));
1623    assert_eq!(iter.next_node(&g), Some(n1));
1624    assert_eq!(iter.next_node(&g), None);
1625    assert_eq!(iter.next_node(&g), None);
1626
1627    let mut iter = g.neighbor_set_walker(n1, NeighborSet::RunningIncoming);
1628    assert_eq!(iter.next_node(&g), Some(n2));
1629    assert_eq!(iter.next_node(&g), None);
1630    assert_eq!(iter.next_node(&g), None);
1631
1632    let mut iter = g.neighbor_set_walker(n2, NeighborSet::RunningIncoming);
1633    assert_eq!(iter.next_node(&g), None);
1634    assert_eq!(iter.next_node(&g), None);
1635  }
1636
1637  #[test]
1638  fn test_rrtx_graph_parent() {
1639    let goal_coord = [1.5, 1.5].into();
1640
1641    let mut g = RrtxGraph::new(goal_coord);
1642    let goal = g.get_goal_idx();
1643
1644    let n1_coord = [2.0, 2.0].into();
1645    let n1 = Node::with_cost(n1_coord, Cost::new(0.1, 0.2));
1646    let n1 = g.graph.add_node(n1);
1647
1648    g.add_edge(
1649      n1,
1650      goal,
1651      NeighborType::Original,
1652      NeighborType::Running,
1653      true,
1654      EuclideanTrajectory::new(),
1655    );
1656    g.add_edge(
1657      goal,
1658      n1,
1659      NeighborType::Running,
1660      NeighborType::Original,
1661      false,
1662      EuclideanTrajectory::new(),
1663    );
1664
1665    let n2_coord = [-2.0, -2.0].into();
1666    let n2 = Node::with_cost(n2_coord, Cost::new(0.5, 0.6));
1667    let n2 = g.graph.add_node(n2);
1668
1669    g.add_edge(
1670      n2,
1671      goal,
1672      NeighborType::Original,
1673      NeighborType::Running,
1674      true,
1675      EuclideanTrajectory::new(),
1676    );
1677    g.add_edge(
1678      goal,
1679      n2,
1680      NeighborType::Running,
1681      NeighborType::Original,
1682      false,
1683      EuclideanTrajectory::new(),
1684    );
1685
1686    g.add_edge(
1687      n2,
1688      n1,
1689      NeighborType::Original,
1690      NeighborType::Running,
1691      false,
1692      EuclideanTrajectory::new(),
1693    );
1694    // No edge possible from n1 -> n2
1695
1696    // Parents
1697    assert_eq!(g.parent(goal), None);
1698    assert_eq!(g.parent(n1), Some(goal));
1699    assert_eq!(g.parent(n2), Some(goal));
1700  }
1701
1702  #[test]
1703  fn test_rrtx_graph_children() {
1704    let goal_coord = [1.5, 1.5].into();
1705
1706    let mut g = RrtxGraph::new(goal_coord);
1707    let goal = g.get_goal_idx();
1708
1709    let n1_coord = [2.0, 2.0].into();
1710    let n1 = Node::with_cost(n1_coord, Cost::new(0.1, 0.2));
1711    let n1 = g.graph.add_node(n1);
1712
1713    g.add_edge(
1714      n1,
1715      goal,
1716      NeighborType::Original,
1717      NeighborType::Running,
1718      true,
1719      EuclideanTrajectory::new(),
1720    );
1721    g.add_edge(
1722      goal,
1723      n1,
1724      NeighborType::Running,
1725      NeighborType::Original,
1726      false,
1727      EuclideanTrajectory::new(),
1728    );
1729
1730    let n2_coord = [-2.0, -2.0].into();
1731    let n2 = Node::with_cost(n2_coord, Cost::new(0.5, 0.6));
1732    let n2 = g.graph.add_node(n2);
1733
1734    g.add_edge(
1735      n2,
1736      goal,
1737      NeighborType::Original,
1738      NeighborType::Running,
1739      true,
1740      EuclideanTrajectory::new(),
1741    );
1742    g.add_edge(
1743      goal,
1744      n2,
1745      NeighborType::Running,
1746      NeighborType::Original,
1747      false,
1748      EuclideanTrajectory::new(),
1749    );
1750
1751    g.add_edge(
1752      n2,
1753      n1,
1754      NeighborType::Original,
1755      NeighborType::Running,
1756      false,
1757      EuclideanTrajectory::new(),
1758    );
1759    // No edge possible from n1 -> n2
1760
1761    // Childern
1762    let mut iter = g.children(goal);
1763    assert_eq!(iter.next_node(), Some(n2));
1764    assert_eq!(iter.next_node(), Some(n1));
1765    assert_eq!(iter.next_node(), None);
1766
1767    let mut iter = g.children(n1);
1768    assert_eq!(iter.next_node(), None);
1769
1770    let mut iter = g.children(n2);
1771    assert_eq!(iter.next_node(), None);
1772  }
1773
1774  #[test]
1775  fn test_rrtx_graph_children_walker() {
1776    let goal_coord = [1.5, 1.5].into();
1777
1778    let mut g = RrtxGraph::new(goal_coord);
1779    let goal = g.get_goal_idx();
1780
1781    let n1_coord = [2.0, 2.0].into();
1782    let n1 = Node::with_cost(n1_coord, Cost::new(0.1, 0.2));
1783    let n1 = g.graph.add_node(n1);
1784
1785    g.add_edge(
1786      n1,
1787      goal,
1788      NeighborType::Original,
1789      NeighborType::Running,
1790      true,
1791      EuclideanTrajectory::new(),
1792    );
1793    g.add_edge(
1794      goal,
1795      n1,
1796      NeighborType::Running,
1797      NeighborType::Original,
1798      false,
1799      EuclideanTrajectory::new(),
1800    );
1801
1802    let n2_coord = [-2.0, -2.0].into();
1803    let n2 = Node::with_cost(n2_coord, Cost::new(0.5, 0.6));
1804    let n2 = g.graph.add_node(n2);
1805
1806    g.add_edge(
1807      n2,
1808      goal,
1809      NeighborType::Original,
1810      NeighborType::Running,
1811      true,
1812      EuclideanTrajectory::new(),
1813    );
1814    g.add_edge(
1815      goal,
1816      n2,
1817      NeighborType::Running,
1818      NeighborType::Original,
1819      false,
1820      EuclideanTrajectory::new(),
1821    );
1822
1823    g.add_edge(
1824      n2,
1825      n1,
1826      NeighborType::Original,
1827      NeighborType::Running,
1828      false,
1829      EuclideanTrajectory::new(),
1830    );
1831    // No edge possible from n1 -> n2
1832
1833    // Childern Walker
1834    let mut iter = g.children_walker(goal);
1835    assert_eq!(iter.next_node(&g), Some(n2));
1836    assert_eq!(iter.next_node(&g), Some(n1));
1837    assert_eq!(iter.next_node(&g), None);
1838
1839    let mut iter = g.children_walker(n1);
1840    assert_eq!(iter.next_node(&g), None);
1841
1842    let mut iter = g.children_walker(n2);
1843    assert_eq!(iter.next_node(&g), None);
1844  }
1845
1846  #[test]
1847  fn test_get_bit() {
1848    let edge_1 = Edge::<f32, _, 3>::new(
1849      NeighborType::Original,
1850      NeighborType::Running,
1851      true,
1852      EuclideanTrajectory::new(),
1853    );
1854    let edge_2 = Edge::<f32, _, 3>::new(
1855      NeighborType::Original,
1856      NeighborType::Running,
1857      false,
1858      EuclideanTrajectory::new(),
1859    );
1860
1861    assert_eq!(edge_1.is_parent(), true);
1862    assert_eq!(edge_2.is_parent(), false);
1863  }
1864}
path_planning/rrtx/rrtx_graph.rs

path_planning/rrtx/
rrtx_graph.rs
