From 164a1e9ed4f510dd953791da95fa9fbde266c5d8 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Mon, 8 Jul 2024 11:33:55 +0800 Subject: [PATCH 1/8] added rse Signed-off-by: Reuben Thomas --- Cargo.toml | 5 ++--- mapf-rse/Cargo.toml | 9 +++++++++ mapf-rse/src/main.rs | 36 ++++++++++++++++++++++++++++++++++++ 3 files changed, 47 insertions(+), 3 deletions(-) create mode 100644 mapf-rse/Cargo.toml create mode 100644 mapf-rse/src/main.rs diff --git a/Cargo.toml b/Cargo.toml index 8dd3043..1ab6779 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,7 +1,6 @@ [workspace] members = [ - "mapf", - "mapf-viz", -] + "mapf" +, "mapf-rse"] resolver = "2" diff --git a/mapf-rse/Cargo.toml b/mapf-rse/Cargo.toml new file mode 100644 index 0000000..8065e26 --- /dev/null +++ b/mapf-rse/Cargo.toml @@ -0,0 +1,9 @@ +[package] +name = "mapf-rse" +version = "0.1.0" +edition = "2021" + +[dependencies] +rmf_site_format = { path = "../../rmf_site/rmf_site_format" } +rmf_site_editor = { path = "../../rmf_site/rmf_site_editor" } +bevy = { version = "0.12", features = ["pnm", "jpeg", "tga"] } \ No newline at end of file diff --git a/mapf-rse/src/main.rs b/mapf-rse/src/main.rs new file mode 100644 index 0000000..8c9aee5 --- /dev/null +++ b/mapf-rse/src/main.rs @@ -0,0 +1,36 @@ +use bevy::prelude::{App, Entity, Query, Res}; +use librmf_site_editor::{ + site::NameOfSite, widgets::prelude::*, workspace::CurrentWorkspace, SiteEditor, +}; + +#[derive(SystemParam)] +pub struct HelloSiteWidget<'w, 's> { + sites: Query<'w, 's, &'static NameOfSite>, + current: Res<'w, CurrentWorkspace>, +} + +impl<'w, 's> WidgetSystem for HelloSiteWidget<'w, 's> { + fn show(_: Tile, ui: &mut Ui, state: &mut SystemState, world: &mut World) { + let mut params = state.get_mut(world); + if let Some(name) = params + .current + .root + .map(|e| params.sites.get(e).ok()) + .flatten() + { + ui.separator(); + ui.add_space(50.0); + ui.heading(format!("MAPF Settings")); + } + } +} + +fn main() { + let mut app = App::new(); + app.add_plugins(( + SiteEditor::default(), + PropertiesTilePlugin::::new(), + )); + + app.run(); +} From 3a2e733d6fb070f591f07c0910e1d68c54a10fbb Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Mon, 29 Jul 2024 11:14:05 +0800 Subject: [PATCH 2/8] update Signed-off-by: Reuben Thomas --- .vscode/settings.json | 2 + Cargo.toml | 4 +- mapf-viz/examples/grid.rs => grid.rs | 0 mapf-rse/Cargo.toml | 4 +- mapf-rse/src/lib.rs | 41 + mapf-rse/src/main.rs | 46 +- mapf-rse/src/negotiation/animate.rs | 0 mapf-rse/src/negotiation/control_tile.rs | 162 ++++ mapf-rse/src/negotiation/debug_panel.rs | 0 mapf-rse/src/negotiation/mod.rs | 177 ++++ mapf-rse/src/simulation.rs | 172 ++++ mapf-viz/.gitignore | 2 - mapf-viz/Cargo.toml | 23 - mapf-viz/scenarios/T-crossing.yaml | 88 -- mapf-viz/scenarios/sizes.yaml | 56 -- mapf-viz/scenarios/warehouse.yaml | 868 ------------------ mapf-viz/src/accessibility_visual.rs | 243 ----- mapf-viz/src/grid.rs | 99 -- mapf-viz/src/lib.rs | 40 - mapf-viz/src/spatial_canvas.rs | 454 --------- mapf-viz/src/toggle.rs | 239 ----- mapf-viz/src/visibility_visual.rs | 374 -------- .../se2/differential_drive_line_follow.rs | 2 +- mapf/src/motion/se2/timed_position.rs | 6 +- mapf/src/premade/search_se2.rs | 2 +- 25 files changed, 583 insertions(+), 2521 deletions(-) create mode 100644 .vscode/settings.json rename mapf-viz/examples/grid.rs => grid.rs (100%) create mode 100644 mapf-rse/src/lib.rs create mode 100644 mapf-rse/src/negotiation/animate.rs create mode 100644 mapf-rse/src/negotiation/control_tile.rs create mode 100644 mapf-rse/src/negotiation/debug_panel.rs create mode 100644 mapf-rse/src/negotiation/mod.rs create mode 100644 mapf-rse/src/simulation.rs delete mode 100644 mapf-viz/.gitignore delete mode 100644 mapf-viz/Cargo.toml delete mode 100644 mapf-viz/scenarios/T-crossing.yaml delete mode 100644 mapf-viz/scenarios/sizes.yaml delete mode 100644 mapf-viz/scenarios/warehouse.yaml delete mode 100644 mapf-viz/src/accessibility_visual.rs delete mode 100644 mapf-viz/src/grid.rs delete mode 100644 mapf-viz/src/lib.rs delete mode 100644 mapf-viz/src/spatial_canvas.rs delete mode 100644 mapf-viz/src/toggle.rs delete mode 100644 mapf-viz/src/visibility_visual.rs diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..7a73a41 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,2 @@ +{ +} \ No newline at end of file diff --git a/Cargo.toml b/Cargo.toml index 1ab6779..00c95d8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,4 @@ [workspace] -members = [ - "mapf" -, "mapf-rse"] +members = [ "mapf", "mapf-rse"] resolver = "2" diff --git a/mapf-viz/examples/grid.rs b/grid.rs similarity index 100% rename from mapf-viz/examples/grid.rs rename to grid.rs diff --git a/mapf-rse/Cargo.toml b/mapf-rse/Cargo.toml index 8065e26..2b24835 100644 --- a/mapf-rse/Cargo.toml +++ b/mapf-rse/Cargo.toml @@ -4,6 +4,8 @@ version = "0.1.0" edition = "2021" [dependencies] +mapf = { path="../mapf" } rmf_site_format = { path = "../../rmf_site/rmf_site_format" } rmf_site_editor = { path = "../../rmf_site/rmf_site_editor" } -bevy = { version = "0.12", features = ["pnm", "jpeg", "tga"] } \ No newline at end of file +bevy = { version = "0.12", features = ["pnm", "jpeg", "tga"] } +bevy_egui = "0.23.0" diff --git a/mapf-rse/src/lib.rs b/mapf-rse/src/lib.rs new file mode 100644 index 0000000..7e10b63 --- /dev/null +++ b/mapf-rse/src/lib.rs @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +pub mod simulation; +pub use simulation::*; + +pub mod negotiation; +use librmf_site_editor::widgets::PropertiesTilePlugin; +pub use negotiation::*; + +use bevy::prelude::*; + +#[derive(Default)] +pub struct MapfRsePlugin; + +impl Plugin for MapfRsePlugin { + fn build(&self, app: &mut App) { + app.add_state::() + .init_resource::() + .add_plugins(PropertiesTilePlugin::::new()); + + app.add_event::() + .init_resource::() + .add_plugins(PropertiesTilePlugin::::new()) + .add_systems(Update, generate_plan); + } +} diff --git a/mapf-rse/src/main.rs b/mapf-rse/src/main.rs index 8c9aee5..a5c00b4 100644 --- a/mapf-rse/src/main.rs +++ b/mapf-rse/src/main.rs @@ -1,35 +1,29 @@ -use bevy::prelude::{App, Entity, Query, Res}; -use librmf_site_editor::{ - site::NameOfSite, widgets::prelude::*, workspace::CurrentWorkspace, SiteEditor, -}; +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ -#[derive(SystemParam)] -pub struct HelloSiteWidget<'w, 's> { - sites: Query<'w, 's, &'static NameOfSite>, - current: Res<'w, CurrentWorkspace>, -} - -impl<'w, 's> WidgetSystem for HelloSiteWidget<'w, 's> { - fn show(_: Tile, ui: &mut Ui, state: &mut SystemState, world: &mut World) { - let mut params = state.get_mut(world); - if let Some(name) = params - .current - .root - .map(|e| params.sites.get(e).ok()) - .flatten() - { - ui.separator(); - ui.add_space(50.0); - ui.heading(format!("MAPF Settings")); - } - } -} +use bevy::prelude::*; +use librmf_site_editor::*; +use mapf_rse::MapfRsePlugin; fn main() { let mut app = App::new(); app.add_plugins(( SiteEditor::default(), - PropertiesTilePlugin::::new(), + MapfRsePlugin::default(), )); app.run(); diff --git a/mapf-rse/src/negotiation/animate.rs b/mapf-rse/src/negotiation/animate.rs new file mode 100644 index 0000000..e69de29 diff --git a/mapf-rse/src/negotiation/control_tile.rs b/mapf-rse/src/negotiation/control_tile.rs new file mode 100644 index 0000000..346fb8e --- /dev/null +++ b/mapf-rse/src/negotiation/control_tile.rs @@ -0,0 +1,162 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use super::{Negotiate, NegotiationData}; +use bevy::{ecs::system::SystemParam, prelude::*}; +use bevy_egui::egui::{ + Align, CollapsingHeader, Color32, ComboBox, DragValue, Frame, Layout, ScrollArea, Slider, + Stroke, Ui, +}; +use librmf_site_editor::{ + interaction::{Select, Selection}, + site::{ + location, mobile_robot, Category, Change, ChangeCurrentScenario, CurrentScenario, Delete, + Group, MobileRobotMarker, NameInSite, Scenario, ScenarioMarker, SiteParent, Task, Tasks, + }, + widgets::{prelude::*, view_scenarios::ScenarioDisplay, Icons}, +}; +use rmf_site_format::SiteID; + +#[derive(SystemParam)] +pub struct NegotiationControlTile<'w, 's> { + commands: Commands<'w, 's>, + selection: Res<'w, Selection>, + change_tasks: EventWriter<'w, Change>>, + select: EventWriter<'w, Select>, + mobile_robots: Query< + 'w, + 's, + ( + Entity, + &'static NameInSite, + Option<&'static SiteID>, + &'static mut Tasks, + ), + (With, Without), + >, + negotiation_data: Res<'w, NegotiationData>, + negotiation_request: EventWriter<'w, Negotiate>, + locations: Query<'w, 's, (&'static NameInSite, Option<&'static SiteID>)>, +} + +impl<'w, 's> WidgetSystem for NegotiationControlTile<'w, 's> { + fn show(_: Tile, ui: &mut Ui, state: &mut SystemState, world: &mut World) -> () { + let mut params = state.get_mut(world); + CollapsingHeader::new("Negotiation") + .default_open(false) + .show(ui, |ui| { + // Add button + ui.add_enabled_ui(!params.negotiation_data.is_generating, |ui| { + if ui.button("Generate Plan").clicked() { + params.negotiation_request.send(Negotiate); + } + }); + + // Tasks + ui.label("Tasks"); + task_frame(ui, |ui| { + let mut count = 0; + for (robot_entity, robot_name, robot_site_id, robot_tasks) in + params.mobile_robots.iter() + { + for task in robot_tasks.0.iter() { + if let Task::GoToPlace(SiteParent(Some(location_entity))) = task { + if let Ok((location_name, location_site_id)) = + params.locations.get(*location_entity) + { + show_task( + ui, + robot_name.0.as_str(), + &robot_entity, + robot_site_id.unwrap(), + location_name.0.as_str(), + location_entity, + location_site_id.unwrap(), + ¶ms.selection, + &mut params.select, + ); + count += 1; + break; + } + } + } + } + if count == 0 { + ui.label("No Tasks"); + } + }); + }); + } +} + +fn task_frame(ui: &mut Ui, add_contents: impl FnOnce(&mut Ui) -> R) { + Frame::default() + .inner_margin(4.0) + .rounding(2.0) + .stroke(Stroke::new(1.0, Color32::GRAY)) + .show(ui, |ui| { + ui.set_min_width(ui.available_width()); + add_contents(ui); + }); +} + +fn show_task( + ui: &mut Ui, + robot_name: &str, + robot_entity: &Entity, + robot_site_id: &u32, + location_name: &str, + location_entity: &Entity, + location_site_id: &u32, + selected: &Selection, + select: &mut EventWriter, -) { - let mut is_deleted = false; - Frame::default() - .inner_margin(4.0) - .fill(Color32::DARK_GRAY) - .rounding(2.0) - .show(ui, |ui| { - ui.set_min_width(ui.available_width()); - // Mobile Robot - ui.horizontal(|ui| { - ui.label("Robot"); - if ui - .selectable_label( - selected.0.is_some_and(|s| s == *robot_entity), - format!("Model #{} [{}]", robot_site_id, robot_name).to_string(), - ) - .clicked() - { - select.send(Select(Some(*robot_entity))); - } - }); - // Goal - ui.horizontal(|ui| { - ui.label("To: "); - if ui - .selectable_label( - selected.0.is_some_and(|s| s == *location_entity), - format!("Location #{} [{}]", location_site_id, location_name).to_string(), - ) - .clicked() - { - select.send(Select(Some(*location_entity))); - } - }); - }); -} diff --git a/mapf-rse/src/negotiation/debug_panel.rs b/mapf-rse/src/negotiation/debug_panel.rs new file mode 100644 index 0000000..81a3287 --- /dev/null +++ b/mapf-rse/src/negotiation/debug_panel.rs @@ -0,0 +1,16 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ diff --git a/mapf-rse/src/negotiation/mod.rs b/mapf-rse/src/negotiation/mod.rs index 3383dd8..6e42f6c 100644 --- a/mapf-rse/src/negotiation/mod.rs +++ b/mapf-rse/src/negotiation/mod.rs @@ -16,50 +16,181 @@ */ use bevy::{ - ecs::system::{CommandQueue, SystemState}, prelude::*, tasks::{block_on, AsyncComputeTaskPool, Task}, - time::Stopwatch, }; +use futures_lite::future; use std::{ collections::{BTreeMap, HashMap}, fmt::Debug, - time::Duration, + time::{Duration, Instant}, }; +use mapf::negotiation::*; use rmf_site_editor::{ - interaction::{Select, Selection}, occupancy::{Cell, Grid}, site::{ - level, location, Anchor, Category, Change, ChangeCurrentScenario, CurrentLevel, - CurrentScenario, Delete, DifferentialDrive, Group, LocationTags, MobileRobotMarker, - ModelMarker, NameInSite, Point, Pose, Scenario, ScenarioMarker, SiteParent, - Task as RobotTask, Tasks as RobotTasks, + Anchor, CurrentLevel, DifferentialDrive, Group, LocationTags, ModelMarker, NameInSite, + Point, Pose, Task as RobotTask, Tasks as RobotTasks, }, }; use mapf::negotiation::{Agent, Obstacle, Scenario as MapfScenario}; -pub mod control_tile; -pub use control_tile::*; +pub mod debug_panel; +pub use debug_panel::*; -// pub mod debug_panel; -// pub use debug_panel::*; +#[derive(Default)] +pub struct NegotiationPlugin; + +impl Plugin for NegotiationPlugin { + fn build(&self, app: &mut App) { + app.add_event::() + .init_resource::() + .init_resource::() + .add_systems( + Update, + ( + start_compute_negotiation, + handle_compute_negotiation_complete, + ), + ); + } +} #[derive(Event)] -pub struct Negotiate; +pub struct NegotiationRequest; + +#[derive(Debug, Clone, Resource)] +pub struct NegotiationParams { + pub queue_length_limit: usize, +} + +impl Default for NegotiationParams { + fn default() -> Self { + Self { + queue_length_limit: 1_000_000, + } + } +} + +#[derive(Debug, Default, Clone, Resource)] +pub enum NegotiationData { + #[default] + NotStarted, + InProgress { + start_time: Instant, + }, + Complete { + elapsed_time: Duration, + solution: Option, + negotiation_history: Vec, + entity_id_map: HashMap, + error_message: Option, + conflicting_endpoints: Vec<(Entity, Entity)>, + }, +} + +impl NegotiationData { + pub fn is_in_progress(&self) -> bool { + matches!(self, NegotiationData::InProgress { .. }) + } +} #[derive(Component)] -pub struct ComputeNegotiateTask(Task); +pub struct ComputeNegotiateTask( + Task< + Result< + ( + NegotiationNode, + Vec, + HashMap, + ), + NegotiationError, + >, + >, +); + +pub fn handle_compute_negotiation_complete( + mut commands: Commands, + mut compute_negotiation_task: Query<(Entity, &mut ComputeNegotiateTask)>, + mut negotiation_data: ResMut, +) { + let NegotiationData::InProgress { start_time } = *negotiation_data else { + return; + }; + + fn bits_string_to_entity(bits_string: &str) -> Entity { + let bits = u64::from_str_radix(bits_string, 10).expect("Invalid entity id"); + Entity::from_bits(bits) + } -#[derive(Default, Debug, Clone, Resource)] -pub struct NegotiationData { - pub is_generating: bool, - pub cell_size: f32, - pub planning_elapsed_time: Duration, + for (mut entity, mut task) in &mut compute_negotiation_task { + if let Some(result) = block_on(future::poll_once(&mut task.0)) { + let elapsed_time = start_time.elapsed(); + + match result { + Ok((solution, negotiation_history, name_map)) => { + *negotiation_data = NegotiationData::Complete { + elapsed_time, + solution: Some(solution), + negotiation_history, + entity_id_map: name_map + .into_iter() + .map(|(id, bits_string)| (id, bits_string_to_entity(&bits_string))) + .collect(), + error_message: None, + conflicting_endpoints: Vec::new(), + }; + } + Err(err) => match err { + NegotiationError::PlanningImpossible(msg) => { + *negotiation_data = NegotiationData::Complete { + elapsed_time, + solution: None, + negotiation_history: Vec::new(), + entity_id_map: HashMap::new(), + error_message: Some(msg), + conflicting_endpoints: Vec::new(), + }; + } + NegotiationError::ConflictingEndpoints(conflicts) => { + *negotiation_data = NegotiationData::Complete { + elapsed_time, + solution: None, + negotiation_history: Vec::new(), + entity_id_map: HashMap::new(), + error_message: None, + conflicting_endpoints: conflicts + .into_iter() + .map(|(a, b)| { + (bits_string_to_entity(&a), bits_string_to_entity(&b)) + }) + .collect(), + }; + } + NegotiationError::PlanningFailed((negotiation_history, name_map)) => { + println!("HERE"); + *negotiation_data = NegotiationData::Complete { + elapsed_time, + solution: None, + negotiation_history, + entity_id_map: name_map + .into_iter() + .map(|(id, bits_string)| (id, bits_string_to_entity(&bits_string))) + .collect(), + error_message: None, + conflicting_endpoints: Vec::new(), + }; + } + }, + }; + commands.entity(entity).remove::(); + } + } } -pub fn generate_plan( +pub fn start_compute_negotiation( mut commands: Commands, mobile_robots: Query< ( @@ -71,10 +202,10 @@ pub fn generate_plan( ), (With, Without), >, - locations: Query<(Entity, &Point), With>, // locations: Query<'w, 's, (&'static NameInSite, Option<&'static SiteID>)>, + locations: Query<(Entity, &Point), With>, anchors: Query<(Entity, &Anchor, &Transform)>, - negotiation_request: EventReader, - negotiation_data: Res, + negotiation_request: EventReader, + mut negotiation_data: ResMut, current_level: Res, grids: Query<(Entity, &Grid)>, parents: Query<&Parent>, @@ -82,10 +213,15 @@ pub fn generate_plan( if negotiation_request.len() == 0 { return; } + if let NegotiationData::InProgress { .. } = *negotiation_data { + warn!("Negotiation requested while another negotiation is in progress"); + return; + } // Occupancy let mut occupancy = HashMap::>::new(); - let Some(grid) = grids + let mut cell_size = 1.0; + let grid = grids .iter() .filter_map(|(grid_entity, grid)| { if let Some(level_entity) = current_level.0 { @@ -101,31 +237,32 @@ pub fn generate_plan( None } }) - .next() - else { - println!("No occupancy map available"); - return; - }; - let cell_size = grid.cell_size; - for cell in grid.occupied.iter() { - occupancy.entry(cell.y).or_default().push(cell.x); - } - - for (_, column) in &mut occupancy { - column.sort_unstable(); + .next(); + match grid { + Some(grid) => { + cell_size = grid.cell_size; + for cell in grid.occupied.iter() { + occupancy.entry(cell.y).or_default().push(cell.x); + } + for (_, column) in &mut occupancy { + column.sort_unstable(); + } + } + None => { + occupancy.entry(0).or_default().push(0); + warn!("No occupancy grid found, defaulting to empty"); + } } - println!("Y_MAX: {:?}", occupancy.len()); - println!("X_MAX: {:?}", occupancy.values().map(|x| x.len()).max()); // Agent let mut agents = BTreeMap::::new(); - for (robot_entity, robot_name, robot_pose, differential_drive, tasks) in mobile_robots.iter() { + for (robot_entity, _, robot_pose, differential_drive, tasks) in mobile_robots.iter() { let Some(location_entity) = tasks .0 .iter() .filter_map(|task| { - if let RobotTask::GoToPlace(SiteParent(Some(location_entity))) = task { - Some(location_entity) + if let RobotTask::GoToPlace { location } = task { + Some(location.0) } else { None } @@ -134,7 +271,7 @@ pub fn generate_plan( else { continue; }; - let Ok((_, Point(anchor_entity))) = locations.get(*location_entity) else { + let Ok((_, Point(anchor_entity))) = locations.get(location_entity) else { continue; }; let Ok((_, _, goal_transform)) = anchors.get(*anchor_entity) else { @@ -149,46 +286,37 @@ pub fn generate_plan( goal_transform.translation.y, cell_size, ), - radius: 0.5, + radius: 0.2, speed: f64::from(differential_drive.translational_speed), spin: f64::from(differential_drive.rotational_speed), }; - agents.insert(format!("{:?}", robot_entity), agent); + let agent_id = robot_entity.to_bits().to_string(); + agents.insert(agent_id, agent); } if agents.len() == 0 { - println!("No active agents"); + warn!("No agents with valid GoToPlace task"); return; } - // Obstacles - let mut obstacles = Vec::::new(); - let scenario = MapfScenario { agents: agents, - obstacles: obstacles, + obstacles: Vec::::new(), occupancy: occupancy, cell_size: f64::from(cell_size), camera_bounds: None, }; // Execute asynchronously + let start_time = Instant::now(); + *negotiation_data = NegotiationData::InProgress { start_time }; + let thread_pool = AsyncComputeTaskPool::get(); - let start_time = std::time::Instant::now(); - let task = thread_pool.spawn(async move { - let res = mapf::negotiation::negotiate(&scenario, Some(1_000_000)); - let elapsed_time = start_time.elapsed(); - println!("Elapsed time: {:?}", elapsed_time); - elapsed_time - }); - let task_entity = commands.spawn_empty().id(); - commands - .entity(task_entity) - .insert(ComputeNegotiateTask(task)); + let task = thread_pool.spawn(async move { negotiate(&scenario, Some(1000000)) }); + + commands.spawn(ComputeNegotiateTask(task)); } -pub fn to_cell(x: f32, y: f32, cell_size: f32) -> [i64; 2] { - println!("TO_CELL: {:?}, {:?}, {:?}", x, y, cell_size); +fn to_cell(x: f32, y: f32, cell_size: f32) -> [i64; 2] { let cell = Cell::from_point(Vec2::new(x, y), cell_size); - println!("CELL: {:?}", cell); [cell.x, cell.y] } diff --git a/mapf-rse/src/negotiation/animate.rs b/mapf-rse/src/negotiation/visual.rs similarity index 100% rename from mapf-rse/src/negotiation/animate.rs rename to mapf-rse/src/negotiation/visual.rs diff --git a/mapf-rse/src/simulation.rs b/mapf-rse/src/simulation.rs index 683f763..40474f1 100644 --- a/mapf-rse/src/simulation.rs +++ b/mapf-rse/src/simulation.rs @@ -119,54 +119,3 @@ impl<'w> WidgetSystem for SimulationControlTile<'w> { }); } } - -#[derive(Resource, Debug, Clone)] -pub struct SimulationConfig { - pub is_playing: bool, - pub speed: f32, - pub current_time: f32, - pub end_time: f32, - pub current_step: u32, - pub end_step: u32, -} - -impl Default for SimulationConfig { - fn default() -> Self { - Self { - is_playing: false, - speed: 1.0, - current_time: 0.0, - end_time: 0.0, - current_step: 0, - end_step: 0, - } - } -} - -#[derive(Clone, Default, Eq, PartialEq, Debug, Hash, States)] -pub enum DebugMode { - #[default] - Negotation, - Planner, -} - -impl DebugMode { - pub fn labels() -> Vec<&'static str> { - vec!["Negotation", "Planner"] - } - - pub fn label(&self) -> &str { - match self { - DebugMode::Negotation => Self::labels()[0], - DebugMode::Planner => Self::labels()[1], - } - } - - pub fn from_label(label: &str) -> Self { - if label == Self::labels()[0] { - return DebugMode::Negotation; - } else { - return DebugMode::Planner; - } - } -} From 556dbcf0d8e89caf3bd7c6bf5c1d38b6f11677de Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Tue, 30 Jul 2024 14:36:59 +0800 Subject: [PATCH 6/8] negotiation test Signed-off-by: Reuben Thomas --- mapf-rse/src/config_widget.rs | 57 +++++++++++++++++++++++++++++++++ mapf-rse/src/negotiation/mod.rs | 16 ++++----- 2 files changed, 65 insertions(+), 8 deletions(-) diff --git a/mapf-rse/src/config_widget.rs b/mapf-rse/src/config_widget.rs index dc60dd2..5f4a957 100644 --- 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 = 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 = Vec::new(); + let occupancy: HashMap> = 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, + ] +} \ No newline at end of file diff --git a/mapf-rse/src/negotiation/mod.rs b/mapf-rse/src/negotiation/mod.rs index 6e42f6c..22475de 100644 --- 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::() .init_resource::() - .init_resource::() - .add_systems( - Update, - ( - start_compute_negotiation, - handle_compute_negotiation_complete, - ), - ); + .init_resource::(); + // .add_systems( + // Update, + // ( + // start_compute_negotiation, + // handle_compute_negotiation_complete, + // ), + // ); } } From 15dd7a40f3b6370b3208546ab5a7ad2583260382 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Wed, 31 Jul 2024 09:26:28 +0800 Subject: [PATCH 7/8] selectable negotiation nodes and gizmo visualisation Signed-off-by: Reuben Thomas --- mapf-rse/src/config_widget.rs | 150 +++++++---------- mapf-rse/src/negotiation/debug_panel.rs | 211 +++++++++++++++++++++++- mapf-rse/src/negotiation/mod.rs | 47 +++++- mapf-rse/src/negotiation/visual.rs | 79 +++++++++ mapf/src/negotiation/mod.rs | 20 +++ 5 files changed, 407 insertions(+), 100 deletions(-) diff --git a/mapf-rse/src/config_widget.rs b/mapf-rse/src/config_widget.rs index 5f4a957..5a26d5f 100644 --- a/mapf-rse/src/config_widget.rs +++ b/mapf-rse/src/config_widget.rs @@ -17,25 +17,13 @@ use super::*; use bevy::{ecs::system::SystemParam, prelude::*}; -use bevy_egui::egui::{ - Button, CollapsingHeader, Color32, ComboBox, DragValue, Frame, Grid as EguiGrid, ScrollArea, - Slider, Stroke, Ui, -}; +use bevy_egui::egui::{CollapsingHeader, ComboBox, DragValue, Grid as EguiGrid, Ui}; use rmf_site_editor::{ - interaction::{Select, Selection}, occupancy::Grid, - site::{ - Category, Change, ChangeCurrentScenario, CurrentLevel, CurrentScenario, Delete, Group, - MobileRobotMarker, NameInSite, Scenario, ScenarioMarker, Task, Tasks, - }, + site::{CurrentLevel, Group, MobileRobotMarker, Task, Tasks}, widgets::prelude::*, - 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>, @@ -48,6 +36,7 @@ pub struct MapfConfigWidget<'w, 's> { negotiation_request: EventWriter<'w, NegotiationRequest>, negotiation_params: ResMut<'w, NegotiationParams>, negotiation_data: ResMut<'w, NegotiationData>, + negotiation_debug: ResMut<'w, NegotiationDebugData>, } impl<'w, 's> WidgetSystem for MapfConfigWidget<'w, 's> { @@ -58,18 +47,24 @@ impl<'w, 's> WidgetSystem for MapfConfigWidget<'w, 's> { CollapsingHeader::new("MAPF Configuration") .default_open(true) .show(ui, |ui| { - ComboBox::from_id_source("mapf_debug_mode") - .selected_text(params.debug_mode.get().label()) - .show_ui(ui, |ui| { - for label in DebugMode::labels() { - if ui - .selectable_label(params.debug_mode.get().label() == label, label) - .clicked() - { - params.debug_mode_next.set(DebugMode::from_label(label)); + ui.horizontal(|ui| { + ui.label("Mode"); + ComboBox::from_id_source("mapf_debug_mode") + .selected_text(params.debug_mode.get().label()) + .show_ui(ui, |ui| { + for label in DebugMode::labels() { + if ui + .selectable_label( + params.debug_mode.get().label() == label, + label, + ) + .clicked() + { + params.debug_mode_next.set(DebugMode::from_label(label)); + } } - } - }); + }); + }); match params.debug_mode.get() { DebugMode::Negotiation => params.show_negotiation(ui), @@ -81,7 +76,26 @@ impl<'w, 's> WidgetSystem for MapfConfigWidget<'w, 's> { impl<'w, 's> MapfConfigWidget<'w, 's> { pub fn show_negotiation(&mut self, ui: &mut Ui) { - // Agents with Task + // Debug Parameters + // Visualize + ui.horizontal(|ui| { + ui.label("Visualize"); + ui.checkbox( + &mut self.negotiation_debug.visualize_trajectories, + "Trajectories", + ); + ui.checkbox(&mut self.negotiation_debug.visualize_conflicts, "Conflicts"); + ui.checkbox(&mut self.negotiation_debug.visualize_keys, "Keys") + }); + // Toggle debug panel + ui.horizontal(|ui| { + ui.label("Debug Panel"); + ui.checkbox(&mut self.negotiation_debug.show_debug_panel, "Enabled"); + }); + + // Negotiation Request Properties + // Agent tasks + ui.separator(); let num_tasks = self .mobile_robots .iter() @@ -117,19 +131,31 @@ impl<'w, 's> MapfConfigWidget<'w, 's> { }) .next(); ui.label("Occupancy"); - ui.indent("occupancy_map_info", |ui| { + ui.indent("occupancy_grid_info", |ui| { if let Some(grid) = occupancy_grid { - ui.label(format!(" cell size: {}m", grid.cell_size)); - ui.label(format!( - " grid size: {} x {}", - grid.range.max_cell().x - grid.range.min_cell().x, - grid.range.max_cell().y - grid.range.min_cell().y - )); + EguiGrid::new("occupancy_map_info") + .num_columns(2) + .show(ui, |ui| { + ui.label("min cell"); + ui.label(format!("{:?}", grid.range.min_cell())); + ui.end_row(); + ui.label("max cell"); + ui.label(format!("{:?}", grid.range.max_cell())); + ui.end_row(); + ui.label("dimension"); + ui.label(format!( + "{} x {}", + grid.range.max_cell().x - grid.range.min_cell().x, + grid.range.max_cell().y - grid.range.min_cell().y + )); + ui.end_row(); + ui.label("cell size"); + ui.label(format!("{} m", grid.cell_size)); + }); } else { ui.label("None"); } }); - // Generate Plan ui.horizontal(|ui| { let allow_generate_plan = num_tasks > 0 @@ -148,10 +174,6 @@ impl<'w, 's> MapfConfigWidget<'w, 's> { ); }); - if ui.button("Negotiation Test").clicked() { - negotiation_test(); - } - // Results ui.separator(); match self.negotiation_data.as_ref() { @@ -185,61 +207,9 @@ impl<'w, 's> MapfConfigWidget<'w, 's> { } _ => {} } - - ui.label("Nodes: "); - ui.label("Successful in : "); } pub fn show_planner(&mut self, ui: &mut Ui) { ui.label("Unavailable"); } } - -pub fn negotiation_test() { - let mut agents: BTreeMap = 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 = Vec::new(); - let occupancy: HashMap> = 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, - ] -} \ No newline at end of file diff --git a/mapf-rse/src/negotiation/debug_panel.rs b/mapf-rse/src/negotiation/debug_panel.rs index 81a3287..81d8d7f 100644 --- a/mapf-rse/src/negotiation/debug_panel.rs +++ b/mapf-rse/src/negotiation/debug_panel.rs @@ -1,5 +1,5 @@ /* - * Copyright (C) 2024 Open Source Robotics Foundation + * Copyright (C) 2024 active Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -14,3 +14,212 @@ * limitations under the License. * */ + +use super::*; +use bevy::{ecs::system::SystemParam, prelude::*, ui}; +use bevy_egui::egui::{ + self, Align, CollapsingHeader, Color32, Frame, Grid, ImageSource, InnerResponse, Response, + RichText, ScrollArea, Stroke, Ui, Window, +}; +use mapf::negotiation::Negotiation; +use rmf_site_editor::{ + interaction::{outline, ChangeMode, ModelPreviewCamera, SelectAnchor3D}, + site::{ + AssetSource, FuelClient, Group, IsStatic, Model, ModelDescriptionBundle, ModelMarker, + ModelProperty, NameInSite, Scale, SetFuelApiKey, UpdateFuelCache, + }, + widgets::prelude::*, +}; + +#[derive(Default)] +pub struct NegotiationDebugPlugin; + +impl Plugin for NegotiationDebugPlugin { + fn build(&self, app: &mut App) { + app.init_resource::(); + let panel = PanelWidget::new(negotiation_debug_panel, &mut app.world); + let widget = Widget::new::(&mut app.world); + app.world.spawn((panel, widget)); + } +} + +#[derive(SystemParam)] +pub struct NegotiationDebugWidget<'w, 's> { + commands: Commands<'w, 's>, + negotiation_data: ResMut<'w, NegotiationData>, + negotiation_debug_data: ResMut<'w, NegotiationDebugData>, +} + +fn negotiation_debug_panel(In(input): In, world: &mut World) { + if world.resource::().show_debug_panel { + egui::SidePanel::left("negotiation_debug_panel") + .resizable(true) + .min_width(320.0) + .show(&input.context, |ui| { + if let Err(err) = world.try_show(input.id, ui) { + error!("Unable to display asset gallery: {err:?}"); + } + }); + } +} + +impl<'w, 's> WidgetSystem for NegotiationDebugWidget<'w, 's> { + fn show(_: (), ui: &mut Ui, state: &mut SystemState, world: &mut World) { + let mut params = state.get_mut(world); + + ui.heading("Negotiation Debugger"); + match params.negotiation_data.as_ref() { + NegotiationData::Complete { .. } => { + params.show_completed(ui); + } + NegotiationData::InProgress { start_time } => { + ui.label(format!( + "In Progresss: {} s", + start_time.elapsed().as_secs_f32() + )); + } + NegotiationData::NotStarted => { + ui.label("No negotiation started"); + } + } + } +} + +impl<'w, 's> NegotiationDebugWidget<'w, 's> { + pub fn show_completed(&mut self, ui: &mut Ui) { + let NegotiationData::Complete { + elapsed_time, + solution, + negotiation_history, + entity_id_map, + error_message, + conflicting_endpoints, + } = self.negotiation_data.as_ref() + else { + return; + }; + + ui.add_space(10.0); + ui.label(format!( + "Solution [found in {} s]", + elapsed_time.as_secs_f32() + )); + match solution { + Some(solution) => { + show_negotiation_node( + ui, + &mut HashMap::new(), + &mut self.negotiation_debug_data, + solution, + ); + if self + .negotiation_debug_data + .selected_negotiation_node + .is_none() + { + self.negotiation_debug_data.selected_negotiation_node = Some(solution.id); + } + } + None => { + outline_frame(ui, |ui| { + ui.label("No solution found"); + }); + } + } + + ui.add_space(10.0); + ui.label("Errors"); + if let Some(error_message) = error_message { + outline_frame(ui, |ui| { + ui.horizontal_wrapped(|ui| { + ui.label(error_message.clone()); + }); + }); + } else { + outline_frame(ui, |ui| { + ui.label("No errors"); + }); + } + + ui.add_space(10.0); + ui.label("Negotiation History"); + + let mut id_response_map = HashMap::::new(); + ScrollArea::vertical().show(ui, |ui| { + for negotiation_node in negotiation_history { + let id = negotiation_node.id; + let mut response = show_negotiation_node( + ui, + &mut id_response_map, + &mut self.negotiation_debug_data, + negotiation_node, + ); + // id_response_map.insert(id, &mut response); + } + }); + } +} + +fn show_negotiation_node( + mut ui: &mut Ui, + mut id_response_map: &mut HashMap, + mut negotiation_debug_data: &mut ResMut, + node: &NegotiationNode, +) -> Response { + Frame::default() + .inner_margin(4.0) + .fill(Color32::DARK_GRAY) + .rounding(2.0) + .show(ui, |ui| { + ui.set_width(ui.available_width()); + + let id = node.id; + ui.horizontal(|ui| { + let selected = negotiation_debug_data.selected_negotiation_node == Some(id); + if ui.radio(selected, format!("#{}", node.id)).clicked() { + negotiation_debug_data.selected_negotiation_node = Some(id); + } + ui.label("|"); + ui.label(format!("Keys: {}", node.keys.len())); + ui.label("|"); + ui.label(format!("Conflicts: {}", node.negotiation.conflicts.len())); + ui.label("|"); + ui.label("Parent"); + match node.parent { + Some(parent) => { + if ui.button(format!("#{}", parent)).clicked() { + if let Some(response) = id_response_map.get_mut(&parent) { + response.scroll_to_me(Some(Align::Center)); + } + } + } + None => { + ui.label("None"); + } + } + }); + + CollapsingHeader::new("Information") + .id_source(id.to_string() + "node_info") + .default_open(false) + .show(ui, |ui| { + ui.label("Keys"); + for key in &node.keys { + ui.label(format!("{:?}", key)); + } + }); + }) + .response +} + +fn outline_frame(ui: &mut Ui, add_body: impl FnOnce(&mut Ui) -> R) -> Response { + Frame::default() + .inner_margin(4.0) + .stroke(Stroke::new(1.0, Color32::GRAY)) + .rounding(2.0) + .show(ui, |ui| { + ui.set_width(ui.available_width()); + ui.add_enabled_ui(true, add_body); + }) + .response +} diff --git a/mapf-rse/src/negotiation/mod.rs b/mapf-rse/src/negotiation/mod.rs index 22475de..f5b2a88 100644 --- a/mapf-rse/src/negotiation/mod.rs +++ b/mapf-rse/src/negotiation/mod.rs @@ -16,6 +16,7 @@ */ use bevy::{ + ecs::query::QueryEntityError, prelude::*, tasks::{block_on, AsyncComputeTaskPool, Task}, }; @@ -40,6 +41,9 @@ use mapf::negotiation::{Agent, Obstacle, Scenario as MapfScenario}; pub mod debug_panel; pub use debug_panel::*; +pub mod visual; +pub use visual::*; + #[derive(Default)] pub struct NegotiationPlugin; @@ -47,14 +51,16 @@ impl Plugin for NegotiationPlugin { fn build(&self, app: &mut App) { app.add_event::() .init_resource::() - .init_resource::(); - // .add_systems( - // Update, - // ( - // start_compute_negotiation, - // handle_compute_negotiation_complete, - // ), - // ); + .init_resource::() + .add_plugins(NegotiationDebugPlugin::default()) + .add_systems( + Update, + ( + start_compute_negotiation, + handle_compute_negotiation_complete, + visualise_selected_node, + ), + ); } } @@ -97,6 +103,27 @@ impl NegotiationData { } } +#[derive(Resource)] +pub struct NegotiationDebugData { + pub show_debug_panel: bool, + pub selected_negotiation_node: Option, + pub visualize_keys: bool, + pub visualize_conflicts: bool, + pub visualize_trajectories: bool, +} + +impl Default for NegotiationDebugData { + fn default() -> Self { + Self { + show_debug_panel: false, + selected_negotiation_node: None, + visualize_keys: false, + visualize_conflicts: true, + visualize_trajectories: true, + } + } +} + #[derive(Component)] pub struct ComputeNegotiateTask( Task< @@ -205,6 +232,7 @@ pub fn start_compute_negotiation( locations: Query<(Entity, &Point), With>, anchors: Query<(Entity, &Anchor, &Transform)>, negotiation_request: EventReader, + negotiation_params: Res, mut negotiation_data: ResMut, current_level: Res, grids: Query<(Entity, &Grid)>, @@ -305,13 +333,14 @@ pub fn start_compute_negotiation( cell_size: f64::from(cell_size), camera_bounds: None, }; + let queue_length_limit = negotiation_params.queue_length_limit; // Execute asynchronously let start_time = Instant::now(); *negotiation_data = NegotiationData::InProgress { start_time }; let thread_pool = AsyncComputeTaskPool::get(); - let task = thread_pool.spawn(async move { negotiate(&scenario, Some(1000000)) }); + let task = thread_pool.spawn(async move { negotiate(&scenario, Some(queue_length_limit)) }); commands.spawn(ComputeNegotiateTask(task)); } diff --git a/mapf-rse/src/negotiation/visual.rs b/mapf-rse/src/negotiation/visual.rs index e69de29..91b8110 100644 --- a/mapf-rse/src/negotiation/visual.rs +++ b/mapf-rse/src/negotiation/visual.rs @@ -0,0 +1,79 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use super::*; +use bevy::prelude::*; +use mapf::motion::se2::WaypointSE2; + +pub fn visualise_selected_node( + mut gizmos: Gizmos, + negotiation_data: Res, + mut debug_data: ResMut, +) { + // Return unless complete + let NegotiationData::Complete { + elapsed_time, + solution, + negotiation_history, + entity_id_map, + error_message, + conflicting_endpoints, + } = negotiation_data.as_ref() + else { + return; + }; + + let selected_node = if let Some(solution_node) = solution { + debug_data.selected_negotiation_node = Some(solution_node.id.clone()); + solution_node + } else if let Some(selected_node_id) = debug_data.selected_negotiation_node { + let node = negotiation_history + .iter() + .find(|node| node.id == selected_node_id); + if node.is_none() { + warn!("Selected negotiation node not found"); + return; + } + node.unwrap() + } else { + return; + }; + + if debug_data.visualize_trajectories { + for proposal in selected_node.proposals.iter() { + let line: Vec<(Vec3, Color)> = proposal + .1 + .meta + .trajectory + .iter() + .map(|WaypointSE2 { time: _, position }| { + ( + Vec3::new( + position.translation.x as f32, + position.translation.y as f32, + 0.1, + ), + Color::GREEN, + ) + }) + .collect(); + gizmos.linestrip_gradient(line); + } + } + + return; +} diff --git a/mapf/src/negotiation/mod.rs b/mapf/src/negotiation/mod.rs index 49f7f78..3364a12 100644 --- a/mapf/src/negotiation/mod.rs +++ b/mapf/src/negotiation/mod.rs @@ -714,6 +714,16 @@ impl std::fmt::Debug for Conflict { } } +impl Conflict { + pub fn time(&self) -> TimePoint { + self.time + } + + pub fn segments(&self) -> &[Segment; 2] { + &self.segments + } +} + pub type SippDecisionRange = DecisionRange>; pub type DecisionRangePair = (SippDecisionRange, SippDecisionRange); @@ -723,6 +733,16 @@ pub struct Segment { range: SippDecisionRange, } +impl Segment { + pub fn agent(&self) -> usize { + self.agent + } + + pub fn range(&self) -> &SippDecisionRange { + &self.range + } +} + fn reasses_conflicts( proposals: &HashMap, profiles: &Vec, From 489ed7bdad3200d3b9f28543a7c45e7567c1e1f2 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Thu, 1 Aug 2024 12:23:14 +0800 Subject: [PATCH 8/8] use std materials for viz Signed-off-by: Reuben Thomas --- mapf-rse/src/config_widget.rs | 36 ++++++-- mapf-rse/src/misc.rs | 1 + mapf-rse/src/negotiation/debug_panel.rs | 13 +-- mapf-rse/src/negotiation/mod.rs | 9 +- mapf-rse/src/negotiation/visual.rs | 109 ++++++++++++++++-------- 5 files changed, 116 insertions(+), 52 deletions(-) diff --git a/mapf-rse/src/config_widget.rs b/mapf-rse/src/config_widget.rs index 5a26d5f..ba46357 100644 --- a/mapf-rse/src/config_widget.rs +++ b/mapf-rse/src/config_widget.rs @@ -19,7 +19,7 @@ use super::*; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{CollapsingHeader, ComboBox, DragValue, Grid as EguiGrid, Ui}; use rmf_site_editor::{ - occupancy::Grid, + occupancy::{CalculateGrid, Grid}, site::{CurrentLevel, Group, MobileRobotMarker, Task, Tasks}, widgets::prelude::*, }; @@ -29,9 +29,11 @@ pub struct MapfConfigWidget<'w, 's> { simulation_config: ResMut<'w, SimulationConfig>, debug_mode: Res<'w, State>, debug_mode_next: ResMut<'w, NextState>, - mobile_robots: Query<'w, 's, &'static Tasks, (With, Without)>, + mobile_robots: + Query<'w, 's, (Entity, &'static Tasks), (With, Without)>, current_level: Res<'w, CurrentLevel>, grids: Query<'w, 's, (Entity, &'static Grid)>, + calculate_grid: EventWriter<'w, CalculateGrid>, parents: Query<'w, 's, &'static Parent>, negotiation_request: EventWriter<'w, NegotiationRequest>, negotiation_params: ResMut<'w, NegotiationParams>, @@ -99,7 +101,7 @@ impl<'w, 's> MapfConfigWidget<'w, 's> { let num_tasks = self .mobile_robots .iter() - .filter(|tasks| { + .filter(|(_, tasks)| { tasks.0.iter().any(|task| { if let Task::GoToPlace { location: _ } = task { true @@ -130,13 +132,37 @@ impl<'w, 's> MapfConfigWidget<'w, 's> { } }) .next(); + ui.horizontal(|ui| { + ui.label("Cell Size: "); + if ui + .add( + DragValue::new(&mut self.negotiation_params.cell_size) + .clamp_range(0.1..=1.0) + .suffix(" m") + .speed(0.01), + ) + .changed() + { + self.calculate_grid.send(CalculateGrid { + cell_size: self.negotiation_params.cell_size, + floor: 0.01, + ceiling: 1.5, + ignore: Some( + self.mobile_robots + .iter() + .map(|(entity, _)| entity) + .collect(), + ), + }); + } + }); ui.label("Occupancy"); ui.indent("occupancy_grid_info", |ui| { if let Some(grid) = occupancy_grid { EguiGrid::new("occupancy_map_info") .num_columns(2) .show(ui, |ui| { - ui.label("min cell"); + ui.label("range"); ui.label(format!("{:?}", grid.range.min_cell())); ui.end_row(); ui.label("max cell"); @@ -149,8 +175,6 @@ impl<'w, 's> MapfConfigWidget<'w, 's> { grid.range.max_cell().y - grid.range.min_cell().y )); ui.end_row(); - ui.label("cell size"); - ui.label(format!("{} m", grid.cell_size)); }); } else { ui.label("None"); diff --git a/mapf-rse/src/misc.rs b/mapf-rse/src/misc.rs index 6cb869d..d1707be 100644 --- a/mapf-rse/src/misc.rs +++ b/mapf-rse/src/misc.rs @@ -56,6 +56,7 @@ pub fn load_tiny_robot( translational_speed: 1.0, rotational_speed: 1.0, bidirectional: false, + collision_radius: 0.3, rotation_center_offset: [0.0, 0.0], }); let mobile_robot_marker = ModelProperty::::default(); diff --git a/mapf-rse/src/negotiation/debug_panel.rs b/mapf-rse/src/negotiation/debug_panel.rs index 81d8d7f..d64d7b7 100644 --- a/mapf-rse/src/negotiation/debug_panel.rs +++ b/mapf-rse/src/negotiation/debug_panel.rs @@ -98,7 +98,7 @@ impl<'w, 's> NegotiationDebugWidget<'w, 's> { else { return; }; - + // Solution node ui.add_space(10.0); ui.label(format!( "Solution [found in {} s]", @@ -112,13 +112,6 @@ impl<'w, 's> NegotiationDebugWidget<'w, 's> { &mut self.negotiation_debug_data, solution, ); - if self - .negotiation_debug_data - .selected_negotiation_node - .is_none() - { - self.negotiation_debug_data.selected_negotiation_node = Some(solution.id); - } } None => { outline_frame(ui, |ui| { @@ -126,7 +119,7 @@ impl<'w, 's> NegotiationDebugWidget<'w, 's> { }); } } - + // Error display ui.add_space(10.0); ui.label("Errors"); if let Some(error_message) = error_message { @@ -140,7 +133,7 @@ impl<'w, 's> NegotiationDebugWidget<'w, 's> { ui.label("No errors"); }); } - + // Negotiatio history ui.add_space(10.0); ui.label("Negotiation History"); diff --git a/mapf-rse/src/negotiation/mod.rs b/mapf-rse/src/negotiation/mod.rs index f5b2a88..830fad8 100644 --- a/mapf-rse/src/negotiation/mod.rs +++ b/mapf-rse/src/negotiation/mod.rs @@ -70,12 +70,14 @@ pub struct NegotiationRequest; #[derive(Debug, Clone, Resource)] pub struct NegotiationParams { pub queue_length_limit: usize, + pub cell_size: f32, } impl Default for NegotiationParams { fn default() -> Self { Self { queue_length_limit: 1_000_000, + cell_size: 0.2, } } } @@ -141,6 +143,7 @@ pub struct ComputeNegotiateTask( pub fn handle_compute_negotiation_complete( mut commands: Commands, mut compute_negotiation_task: Query<(Entity, &mut ComputeNegotiateTask)>, + mut negotiation_debug_data: ResMut, mut negotiation_data: ResMut, ) { let NegotiationData::InProgress { start_time } = *negotiation_data else { @@ -158,6 +161,7 @@ pub fn handle_compute_negotiation_complete( match result { Ok((solution, negotiation_history, name_map)) => { + negotiation_debug_data.selected_negotiation_node = Some(solution.id); *negotiation_data = NegotiationData::Complete { elapsed_time, solution: Some(solution), @@ -234,6 +238,7 @@ pub fn start_compute_negotiation( negotiation_request: EventReader, negotiation_params: Res, mut negotiation_data: ResMut, + mut negotiation_debug_data: ResMut, current_level: Res, grids: Query<(Entity, &Grid)>, parents: Query<&Parent>, @@ -245,6 +250,7 @@ pub fn start_compute_negotiation( warn!("Negotiation requested while another negotiation is in progress"); return; } + negotiation_debug_data.selected_negotiation_node = None; // Occupancy let mut occupancy = HashMap::>::new(); @@ -305,7 +311,6 @@ pub fn start_compute_negotiation( let Ok((_, _, goal_transform)) = anchors.get(*anchor_entity) else { continue; }; - let agent = Agent { start: to_cell(robot_pose.trans[0], robot_pose.trans[1], cell_size), yaw: f64::from(robot_pose.rot.yaw().radians()), @@ -314,7 +319,7 @@ pub fn start_compute_negotiation( goal_transform.translation.y, cell_size, ), - radius: 0.2, + radius: f64::from(differential_drive.collision_radius), speed: f64::from(differential_drive.translational_speed), spin: f64::from(differential_drive.rotational_speed), }; diff --git a/mapf-rse/src/negotiation/visual.rs b/mapf-rse/src/negotiation/visual.rs index 91b8110..2270fe4 100644 --- a/mapf-rse/src/negotiation/visual.rs +++ b/mapf-rse/src/negotiation/visual.rs @@ -16,13 +16,23 @@ */ use super::*; -use bevy::prelude::*; -use mapf::motion::se2::WaypointSE2; +use bevy::render::mesh::{shape::Quad, Mesh}; +use rmf_site_editor::site::line_stroke_transform; + +pub const DEFAULT_PATH_WIDTH: f32 = 0.2; + +#[derive(Component)] +pub struct PathVisualMarker; pub fn visualise_selected_node( - mut gizmos: Gizmos, + mut commands: Commands, negotiation_data: Res, mut debug_data: ResMut, + mut materials: ResMut>, + mut path_visuals: Query>, + mut meshes: ResMut>, + mobile_robots: Query<(Entity, &DifferentialDrive), (With, Without)>, + current_level: Res, ) { // Return unless complete let NegotiationData::Complete { @@ -36,44 +46,75 @@ pub fn visualise_selected_node( else { return; }; + if !debug_data.is_changed() && !negotiation_data.is_changed() { + return; + } + let Some(selected_node) = debug_data + .selected_negotiation_node + .and_then(|selected_id| { + if negotiation_history.is_empty() { + solution.clone() + } else { + negotiation_history + .iter() + .find(|node| node.id == selected_id) + .map(|node| node.clone()) + } + }) + else { + return; + }; - let selected_node = if let Some(solution_node) = solution { - debug_data.selected_negotiation_node = Some(solution_node.id.clone()); - solution_node - } else if let Some(selected_node_id) = debug_data.selected_negotiation_node { - let node = negotiation_history - .iter() - .find(|node| node.id == selected_node_id); - if node.is_none() { - warn!("Selected negotiation node not found"); - return; - } - node.unwrap() - } else { + let Some(level_entity) = current_level.0 else { return; }; + for path_visual in path_visuals.iter() { + commands.entity(path_visual).despawn_recursive(); + } + let mut spawn_path_mesh = |lane_tf, lane_material: Handle, lane_mesh| { + commands + .spawn(PbrBundle { + mesh: lane_mesh, + material: lane_material.clone(), + transform: lane_tf, + ..default() + }) + .insert(PathVisualMarker) + .set_parent(level_entity); + }; + if debug_data.visualize_trajectories { for proposal in selected_node.proposals.iter() { - let line: Vec<(Vec3, Color)> = proposal - .1 - .meta - .trajectory - .iter() - .map(|WaypointSE2 { time: _, position }| { - ( - Vec3::new( - position.translation.x as f32, - position.translation.y as f32, - 0.1, - ), - Color::GREEN, - ) + let collision_radius = entity_id_map + .get(&proposal.0) + .and_then(|e| { + mobile_robots + .get(*e) + .map(|(_, dd)| dd.collision_radius) + .ok() }) - .collect(); - gizmos.linestrip_gradient(line); + .unwrap_or(DEFAULT_PATH_WIDTH / 2.0); + + for (i, _waypoint) in proposal.1.meta.trajectory.iter().enumerate().skip(2) { + let start_pos = proposal.1.meta.trajectory[i - 1].position.translation; + let end_pos = proposal.1.meta.trajectory[i].position.translation; + let start_pos = Vec3::new(start_pos.x as f32, start_pos.y as f32, 0.1); + let end_pos = Vec3::new(end_pos.x as f32, end_pos.y as f32, 0.1); + + spawn_path_mesh( + line_stroke_transform(&start_pos, &end_pos, collision_radius * 2.0), + materials.add(StandardMaterial { + base_color: Color::rgb(0.0, 1.0, 0.0), + unlit: true, + ..Default::default() + }), + meshes.add(Mesh::from(shape::Quad { + size: Vec2::new(collision_radius * 10.0, collision_radius * 2.0), + flip: false, + })), + ); + } } } - - return; }