mapf/algorithm/
tree.rs

1/*
2 * Copyright (C) 2022 Open Source Robotics Foundation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 *     http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16*/
17
18use crate::{
19    algorithm::{MinimumCostBound, Path, QueueLength},
20    domain::{ClosedSet, ClosedStatus},
21    error::ThisError,
22};
23use std::{
24    cmp::{Ordering, Reverse},
25    collections::BinaryHeap,
26};
27
28/// A data structure for storing, managing, and growing a tree of nodes.
29#[derive(Debug)]
30pub struct Tree<Closed, Node, Cost> {
31    /// The set of tree nodes that have been closed. Closing a node usually
32    /// suggests that the node represents the shortest path to the node's state
33    pub closed_set: Closed,
34    /// The frontier queue for the tree, representing edge nodes of the tree
35    /// that should be expanded next, in order from highest to lowest priority.
36    pub queue: TreeFrontierQueue<Cost>,
37    /// The memory arena for the tree which keeps track of all node data.
38    pub arena: Vec<Node>,
39}
40
41impl<Closed, Node: TreeNode> Tree<Closed, Node, Node::Cost> {
42    pub fn new(closed_set: Closed) -> Self
43    where
44        Node: TreeNode,
45        Node::Cost: Ord,
46    {
47        Self {
48            closed_set,
49            queue: Default::default(),
50            arena: Default::default(),
51        }
52    }
53
54    pub fn push_node(&mut self, node: Node) -> Result<(), TreeError>
55    where
56        Node: TreeNode,
57        Closed: ClosedSet<Node::State, usize>,
58        Node::Cost: Ord,
59    {
60        if let ClosedStatus::Closed(prior) = self.closed_set.status(node.state()) {
61            if let Some(prior) = self.arena.get(*prior) {
62                if prior.cost() <= node.cost() {
63                    // The state is already closed with a lower-cost node, so
64                    // we should not push this new node.
65                    return Ok(());
66                }
67            } else {
68                // The closed set is referencing a node that does not exist in
69                // the memory arena. This is a major bug.
70                return Err(TreeError::BrokenReference(*prior));
71            }
72        }
73
74        let node_id = self.arena.len();
75        let evaluation = node.queue_evaluation();
76        let bias = node.queue_bias();
77        self.arena.push(node);
78        self.queue.push(Reverse(TreeQueueTicket {
79            node_id,
80            bias,
81            evaluation,
82        }));
83        Ok(())
84    }
85}
86
87pub trait TreeNode {
88    /// The type used to describe the state of the node.
89    type State;
90
91    /// The type of action that can be taken from one node to another.
92    type Action;
93
94    /// The type used to decide whether to prefer one node over another. Lower
95    /// values are preferable.
96    type Cost;
97
98    /// Get the state of the node.
99    fn state(&self) -> &Self::State;
100
101    /// If the node has a parent, get the identity of that parent and the action
102    /// used to arrive from it.
103    fn parent(&self) -> Option<(usize, &Self::Action)>;
104
105    /// Get the actual cost of arriving at this node from its initial state.
106    fn cost(&self) -> Self::Cost;
107
108    /// Evaluate this node for its placement in the search queue. For an
109    /// uninformed node this would simply return its aggregated cost. For an
110    /// informed node this would return its aggregated cost plus its remaining
111    /// cost estimate.
112    fn queue_evaluation(&self) -> Self::Cost;
113
114    /// Give a bias to this node. When queue_evaluation is exactly equal, the
115    /// node will be ordered by this bias instead. Higher bias will push it
116    /// later in the queue. For an informed node this could be its remaining
117    /// cost estimate.
118    fn queue_bias(&self) -> Option<Self::Cost>;
119}
120
121#[derive(Debug, Clone, Copy)]
122pub struct TreeQueueTicket<Cost> {
123    pub evaluation: Cost,
124    pub bias: Option<Cost>,
125    pub node_id: usize,
126}
127
128pub type TreeFrontierQueue<Cost> = BinaryHeap<Reverse<TreeQueueTicket<Cost>>>;
129
130impl<Cost: PartialEq> PartialEq for TreeQueueTicket<Cost> {
131    fn eq(&self, other: &Self) -> bool {
132        self.evaluation.eq(&other.evaluation)
133    }
134}
135
136impl<Cost: Eq> Eq for TreeQueueTicket<Cost> {}
137
138impl<Cost: PartialOrd> PartialOrd for TreeQueueTicket<Cost> {
139    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
140        match self.evaluation.partial_cmp(&other.evaluation) {
141            // TODO(@mxgrey): Let downstream users to define criteria for
142            // deciding when two evaluations are close enough that the biases
143            // should be compared instead. Small floating point numerical
144            // residue could cause one evaluation to be unfairly favored over
145            // another that should be considered equal.
146            Some(Ordering::Equal) => {
147                if let (Some(l), Some(r)) = (&self.bias, &other.bias) {
148                    l.partial_cmp(r)
149                } else {
150                    Some(Ordering::Equal)
151                }
152            }
153            value => value,
154        }
155    }
156}
157
158impl<Cost: Ord> Ord for TreeQueueTicket<Cost> {
159    fn cmp(&self, other: &Self) -> Ordering {
160        match self.evaluation.cmp(&other.evaluation) {
161            Ordering::Equal => {
162                if let (Some(l), Some(r)) = (&self.bias, &other.bias) {
163                    l.cmp(r)
164                } else {
165                    Ordering::Equal
166                }
167            }
168            value => value,
169        }
170    }
171}
172
173pub trait NodeContainer<N: TreeNode> {
174    fn get_node(&self, index: usize) -> Result<&N, TreeError>;
175    fn retrace(&self, index: usize) -> Result<Path<N::State, N::Action, N::Cost>, TreeError>;
176}
177
178impl<N: TreeNode> NodeContainer<N> for Vec<N>
179where
180    N::State: Clone,
181    N::Action: Clone,
182{
183    fn get_node(&self, index: usize) -> Result<&N, TreeError> {
184        self.get(index)
185            .ok_or_else(|| TreeError::BrokenReference(index))
186    }
187
188    fn retrace(&self, node_id: usize) -> Result<Path<N::State, N::Action, N::Cost>, TreeError> {
189        let total_cost = self.get_node(node_id)?.cost();
190        let mut initial_node_id = node_id;
191        let mut next_node_id = Some(node_id);
192        let mut sequence = Vec::new();
193        while let Some(current_node_id) = next_node_id {
194            initial_node_id = current_node_id;
195            let node = self.get_node(current_node_id)?;
196            next_node_id = if let Some((parent_id, action)) = node.parent() {
197                sequence.push((action.clone(), node.state().clone()));
198                Some(parent_id)
199            } else {
200                None
201            };
202        }
203
204        sequence.reverse();
205
206        let initial_state = self.get_node(initial_node_id)?.state().clone();
207        Ok(Path {
208            initial_state,
209            sequence,
210            total_cost,
211        })
212    }
213}
214
215#[derive(ThisError, Debug)]
216pub enum TreeError {
217    #[error(
218        "A node [{0}] is referenced but does not exist in the search memory. \
219    This is a critical implementation error, please report this to the mapf developers."
220    )]
221    BrokenReference(usize),
222}
223
224impl<Closed, Node: TreeNode> QueueLength for Tree<Closed, Node, Node::Cost> {
225    fn queue_length(&self) -> usize {
226        self.queue.len()
227    }
228}
229
230impl<Closed, Node: TreeNode> MinimumCostBound for Tree<Closed, Node, Node::Cost>
231where
232    Node::Cost: Clone,
233{
234    type Cost = Node::Cost;
235    fn minimum_cost_bound(&self) -> Option<Self::Cost> {
236        self.queue.peek().map(|n| n.0.evaluation.clone())
237    }
238}
239
240/// This is implemented based off of
241/// `std::collections::binary_head::IntoIterSorted` which is an unstable feature
242/// of the standard library.
243pub struct BinaryHeapIntoIterSorted<T> {
244    inner: BinaryHeap<T>,
245}
246
247impl<T: Ord> Iterator for BinaryHeapIntoIterSorted<T> {
248    type Item = T;
249    fn next(&mut self) -> Option<Self::Item> {
250        self.inner.pop()
251    }
252
253    fn size_hint(&self) -> (usize, Option<usize>) {
254        let exact = self.inner.len();
255        (exact, Some(exact))
256    }
257}
258
259pub trait IntoIterSorted<T> {
260    fn binary_heap_into_iter_sorted(self) -> BinaryHeapIntoIterSorted<T>;
261}
262
263impl<T: Ord> IntoIterSorted<T> for BinaryHeap<T> {
264    fn binary_heap_into_iter_sorted(self) -> BinaryHeapIntoIterSorted<T> {
265        BinaryHeapIntoIterSorted { inner: self }
266    }
267}
mapf/algorithm/tree.rs

mapf/algorithm/
tree.rs
