Skip to content

Commit

Permalink
negotiation test
Browse files Browse the repository at this point in the history
Signed-off-by: Reuben Thomas <reubenthomas123@gmail.com>
  • Loading branch information
reuben-thomas committed Jul 30, 2024
1 parent f8bc60f commit 556dbcf
Show file tree
Hide file tree
Showing 2 changed files with 65 additions and 8 deletions.
57 changes: 57 additions & 0 deletions mapf-rse/src/config_widget.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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>,
Expand Down Expand Up @@ -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() {
Expand Down Expand Up @@ -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,
]
}
16 changes: 8 additions & 8 deletions mapf-rse/src/negotiation/mod.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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,
// ),
// );
}
}

Expand Down

0 comments on commit 556dbcf

Please sign in to comment.