Add suboptimal search option

mapf-viz/examples/grid.rs

@@ -1009,7 +1009,11 @@ impl App {
 
             self.canvas.program.layers.3.searches.clear();
 
-            for ticket in self.search_memory.iter().take(self.debug_ticket_size as usize) {
+            for ticket in self
+                .search_memory
+                .iter()
+                .take(self.debug_ticket_size as usize)
+            {
                 if let Some(mt) = search
                     .memory()
                     .0
@@ -1319,9 +1323,9 @@ impl App {
         let mut total_length = 0.0;
         for solution in solution_node.proposals.values() {
             total_length += solution.meta.trajectory.windows(2).fold(0.0, |acc, w| {
-                (w[0].position.translation.vector - w[1].position.translation.vector).magnitude() + acc
+                (w[0].position.translation.vector - w[1].position.translation.vector).magnitude()
+                    + acc
             });
-
         }
         println!("Total length: {total_length}");
         assert!(self.canvas.program.layers.3.solutions.is_empty());
@@ -1997,7 +2001,10 @@ impl Application for App {
                                 .push(iced::Space::with_width(Length::Units(16)))
                                 .push(
                                     Column::new()
-                                        .push(Text::new(format!("Debug Paths: {}", &self.debug_ticket_size)))
+                                        .push(Text::new(format!(
+                                            "Debug Paths: {}",
+                                            &self.debug_ticket_size
+                                        )))
                                         .push(iced::Space::with_height(Length::Units(2)))
                                         .push(
                                             Slider::new(

mapf/src/negotiation/mod.rs

@@ -40,7 +40,7 @@ use crate::{
 };
 use std::{
     cmp::Reverse,
-    collections::{BinaryHeap, HashMap, HashSet},
+    collections::{BTreeSet, HashMap, HashSet},
     sync::Arc,
 };
 
@@ -64,6 +64,21 @@ pub fn negotiate(
         HashMap<usize, String>,
     ),
     NegotiationError,
+> {
+    negotiate_focal(scenario, queue_length_limit, 1.0)
+}
+
+pub fn negotiate_focal(
+    scenario: &Scenario,
+    queue_length_limit: Option<usize>,
+    weight: f64,
+) -> Result<
+    (
+        NegotiationNode,
+        Vec<NegotiationNode>,
+        HashMap<usize, String>,
+    ),
+    NegotiationError,
 > {
     let cs = scenario.cell_size;
     let mut conflicts = HashMap::new();
@@ -200,14 +215,34 @@ pub fn negotiate(
         };
 
         for root in negotiations.values() {
-            let mut queue: BinaryHeap<QueueEntry> = BinaryHeap::new();
+            let mut queue: BTreeSet<QueueEntry> = BTreeSet::new();
             let root = NegotiationNode::from_root(root, &ideal, base_env.clone(), arena.len());
             arena.push(root.clone());
-            queue.push(QueueEntry::new(root));
+            queue.insert(QueueEntry::new(root));
 
             let mut solution = None;
             let mut iters = 0;
-            while let Some(mut top) = queue.pop() {
+            while !queue.is_empty() {
+                let top = {
+                    let focal_weight = weight;
+                    let min_f = queue.first().unwrap().node.cost.0;
+                    let threshold = min_f * focal_weight;
+
+                    let mut best_entry = queue.first().unwrap();
+                    for entry in queue.iter().take_while(|e| e.node.cost.0 <= threshold) {
+                        if entry.node.negotiation.conflicts.len()
+                            < best_entry.node.negotiation.conflicts.len()
+                        {
+                            best_entry = entry;
+                        }
+                    }
+                    best_entry.clone()
+                };
+                if !queue.remove(&top) {
+                    panic!("Failed to remove node {} from queue!", top.node.id);
+                }
+                let mut top = top;
+
                 iters += 1;
                 if iters % 10 == 0 {
                     dbg!(iters);
@@ -217,7 +252,7 @@ pub fn negotiate(
 
                     // Dump the remaining queue into the node history
                     println!("Queue begins at {}", arena.len() + 1);
-                    while let Some(remainder) = queue.pop() {
+                    while let Some(remainder) = queue.pop_first() {
                         arena.push(remainder.node);
                     }
 
@@ -395,7 +430,7 @@ pub fn negotiate(
                         arena.len(),
                     );
                     arena.push(node.clone());
-                    queue.push(QueueEntry::new(node));
+                    queue.insert(QueueEntry::new(node));
                 }
             }
 
@@ -660,27 +695,23 @@ struct QueueEntry {
 
 impl PartialOrd for QueueEntry {
     fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
-        if f64::abs(self.node.cost.0 - other.node.cost.0) < 0.1 {
-            Reverse(self.node.depth).partial_cmp(&Reverse(other.node.depth))
-        } else {
-            Reverse(self.node.cost).partial_cmp(&Reverse(other.node.cost))
-        }
+        Some(self.cmp(other))
     }
 }
 
 impl PartialEq for QueueEntry {
     fn eq(&self, other: &Self) -> bool {
-        self.node.cost.eq(&other.node.cost)
+        self.node.id == other.node.id
     }
 }
 
 impl Ord for QueueEntry {
     fn cmp(&self, other: &Self) -> std::cmp::Ordering {
-        if f64::abs(self.node.cost.0 - other.node.cost.0) < 0.1 {
-            self.node.depth.cmp(&self.node.depth)
-        } else {
-            Reverse(self.node.cost).cmp(&Reverse(other.node.cost))
-        }
+        self.node
+            .cost
+            .cmp(&other.node.cost)
+            .then_with(|| Reverse(self.node.depth).cmp(&Reverse(other.node.depth)))
+            .then_with(|| self.node.id.cmp(&other.node.id))
     }
 }
 impl Eq for QueueEntry {}