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 8dd3043..00c95d8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,7 +1,4 @@ [workspace] -members = [ - "mapf", - "mapf-viz", -] +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 new file mode 100644 index 0000000..c52a857 --- /dev/null +++ b/mapf-rse/Cargo.toml @@ -0,0 +1,12 @@ +[package] +name = "mapf-rse" +version = "0.1.0" +edition = "2021" + +[dependencies] +mapf = { path="../mapf" } +rmf_site_format = { git = "https://github.com/reuben-thomas/rmf_site.git", branch = "scenarios", package = "rmf_site_format" } +rmf_site_editor = { git = "https://github.com/reuben-thomas/rmf_site.git", branch = "scenarios", package = "rmf_site_editor" } +bevy = { version = "0.12", features = ["pnm", "jpeg", "tga"] } +bevy_egui = "0.23.0" +futures-lite = "2.3.0" diff --git a/mapf-rse/src/config_widget.rs b/mapf-rse/src/config_widget.rs new file mode 100644 index 0000000..ba46357 --- /dev/null +++ b/mapf-rse/src/config_widget.rs @@ -0,0 +1,239 @@ +/* + * 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::{ecs::system::SystemParam, prelude::*}; +use bevy_egui::egui::{CollapsingHeader, ComboBox, DragValue, Grid as EguiGrid, Ui}; +use rmf_site_editor::{ + occupancy::{CalculateGrid, Grid}, + site::{CurrentLevel, Group, MobileRobotMarker, Task, Tasks}, + widgets::prelude::*, +}; + +#[derive(SystemParam)] +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, (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>, + negotiation_data: ResMut<'w, NegotiationData>, + negotiation_debug: ResMut<'w, NegotiationDebugData>, +} + +impl<'w, 's> WidgetSystem for MapfConfigWidget<'w, 's> { + fn show(_: Tile, ui: &mut Ui, state: &mut SystemState, world: &mut World) -> () { + let mut params = state.get_mut(world); + ui.separator(); + + CollapsingHeader::new("MAPF Configuration") + .default_open(true) + .show(ui, |ui| { + 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), + DebugMode::Planner => params.show_planner(ui), + } + }); + } +} + +impl<'w, 's> MapfConfigWidget<'w, 's> { + pub fn show_negotiation(&mut self, ui: &mut Ui) { + // 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() + .filter(|(_, tasks)| { + tasks.0.iter().any(|task| { + if let Task::GoToPlace { location: _ } = task { + true + } else { + false + } + }) + }) + .count(); + ui.label(format!("Tasks: {}", num_tasks)); + // Grid Info + let occupancy_grid = self + .grids + .iter() + .filter_map(|(grid_entity, grid)| { + if let Some(level_entity) = self.current_level.0 { + if self + .parents + .get(grid_entity) + .is_ok_and(|parent_entity| parent_entity.get() == level_entity) + { + Some(grid) + } else { + None + } + } else { + None + } + }) + .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("range"); + 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(); + }); + } else { + ui.label("None"); + } + }); + // Generate Plan + ui.horizontal(|ui| { + let allow_generate_plan = num_tasks > 0 + && self.negotiation_params.queue_length_limit > 0 + && !self.negotiation_data.is_in_progress(); + + ui.add_enabled_ui(allow_generate_plan, |ui| { + if ui.button("Generate Plan").clicked() { + self.negotiation_request.send(NegotiationRequest); + } + }); + ui.add( + DragValue::new(&mut self.negotiation_params.queue_length_limit) + .clamp_range(0..=std::usize::MAX) + .speed(1000), + ); + }); + + // Results + ui.separator(); + match self.negotiation_data.as_ref() { + NegotiationData::Complete { + elapsed_time, + solution, + negotiation_history, + entity_id_map, + error_message, + conflicting_endpoints, + } => { + EguiGrid::new("negotiation_data") + .num_columns(2) + .show(ui, |ui| { + ui.label("execution time"); + ui.label(format!("{:.2} s", elapsed_time.as_secs_f32())); + ui.end_row(); + ui.label("negotiation history"); + ui.label(format!("{}", negotiation_history.len())); + ui.end_row(); + ui.label("endpoint conflicts"); + ui.label(format!("{}", conflicting_endpoints.len())); + ui.end_row(); + ui.label("error message"); + ui.label(error_message.clone().unwrap_or("None".to_string())); + }); + } + NegotiationData::InProgress { start_time } => { + let elapsed_time = start_time.elapsed(); + ui.label(format!("In Progress: {}", elapsed_time.as_secs_f32())); + } + _ => {} + } + } + + pub fn show_planner(&mut self, ui: &mut Ui) { + ui.label("Unavailable"); + } +} diff --git a/mapf-rse/src/lib.rs b/mapf-rse/src/lib.rs new file mode 100644 index 0000000..3a8aef1 --- /dev/null +++ b/mapf-rse/src/lib.rs @@ -0,0 +1,93 @@ +/* + * 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 negotiation; +pub use negotiation::*; + +pub mod config_widget; +pub use config_widget::*; + +pub mod misc; +pub use misc::*; + +use rmf_site_editor::widgets::PropertiesTilePlugin; + +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(NegotiationPlugin) + .add_plugins(PropertiesTilePlugin::::new()) + .add_systems(Update, load_tiny_robot); + } +} + +#[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] + Negotiation, + Planner, +} + +impl DebugMode { + pub fn labels() -> Vec<&'static str> { + vec!["Negotiation", "Planner"] + } + + pub fn label(&self) -> &str { + match self { + DebugMode::Negotiation => Self::labels()[0], + DebugMode::Planner => Self::labels()[1], + } + } + + pub fn from_label(label: &str) -> Self { + if label == Self::labels()[0] { + return DebugMode::Negotiation; + } else { + return DebugMode::Planner; + } + } +} diff --git a/mapf-viz/src/lib.rs b/mapf-rse/src/main.rs similarity index 50% rename from mapf-viz/src/lib.rs rename to mapf-rse/src/main.rs index 913c139..8aae3bc 100644 --- a/mapf-viz/src/lib.rs +++ b/mapf-rse/src/main.rs @@ -1,5 +1,5 @@ /* - * Copyright (C) 2022 Open Source Robotics Foundation + * 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. @@ -15,26 +15,12 @@ * */ -pub mod spatial_canvas; -pub use spatial_canvas::SpatialCanvasProgram; +use bevy::prelude::*; +use mapf_rse::MapfRsePlugin; +use rmf_site_editor::*; -pub mod visibility_visual; -pub use visibility_visual::{SparseGridVisibilityVisual, VisibilityVisual}; - -pub mod accessibility_visual; -pub use accessibility_visual::{AccessibilityVisual, SparseGridAccessibilityVisual}; - -pub mod grid; -pub use grid::InfiniteGrid; - -pub mod toggle; -pub use toggle::{Toggle, Toggler}; - -#[cfg(test)] -mod tests { - #[test] - fn it_works() { - let result = 2 + 2; - assert_eq!(result, 4); - } +fn main() { + let mut app = App::new(); + app.add_plugins((SiteEditor::default(), MapfRsePlugin::default())); + app.run(); } diff --git a/mapf-rse/src/misc.rs b/mapf-rse/src/misc.rs new file mode 100644 index 0000000..d1707be --- /dev/null +++ b/mapf-rse/src/misc.rs @@ -0,0 +1,71 @@ +/* + * 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 bevy::prelude::*; +use rmf_site_editor::{site::site, workspace::CurrentWorkspace}; +use rmf_site_format::{ + AssetSource, Category, DifferentialDrive, Group, IsStatic, MobileRobotMarker, + ModelDescriptionBundle, ModelMarker, ModelProperty, NameInSite, Scale, +}; + +/// Loads the TinyRobot model into the scene with differential drive and a mobile robot marker for convenience +pub fn load_tiny_robot( + mut commands: Commands, + mobile_robots: Query<&NameInSite, (With, With)>, + current_workspace: Res, +) { + if !current_workspace.is_changed() { + return; + } + let Some(site_entity) = current_workspace.root else { + return; + }; + if mobile_robots + .iter() + .find(|name| name.0 == "TinyRobot") + .is_some() + { + return; + } + + let description = ModelDescriptionBundle { + name: NameInSite("TinyRobot".to_string()), + source: ModelProperty(AssetSource::Remote("Luca/TinyRobot/model.sdf".to_string())), + is_static: ModelProperty(IsStatic(false)), + scale: ModelProperty(Scale::default()), + marker: ModelMarker, + group: Group, + }; + // Wrapping differential_drive and mobile_robot_marker in ModelProperty allows us + // to apply the same properties to its instances + let differential_drive = ModelProperty(DifferentialDrive { + 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(); + commands + .spawn(description) + .insert(differential_drive) + .insert(Category::ModelDescription) + .insert(mobile_robot_marker) + .set_parent(site_entity); + + println!("LOADED"); +} diff --git a/mapf-rse/src/negotiation/debug_panel.rs b/mapf-rse/src/negotiation/debug_panel.rs new file mode 100644 index 0000000..d64d7b7 --- /dev/null +++ b/mapf-rse/src/negotiation/debug_panel.rs @@ -0,0 +1,218 @@ +/* + * 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. + * 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::{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; + }; + // Solution node + 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, + ); + } + None => { + outline_frame(ui, |ui| { + ui.label("No solution found"); + }); + } + } + // Error display + 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"); + }); + } + // Negotiatio history + 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 new file mode 100644 index 0000000..830fad8 --- /dev/null +++ b/mapf-rse/src/negotiation/mod.rs @@ -0,0 +1,356 @@ +/* + * 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 bevy::{ + ecs::query::QueryEntityError, + prelude::*, + tasks::{block_on, AsyncComputeTaskPool, Task}, +}; +use futures_lite::future; +use std::{ + collections::{BTreeMap, HashMap}, + fmt::Debug, + time::{Duration, Instant}, +}; + +use mapf::negotiation::*; +use rmf_site_editor::{ + occupancy::{Cell, Grid}, + site::{ + 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 debug_panel; +pub use debug_panel::*; + +pub mod visual; +pub use visual::*; + +#[derive(Default)] +pub struct NegotiationPlugin; + +impl Plugin for NegotiationPlugin { + fn build(&self, app: &mut App) { + app.add_event::() + .init_resource::() + .init_resource::() + .add_plugins(NegotiationDebugPlugin::default()) + .add_systems( + Update, + ( + start_compute_negotiation, + handle_compute_negotiation_complete, + visualise_selected_node, + ), + ); + } +} + +#[derive(Event)] +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, + } + } +} + +#[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(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< + Result< + ( + NegotiationNode, + Vec, + HashMap, + ), + NegotiationError, + >, + >, +); + +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 { + 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) + } + + 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_debug_data.selected_negotiation_node = Some(solution.id); + *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 start_compute_negotiation( + mut commands: Commands, + mobile_robots: Query< + ( + Entity, + &NameInSite, + &Pose, + &DifferentialDrive, + &RobotTasks, + ), + (With, Without), + >, + locations: Query<(Entity, &Point), With>, + anchors: Query<(Entity, &Anchor, &Transform)>, + 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>, +) { + if negotiation_request.len() == 0 { + return; + } + if let NegotiationData::InProgress { .. } = *negotiation_data { + warn!("Negotiation requested while another negotiation is in progress"); + return; + } + negotiation_debug_data.selected_negotiation_node = None; + + // Occupancy + let mut occupancy = HashMap::>::new(); + let mut cell_size = 1.0; + let grid = grids + .iter() + .filter_map(|(grid_entity, grid)| { + if let Some(level_entity) = current_level.0 { + if parents + .get(grid_entity) + .is_ok_and(|parent_entity| parent_entity.get() == level_entity) + { + Some(grid) + } else { + None + } + } else { + None + } + }) + .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"); + } + } + + // Agent + let mut agents = BTreeMap::::new(); + 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 { location } = task { + Some(location.0) + } else { + None + } + }) + .next() + else { + continue; + }; + let Ok((_, Point(anchor_entity))) = locations.get(location_entity) else { + continue; + }; + 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()), + goal: to_cell( + goal_transform.translation.x, + goal_transform.translation.y, + cell_size, + ), + radius: f64::from(differential_drive.collision_radius), + speed: f64::from(differential_drive.translational_speed), + spin: f64::from(differential_drive.rotational_speed), + }; + let agent_id = robot_entity.to_bits().to_string(); + agents.insert(agent_id, agent); + } + if agents.len() == 0 { + warn!("No agents with valid GoToPlace task"); + return; + } + + let scenario = MapfScenario { + agents: agents, + obstacles: Vec::::new(), + occupancy: occupancy, + 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(queue_length_limit)) }); + + commands.spawn(ComputeNegotiateTask(task)); +} + +fn to_cell(x: f32, y: f32, cell_size: f32) -> [i64; 2] { + let cell = Cell::from_point(Vec2::new(x, y), cell_size); + [cell.x, cell.y] +} diff --git a/mapf-rse/src/negotiation/visual.rs b/mapf-rse/src/negotiation/visual.rs new file mode 100644 index 0000000..2270fe4 --- /dev/null +++ b/mapf-rse/src/negotiation/visual.rs @@ -0,0 +1,120 @@ +/* + * 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::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 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 { + elapsed_time, + solution, + negotiation_history, + entity_id_map, + error_message, + conflicting_endpoints, + } = negotiation_data.as_ref() + 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 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 collision_radius = entity_id_map + .get(&proposal.0) + .and_then(|e| { + mobile_robots + .get(*e) + .map(|(_, dd)| dd.collision_radius) + .ok() + }) + .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, + })), + ); + } + } + } +} diff --git a/mapf-rse/src/simulation.rs b/mapf-rse/src/simulation.rs new file mode 100644 index 0000000..40474f1 --- /dev/null +++ b/mapf-rse/src/simulation.rs @@ -0,0 +1,121 @@ +/* + * 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 bevy::{ecs::system::SystemParam, prelude::*}; +use bevy_egui::egui::{ + Button, CollapsingHeader, Color32, ComboBox, DragValue, Frame, ScrollArea, Slider, Stroke, Ui, +}; +use rmf_site_editor::{ + interaction::{Select, Selection}, + site::{ + Category, Change, ChangeCurrentScenario, CurrentScenario, Delete, Group, MobileRobotMarker, + NameInSite, Scenario, ScenarioMarker, Tasks, + }, + widgets::prelude::*, + widgets::{view_scenarios::ScenarioDisplay, Icons}, +}; + +#[derive(SystemParam)] +pub struct SimulationControlTile<'w> { + simulation_config: ResMut<'w, SimulationConfig>, + debug_mode: Res<'w, State>, + debug_mode_next: ResMut<'w, NextState>, +} + +impl<'w> WidgetSystem for SimulationControlTile<'w> { + fn show(_: Tile, ui: &mut Ui, state: &mut SystemState, world: &mut World) -> () { + let mut params = state.get_mut(world); + ui.separator(); + + CollapsingHeader::new("Simulation") + .default_open(true) + .show(ui, |ui| { + // Play/pause, speed, stepo + ui.horizontal(|ui| { + if ui + .selectable_label(!params.simulation_config.is_playing, "⏸") + .clicked() + { + params.simulation_config.is_playing = !params.simulation_config.is_playing; + }; + if ui + .selectable_label(params.simulation_config.is_playing, "▶") + .clicked() + { + params.simulation_config.is_playing = !params.simulation_config.is_playing; + }; + + ui.add( + DragValue::new(&mut params.simulation_config.speed) + .clamp_range(0_f32..=10.0) + .suffix(" ×") + .speed(0.01), + ); + ui.horizontal(|ui| match params.debug_mode.get() { + DebugMode::Negotation => { + let end_time = params.simulation_config.end_time.clone(); + ui.add( + DragValue::new(&mut params.simulation_config.current_time) + .clamp_range(0_f32..=end_time) + .suffix(format!(" / {} s", end_time.to_string())) + .speed(0.01), + ); + } + DebugMode::Planner => { + let end_step = params.simulation_config.end_step.clone(); + ui.add( + DragValue::new(&mut params.simulation_config.current_step) + .clamp_range(0_u32..=end_step) + .suffix(format!(" / {} steps", end_step.to_string())) + .speed(0.01), + ); + } + }); + }); + + // Time/step control + ui.scope(|ui| { + ui.spacing_mut().slider_width = ui.available_width(); + ui.horizontal(|ui| match params.debug_mode.get() { + DebugMode::Negotation => { + let end_time = params.simulation_config.end_time.clone(); + ui.add( + Slider::new( + &mut params.simulation_config.current_time, + 0.0..=end_time, + ) + .show_value(false) + .clamp_to_range(true), + ); + } + DebugMode::Planner => { + let end_step = params.simulation_config.end_step.clone(); + ui.add( + Slider::new( + &mut params.simulation_config.current_step, + 0..=end_step, + ) + .show_value(false) + .step_by(1.0) + .clamp_to_range(true), + ); + } + }); + }); + }); + } +} diff --git a/mapf-viz/.gitignore b/mapf-viz/.gitignore deleted file mode 100644 index 96ef6c0..0000000 --- a/mapf-viz/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -/target -Cargo.lock diff --git a/mapf-viz/Cargo.toml b/mapf-viz/Cargo.toml deleted file mode 100644 index 85ac038..0000000 --- a/mapf-viz/Cargo.toml +++ /dev/null @@ -1,23 +0,0 @@ -[package] -name = "mapf-viz" -version = "0.1.1" -edition = "2021" - -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html - -[dependencies] -mapf = { path="../mapf" } -iced = { git = "https://github.com/mxgrey/iced", branch = "asymmetric_scale", features = ["canvas", "smol"] } -iced_aw = { git = "https://github.com/iced-rs/iced_aw", tag = "v0.1.0" } -iced_native = { git = "https://github.com/mxgrey/iced", branch = "asymmetric_scale" } -serde = { version="1.0", features = ["derive"] } -serde_yaml = "*" -async-std = "1.0" -directories-next = "2.0" -native-dialog = "*" -derivative = "*" -lyon = "*" -nalgebra = "*" -arrayvec = "*" -time-point = "*" -clap = { version = "4.2.1", features = ["derive"] } diff --git a/mapf-viz/scenarios/T-crossing.yaml b/mapf-viz/scenarios/T-crossing.yaml deleted file mode 100644 index 994978c..0000000 --- a/mapf-viz/scenarios/T-crossing.yaml +++ /dev/null @@ -1,88 +0,0 @@ -agents: - A: - start: - - -2 - - -16 - yaw: 0.0 - goal: - - 1 - - -16 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - B: - start: - - 8 - - -16 - yaw: 2.5949555318651694 - goal: - - -1 - - -16 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - C: - start: - - 6 - - -16 - yaw: 0.0 - goal: - - 0 - - -16 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 -obstacles: [] -occupancy: - -13: - - 2 - - 4 - -10: - - 2 - - 3 - - 4 - -11: - - 2 - - 4 - -12: - - 2 - - 4 - -16: - - -3 - - 9 - -15: - - -3 - - -2 - - -1 - - 0 - - 1 - - 2 - - 4 - - 5 - - 6 - - 7 - - 8 - - 9 - -17: - - -3 - - -2 - - -1 - - 0 - - 1 - - 2 - - 3 - - 4 - - 5 - - 6 - - 7 - - 8 - - 9 - -14: - - 2 - - 4 -cell_size: 1.0 -camera_bounds: -- - -6.242058 - - -20.061266 -- - 13.77696 - - -7.475301 diff --git a/mapf-viz/scenarios/sizes.yaml b/mapf-viz/scenarios/sizes.yaml deleted file mode 100644 index 66cdb0c..0000000 --- a/mapf-viz/scenarios/sizes.yaml +++ /dev/null @@ -1,56 +0,0 @@ -agents: - A: - start: - - 0 - - 2 - yaw: 0.0 - goal: - - -2 - - -5 - radius: 0.96575 - speed: 0.75 - spin: 1.0575648069534442 - B: - start: - - 5 - - -6 - yaw: 1.5582299561805382 - goal: - - -3 - - 3 - radius: 0.6539 - speed: 0.75 - spin: 1.0471975511965976 - C: - start: - - 6 - - -5 - yaw: -3.141592653589793 - goal: - - -2 - - 0 - radius: 0.248 - speed: 0.75 - spin: 1.0471975511965976 -obstacles: [] -occupancy: - -5: - - 1 - 0: - - -1 - - 0 - - 1 - - 2 - -2: - - -4 - - -3 - - -2 - -1: - - -4 - - 1 -cell_size: 1.0 -camera_bounds: -- - -7.9288683 - - -12.292385 -- - 11.30519 - - 4.3310356 diff --git a/mapf-viz/scenarios/warehouse.yaml b/mapf-viz/scenarios/warehouse.yaml deleted file mode 100644 index d3144d2..0000000 --- a/mapf-viz/scenarios/warehouse.yaml +++ /dev/null @@ -1,868 +0,0 @@ -agents: - A: - start: - - -31 - - 11 - yaw: 0.0 - goal: - - 19 - - 1 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - AA: - start: - - 16 - - -6 - yaw: 0.0 - goal: - - 16 - - 5 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - AB: - start: - - -14 - - 3 - yaw: 0.0 - goal: - - -20 - - 8 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - AC: - start: - - 10 - - -6 - yaw: 0.0 - goal: - - 10 - - 11 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - AD: - start: - - -24 - - 9 - yaw: 0.0 - goal: - - -20 - - 14 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - B: - start: - - 19 - - 6 - yaw: -2.8776988706882505 - goal: - - -31 - - 13 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - C: - start: - - -28 - - 13 - yaw: 0.0 - goal: - - -2 - - -7 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - D: - start: - - -14 - - 11 - yaw: 0.0 - goal: - - 1 - - 11 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - E: - start: - - -14 - - -7 - yaw: 0.0 - goal: - - 13 - - -7 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - F: - start: - - -20 - - 11 - yaw: 0.0 - goal: - - 13 - - 9 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - G: - start: - - 4 - - -7 - yaw: 0.0 - goal: - - 7 - - 10 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - H: - start: - - 13 - - 5 - yaw: -2.8902652413026098 - goal: - - -8 - - 9 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - I: - start: - - 1 - - -7 - yaw: 3.0347785033677406 - goal: - - 13 - - -5 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - J: - start: - - 13 - - -9 - yaw: 0.0 - goal: - - -11 - - -6 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - K: - start: - - -5 - - 8 - yaw: 0.0 - goal: - - 4 - - 7 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - L: - start: - - -5 - - -7 - yaw: 3.103893541746716 - goal: - - -14 - - 8 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - M: - start: - - 7 - - 7 - yaw: 0.0 - goal: - - -11 - - 12 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - N: - start: - - 7 - - -7 - yaw: -3.066194429903638 - goal: - - 10 - - 9 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - O: - start: - - -11 - - 4 - yaw: 3.141592653589793 - goal: - - -24 - - 11 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - P: - start: - - 19 - - -6 - yaw: -2.971946650295944 - goal: - - -14 - - -5 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - Q: - start: - - 4 - - 10 - yaw: 0.0 - goal: - - -28 - - 9 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - R: - start: - - -2 - - 9 - yaw: 0.0 - goal: - - 19 - - 11 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - S: - start: - - -17 - - 10 - yaw: -3.009645762139022 - goal: - - 19 - - -8 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - T: - start: - - -8 - - -6 - yaw: 0.0 - goal: - - -5 - - 5 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - U: - start: - - -8 - - 4 - yaw: 0.0 - goal: - - 1 - - -5 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - V: - start: - - -11 - - -8 - yaw: -3.1164599123610746 - goal: - - 7 - - -9 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - W: - start: - - 7 - - 4 - yaw: 0.0 - goal: - - -24 - - 13 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - X: - start: - - -2 - - 4 - yaw: 0.0 - goal: - - 13 - - 2 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - Y: - start: - - 16 - - 9 - yaw: 0.0 - goal: - - -11 - - 9 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 - Z: - start: - - -5 - - 12 - yaw: -0.2764601535159019 - goal: - - 1 - - 5 - radius: 0.45 - speed: 0.75 - spin: 1.0471975511965976 -obstacles: [] -occupancy: - -14: - - -20 - - -19 - - -18 - - -17 - - -16 - - -15 - - -14 - - -13 - - -12 - - -11 - - -10 - - -9 - - -8 - - -7 - - -6 - - -5 - - -4 - - -3 - - -2 - - -1 - - 0 - - 1 - - 2 - - 3 - - 4 - - 5 - - 6 - - 7 - - 8 - - 9 - - 10 - - 11 - - 12 - - 13 - - 14 - - 15 - - 16 - - 17 - - 18 - - 19 - - 20 - - 21 - - 22 - - 23 - -11: - - -20 - - 23 - -9: - - -20 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - 8: - - -33 - - -30 - - -27 - - -26 - - -23 - - -22 - - -19 - - -18 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - 1: - - -20 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - 19: - - -33 - - -32 - - -31 - - -30 - - -29 - - -28 - - -27 - - -26 - - -25 - - -24 - - -23 - - -22 - - -21 - - -20 - - -19 - - -18 - - -17 - - -16 - - -15 - - -14 - - -13 - - -12 - - -11 - - -10 - - -9 - - -8 - - -7 - - -6 - - -5 - - -4 - - -3 - - -2 - - -1 - - 0 - - 1 - - 2 - - 3 - - 4 - - 5 - - 6 - - 7 - - 8 - - 9 - - 10 - - 11 - - 12 - - 13 - - 14 - - 15 - - 16 - - 17 - - 18 - - 19 - - 20 - - 21 - - 22 - - 23 - -4: - - -20 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - -10: - - -20 - - 23 - 4: - - -33 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - 14: - - -33 - - -30 - - -27 - - -26 - - -23 - - -22 - - -19 - - -18 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - 3: - - -33 - - -32 - - -31 - - -30 - - -29 - - -28 - - -27 - - -26 - - -25 - - -24 - - -23 - - -22 - - -21 - - -20 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - 12: - - -33 - - -30 - - -27 - - -26 - - -23 - - -22 - - -19 - - -18 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - 0: - - -20 - - 23 - -2: - - -20 - - 23 - -6: - - -20 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - 10: - - -33 - - -30 - - -27 - - -26 - - -23 - - -22 - - -19 - - -18 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - 17: - - -33 - - 23 - 2: - - -20 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - -3: - - -20 - - 23 - 11: - - -33 - - -30 - - -27 - - -26 - - -23 - - -22 - - -19 - - -18 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - 7: - - -33 - - -30 - - -27 - - -26 - - -23 - - -22 - - -19 - - -18 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - 15: - - -33 - - -30 - - -27 - - -26 - - -23 - - -22 - - -19 - - -18 - - 23 - 16: - - -33 - - -30 - - -27 - - -26 - - -23 - - -22 - - -19 - - -18 - - 23 - 5: - - -33 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - -13: - - -20 - - 23 - -7: - - -20 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - 9: - - -33 - - -30 - - -27 - - -26 - - -23 - - -22 - - -19 - - -18 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - -12: - - -20 - - 23 - 18: - - -33 - - 23 - 13: - - -33 - - -30 - - -27 - - -26 - - -23 - - -22 - - -19 - - -18 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - 6: - - -33 - - -30 - - -27 - - -26 - - -23 - - -22 - - -19 - - -18 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - -5: - - -20 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - -8: - - -20 - - -13 - - -12 - - -7 - - -6 - - -1 - - 0 - - 5 - - 6 - - 11 - - 12 - - 17 - - 18 - - 23 - -1: - - -20 - - 23 -cell_size: 1.0 -camera_bounds: -- - -34.651337 - - -22.075638 -- - 26.880142 - - 23.979637 diff --git a/mapf-viz/src/accessibility_visual.rs b/mapf-viz/src/accessibility_visual.rs deleted file mode 100644 index c473880..0000000 --- a/mapf-viz/src/accessibility_visual.rs +++ /dev/null @@ -1,243 +0,0 @@ -/* - * Copyright (C) 2023 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::{ - spatial_canvas::{InclusionZone, SpatialCache, SpatialCanvasProgram}, - toggle::{DragToggler, FillToggler, Toggle, Toggler}, -}; -use derivative::Derivative; -use iced::{ - canvas::{ - event::{self, Event}, - Cursor, Frame, Path, - }, - keyboard, mouse, Rectangle, -}; -use mapf::graph::occupancy::{ - accessibility_graph::CellAccessibility, Accessibility, Cell, Grid, SparseGrid, -}; - -#[derive(Derivative)] -#[derivative(Debug)] -pub struct AccessibilityVisual { - #[derivative(Debug = "ignore")] - accessibility: Accessibility, - - // NOTE: After changing one of the public fields below, you must clear the - // cache of the SpatialCanvas that this program belongs to before the change - // will be rendered. - pub occupancy_color: iced::Color, - pub inaccessible_color: iced::Color, - pub open_corner_color: iced::Color, - pub show_accessibility: bool, - - #[derivative(Debug = "ignore")] - pub cell_toggler: Option>, - - #[derivative(Debug = "ignore")] - pub on_occupancy_change: Option Message>>, - - _ignore: std::marker::PhantomData, -} - -impl AccessibilityVisual { - pub fn new( - grid: G, - robot_radius: f32, - on_occupancy_change: Option Message>>, - ) -> Self { - Self { - accessibility: Accessibility::new(grid, robot_radius as f64), - occupancy_color: iced::Color::from_rgb8(0x40, 0x44, 0x4B), - inaccessible_color: iced::Color::from_rgb8(204, 210, 219), - open_corner_color: iced::Color::from_rgb8(240, 240, 240), - show_accessibility: true, - cell_toggler: Some(Box::new(FillToggler::new( - DragToggler::new( - None, - Some((keyboard::Modifiers::SHIFT, mouse::Button::Left)), - ), - DragToggler::new( - None, - Some((keyboard::Modifiers::SHIFT, mouse::Button::Right)), - ), - ))), - on_occupancy_change, - _ignore: Default::default(), - } - } - - pub fn showing_accessibility(mut self, showing: bool) -> Self { - self.show_accessibility = showing; - self - } - - pub fn set_robot_radius(&mut self, radius: f32) { - self.accessibility.change_agent_radius(radius as f64); - } - - pub fn accessibility(&self) -> &Accessibility { - &self.accessibility - } - - pub fn accessibiltiy_mut(&mut self) -> &mut Accessibility { - &mut self.accessibility - } - - pub fn grid(&self) -> &G { - &self.accessibility.grid() - } - - pub fn set_grid(&mut self, grid: G) { - self.accessibility = Accessibility::new(grid, self.accessibility.agent_radius()); - } - - fn toggle(&mut self, p: iced::Point) -> bool { - if let Some(cell_toggler) = &self.cell_toggler { - let cell = Cell::from_point( - [p.x as f64, p.y as f64].into(), - self.accessibility.grid().cell_size(), - ); - match cell_toggler.state() { - Toggle::On => { - return self.accessibility.change_cells([(cell, true)].into()); - } - Toggle::Off => { - return self.accessibility.change_cells([(cell, false)].into()); - } - Toggle::NoChange => { - return false; - } - } - } - - return false; - } -} - -impl SpatialCanvasProgram for AccessibilityVisual { - fn update( - &mut self, - event: Event, - cursor: Cursor, - ) -> (SpatialCache, event::Status, Option) { - if let Some(cell_toggler) = &mut self.cell_toggler { - cell_toggler.toggle(event); - } - - if let Some(p) = cursor.position() { - if self.toggle(p) { - return ( - SpatialCache::Refresh, - event::Status::Captured, - self.on_occupancy_change.as_ref().map(|x| x()), - ); - } - } - - (SpatialCache::Unchanged, event::Status::Ignored, None) - } - - fn draw_in_space(&self, frame: &mut Frame, _: Rectangle, _: Cursor) { - let cell_size = self.accessibility.grid().cell_size(); - for cell in self.accessibility.grid().occupied_cells() { - let p = cell.bottom_left_point(cell_size); - frame.fill_rectangle( - [p.x as f32, p.y as f32].into(), - iced::Size::new(cell_size as f32, cell_size as f32), - self.occupancy_color, - ); - } - - if self.show_accessibility { - for (cell, a) in self.accessibility.iter_accessibility() { - if a.is_inaccessible() { - let p = cell.bottom_left_point(cell_size); - frame.fill_rectangle( - [p.x as f32, p.y as f32].into(), - iced::Size::new(cell_size as f32, cell_size as f32), - self.inaccessible_color, - ); - } - } - - for (cell, a) in self.accessibility.iter_accessibility() { - if let CellAccessibility::Accessible(directions) = a { - let p0 = cell.center_point(cell_size); - let p0 = iced::Point::new(p0.x as f32, p0.y as f32); - for [i, j] in directions.iter() { - if i == 0 || j == 0 { - continue; - } - - let i_bl = (i + 1) / 2; - let j_bl = (j + 1) / 2; - - let show_corner = 'corner: { - for [k, m] in [[0, 0], [-1, 0], [-1, -1], [0, -1]] { - let check = cell.shifted(i_bl, j_bl).shifted(k, m); - if self.accessibility.is_inaccessible(&check) { - break 'corner true; - } - } - - false - }; - - if show_corner { - let p1 = cell.shifted(i, j).center_point(cell_size); - let p1 = iced::Point::new(p1.x as f32, p1.y as f32); - let ratio = 1.0 / 4.0 as f32; - let w = cell_size as f32 * ratio; - let path = Path::new(|builder| { - builder.move_to(p0 + iced::Vector::new(w, 0.0)); - builder.line_to(p1 + iced::Vector::new(w, 0.0)); - builder.line_to(p1 + iced::Vector::new(-w, 0.0)); - builder.line_to(p0 + iced::Vector::new(-w, 0.0)); - }); - - frame.fill(&path, self.open_corner_color); - } - } - } - } - } - } - - fn estimate_bounds(&self) -> InclusionZone { - let mut zone = InclusionZone::Empty; - let r = self.accessibility.agent_radius() + self.accessibility.grid().cell_size(); - let r = r as f32; - for cell in self - .accessibility - .grid() - .occupied_cells() - .into_iter() - .chain(self.accessibility.iter_accessibility().map(|(c, _)| c)) - { - let p = cell.center_point(self.accessibility.grid().cell_size()); - let p: iced::Point = [p.x as f32, p.y as f32].into(); - let d = iced::Vector::new(r, r); - zone.include(p + d); - zone.include(p - d); - } - - zone - } -} - -pub type SparseGridAccessibilityVisual = AccessibilityVisual; diff --git a/mapf-viz/src/grid.rs b/mapf-viz/src/grid.rs deleted file mode 100644 index 684f1aa..0000000 --- a/mapf-viz/src/grid.rs +++ /dev/null @@ -1,99 +0,0 @@ -/* - * Copyright (C) 2022 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 iced::{ - canvas::{Cursor, Frame}, - Point, Rectangle, -}; - -use super::{spatial_canvas::InclusionZone, SpatialCanvasProgram}; - -#[derive(Debug)] -pub struct InfiniteGrid { - origin: Point, - cell_size: f32, - width: f32, - _msg: std::marker::PhantomData, -} - -impl InfiniteGrid { - pub fn new(cell_size: f32) -> Self { - Self { - origin: Point::ORIGIN, - cell_size, - width: cell_size / 10_f32, - _msg: Default::default(), - } - } - - fn traverse( - &self, - frame: &mut Frame, - origin_v: f32, - v_min: f32, - v_max: f32, - f: impl Fn(f32, &mut Frame), - ) { - let mut v = origin_v; - while v >= v_min { - if v <= v_max { - f(v, frame); - } - - v -= self.cell_size; - } - - v = origin_v + self.cell_size; - while v <= v_max { - if v >= v_min { - f(v, frame); - } - - v += self.cell_size; - } - } -} - -impl SpatialCanvasProgram for InfiniteGrid { - fn draw_in_space(&self, frame: &mut Frame, spatial_bounds: Rectangle, _: Cursor) { - let x_min = spatial_bounds.x; - let x_max = spatial_bounds.x + spatial_bounds.width; - let y_min = spatial_bounds.y; - let y_max = spatial_bounds.y + spatial_bounds.height; - let line_color = iced::Color::from_rgb8(200, 204, 213); - - self.traverse(frame, self.origin.x, x_min, x_max, |x, frame| { - frame.fill_rectangle( - iced::Point::new(x - self.width / 2.0, y_min), - iced::Size::new(self.width, y_max - y_min), - line_color, - ); - }); - - self.traverse(frame, self.origin.y, y_min, y_max, |y, frame| { - frame.fill_rectangle( - iced::Point::new(x_min, y - self.width / 2.0), - iced::Size::new(x_max - x_min, self.width), - line_color, - ); - }); - } - - fn estimate_bounds(&self) -> InclusionZone { - InclusionZone::Empty - } -} diff --git a/mapf-viz/src/spatial_canvas.rs b/mapf-viz/src/spatial_canvas.rs deleted file mode 100644 index 00daf5d..0000000 --- a/mapf-viz/src/spatial_canvas.rs +++ /dev/null @@ -1,454 +0,0 @@ -/* - * Copyright (C) 2022 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::{toggle::DragToggler, Toggle, Toggler}; -use derivative::Derivative; -use iced::{ - canvas::{ - self, - event::{self, Event}, - Cache, Canvas, Cursor, Frame, Geometry, Path, - }, - keyboard, mouse, Element, Length, Rectangle, -}; -use lyon::math::{Point, Transform, Vector}; - -pub struct SpatialHUD<'a> { - base_frame: &'a mut Frame, - zoom: f32, -} - -impl<'a> SpatialHUD<'a> { - pub fn at<'b>(&'b mut self, position: iced::Point, f: impl FnOnce(&mut Frame)) - where - 'a: 'b, - { - self.base_frame.with_save(|frame| { - frame.translate(position - iced::Point::ORIGIN); - frame.asymmetric_scale(1.0 / self.zoom, -1.0 / self.zoom); - frame.with_save(f); - }) - } -} - -#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] -pub enum SpatialCache { - Refresh, - Unchanged, -} - -#[derive(Debug, Clone, Copy, PartialEq)] -pub enum InclusionZone { - Empty, - Some { - lower: iced::Point, - upper: iced::Point, - }, -} - -pub trait SpatialCanvasProgram: std::fmt::Debug { - fn update( - &mut self, - _event: Event, - _cursor: Cursor, - ) -> (SpatialCache, event::Status, Option) { - // Default: Do nothing - (SpatialCache::Unchanged, event::Status::Ignored, None) - } - - fn draw_in_space( - &self, - _frame: &mut Frame, - _spatial_bounds: Rectangle, - _spatial_cursor: Cursor, - ) { - // Default: Do nothing - } - - fn draw_on_hud<'a, 'b: 'a>( - &self, - _hud: &'a mut SpatialHUD<'b>, - _spatial_bounds: Rectangle, - _spatial_cursor: Cursor, - ) { - // Default: Do nothing - } - - fn estimate_bounds(&self) -> InclusionZone; -} - -impl InclusionZone { - pub fn include(&mut self, p: iced::Point) { - match self { - Self::Empty => { - *self = Self::Some { lower: p, upper: p }; - } - Self::Some { lower, upper } => { - lower.x = lower.x.min(p.x); - lower.y = lower.y.min(p.y); - upper.x = upper.x.max(p.x); - upper.y = upper.y.max(p.y); - } - } - } - - pub fn with(mut self, p: iced::Point) -> Self { - self.include(p); - self - } - - pub fn merge(&mut self, other: Self) { - match self { - Self::Empty => *self = other, - Self::Some { lower, upper } => { - if let Self::Some { - lower: other_lower, - upper: other_upper, - } = other - { - lower.x = lower.x.min(other_lower.x); - lower.y = lower.y.min(other_lower.y); - upper.x = upper.x.max(other_upper.x); - upper.y = upper.y.max(other_upper.y); - } - } - } - } -} - -#[derive(Derivative)] -#[derivative(Debug)] -pub struct SpatialCanvas> { - pub program: Program, - pub cache: Cache, - pub zoom: f32, - #[derivative(Debug = "ignore")] - pub pan_toggler: Option>, - pub scroll_to_zoom: bool, - offset: Vector, - bounds: Option, - drag_start_point: Option, - _msg: std::marker::PhantomData, -} - -impl> SpatialCanvas { - pub fn new(program: Program) -> Self { - Self { - program, - cache: Cache::new(), - zoom: 1.0, - pan_toggler: Some(Box::new(DragToggler::new( - Some(mouse::Button::Middle), - Some((keyboard::Modifiers::CTRL, mouse::Button::Left)), - ))), - scroll_to_zoom: true, - offset: Vector::new(0.0, 0.0), - bounds: None, - drag_start_point: None, - _msg: Default::default(), - } - } - - pub fn fit_to_bounds(&mut self) -> bool { - self.fit_to_zone(self.program.estimate_bounds()) - } - - pub fn camera_bounds(&self) -> InclusionZone { - let Some(bounds) = &self.bounds else { - return InclusionZone::Empty; - }; - let Some(s_inv) = Transform::scale(self.zoom, -self.zoom).inverse() else { - return InclusionZone::Empty; - }; - let bound_center = Point::new(bounds.width / 2.0, bounds.height / 2.0); - let p0 = s_inv.transform_point(bound_center - self.offset) - bound_center / self.zoom; - let p1 = iced::Point::new( - bounds.width / self.zoom + p0.x, - bounds.height / self.zoom + p0.y, - ); - InclusionZone::Empty - .with(iced::Point::new(p0.x, p0.y)) - .with(p1) - } - - pub fn fit_to_zone(&mut self, zone: InclusionZone) -> bool { - let Some(bounds) = &mut self.bounds else { - return false; - }; - - if let InclusionZone::Some { lower, upper } = zone { - let x_zoom = bounds.width / (upper.x - lower.x); - let y_zoom = bounds.height / (upper.y - lower.y); - self.zoom = x_zoom.min(y_zoom); - - let bound_center = Point::new(bounds.width / 2.0, bounds.height / 2.0); - - let space_center = Point::new((lower.x + upper.x) / 2.0, (lower.y + upper.y) / 2.0); - let s = Transform::scale(self.zoom, -self.zoom); - self.offset = bound_center - s.transform_point(space_center); - self.cache.clear(); - } - - true - } - - pub fn view<'a>(&'a mut self) -> Element<'a, Message> - where - Message: 'static, - { - Canvas::new(self) - .width(Length::Fill) - .height(Length::Fill) - .into() - } - - pub fn transform(&self) -> Transform { - Transform::translation(self.offset.x, self.offset.y) - .pre_scale(self.zoom, -self.zoom) - .inverse() - .unwrap() - } -} - -impl<'a, Message, Program: SpatialCanvasProgram> canvas::Program - for SpatialCanvas -{ - fn update( - &mut self, - event: Event, - bounds: Rectangle, - cursor: Cursor, - ) -> (event::Status, Option) { - self.bounds = Some(bounds); - - if let Some(p_raw) = cursor.position_in(&bounds) { - let p = self - .transform() - .transform_point(Point::new(p_raw.x, p_raw.y)); - let spatial_cursor = Cursor::Available(iced::Point::new(p.x, p.y)); - - let (cache, status, message) = self.program.update(event, spatial_cursor); - if cache == SpatialCache::Refresh { - self.cache.clear(); - } - - if status == event::Status::Captured { - return (status, message); - } - - if message.is_some() { - // TODO(@mxgrey): This implementation only allows us to generate - // one message per update. It would be better if the - // canvas::Program::update pub trait function could return a - // command instead of just an Option - return (status, message); - } - - if let Some(pan_toggler) = &mut self.pan_toggler { - match pan_toggler.toggle(event) { - Toggle::On => { - if self.drag_start_point.is_none() { - self.drag_start_point = Some(p); - } - return (event::Status::Captured, None); - } - Toggle::Off => { - self.drag_start_point = None; - return (event::Status::Captured, None); - } - Toggle::NoChange => { - // Do nothing - } - } - } - - if let Event::Mouse(mouse::Event::CursorMoved { .. }) = event { - if let Some(p0) = self.drag_start_point { - let p_raw = Point::new(p_raw.x, p_raw.y); - self.offset = - p_raw - Transform::scale(self.zoom, -self.zoom).transform_point(p0); - - self.cache.clear(); - return (event::Status::Captured, None); - } - } - - if self.scroll_to_zoom { - if let Event::Mouse(mouse::Event::WheelScrolled { delta }) = event { - match delta { - mouse::ScrollDelta::Lines { y, .. } - | mouse::ScrollDelta::Pixels { y, .. } => { - let p0 = p; - self.zoom = self.zoom * (1.0 + y / 10.0); - let p_raw = Point::new(p_raw.x, p_raw.y); - self.offset = - p_raw - Transform::scale(self.zoom, -self.zoom).transform_point(p0); - self.cache.clear(); - } - } - } - } - } else { - // Release the canvas drag if the cursor has left the panel - self.drag_start_point = None; - let spatial_cursor = Cursor::Unavailable; - - let (cache, status, message) = self.program.update(event, spatial_cursor); - if cache == SpatialCache::Refresh { - self.cache.clear(); - } - - if status == event::Status::Captured { - return (status, message); - } - - if let Some(message) = message { - // TODO(@mxgrey): This implementation only allows us to generate - // one message per update. It would be better if the - // canvas::Program::update pub trait function could return a - // command instead of just an Option - return (status, Some(message)); - } - } - - return (event::Status::Ignored, None); - } - - fn draw(&self, bounds: Rectangle, mut cursor: Cursor) -> Vec { - if let Some(p_raw) = cursor.position_in(&bounds) { - let p = self - .transform() - .transform_point(Point::new(p_raw.x, p_raw.y)); - cursor = Cursor::Available(iced::Point::new(p.x, p.y)); - } - - let tf = self.transform(); - let origin = tf.transform_point(Point::origin()); - let corner = tf.transform_point(Point::origin() + Vector::new(bounds.width, bounds.height)); - let spatial_bounds = Rectangle { - x: origin.x, - y: corner.y, - width: (corner.x - origin.x).abs(), - height: (corner.y - origin.y).abs(), - }; - - let background = self.cache.draw(bounds.size(), |frame| { - let background = Path::rectangle(iced::Point::ORIGIN, frame.size()); - frame.fill(&background, iced::Color::from_rgb8(240, 240, 240)); - - frame.with_save(|frame| { - frame.translate(iced::Vector::new(self.offset.x, self.offset.y)); - frame.asymmetric_scale(self.zoom, -self.zoom); - - self.program.draw_in_space(frame, spatial_bounds, cursor); - self.program.draw_on_hud( - &mut SpatialHUD { - base_frame: frame, - zoom: self.zoom, - }, - spatial_bounds, - cursor, - ); - }); - }); - - return vec![background]; - } - - fn mouse_interaction(&self, _: Rectangle, _: Cursor) -> mouse::Interaction { - if let Some(_) = self.drag_start_point { - return mouse::Interaction::Grabbing; - } - - mouse::Interaction::default() - } -} - -#[macro_export] -macro_rules! spatial_layers { - ($name:ident<$message:ident>: $($types:ident),+) => { - #[derive(Debug)] - struct $name { - layers: ($($types<$message>),+) - } - - impl mapf_viz::spatial_canvas::SpatialCanvasProgram<$message> for $name { - - #[allow(non_snake_case, unused_assignments)] - fn update( - &mut self, - event: iced::canvas::event::Event, - cursor: iced::canvas::Cursor, - ) -> (mapf_viz::spatial_canvas::SpatialCache, iced::canvas::event::Status, Option<$message>) { - let mut refresh = mapf_viz::spatial_canvas::SpatialCache::Unchanged; - let ($(ref mut $types,)+) = &mut self.layers; - $( - let (cache, status, message) = $types.update(event, cursor); - if (cache == mapf_viz::spatial_canvas::SpatialCache::Refresh) { - refresh = cache; - } - - if status == iced::canvas::event::Status::Captured { - return (refresh, status, message); - } - - if message.is_some() { - return (refresh, status, message); - } - )+ - - return (refresh, iced::canvas::event::Status::Ignored, None); - } - - #[allow(non_snake_case, unused_assignments)] - fn draw_in_space( - &self, - frame: &mut iced::canvas::Frame, - spatial_bounds: iced::Rectangle, - spatial_cursor: iced::canvas::Cursor, - ) { - let ($(ref $types,)+) = &self.layers; - $( - $types.draw_in_space(frame, spatial_bounds, spatial_cursor); - )+ - } - - #[allow(non_snake_case, unused_assignments)] - fn draw_on_hud<'a, 'b: 'a>( - &self, - hud: &'a mut mapf_viz::spatial_canvas::SpatialHUD<'b>, - spatial_bounds: iced::Rectangle, - spatial_cursor: iced::canvas::Cursor, - ) { - let ($(ref $types,)+) = &self.layers; - $( - $types.draw_on_hud(&mut *hud, spatial_bounds, spatial_cursor); - )+ - } - - #[allow(non_snake_case, unused_assignments)] - fn estimate_bounds(&self) -> mapf_viz::spatial_canvas::InclusionZone { - let mut bounds = mapf_viz::spatial_canvas::InclusionZone::Empty; - let ($(ref $types,)+) = &self.layers; - $( - bounds.merge($types.estimate_bounds()); - )+ - return bounds; - } - } - }; -} diff --git a/mapf-viz/src/toggle.rs b/mapf-viz/src/toggle.rs deleted file mode 100644 index e139ac6..0000000 --- a/mapf-viz/src/toggle.rs +++ /dev/null @@ -1,239 +0,0 @@ -/* - * Copyright (C) 2022 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 iced::{canvas::event::Event, keyboard, mouse}; -use std::collections::HashMap; - -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -pub enum Toggle { - On, - Off, - NoChange, -} - -impl Toggle { - fn change_to(&mut self, value: Toggle) -> Toggle { - if *self == value { - return Toggle::NoChange; - } - - *self = value; - return value; - } -} - -pub trait Toggler { - fn toggle(&mut self, event: Event) -> Toggle; - fn state(&self) -> Toggle; -} - -#[derive(Debug, Clone, PartialEq, Eq)] -pub struct DragToggler { - buttons: HashMap, - active_modifiers: keyboard::Modifiers, - state: Toggle, -} - -impl DragToggler { - pub fn new( - main_button: Option, - alt_button: Option<(keyboard::Modifiers, mouse::Button)>, - ) -> Self { - let mut buttons = HashMap::new(); - if let Some(main_button) = main_button { - buttons.insert(keyboard::Modifiers::empty(), main_button); - } - - if let Some(alt_button) = alt_button { - buttons.insert(alt_button.0, alt_button.1); - } - - Self { - buttons, - active_modifiers: keyboard::Modifiers::empty(), - state: Toggle::Off, - } - } - - fn button_matches(&self, button: mouse::Button) -> bool { - if let Some(expected_button) = self.buttons.get(&self.active_modifiers) { - if *expected_button == button { - return true; - } - } - - return false; - } -} - -impl Default for DragToggler { - fn default() -> Self { - Self::new( - Some(mouse::Button::Middle), - Some((keyboard::Modifiers::CTRL, mouse::Button::Left)), - ) - } -} - -impl Toggler for DragToggler { - fn toggle(&mut self, event: Event) -> Toggle { - match event { - Event::Mouse(event) => { - if let mouse::Event::ButtonPressed(button) = event { - if self.button_matches(button) { - return self.state.change_to(Toggle::On); - } else { - return self.state.change_to(Toggle::Off); - } - } - - if let mouse::Event::ButtonReleased(button) = event { - if self.button_matches(button) { - return self.state.change_to(Toggle::Off); - } - } - } - Event::Keyboard(event) => { - if let keyboard::Event::ModifiersChanged(modifiers) = event { - if modifiers != self.active_modifiers { - self.active_modifiers = modifiers; - return self.state.change_to(Toggle::Off); - } - } - } - } - - return Toggle::NoChange; - } - - fn state(&self) -> Toggle { - return self.state; - } -} - -#[derive(Debug, Clone, PartialEq, Eq)] -pub struct FillToggler { - on: On, - off: Off, - state: Toggle, -} - -impl FillToggler { - pub fn new(on: On, off: Off) -> Self { - Self { - on, - off, - state: Toggle::NoChange, - } - } -} - -impl Toggler for FillToggler { - fn toggle(&mut self, event: Event) -> Toggle { - self.on.toggle(event); - self.off.toggle(event); - - if self.on.state() == self.off.state() { - // If on and off are equal, then we cannot choose between filling or - // erasing, so we will say that neither should be done. - self.state = Toggle::NoChange; - } else if self.on.state() == Toggle::On { - self.state = Toggle::On; - } else if self.off.state() == Toggle::On { - self.state = Toggle::Off; - } - - return self.state; - } - - fn state(&self) -> Toggle { - return self.state; - } -} - -#[derive(Debug, Clone, PartialEq, Eq)] -pub struct KeyToggler { - key: keyboard::KeyCode, - modifiers: Option, - state: Toggle, -} - -impl KeyToggler { - pub fn for_key(key: keyboard::KeyCode) -> Self { - Self { - key, - modifiers: None, - state: Toggle::Off, - } - } - - pub fn for_key_with_modifiers(key: keyboard::KeyCode, modifiers: keyboard::Modifiers) -> Self { - Self { - key, - modifiers: Some(modifiers), - state: Toggle::Off, - } - } - - pub fn key_toggle(&mut self, event: keyboard::Event) -> Toggle { - if let keyboard::Event::KeyPressed { - key_code, - modifiers, - } = event - { - if let Some(expected_modifiers) = self.modifiers { - if expected_modifiers != modifiers { - return self.state.change_to(Toggle::Off); - } - } - - if key_code == self.key { - return self.state.change_to(Toggle::On); - } - } - - if let keyboard::Event::KeyReleased { key_code, .. } = event { - if key_code == self.key { - return self.state.change_to(Toggle::Off); - } - } - - if let Some(expected_modifiers) = self.modifiers { - if let keyboard::Event::ModifiersChanged(modifiers) = event { - if expected_modifiers != modifiers { - return self.state.change_to(Toggle::Off); - } - } - } - - return Toggle::NoChange; - } -} - -impl Toggler for KeyToggler { - fn toggle(&mut self, event: Event) -> Toggle { - if let Event::Keyboard(event) = event { - return self.key_toggle(event); - } - - return Toggle::NoChange; - } - - fn state(&self) -> Toggle { - self.state - } -} diff --git a/mapf-viz/src/visibility_visual.rs b/mapf-viz/src/visibility_visual.rs deleted file mode 100644 index 1e43600..0000000 --- a/mapf-viz/src/visibility_visual.rs +++ /dev/null @@ -1,375 +0,0 @@ -/* - * Copyright (C) 2022 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::{ - spatial_canvas::{self, InclusionZone, SpatialCache, SpatialCanvasProgram}, - toggle::{DragToggler, FillToggler, Toggle, Toggler}, -}; -use derivative::Derivative; -use iced::{ - canvas::{ - event::{self, Event}, - Cursor, Frame, Path, Stroke, - }, - keyboard, mouse, Rectangle, -}; -use mapf::graph::occupancy::{Cell, Grid, Point, SparseGrid, Visibility}; -use std::collections::HashMap; - -#[derive(Derivative)] -#[derivative(Debug)] -pub struct VisibilityVisual { - #[derivative(Debug = "ignore")] - visibility: Visibility, - - // NOTE: After changing one of the public fields below, you must clear the - // cache of the SpatialCanvas that this program belongs to before the change - // will be rendered. - pub occupancy_color: iced::Color, - pub default_visibility_color: iced::Color, - pub special_visibility_color: HashMap, - pub show_visibility_graph: bool, - pub show_details: bool, - - #[derivative(Debug = "ignore")] - pub cell_toggler: Option>, - - #[derivative(Debug = "ignore")] - pub corner_select_toggler: Option>, - - #[derivative(Debug = "ignore")] - pub on_corner_select: Option Message>>, - - #[derivative(Debug = "ignore")] - pub on_occupancy_change: Option Message>>, - - _ignore: std::marker::PhantomData, -} - -impl VisibilityVisual { - pub fn new( - grid: G, - robot_radius: f32, - on_corner_select: Option Message>>, - on_occupancy_change: Option Message>>, - ) -> Self { - Self { - visibility: Visibility::new(grid, robot_radius as f64), - occupancy_color: iced::Color::from_rgb8(0x40, 0x44, 0x4B), - default_visibility_color: iced::Color::from_rgb8(230, 166, 33), - special_visibility_color: Default::default(), - show_visibility_graph: true, - show_details: false, - cell_toggler: Some(Box::new(FillToggler::new( - DragToggler::new( - None, - Some((keyboard::Modifiers::SHIFT, mouse::Button::Left)), - ), - DragToggler::new( - None, - Some((keyboard::Modifiers::SHIFT, mouse::Button::Right)), - ), - ))), - corner_select_toggler: Some(Box::new(FillToggler::new( - DragToggler::new(None, Some((keyboard::Modifiers::ALT, mouse::Button::Left))), - DragToggler::new(None, Some((keyboard::Modifiers::ALT, mouse::Button::Right))), - ))), - on_corner_select, - on_occupancy_change, - _ignore: Default::default(), - } - } - - pub fn showing_visibility_graph(mut self, showing: bool) -> Self { - self.show_visibility_graph = showing; - self - } - - pub fn set_robot_radius(&mut self, radius: f32) { - self.visibility.change_agent_radius(radius as f64); - } - - pub fn visibility(&self) -> &Visibility { - &self.visibility - } - - pub fn visibility_mut(&mut self) -> &mut Visibility { - &mut self.visibility - } - - pub fn grid(&self) -> &G { - &self.visibility.grid() - } - - pub fn set_grid(&mut self, grid: G) { - self.visibility = Visibility::new(grid, self.visibility.agent_radius()); - } - - /// The cache of the spatial canvas needs to be cleared after changing this - /// value or else the change will not show. - pub fn show_details(&mut self, show: bool) { - self.show_details = show; - } - - fn toggle(&mut self, p: iced::Point) -> bool { - if let Some(cell_toggler) = &self.cell_toggler { - let cell = Cell::from_point( - [p.x as f64, p.y as f64].into(), - self.visibility.grid().cell_size(), - ); - match cell_toggler.state() { - Toggle::On => { - return self.visibility.change_cells(&[(cell, true)].into()); - } - Toggle::Off => { - return self.visibility.change_cells(&[(cell, false)].into()); - } - Toggle::NoChange => { - return false; - } - } - } - - return false; - } - - fn find_closest(&self, p: iced::Point) -> Option { - let mut closest: Option<(Cell, f64)> = None; - let r = self.visibility.agent_radius(); - let r_squared = r * r; - let p = Point::new(p.x as f64, p.y as f64); - - for (cell, _) in self.visibility.iter_points() { - let p_cell = cell.center_point(self.grid().cell_size()); - let dist_squared = (p_cell - p).norm_squared(); - if dist_squared <= r_squared { - if let Some((_, old_dist_squared)) = closest { - if dist_squared < old_dist_squared { - closest = Some((*cell, dist_squared)); - } - } else { - closest = Some((*cell, dist_squared)); - } - } - } - - return closest.map(|x| x.0); - } -} - -impl SpatialCanvasProgram for VisibilityVisual { - fn update( - &mut self, - event: Event, - cursor: Cursor, - ) -> (SpatialCache, event::Status, Option) { - if let Some(cell_toggler) = &mut self.cell_toggler { - cell_toggler.toggle(event); - } - - if let Some(p) = cursor.position() { - if self.toggle(p) { - return ( - SpatialCache::Refresh, - event::Status::Captured, - self.on_occupancy_change.as_ref().map(|x| x()), - ); - } - - if let Some(corner_select_toggler) = &mut self.corner_select_toggler { - if let Some(on_corner_select) = &self.on_corner_select { - corner_select_toggler.toggle(event); - match corner_select_toggler.state() { - Toggle::On => { - if let Some(cell) = self.find_closest(p) { - return ( - SpatialCache::Unchanged, - event::Status::Captured, - Some(on_corner_select(cell, true)), - ); - } - } - Toggle::Off => { - if let Some(cell) = self.find_closest(p) { - return ( - SpatialCache::Unchanged, - event::Status::Captured, - Some(on_corner_select(cell, false)), - ); - } - } - Toggle::NoChange => { - // Do nothing - } - } - } - } - } - - (SpatialCache::Unchanged, event::Status::Ignored, None) - } - - fn draw_in_space(&self, frame: &mut Frame, _: Rectangle, _: Cursor) { - let cell_size = self.visibility.grid().cell_size(); - let robot_radius = self.visibility.agent_radius() as f32; - for cell in self.visibility.grid().occupied_cells() { - let p = cell.bottom_left_point(cell_size); - frame.fill_rectangle( - [p.x as f32, p.y as f32].into(), - iced::Size::new(cell_size as f32, cell_size as f32), - self.occupancy_color, - ); - } - - if self.show_visibility_graph { - for (cell, _) in self.visibility.iter_points() { - let p = cell.center_point(cell_size); - let color = self - .special_visibility_color - .get(cell) - .unwrap_or(&self.default_visibility_color); - - frame.fill( - &Path::circle([p.x as f32, p.y as f32].into(), robot_radius), - *color, - ); - } - - for (cell_i, cell_j) in self.visibility.iter_edges() { - let p_i = cell_i.center_point(cell_size); - let p_j = cell_j.center_point(cell_size); - frame.stroke( - &Path::line( - [p_i.x as f32, p_i.y as f32].into(), - [p_j.x as f32, p_j.y as f32].into(), - ), - Stroke { - color: self.default_visibility_color, - width: 5_f32, - ..Default::default() - }, - ); - - // if self.show_details { - // let p0 = Point::new(p0.x, p0.y); - // let p1 = Point::new(p1.x, p1.y); - // let dist = (p1 - p0).length(); - // if dist < 1e-8 { - // continue; - // } - // let v = (p1 - p0)/dist; - // let n = Vector::new(-v.y, v.x); - - // let points = [ - // p0 + n*self.robot_radius, - // p0 - n*self.robot_radius, - // p1 + n*self.robot_radius, - // p1 - n*self.robot_radius, - // ]; - - // let lines = [ - // (points[0], points[1]), - // (points[0], points[2]), - // (points[1], points[3]), - // (points[2], points[3]), - // ]; - - // let cell_x_min = ( - // points.iter().min_by( - // |p_l, p_r| p_l.x.partial_cmp(&p_r.x).unwrap() - // ).unwrap().x / self.cell_size - // ).floor() as i64; - - // let cell_x_max = ( - // points.iter().max_by( - // |p_l, p_r| p_l.x.partial_cmp(&p_r.x).unwrap() - // ).unwrap().x / self.cell_size - // ).ceil() as i64; - - // for line in &lines { - // frame.stroke( - // &Path::line( - // iced::Point::new(line.0.x, line.0.y), - // iced::Point::new(line.1.x, line.1.y), - // ), - // Stroke{ - // color: iced::Color::from_rgb(0.0, 1.0, 0.0), - // width: 8_f32, - // ..Default::default() - // } - // ); - // } - - // for (cell_x, _) in self.occupancy_map.range(cell_x_min .. cell_x_max) { - // let x_low = *cell_x as f32 * self.cell_size; - // let x_high = (cell_x + 1) as f32 * self.cell_size; - // for x in [x_low, x_high] { - // for line in &lines { - // for y in line.vertical_intersect(x) { - // frame.fill( - // &Path::circle(iced::Point::new(x, y), self.robot_radius/10_f32), - // iced::Color::from_rgb(0.0, 0.0, 1.0) - // ); - // } - // } - // } - // } - // } - } - } - } - - fn draw_on_hud<'a, 'b: 'a>( - &self, - hud: &'a mut spatial_canvas::SpatialHUD<'b>, - bound: Rectangle, - _: Cursor, - ) { - if !self.show_details { - return; - } - - for (cell, _) in self.visibility.iter_points() { - let p = cell.center_point(self.visibility.grid().cell_size()); - let r = self.visibility.agent_radius() as f32 / 2_f32.sqrt(); - let delta = iced::Vector::new(r, -r); - let p = iced::Point::new(p.x as f32, p.y as f32) + delta; - - if bound.contains(p) { - hud.at(p, |frame| { - frame.fill_text(format!("({}, {})", cell.x, cell.y).to_string()); - }); - } - } - } - - fn estimate_bounds(&self) -> InclusionZone { - let mut zone = InclusionZone::Empty; - let r = self.visibility.agent_radius() as f32; - for (cell, _) in self.visibility.iter_points() { - let p = cell.center_point(self.visibility.grid().cell_size()); - let p: iced::Point = [p.x as f32, p.y as f32].into(); - let d = iced::Vector::new(r, r); - zone.include(p + d); - zone.include(p - d); - } - - zone - } -} - -pub type SparseGridVisibilityVisual = VisibilityVisual; diff --git a/mapf/src/motion/se2/differential_drive_line_follow.rs b/mapf/src/motion/se2/differential_drive_line_follow.rs index 5438e16..fe88989 100644 --- a/mapf/src/motion/se2/differential_drive_line_follow.rs +++ b/mapf/src/motion/se2/differential_drive_line_follow.rs @@ -700,7 +700,7 @@ mod tests { #[test] fn test_extrapolation() { let t0 = time_point::TimePoint::from_secs_f64(3.0); - let wp0 = WaypointSE2::new(t0, 1.0, -3.0, -40f64.to_radians()); + let wp0 = WaypointSE2::new(t0, 1.0, -3.0, -(40f64).to_radians()); let movement = DifferentialDriveLineFollow::new(2.0, 3.0) .expect("Failed to make DifferentialLineFollow"); let p_target = Position::new(Vector::new(1.0, 3.0), 60f64.to_radians()); diff --git a/mapf/src/motion/se2/timed_position.rs b/mapf/src/motion/se2/timed_position.rs index 9042a3a..81ee1ab 100644 --- a/mapf/src/motion/se2/timed_position.rs +++ b/mapf/src/motion/se2/timed_position.rs @@ -239,21 +239,25 @@ mod tests { let t0 = time_point::TimePoint::new(0); let t1 = t0 + time_point::Duration::from_secs_f64(2.0); let wp0 = WaypointSE2::new(t0, 1.0, 5.0, 10f64.to_radians()); - let wp1 = WaypointSE2::new(t1, 1.0, 10.0, -20f64.to_radians()); + let wp1 = WaypointSE2::new(t1, 1.0, 10.0, -(20f64).to_radians()); let motion = wp0.interpolate(&wp1); let t = (t1 - t0) / 2f64 + t0; let p = motion.compute_position(&t).ok().unwrap(); assert_relative_eq!(p.translation.vector[0], 1f64, max_relative = 0.001); assert_relative_eq!(p.translation.vector[1], 7.5f64, max_relative = 0.001); - assert_relative_eq!(p.rotation.angle(), -5f64.to_radians(), max_relative = 0.001); + assert_relative_eq!( + p.rotation.angle(), + -(5f64).to_radians(), + max_relative = 0.001 + ); let v = motion.compute_velocity(&t).ok().unwrap(); assert_relative_eq!(v.translational[0], 0f64, max_relative = 0.001); assert_relative_eq!(v.translational[1], 5.0 / 2.0, max_relative = 0.001); assert_relative_eq!( v.rotational, - -30f64.to_radians() / 2.0, + -(30f64).to_radians() / 2.0, max_relative = 0.001 ); } 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, diff --git a/mapf/src/premade/search_se2.rs b/mapf/src/premade/search_se2.rs index 913f95f..1a2f836 100644 --- a/mapf/src/premade/search_se2.rs +++ b/mapf/src/premade/search_se2.rs @@ -200,7 +200,7 @@ mod tests { .plan( (Cell::new(-3, -3), 20_f64.to_radians()), GoalSE2::new(Cell::new(10, 10)) - .with_orientation(Some(Orientation::from_angle(-60_f64.to_radians()))), + .with_orientation(Some(Orientation::from_angle((-60_f64).to_radians()))), ) .unwrap() .solve()