negotiation test

Signed-off-by: Reuben Thomas <[email protected]>
open-rmf · Jul 30, 2024 · 556dbcf · 556dbcf
1 parent f8bc60f
commit 556dbcf
Show file tree

Hide file tree

Showing 2 changed files with 65 additions and 8 deletions.
diff --git a/mapf-rse/src/config_widget.rs b/mapf-rse/src/config_widget.rs
@@ -32,6 +32,10 @@ use rmf_site_editor::{
     widgets::{view_scenarios::ScenarioDisplay, Icons},
 };
 
+use mapf::negotiation::{Agent, Obstacle, Scenario as MapfScenario};
+use mapf::negotiation::*;
+use std::collections::{BTreeMap, HashMap};
+
 #[derive(SystemParam)]
 pub struct MapfConfigWidget<'w, 's> {
     simulation_config: ResMut<'w, SimulationConfig>,
@@ -144,6 +148,10 @@ impl<'w, 's> MapfConfigWidget<'w, 's> {
             );
         });
 
+        if ui.button("Negotiation Test").clicked() {
+            negotiation_test();
+        }
+
         // Results
         ui.separator();
         match self.negotiation_data.as_ref() {
@@ -186,3 +194,52 @@ impl<'w, 's> MapfConfigWidget<'w, 's> {
         ui.label("Unavailable");
     }
 }
+
+pub fn negotiation_test() {
+    let mut agents: BTreeMap<String, Agent> = BTreeMap::new();
+    agents.insert(
+        "A".to_string(),
+        Agent {
+            start: get_cell(5.0, 5.0, 1.0),
+            goal: get_cell(1.0, 1.0, 1.0),
+            yaw: 1.0,
+            radius: 0.5,
+            speed: 1.0,
+            spin: 1.0,
+        },
+    );
+    let obstacles: Vec<Obstacle> = Vec::new();
+    let occupancy: HashMap<i64, Vec<i64>> = HashMap::new();
+    let cell_size = 0.2;
+
+    let scenario = MapfScenario {
+        agents,
+        obstacles,
+        occupancy,
+        cell_size,
+        camera_bounds: None,
+    };
+
+    let res = match negotiate(&scenario, Some(1_000_000)) {
+        Ok(res) => res,
+        Err(err) => {
+            match err {
+                NegotiationError::PlanningFailed((nodes, name_map)) => {
+                    println!("Unable to find a solution");
+                    for node in nodes {
+                        println!("{:?}", node);
+                    }
+                }
+                err => println!("Error while planning: {err:?}"),
+            };
+            return;
+        }
+    };
+}
+
+pub fn get_cell(x: f64, y: f64, cell_size: f64) -> [i64; 2] {
+    [
+        (x / cell_size).floor() as i64,
+        (y / cell_size).floor() as i64,
+    ]
+}
diff --git a/mapf-rse/src/negotiation/mod.rs b/mapf-rse/src/negotiation/mod.rs
@@ -47,14 +47,14 @@ impl Plugin for NegotiationPlugin {
     fn build(&self, app: &mut App) {
         app.add_event::<NegotiationRequest>()
             .init_resource::<NegotiationParams>()
-            .init_resource::<NegotiationData>()
-            .add_systems(
-                Update,
-                (
-                    start_compute_negotiation,
-                    handle_compute_negotiation_complete,
-                ),
-            );
+            .init_resource::<NegotiationData>();
+            // .add_systems(
+            //     Update,
+            //     (
+            //         start_compute_negotiation,
+            //         handle_compute_negotiation_complete,
+            //     ),
+            // );
     }
 }