From e910cfac491b25c9ab270c4c63578f3f503e8fea Mon Sep 17 00:00:00 2001 From: Grey Date: Thu, 20 Apr 2023 22:40:13 +0800 Subject: [PATCH] Continuous-time Conflict Based Search prototype (#15) Signed-off-by: Michael X. Grey --- .github/workflows/ci_linux.yaml | 26 +- .github/workflows/ci_windows.yaml | 45 + README.md | 61 + mapf-viz/Cargo.toml | 3 +- mapf-viz/examples/grid.rs | 1966 +++++++++++++++++ mapf-viz/examples/grid_planner.rs | 1293 ----------- mapf-viz/examples/occupancy.rs | 319 --- 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/lib.rs | 7 +- mapf-viz/src/spatial_canvas.rs | 33 +- .../{occupancy.rs => visibility_visual.rs} | 108 +- mapf/Cargo.toml | 3 + mapf/src/algorithm/a_star.rs | 22 +- mapf/src/algorithm/dijkstra/backward.rs | 17 +- mapf/src/algorithm/dijkstra/forward.rs | 7 +- mapf/src/algorithm/mod.rs | 11 +- mapf/src/algorithm/path.rs | 226 +- mapf/src/algorithm/tree.rs | 13 +- mapf/src/domain/configurable.rs | 11 +- mapf/src/domain/conflict.rs | 7 +- mapf/src/domain/extrapolator.rs | 37 +- mapf/src/domain/reversible.rs | 12 +- .../graph/occupancy/accessibility_graph.rs | 427 ++++ mapf/src/graph/occupancy/mod.rs | 87 +- mapf/src/graph/occupancy/sparse_grid.rs | 106 +- mapf/src/graph/occupancy/util.rs | 6 +- mapf/src/graph/occupancy/visibility_graph.rs | 34 +- mapf/src/graph/shared_graph.rs | 15 + mapf/src/lib.rs | 2 + mapf/src/motion/conflict.rs | 149 +- mapf/src/motion/environment.rs | 151 +- mapf/src/motion/mod.rs | 4 +- mapf/src/motion/r2/direct_travel.rs | 7 +- mapf/src/motion/r2/line_follow.rs | 128 +- mapf/src/motion/r2/space.rs | 10 +- mapf/src/motion/r2/timed_position.rs | 19 +- mapf/src/motion/reach.rs | 47 - .../safe_interval.rs} | 309 +-- .../se2/differential_drive_line_follow.rs | 256 ++- mapf/src/motion/se2/mod.rs | 11 +- mapf/src/motion/se2/quickest_path.rs | 199 +- mapf/src/motion/se2/space.rs | 56 +- mapf/src/motion/se2/timed_position.rs | 21 +- mapf/src/motion/timed.rs | 24 +- mapf/src/motion/trajectory.rs | 136 +- mapf/src/motion/travel_effort_cost.rs | 2 +- mapf/src/motion/travel_time_cost.rs | 2 +- mapf/src/motion/waypoint.rs | 6 + mapf/src/negotiation/mod.rs | 1198 ++++++++++ mapf/src/negotiation/scenario.rs | 136 ++ mapf/src/planner/halt.rs | 18 +- mapf/src/planner/mod.rs | 9 +- mapf/src/premade/mod.rs | 3 - mapf/src/premade/search_se2.rs | 17 +- mapf/src/premade/sipp_se2.rs | 398 +++- mapf/src/templates/conflict_avoidance.rs | 31 +- mapf/src/templates/graph_motion.rs | 12 +- .../src/templates/incremental_graph_motion.rs | 31 +- mapf/src/templates/lazy_graph_motion.rs | 11 +- 62 files changed, 7001 insertions(+), 2559 deletions(-) create mode 100644 .github/workflows/ci_windows.yaml create mode 100644 mapf-viz/examples/grid.rs delete mode 100644 mapf-viz/examples/grid_planner.rs delete mode 100644 mapf-viz/examples/occupancy.rs create mode 100644 mapf-viz/scenarios/T-crossing.yaml create mode 100644 mapf-viz/scenarios/sizes.yaml create mode 100644 mapf-viz/scenarios/warehouse.yaml create mode 100644 mapf-viz/src/accessibility_visual.rs rename mapf-viz/src/{occupancy.rs => visibility_visual.rs} (79%) create mode 100644 mapf/src/graph/occupancy/accessibility_graph.rs delete mode 100644 mapf/src/motion/reach.rs rename mapf/src/{premade/safe_interval_motion.rs => motion/safe_interval.rs} (50%) create mode 100644 mapf/src/negotiation/mod.rs create mode 100644 mapf/src/negotiation/scenario.rs diff --git a/.github/workflows/ci_linux.yaml b/.github/workflows/ci_linux.yaml index 0e0fbd8..0eced17 100644 --- a/.github/workflows/ci_linux.yaml +++ b/.github/workflows/ci_linux.yaml @@ -3,9 +3,14 @@ name: ci_linux on: schedule: # * is a special character in YAML so you have to quote this string - - cron: '30 2 * * *' + # The final 1 indicates that we want to run this test on Tuesdays, making + # this a weekly test. + - cron: '30 2 * * 1' + workflow_dispatch: pull_request: - + push: + branches: [main] + env: CARGO_TERM_COLOR: always @@ -15,16 +20,13 @@ jobs: runs-on: ubuntu-latest steps: - - name: Instally nightly + - name: Install nightly run: | - rustup toolchain install nightly rustup default nightly - uses: actions/checkout@v3 - - name: Build mapf - run: cd mapf && cargo build - - name: Run tests mapf - run: cd mapf && cargo test - - name: Build mapf-viz - run: cd mapf-viz && cargo build - - name: Run tests mapf-viz - run: cd mapf-viz && cargo test + - name: Build workspace + run: | + cargo build --workspace --tests + - name: Run tests + run: | + cargo test --tests diff --git a/.github/workflows/ci_windows.yaml b/.github/workflows/ci_windows.yaml new file mode 100644 index 0000000..296a63d --- /dev/null +++ b/.github/workflows/ci_windows.yaml @@ -0,0 +1,45 @@ +name: ci_windows +on: + schedule: + # * is a special character in YAML so you have to quote this string + # The final 1 indicates that we want to run this test on Tuesdays, making + # this a weekly test. + - cron: '30 2 * * 1' + workflow_dispatch: + pull_request: + push: + branches: [main] + +env: + CARGO_TERM_COLOR: always + +jobs: + build: + runs-on: windows-latest + + steps: + - name: checkout + uses: actions/checkout@v3 + + - name: Install Rustup using win.rustup.rs + run: | + # Disable the download progress bar which can cause perf issues + $ProgressPreference = "SilentlyContinue" + Invoke-WebRequest https://win.rustup.rs/ -OutFile rustup-init.exe + .\rustup-init.exe -y --default-host=x86_64-pc-windows-msvc --default-toolchain=none + del rustup-init.exe + rustup default nightly + rustup target add x86_64-pc-windows-msvc + shell: powershell + + - name: build + run: | + rustc -Vv + cargo -V + cargo build --workspace --tests + shell: cmd + + - name: test + run: | + cargo test --tests + shell: cmd diff --git a/README.md b/README.md index 76a6c44..f225866 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,7 @@ +[![style](https://github.com/open-rmf/mapf/actions/workflows/style.yaml/badge.svg)](https://github.com/open-rmf/mapf/actions/workflows/style.yaml) +[![ci_linux](https://github.com/open-rmf/mapf/actions/workflows/ci_linux.yaml/badge.svg)](https://github.com/open-rmf/mapf/actions/workflows/ci_linux.yaml) +[![ci_windows](https://github.com/open-rmf/mapf/actions/workflows/ci_windows.yaml/badge.svg)](https://github.com/open-rmf/mapf/actions/workflows/ci_windows.yaml) + # multi-agent (path finding) planning framework Mapf is a (currently experimental) Rust library for multi-agent planning, with @@ -10,3 +14,60 @@ This is being developed as part of the [Open-RMF](https://github.com/open-rmf) project which provides an open source framework for enabling interoperability between heterogeneous fleets of mobile robots, including cooperation across different platforms and vendors. + +# Helpful Links + +* [Rust Book](https://doc.rust-lang.org/stable/book/) + +# Install dependencies + +## Ubuntu / MacOS / Linux / Unix + +We tend to always target the latest versions of Rust, so we recommend using the `rustup` tool: https://www.rust-lang.org/tools/install + +```bash +$ curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh +``` + +If you already have `rustup` then you can use this to bring your installation up to date: +```bash +$ rustup update +``` + +## Windows + +Follow instructions for installing rustup-init.exe [here](https://forge.rust-lang.org/infra/other-installation-methods.html#other-ways-to-install-rustup). + +# Use nightly + +We are currently using a few unstable features from the nightly toolchain of Rust +because they allow `mapf` to have extreme customizability without any loss to +performance. The easiest way to use the nightly version is to make it your default +toolchain with this command: + +```bash +$ rustup default nightly +``` + +# Run an example + +From the root directory: + +```bash +$ cargo run --release --example grid +``` + +This example will allow you to experiment with multiple agents cooperatively +planning from their starts to their goals in a grid environment. Use left/right +click to set the start/goal cells of each agent. Use shift+left/right click to +add/remove occupancy from cells. The velocities and sizes of each agent can be +customized independently, and you can save/load scenarios into yaml files. + +Some premade scenarios can be found in the `mapf-viz/scenarios` folder. Load a +scenario on startup by passing the scenario name as an executable argument, e.g.: + +```bash +$ cargo run --release --example grid -- mapf-viz/scenarios/sizes.yaml +``` + +Note that the scenario filename to load at startup must come after a `--` with a space on each side. diff --git a/mapf-viz/Cargo.toml b/mapf-viz/Cargo.toml index 6bd7fa1..493d555 100644 --- a/mapf-viz/Cargo.toml +++ b/mapf-viz/Cargo.toml @@ -11,7 +11,7 @@ iced = { git = "https://github.com/mxgrey/iced", branch = "asymmetric_scale", fe iced_aw = { git = "https://github.com/iced-rs/iced_aw", branch = "main" } iced_native = { git = "https://github.com/mxgrey/iced", branch = "asymmetric_scale" } serde = { version="1.0", features = ["derive"] } -serde_json = "1.0" +serde_yaml = "*" async-std = "1.0" directories-next = "2.0" native-dialog = "*" @@ -20,3 +20,4 @@ lyon = "*" nalgebra = "*" arrayvec = "*" time-point = "*" +clap = { version = "4.2.1", features = ["derive"] } diff --git a/mapf-viz/examples/grid.rs b/mapf-viz/examples/grid.rs new file mode 100644 index 0000000..cced2c6 --- /dev/null +++ b/mapf-viz/examples/grid.rs @@ -0,0 +1,1966 @@ +/* + * 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. + * +*/ +#![feature(array_windows, binary_heap_into_iter_sorted)] + +use time_point::{TimePoint, Duration}; +use iced::{ + Application, Alignment, Element, Length, Command, Column, Row, Text, Container, Radio, + slider::{self, Slider}, + scrollable::{self, Scrollable}, + text_input::{self, TextInput}, + button::{self, Button}, + canvas::{self, Event, Path, Stroke}, + executor, keyboard, mouse, + pick_list::{self, PickList}, +}; +use iced_native; +use mapf::{ + graph::{ + SharedGraph, + occupancy::{SparseGrid, Cell, Grid, AccessibilityGraph, Accessibility}, + }, + motion::{ + Trajectory, Motion, CcbsEnvironment, DynamicEnvironment, + CircularProfile, DynamicCircularObstacle, TravelEffortCost, + se2::{ + Point, LinearTrajectorySE2, Vector, WaypointSE2, GoalSE2, + DifferentialDriveLineFollow, + }, + }, + templates::InformedSearch, + planner::{Planner, Search}, + premade::SippSE2, + algorithm::{AStarConnect, SearchStatus, tree::NodeContainer, QueueLength}, + planner::halt::QueueLengthLimit, + negotiation::*, +}; +use mapf_viz::{ + SparseGridAccessibilityVisual, InfiniteGrid, + spatial_canvas::{SpatialCanvas, SpatialCanvasProgram, SpatialCache, InclusionZone}, + toggle::{Toggle, Toggler, KeyToggler}, +}; +use mapf_viz::spatial_layers; +use std::{ + collections::{HashMap, BTreeMap}, + sync::Arc, +}; +use native_dialog::FileDialog; +use clap::Parser; + +type SparseAccessibility = Accessibility; + +const ASCII_UPPER: [char; 26] = [ + 'A', 'B', 'C', 'D', 'E', + 'F', 'G', 'H', 'I', 'J', + 'K', 'L', 'M', 'N', 'O', + 'P', 'Q', 'R', 'S', 'T', + 'U', 'V', 'W', 'X', 'Y', + 'Z', +]; + +fn generate_robot_name(mut index: usize) -> String { + let mut chars = Vec::new(); + let mut offset = 0; + while index >= 26 { + chars.push(index % 26); + index = index / 26; + offset = 1; + } + chars.push(index % 26 - offset); + chars.reverse(); + String::from_iter(chars.into_iter().map(|i| ASCII_UPPER[i])) +} + +const MIN_AGENT_RADIUS: f64 = 0.05; +const MAX_AGENT_RADIUS: f64 = 5.0; +const MIN_AGENT_SPEED: f64 = 0.01; +const MAX_AGENT_SPEED: f64 = 10.0; +const MIN_AGENT_SPIN: f64 = 1.0 * std::f64::consts::PI / 180.0; +const MAX_AGENT_SPIN: f64 = 360.0 * std::f64::consts::PI / 180.0; +const MIN_AGENT_YAW: f64 = -std::f64::consts::PI; +const MAX_AGENT_YAW: f64 = std::f64::consts::PI; + +fn triangular_for( + mut outer_iter: impl Iterator + Clone, + mut f: impl FnMut(&Item, Item), +) { + while let Some(outer_value) = outer_iter.next() { + let mut inner_iter = outer_iter.clone(); + while let Some(inner_value) = inner_iter.next() { + f(&outer_value, inner_value); + } + } +} + +fn serialize_grid(grid: &SparseGrid) -> HashMap> { + let mut ser: HashMap> = HashMap::new(); + for cell in grid.occupied_cells() { + ser.entry(cell.y).or_default().push(cell.x); + } + + for (_, column) in &mut ser { + column.sort_unstable(); + } + + ser +} + +pub(crate) struct Minimum std::cmp::Ordering> { + value: Option, + f: F, +} + +impl std::cmp::Ordering> Minimum { + pub(crate) fn new(f: F) -> Self { + Self{value: None, f} + } + + pub(crate) fn consider(&mut self, other: &T) -> bool { + if let Some(value) = &self.value { + if std::cmp::Ordering::Less == (self.f)(other, value) { + self.value = Some(other.clone()); + return true; + } + } else { + self.value = Some(other.clone()); + return true; + } + + return false; + } + + pub(crate) fn result(self) -> Option { + self.value + } + + pub(crate) fn has_value(&self) -> bool { + self.value.is_some() + } +} + + +fn draw_agent( + frame: &mut canvas::Frame, + point: Point, + angle: Option, + agent_radius: f32, + color: iced::Color, +) { + frame.with_save( + |frame| { + frame.translate([point.x as f32, point.y as f32].into()); + frame.fill(&Path::circle([0, 0].into(), agent_radius), color); + + if let Some(angle) = angle { + frame.rotate(angle as f32); + let r = 0.8 * agent_radius; + let angle_to_point = |angle: f32| { + iced::Point::new(r * f32::cos(angle), r * f32::sin(angle)) + }; + + let points: [iced::Point; 3] = [ + angle_to_point(-150_f32.to_radians()), + angle_to_point(0_f32.to_radians()), + angle_to_point(150_f32.to_radians()), + ]; + + frame.fill( + &Path::new( + |builder| { + builder.move_to(points[0]); + builder.line_to(points[1]); + builder.line_to(points[2]); + builder.line_to(points[0]); + } + ), + iced::Color::from_rgb(1.0, 1.0, 1.0) + ); + } + } + ); +} + +fn draw_trajectory( + frame: &mut canvas::Frame, + trajectory: &LinearTrajectorySE2, + color: iced::Color, + width: Option, +) { + for [wp0, wp1] in trajectory.array_windows() { + let p0 = wp0.position.translation; + let p1 = wp1.position.translation; + frame.stroke( + &Path::line( + [p0.x as f32, p0.y as f32].into(), + [p1.x as f32, p1.y as f32].into(), + ), + Stroke{ + color, + width: width.unwrap_or(3_f32), + ..Default::default() + } + ); + } +} + +#[derive(Debug, Clone)] +struct EndpointSelector { + pub agents: BTreeMap, + pub cell_size: f64, + pub invalid_color: iced::Color, + pub selected_color: iced::Color, + pub start_color: iced::Color, + pub goal_color: iced::Color, + pub show_details: bool, + pub selected_agent: Option, + shift: KeyToggler, + _ignore: std::marker::PhantomData, +} + +#[derive(Debug, Clone)] +struct AgentContext { + agent: Agent, + start_open: bool, + start_available: bool, + goal_open: bool, + goal_available: bool, + start_sees_goal: bool, +} + +impl AgentContext { + fn new(agent: Agent) -> Self { + Self { + agent, + start_open: false, + start_available: false, + goal_open: false, + goal_available: false, + start_sees_goal: false, + } + } + + fn endpoints_valid(&self) -> bool { + self.start_valid() && self.goal_valid() + } + + fn start_valid(&self) -> bool { + self.start_open && self.start_available + } + + fn goal_valid(&self) -> bool { + self.goal_open && self.goal_available + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +enum Endpoint { + Start, + Goal, +} + +impl Endpoint { + fn of(&self, agent: &Agent) -> Cell { + match self { + Endpoint::Start => agent.start.into(), + Endpoint::Goal => agent.goal.into(), + } + } + + fn set_availability(&self, ctx: &mut AgentContext, value: bool) { + match self { + Endpoint::Start => ctx.start_available = value, + Endpoint::Goal => ctx.goal_available = value, + } + } + + fn set_accessibility(&self, ctx: &mut AgentContext, value: bool) { + match self { + Endpoint::Start => ctx.start_open = value, + Endpoint::Goal => ctx.goal_open = value, + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +enum EndpointSelection { + Cell(Endpoint), +} + +impl EndpointSelector { + + fn new( + agents: BTreeMap, + cell_size: f64, + invalid_color: iced::Color, + selected_color: iced::Color, + start_color: iced::Color, + goal_color: iced::Color, + ) -> Self { + let selected_agent = agents.iter().next().map(|r| r.0.clone()); + Self{ + agents: agents.into_iter().map(|(name, a)| (name, AgentContext::new(a))).collect(), + selected_agent, + cell_size, + invalid_color, + selected_color, + start_color, + goal_color, + show_details: false, + shift: KeyToggler::for_key(keyboard::KeyCode::LShift), + _ignore: Default::default(), + } + } + + fn selected_agent(&self) -> Option<&AgentContext> { + if let Some(selected_agent) = &self.selected_agent { + self.agents.get(selected_agent) + } else { + None + } + } + + fn selected_agent_mut(&mut self) -> Option<&mut AgentContext> { + if let Some(selected_agent) = &self.selected_agent { + self.agents.get_mut(selected_agent) + } else { + None + } + } + + fn update_endpoint_conflicts( + &mut self, endpoint: Endpoint, + accessibility: &SparseAccessibility, + ) { + let Some(selected) = &self.selected_agent else { return }; + let cell: Cell = if let Some(ctx) = self.agents.get(selected) { + endpoint.of(&ctx.agent) + } else { + return + }; + + let cs = accessibility.grid().cell_size(); + let p = cell.center_point(cs); + + // Start by assuming all are valid + for ctx in self.agents.values_mut() { + endpoint.set_availability(ctx, true); + + endpoint.set_accessibility( + ctx, + accessibility.grid().is_circle_occupied(p, ctx.agent.radius).is_none(), + ); + } + + // Test O(N^2) for invalidity. We could make this way more efficient. + let mut invalidated = Vec::new(); + triangular_for(self.agents.iter(), |(name_i, ctx_i), (name_j, ctx_j)| { + let p_i = endpoint.of(&ctx_i.agent).center_point(cs); + let p_j = endpoint.of(&ctx_j.agent).center_point(cs); + let dist = (p_i - p_j).norm(); + let min_dist = ctx_i.agent.radius + ctx_j.agent.radius; + if dist < min_dist { + invalidated.push((*name_i).clone()); + invalidated.push(name_j.clone()); + } + }); + + for name in invalidated { + let Some(ctx) = self.agents.get_mut(&name) else { continue }; + endpoint.set_availability(ctx, false); + } + } + + fn endpoint_cell(&self, choice: Endpoint) -> Option<&[i64; 2]> { + if let Some(ctx) = self.selected_agent() { + match choice { + Endpoint::Start => Some(&ctx.agent.start), + Endpoint::Goal => Some(&ctx.agent.goal), + } + } else { + None + } + } + + fn endpoint_cell_mut(&mut self, choice: Endpoint) -> Option<&mut [i64; 2]> { + if let Some(ctx) = self.selected_agent_mut() { + match choice { + Endpoint::Start => Some(&mut ctx.agent.start), + Endpoint::Goal => Some(&mut ctx.agent.goal), + } + } else { + None + } + } + + fn set_endpoint_valid(&mut self, choice: Endpoint, valid: bool) { + if let Some(ctx) = self.selected_agent_mut() { + match choice { + Endpoint::Start => { ctx.start_open = valid; }, + Endpoint::Goal => { ctx.goal_open = valid; }, + } + } + } + + fn set_agent_radius(&mut self, radius: f64) -> bool { + match self.selected_agent_mut() { + Some(ctx) => { + ctx.agent.radius = radius; + true + } + None => false, + } + } +} + +impl SpatialCanvasProgram for EndpointSelector { + fn update( + &mut self, + event: iced::canvas::Event, + cursor: iced::canvas::Cursor + ) -> (SpatialCache, iced::canvas::event::Status, Option) { + if let Some(p) = cursor.position() { + match event { + Event::Mouse(event) => { + if self.shift.state() == Toggle::Off { + let endpoint = match event { + mouse::Event::ButtonPressed(mouse::Button::Left) => Endpoint::Start, + mouse::Event::ButtonPressed(mouse::Button::Right) => Endpoint::Goal, + _ => return (SpatialCache::Unchanged, canvas::event::Status::Ignored, None), + }; + + let cell_size = self.cell_size; + if let Some(cell) = self.endpoint_cell_mut(endpoint) { + *cell = Cell::from_point([p.x as f64, p.y as f64].into(), cell_size).into(); + } + + return ( + SpatialCache::Refresh, + canvas::event::Status::Captured, + Some(Message::EndpointSelected( + EndpointSelection::Cell(endpoint) + )) + ); + } + }, + Event::Keyboard(event) => { + self.shift.key_toggle(event); + } + } + } + + return (SpatialCache::Unchanged, canvas::event::Status::Ignored, None); + } + + fn draw_in_space(&self, frame: &mut canvas::Frame, _spatial_bounds: iced::Rectangle, _spatial_cursor: canvas::Cursor) { + for (name, ctx) in &self.agents { + if self.show_details { + if ctx.start_sees_goal { + let cell_s: Cell = ctx.agent.start.into(); + let cell_g: Cell = ctx.agent.goal.into(); + let p_start = cell_s.center_point(self.cell_size); + let p_goal = cell_g.center_point(self.cell_size); + frame.stroke( + &Path::line( + [p_start.x as f32, p_start.y as f32].into(), + [p_goal.x as f32, p_goal.y as f32].into() + ), + Stroke{ + color: self.selected_color, + width: 5_f32, + ..Default::default() + } + ); + } + } + + for (cell, angle, color, valid) in [ + (ctx.agent.start, Some(ctx.agent.yaw), self.start_color, ctx.start_valid()), + (ctx.agent.goal, None, self.goal_color, ctx.goal_valid()) + ] { + let radius = ctx.agent.radius as f32; + let cell: Cell = cell.into(); + let p = cell.center_point(self.cell_size); + + draw_agent(frame, p, angle, radius, color); + let is_selected = self.selected_agent.as_ref().filter(|n| **n == *name).is_some(); + if !valid || is_selected { + let color = if !valid { + self.invalid_color + } else { + self.selected_color + }; + + frame.stroke( + &Path::circle([p.x as f32, p.y as f32].into(), radius), + Stroke{ + color, + width: 5_f32, + ..Default::default() + }, + ); + } + } + } + } + + fn draw_on_hud<'a, 'b: 'a>( + &self, + hud: &'a mut mapf_viz::spatial_canvas::SpatialHUD<'b>, + bounds: iced::Rectangle, + _cursor: canvas::Cursor + ) { + for (name, ctx) in &self.agents { + for (kind, cell, offset) in [("Start", ctx.agent.start, -16), ("Goal", ctx.agent.goal, 0)] { + let cell: Cell = cell.into(); + let p = cell.center_point(self.cell_size); + let r = ctx.agent.radius as f32; + let delta = iced::Vector::new(r, -r); + let p = iced::Point::new(p.x as f32, p.y as f32) + delta; + + if bounds.contains(p) { + hud.at( + p, + |frame| { + frame.translate(iced::Vector::new(0.0, offset as f32)); + frame.fill_text(format!("{} {}: ({}, {})", name, kind, cell.x, cell.y).to_owned()); + } + ) + } + } + } + } + + fn estimate_bounds(&self) -> mapf_viz::spatial_canvas::InclusionZone { + let mut zone = InclusionZone::Empty; + for (_, ctx) in &self.agents { + for endpoint in [ctx.agent.start, ctx.agent.goal] { + let cell: Cell = endpoint.into(); + let p = cell.center_point(self.cell_size); + for v in [[1.0, 1.0], [-1.0, -1.0]] { + let v: Vector = v.into(); + let r = p + ctx.agent.radius*v; + zone.include([r.x as f32, r.y as f32].into()); + } + } + } + + return zone; + } +} + +#[derive(Debug, Clone)] +struct SolutionVisual { + pub cell_size: f64, + pub solution_color: iced::Color, + pub solutions: Vec<(f64, LinearTrajectorySE2)>, + pub search_color: iced::Color, + pub searches: Vec<(f64, LinearTrajectorySE2)>, + pub obstacles: Vec<(f64, LinearTrajectorySE2)>, + pub vertex_lookup: HashMap, + tick_start: Option, + now: Option, + _msg: std::marker::PhantomData, +} + +impl SolutionVisual { + fn new( + cell_size: f64, + solution_color: iced::Color, + search_color: iced::Color, + obstacles: Vec<(f64, LinearTrajectorySE2)>, + ) -> Self { + Self { + cell_size, + solution_color, + solutions: Vec::new(), + search_color, + searches: Vec::new(), + obstacles, + vertex_lookup: HashMap::new(), + tick_start: None, + now: None, + _msg: Default::default(), + } + } + + fn reset_time(&mut self) { + self.tick_start = Some(TimePoint::from_std_instant(std::time::Instant::now())); + self.now = Some(Duration::zero()); + } + + fn time_range(&self) -> Option<(TimePoint, TimePoint)> { + let mut earliest = Minimum::new(|l: &TimePoint, r: &TimePoint| l.cmp(r)); + let mut latest = Minimum::new(|l: &TimePoint, r: &TimePoint| r.cmp(l)); + + for (_, traj) in &self.searches { + earliest.consider(&traj.initial_motion_time()); + latest.consider(&traj.finish_motion_time()); + } + + if !earliest.has_value() || !latest.has_value() { + for (_, solution) in &self.solutions { + earliest.consider(&solution.initial_motion_time()); + latest.consider(&solution.finish_motion_time()); + } + } + + if !earliest.has_value() || !latest.has_value() { + for (_, obs) in &self.obstacles { + earliest.consider(&obs.initial_motion_time()); + latest.consider(&obs.finish_motion_time()); + } + } + + if let (Some(earliest), Some(latest)) = (earliest.result(), latest.result()) { + return Some((earliest, latest)); + } + + return None; + } + + fn tick(&mut self) -> bool { + + if let Some((start, end)) = self.time_range() { + let duration = end - start; + if let Some(start) = self.tick_start { + let dt = TimePoint::from_std_instant(std::time::Instant::now()) - start; + if dt > duration + Duration::from_secs(1) { + self.reset_time(); + } else if dt > duration { + self.now = Some(duration); + } else { + self.now = Some(dt); + } + + return true; + } else { + self.reset_time(); + } + } + + return false; + } +} + +impl SpatialCanvasProgram for SolutionVisual { + fn draw_in_space( + &self, + frame: &mut canvas::Frame, + _spatial_bounds: iced::Rectangle, + _spatial_cursor: canvas::Cursor + ) { + if let Some((t0, _)) = self.time_range() { + + for (_, trajectory) in &self.searches { + draw_trajectory(frame, trajectory, self.search_color, None); + } + + for (_, trajectory) in &self.solutions { + draw_trajectory(frame, trajectory, self.solution_color, None); + } + + for (r, obs) in &self.obstacles { + let red = iced::Color::from_rgb(1.0, 0.0, 0.0); + draw_trajectory(frame, obs, red, Some(8_f32)); + + if let Some(now) = self.now { + if let Ok(p) = obs.motion().compute_position(&(t0 + now)) { + draw_agent( + frame, + Point::from(p.translation.vector), + Some(p.rotation.angle()), + *r as f32, + red, + ); + } + } + } + + if let Some(now) = self.now { + for (radius, trajectory) in &self.searches { + if let Ok(p) = trajectory.motion().compute_position(&(t0 + now)) { + draw_agent( + frame, + p.translation.vector.into(), + Some(p.rotation.angle()), + *radius as f32, + self.search_color, + ); + } + } + + for (radius, trajectory) in &self.solutions { + if let Ok(p) = trajectory.motion().compute_position(&(t0 + now)) { + draw_agent( + frame, + Point::from(p.translation.vector), + Some(p.rotation.angle()), + *radius as f32, + self.solution_color, + ); + } + } + } + } + } + + fn draw_on_hud<'a, 'b: 'a>( + &self, + hud: &'a mut mapf_viz::spatial_canvas::SpatialHUD<'b>, + bound: iced::Rectangle, + _spatial_cursor: canvas::Cursor + ) { + for (cell, v) in &self.vertex_lookup { + let p = cell.center_point(self.cell_size); + let p = iced::Point::new(p.x as f32, p.y as f32); + if bound.contains(p) { + hud.at( + p, + |frame| { + frame.fill_text(format!("{v}")); + } + ); + } + } + } + + fn estimate_bounds(&self) -> InclusionZone { + // This layer should always be contained within the other layers, + // so we don't need to provide any bounds for this. + return InclusionZone::Empty; + } +} + +spatial_layers!(GridLayers: InfiniteGrid, SparseGridAccessibilityVisual, EndpointSelector, SolutionVisual); + +type SparseAccessibilityGraph = AccessibilityGraph; + +type MyAlgo = AStarConnect>; +type TreeTicket = mapf::algorithm::tree::TreeQueueTicket>; + +struct App { + save_button: button::State, + load_button: button::State, + select_file_button: button::State, + file_text_input: text_input::State, + file_text_input_value: String, + agent_yaw_slider: slider::State, + agent_radius_slider: slider::State, + agent_speed_slider: slider::State, + agent_spin_slider: slider::State, + add_agent_button: button::State, + remove_agent_button: button::State, + pick_agent_state: pick_list::State, + reset_view_button: button::State, + reset_time_button: button::State, + canvas: SpatialCanvas, + node_list_scroll: scrollable::State, + debug_text_scroll: scrollable::State, + show_details: KeyToggler, + search: Option<(f64, Search, QueueLengthLimit>)>, + step_progress: button::State, + debug_planner_on: bool, + debug_negotiation_on: bool, + search_memory: Vec, + negotiation_history: Vec, + name_map: HashMap, + debug_step_count: u64, + debug_node_selected: Option, + negotiation_node_selected: Option, + next_robot_name_index: usize, +} + +impl App { + const TICK_COUNT: u32 = 1000; + + fn get_tick(x: f64, min: f64, max: f64) -> u32 { + ((x - min)/(max - min) * Self::TICK_COUNT as f64) as u32 + } + + fn from_tick(x: u32, min: f64, max: f64) -> f64 { + x as f64 * (max - min)/(Self::TICK_COUNT as f64) + min + } + + fn default_invalid_color() -> iced::Color { + iced::Color::from_rgb(1.0, 0.0, 0.0) + } + + fn default_selected_color() -> iced::Color { + iced::Color::from_rgb(0.1, 1.0, 1.0) + } + + fn default_endpoint_color(endpoint: Endpoint) -> iced::Color { + match endpoint { + Endpoint::Start => iced::Color::from_rgba(0.1, 0.8, 0.1, 0.9), + Endpoint::Goal => iced::Color::from_rgba(0.1, 0.1, 1.0, 0.9), + } + } + + fn default_solution_color() -> iced::Color { + iced::Color::from_rgb(1.0, 0.1, 1.0) + } + + fn default_search_color() -> iced::Color { + iced::Color::from_rgb8(191, 148, 228) + } + + fn save_file(&self, filename: &String) { + let out_file = match std::fs::File::create(filename) { + Ok(r) => r, + Err(err) => { + println!("Unable to save to file {}: {:?}", self.file_text_input_value, err); + return; + } + }; + + let scenario = self.make_scenario(); + + match serde_yaml::to_writer(out_file, &scenario) { + Ok(()) => {} + Err(err) => { + println!("Unable to save to file {}: {:?}", self.file_text_input_value, err); + return; + } + } + } + + fn set_robot_radius(&mut self, radius: f64) { + self.canvas.program.layers.1.set_robot_radius(radius as f32); + let endpoint_selector = &mut self.canvas.program.layers.2; + if endpoint_selector.set_agent_radius(radius) { + for endpoint in [Endpoint::Start, Endpoint::Goal] { + if let Some(cell) = endpoint_selector.endpoint_cell(endpoint) { + let cell: Cell = (*cell).into(); + let p = cell.center_point(endpoint_selector.cell_size); + let valid = self.canvas.program.layers.1.grid().is_circle_occupied(p, radius).is_none(); + endpoint_selector.set_endpoint_valid(endpoint, valid); + } + } + self.canvas.cache.clear(); + } + + self.update_all_endpoints(); + } + + fn update_all_endpoints(&mut self) { + self.canvas.program.layers.2.update_endpoint_conflicts( + Endpoint::Start, + &self.canvas.program.layers.1.accessibility(), + ); + self.canvas.program.layers.2.update_endpoint_conflicts( + Endpoint::Goal, + &self.canvas.program.layers.1.accessibility(), + ); + } + + fn select_search_node(&mut self, value: usize) { + if self.search_memory.is_empty() { + self.debug_node_selected = Some(0); + } else { + self.debug_node_selected = Some(usize::min(value, self.search_memory.len() - 1)); + } + + self.canvas.program.layers.3.solutions.clear(); + if let Some(ticket) = self.search_memory.get(value) { + if let Some((r, search)) = &self.search { + let solution = search + .memory().0.arena + .retrace(ticket.node_id) + .unwrap() + .make_trajectory() + .unwrap(); + + self.canvas.program.layers.3.solutions.extend(solution.map(|s| (*r, s.trajectory)).into_iter()); + self.canvas.cache.clear(); + + self.canvas.program.layers.3.obstacles.clear(); + if let Some(node) = search.memory().0.arena.get(ticket.node_id) { + if let Some(n) = self.negotiation_node_selected { + if let Some(n) = self.negotiation_history.get(n) { + let agent_id = self.name_map.iter() + .find(|(_, name)| **name == *self.canvas.program.layers.2.selected_agent.as_ref().unwrap()) + .map(|(i, _)| *i); + + if let Some(agent_id) = agent_id { + for obs in n.environment.iter_obstacles_from(node.state().key.vertex, agent_id) { + if let Some(t) = obs.trajectory() { + let r = obs.profile().footprint_radius(); + self.canvas.program.layers.3.obstacles.push((r, t.clone())); + } + } + } + } + } + } + } + } + } + + fn step_progress(&mut self) { + if let Some((radius, search)) = &mut self.search { + self.debug_step_count += 1; + + self.search_memory = search.memory().0 + .queue.clone().into_iter_sorted() + .map(|n| n.0.clone()).collect(); + + // TODO(@mxgrey): Make the number to take configurable + for ticket in self.search_memory.iter().take(10) { + if let Some(mt) = search.memory().0 + .arena.retrace(ticket.node_id).unwrap() + .make_trajectory().unwrap() + { + self.canvas.program.layers.3.searches.push((*radius, mt.trajectory)); + } + } + + if let SearchStatus::Solved(solution) = search.step().unwrap() { + println!("Queue length: {}", search.memory().queue_length()); + println!("Solution: {:#?}", solution); + self.canvas.program.layers.3.solutions.clear(); + self.canvas.program.layers.3.solutions.extend(solution.make_trajectory().unwrap().map(|t| (*radius, t.trajectory)).into_iter()); + self.debug_node_selected = None; + self.search = None; + } else { + println!("Queue length: {}", search.memory().queue_length()); + if let Some(selection) = self.debug_node_selected { + self.select_search_node(selection); + } + } + + self.canvas.cache.clear(); + } + } + + fn add_agent(&mut self) { + let name = { + let mut name = generate_robot_name(self.next_robot_name_index); + while self.canvas.program.layers.2.agents.contains_key(&name) { + self.next_robot_name_index += 1; + name = generate_robot_name(self.next_robot_name_index); + } + + self.next_robot_name_index += 1; + name + }; + + let start = [-30, 0]; + let goal = [-25, 0]; + // let start = [-5, 0]; + // let goal = [5, 0]; + let yaw = 0.0; + let agent = match self.canvas.program.layers.2.selected_agent() { + Some(ctx) => { + Agent { + start, + goal, + yaw, + ..ctx.agent + } + } + None => { + Agent { + start, + goal, + yaw, + radius: default_radius(), + speed: default_speed(), + spin: default_spin(), + } + } + }; + + self.canvas.program.layers.2.agents.insert(name.clone(), AgentContext::new(agent)); + self.canvas.program.layers.2.selected_agent = Some(name); + self.canvas.program.layers.1.set_robot_radius(agent.radius as f32); + self.update_all_endpoints(); + self.canvas.cache.clear(); + self.generate_plan(); + } + + fn remove_agent(&mut self) { + if let Some(selected) = &self.canvas.program.layers.2.selected_agent { + self.canvas.program.layers.2.agents.remove(selected); + self.canvas.program.layers.2.selected_agent = self.canvas.program.layers.2.agents + .iter().next().map(|r| r.0.clone()); + } + } + + fn make_scenario(&self) -> Scenario { + let cell_size = self.canvas.program.layers.2.cell_size; + let camera_bounds = match self.canvas.camera_bounds() { + InclusionZone::Empty => None, + InclusionZone::Some { lower, upper } => Some([[lower.x, lower.y], [upper.x, upper.y]]) + }; + Scenario { + agents: self.canvas.program.layers.2.agents.iter().map(|(n, a)| (n.clone(), a.agent.clone())).collect(), + obstacles: self.canvas.program.layers.3.obstacles.iter().map(|obs| Obstacle::new(obs.0, &obs.1, cell_size)).collect(), + occupancy: serialize_grid(self.canvas.program.layers.1.grid()), + cell_size, + camera_bounds, + } + } + + fn generate_plan(&mut self) { + // Clear all history + self.search_memory.clear(); + self.search = None; + self.canvas.program.layers.3.solutions.clear(); + self.canvas.program.layers.3.searches.clear(); + self.canvas.cache.clear(); + + if self.debug_planner_on { + let endpoints = &self.canvas.program.layers.2; + let accessibility = self.canvas.program.layers.1.accessibility(); + let Some(ctx) = endpoints.selected_agent() else { return }; + + if !ctx.endpoints_valid() { + print!("Cannot plan for {}:", endpoints.selected_agent.as_ref().unwrap()); + if !ctx.start_open { + print!(" invalid start |"); + } + if !ctx.start_available { + print!(" start conflict |"); + } + if !ctx.goal_open { + print!(" invalid goal |"); + } + if !ctx.goal_available { + print!(" goal conflict |"); + } + println!(""); + return; + } + + let r = ctx.agent.radius; + let extrapolator = DifferentialDriveLineFollow::new(ctx.agent.speed, ctx.agent.spin).expect("Bad speeds"); + let profile = CircularProfile::new(r, 0.0, 0.0).expect("Bad profile sizes"); + + let shared_accessibility = Arc::new(accessibility.clone()); + let activity_graph = SharedGraph::new(AccessibilityGraph::new(shared_accessibility)); + let heuristic_graph = activity_graph.clone(); + + let (environment, minimum_time) = 'environment: { + // if let Some(n) = self.negotiation_node_selected { + if let Some(n) = self.negotiation_node_selected { + if let Some(node) = self.negotiation_history.get(n) { + let mut env = node.environment.clone(); + let agent_id = self.name_map.iter() + .find(|(_, name)| **name == *endpoints.selected_agent.as_ref().unwrap()) + .map(|(i, _)| *i); + + // if let Some(agent_id) = agent_id { + if let Some(agent_id) = agent_id { + env.set_mask(Some(agent_id)); + env.overlay_trajectory(agent_id, None).ok(); + self.canvas.program.layers.3.obstacles.clear(); + for obs in env.iter_all_obstacles() { + if let Some(t) = obs.trajectory() { + let r = obs.profile().footprint_radius(); + self.canvas.program.layers.3.obstacles.push((r, t.clone())); + } + } + + let finish_time = node.proposals.values() + .max_by_key(|t| t.meta.trajectory.finish_motion_time()) + .unwrap().meta.trajectory.finish_motion_time(); + + break 'environment (Arc::new(env), Some(finish_time)); + } + + } + } + + let environment = Arc::new( + CcbsEnvironment::new( + Arc::new({ + let mut env = DynamicEnvironment::new(profile); + for (obs_size, obs_traj) in self.canvas.program.layers.3.obstacles.iter() { + env.obstacles.push( + DynamicCircularObstacle::new( + CircularProfile::new(*obs_size, 0.0, 0.0).unwrap() + ).with_trajectory(Some(obs_traj.clone())) + ); + } + env + }) + ) + ); + + let mut minimum_time: Option = None; + for obs in environment.iter_all_obstacles() { + if let Some(t) = obs.trajectory() { + let tf = t.finish_motion_time(); + minimum_time = Some(minimum_time.map(|t| t.min(tf)).unwrap_or(tf)); + } + } + + (environment, minimum_time) + }; + + let domain = InformedSearch::new_sipp_se2( + activity_graph, + heuristic_graph, + extrapolator, + environment, + TravelEffortCost::save_one_second_with_detour_up_to( + 5.0, + 360_f64.to_radians(), + ), + ).unwrap(); + + let start = ctx.agent.make_start(); + let goal = ctx.agent.make_goal().with_minimum_time(minimum_time); + + println!( + "About to plan for {}:\nStart: {start:#?}\nGoal: {goal:#?}", + endpoints.selected_agent.as_ref().unwrap(), + ); + + let planner = Planner::new(AStarConnect(domain)) + .with_halting(QueueLengthLimit(Some(1_000_000))); + // .with_halting(MeasureLimit(None)); + let search = planner.plan(start, goal).unwrap(); + + self.canvas.program.layers.3.searches.clear(); + self.debug_step_count = 0; + if self.debug_planner_on { + self.search_memory = search.memory().0 + .queue.clone().into_iter_sorted() + .map(|n| n.0.clone()).collect(); + self.search = Some((r, search)); + } + self.debug_node_selected = None; + return; + } + + self.negotiation_history.clear(); + self.canvas.program.layers.3.obstacles.clear(); + let scenario = self.make_scenario(); + + if !self.file_text_input_value.is_empty() { + let mut backup = self.file_text_input_value.clone(); + backup += ".backup"; + println!("Saving backup file {backup}"); + self.save_file(&backup); + } + + let start_time = std::time::Instant::now(); + // let propsoals = match negotiate(&scenario, Some(1_000_000)) { + let (solution_node, node_history, name_map) = match negotiate( + &scenario, + // Some(1_000), + Some(1_000_000), + // None + ) { + Ok(solutions) => solutions, + Err(err) => { + match err { + NegotiationError::PlanningFailed((nodes, name_map)) => { + println!("Unable to find a solution"); + self.negotiation_history = nodes; + self.negotiation_history.sort_unstable_by_key(|n| n.id); + self.name_map = name_map; + } + err => println!("Error while planning: {err:?}"), + }; + + let elapsed = start_time.elapsed(); + println!("Elapsed time: {} seconds", elapsed.as_secs_f64()); + return; + } + }; + let elapsed = start_time.elapsed(); + println!("Successful planning took {} seconds", elapsed.as_secs_f64()); + dbg!(node_history.len()); + + assert!(self.canvas.program.layers.3.solutions.is_empty()); + for (i, proposal) in &solution_node.proposals { + let name = name_map.get(i).unwrap(); + let r = scenario.agents.get(name).unwrap().radius; + self.canvas.program.layers.3.solutions.push((r, proposal.meta.trajectory.clone())); + } + + if self.debug_negotiation_on { + self.negotiation_history = node_history; + self.negotiation_history.sort_unstable_by_key(|n| n.id); + } + self.name_map = name_map; + } +} + +#[derive(Parser)] +#[command(author, version, about, long_about = None)] +struct Args { + /// Scenario to load on startup + filename: Option, +} + +fn load_file(filename: &String) -> Option { + let f = match std::fs::File::open(&filename) { + Ok(f) => f, + Err(err) => { + println!("Unable to open file {}: {err:?}", filename); + return None; + } + }; + let scenario: Scenario = match serde_yaml::from_reader(f) { + Ok(scenario) => scenario, + Err(err) => { + println!("Unable to parse scenario in file {}: {err:?}", filename); + return None; + } + }; + Some(scenario) +} + +fn load_scenario(filename: Option<&String>) -> ( + BTreeMap, + Vec<(f64, Trajectory)>, + SparseGrid, + InclusionZone, + bool, +) { + let mut agents = BTreeMap::new(); + let mut obstacles = Vec::new(); + let mut grid = SparseGrid::new(1.0); + let mut zone = InclusionZone::Empty; + let mut success = false; + if let Some(filename) = filename { + if let Some(scenario) = load_file(filename) { + let cell_size = scenario.cell_size; + agents = scenario.agents; + obstacles = scenario.obstacles.into_iter().filter_map(|obs| { + LinearTrajectorySE2::from_iter( + obs.trajectory.into_iter() + .map(|(t, x, y)| { + let p = Cell::new(x, y).center_point(cell_size); + WaypointSE2::new_f64(t, p.x, p.y, 0.0) + }) + ).ok() + .map(|t| + t + .with_indefinite_initial_time(obs.indefinite_start) + .with_indefinite_finish_time(obs.indefinite_finish) + ) + .map(|t| (obs.radius, t)) + }).collect(); + for (y, row) in scenario.occupancy { + for x in row { + grid.change_cells( + &[(Cell::new(x, y), true)].into_iter().collect() + ); + } + } + + if let Some(bounds) = scenario.camera_bounds { + for p in bounds { + zone.include(iced::Point::new(p[0], p[1])); + } + } + success = true; + } + } + (agents, obstacles, grid, zone, success) +} + +impl Application for App { + type Message = Message; + type Executor = executor::Default; + type Flags = Args; + + fn new(flags: Self::Flags) -> (Self, Command) { + let cell_size = 1.0_f32; + let (agents, obstacles, grid, mut zone, _) = load_scenario(flags.filename.as_ref()); + + let mut canvas = SpatialCanvas::new( + GridLayers{ + layers: ( + InfiniteGrid::new(cell_size), + SparseGridAccessibilityVisual::new( + grid, + default_radius() as f32, + Some(Box::new(|| { Message::OccupancyChanged })), + ).showing_accessibility(true), + EndpointSelector::new( + agents, + cell_size as f64, + Self::default_invalid_color(), + Self::default_selected_color(), + Self::default_endpoint_color(Endpoint::Start), + Self::default_endpoint_color(Endpoint::Goal), + ), + SolutionVisual::new( + cell_size as f64, + Self::default_solution_color(), + Self::default_search_color(), + obstacles, + ), + ) + } + ); + canvas.zoom = 20.0; + + let mut app = Self { + save_button: button::State::new(), + load_button: button::State::new(), + select_file_button: button::State::new(), + file_text_input: text_input::State::new(), + file_text_input_value: flags.filename.unwrap_or(String::new()), + agent_yaw_slider: slider::State::new(), + agent_radius_slider: slider::State::new(), + agent_speed_slider: slider::State::new(), + agent_spin_slider: slider::State::new(), + add_agent_button: button::State::new(), + remove_agent_button: button::State::new(), + pick_agent_state: pick_list::State::new(), + reset_view_button: button::State::new(), + reset_time_button: button::State::new(), + canvas, + node_list_scroll: scrollable::State::new(), + debug_text_scroll: scrollable::State::new(), + show_details: KeyToggler::for_key(keyboard::KeyCode::LAlt), + search: None, + step_progress: button::State::new(), + debug_planner_on: false, + debug_negotiation_on: false, + search_memory: Default::default(), + negotiation_history: Default::default(), + name_map: Default::default(), + debug_step_count: 0, + debug_node_selected: None, + negotiation_node_selected: None, + next_robot_name_index: 0, + }; + + if app.canvas.program.layers.2.agents.is_empty() { + app.add_agent(); + zone.include(iced::Point::new(-10.0, -10.0)); + zone.include(iced::Point::new(10.0, 10.0)); + } + + app.update_all_endpoints(); + app.generate_plan(); + + let set_view = Command::perform(async move {}, move |_| Message::SetView(zone)); + let top_agent = app.canvas.program.layers.2.agents.iter().next(); + let set_agent = if let Some((top_agent, _)) = top_agent { + let top_agent = top_agent.clone(); + Some(Command::perform(async move {}, move |_| Message::SelectAgent(top_agent.clone()))) + } else { + None + }; + + (app, Command::batch([set_view].into_iter().chain(set_agent.into_iter()))) + } + + fn title(&self) -> String { + "Grid Planner".to_owned() + } + + fn update( + &mut self, + message: Self::Message + ) -> Command { + match message { + Message::AddAgent => { + self.add_agent(); + self.generate_plan(); + } + Message::RemoveAgent => { + self.remove_agent(); + self.generate_plan(); + } + Message::SaveFile | Message::SaveFileAs => { + if matches!(message, Message::SaveFileAs) { + match FileDialog::new().show_save_single_file() { + Ok(f) => match f { + Some(f) => self.file_text_input_value = f.as_path().as_os_str().to_str().unwrap().to_owned(), + None => return Command::none(), + } + Err(err) => { + println!("Unable to select file: {err:?}"); + return Command::none(); + } + } + } + + self.save_file(&self.file_text_input_value); + } + Message::LoadFile => { + match FileDialog::new().show_open_single_file() { + Ok(f) => match f { + Some(value) => self.file_text_input_value = value.as_path().as_os_str().to_str().unwrap().to_owned(), + None => return Command::none(), + } + Err(err) => { + println!("Unable to load selected file: {err:?}"); + return Command::none(); + } + } + + let (agents, obstacles, grid, zone, success) = load_scenario(Some(&self.file_text_input_value)); + if !success { + return Command::none(); + } + + self.canvas.program.layers.1.set_grid(grid); + self.canvas.program.layers.2.agents = agents.into_iter().map(|(n, a)| (n, AgentContext::new(a))).collect(); + self.canvas.program.layers.3.obstacles = obstacles; + self.update_all_endpoints(); + self.canvas.cache.clear(); + self.generate_plan(); + return Command::perform(async move {}, move |_| Message::SetView(zone)); + } + Message::ScenarioFileInput(value) => { + self.file_text_input_value = value; + } + Message::SelectAgent(new_agent) => { + match self.canvas.program.layers.2.agents.get(&new_agent) { + Some(ctx) => { + self.canvas.program.layers.2.selected_agent = Some(new_agent); + self.canvas.program.layers.1.set_robot_radius(ctx.agent.radius as f32); + self.canvas.cache.clear(); + } + None => {} + } + } + Message::AgentYawSlide(value) => { + if let Some(ctx) = self.canvas.program.layers.2.selected_agent_mut() { + ctx.agent.yaw = Self::from_tick(value, MIN_AGENT_YAW, MAX_AGENT_YAW); + self.generate_plan(); + } + } + Message::AgentRadiusSlide(value) => { + self.set_robot_radius(Self::from_tick(value, MIN_AGENT_RADIUS, MAX_AGENT_RADIUS)); + self.generate_plan(); + } + Message::AgentSpeedSlide(value) => { + if let Some(ctx) = self.canvas.program.layers.2.selected_agent_mut() { + ctx.agent.speed = Self::from_tick(value, MIN_AGENT_SPEED, MAX_AGENT_SPEED); + self.generate_plan(); + } + } + Message::AgentSpinSlide(value) => { + if let Some(ctx) = self.canvas.program.layers.2.selected_agent_mut() { + ctx.agent.spin = Self::from_tick(value, MIN_AGENT_SPIN, MAX_AGENT_SPIN); + self.generate_plan(); + } + } + Message::ResetView => { + if !self.canvas.fit_to_bounds() { + // The canvas was not ready to fit the view, so we need to + // issue this message again + return Command::perform(async move {}, move |_| Message::ResetView); + } + } + Message::SetView(zone) => { + if !self.canvas.fit_to_zone(zone) { + // The canvas was not ready to fit the view, so we need to + // issue this message again + return Command::perform(async move {}, move |_| Message::SetView(zone)); + } + } + Message::ResetTime => { + self.canvas.program.layers.3.reset_time(); + } + Message::EventOccurred(event) => { + if let iced_native::Event::Keyboard(event) = event { + match self.show_details.key_toggle(event) { + Toggle::On => { + // self.canvas.program.layers.1.show_accessibility = true; + self.canvas.program.layers.1.show_accessibility = false; + self.canvas.program.layers.2.show_details = true; + self.canvas.cache.clear(); + }, + Toggle::Off => { + // self.canvas.program.layers.1.show_accessibility = false; + self.canvas.program.layers.1.show_accessibility = true; + self.canvas.program.layers.2.show_details = false; + self.canvas.cache.clear(); + }, + Toggle::NoChange => { + // Do nothing + } + } + + if let keyboard::Event::KeyPressed{key_code: keyboard::KeyCode::D, modifiers} = event { + if modifiers.shift() { + self.debug_planner_on = false; + self.generate_plan(); + } else { + self.debug_planner_on = true; + self.generate_plan(); + } + } + + if let keyboard::Event::KeyPressed { key_code: keyboard::KeyCode::N, modifiers } = event { + if modifiers.shift() { + self.debug_negotiation_on = false; + self.negotiation_history.clear(); + } else { + self.debug_negotiation_on = true; + self.generate_plan(); + } + } + + if let keyboard::Event::KeyPressed{key_code: keyboard::KeyCode::S, ..} = event { + self.step_progress(); + } + + if let keyboard::Event::KeyPressed { key_code: keyboard::KeyCode::Down, .. } = event { + if self.debug_planner_on { + let next_debug_node = self.debug_node_selected.map(|n| n+1).unwrap_or(0); + return Command::perform(async move {}, move |_| { + Message::SelectDebugNode(next_debug_node) + }); + } else { + let next_negotiation_node = self.negotiation_node_selected.map(|n| n+1).unwrap_or(0); + return Command::perform(async move {}, move |_| { + Message::SelectNegotiationNode(next_negotiation_node) + }); + } + } + + if let keyboard::Event::KeyPressed { key_code: keyboard::KeyCode::Up, .. } = event { + if self.debug_planner_on { + let next_debug_node = self.debug_node_selected.map(|n| if n > 0 { n-1 } else { 0 }).unwrap_or(0); + return Command::perform(async move {}, move |_| { + Message::SelectDebugNode(next_debug_node) + }); + } else { + let next_negotiation_node = self.negotiation_node_selected.map(|n| if n > 0 { n-1 } else { 0 }).unwrap_or(0); + return Command::perform(async move {}, move |_| { + Message::SelectNegotiationNode(next_negotiation_node) + }); + } + } + } + }, + Message::EndpointSelected(selection) => { + match selection { + EndpointSelection::Cell(endpoint) => { + self.canvas.program.layers.2.update_endpoint_conflicts( + endpoint, + self.canvas.program.layers.1.accessibility(), + ); + } + } + self.generate_plan(); + }, + Message::OccupancyChanged => { + self.generate_plan(); + }, + Message::SelectDebugNode(value) => { + self.select_search_node(value); + }, + Message::SelectNegotiationNode(value) => { + if self.negotiation_history.is_empty() { + self.negotiation_node_selected = None; + } else { + self.negotiation_node_selected = Some(usize::min(value, self.negotiation_history.len() - 1)); + } + + if let Some(node) = self.negotiation_history.get(value) { + self.canvas.program.layers.3.obstacles.clear(); + for obs in node.environment.iter_all_obstacles() { + if let Some(t) = obs.trajectory() { + let r = obs.profile().footprint_radius(); + self.canvas.program.layers.3.obstacles.push((r, t.clone())); + } + } + + self.canvas.program.layers.3.solutions.clear(); + for (i, proposal) in &node.proposals { + let name = self.name_map.get(i).unwrap(); + let r = self.canvas.program.layers.2.agents.get(name).unwrap().agent.radius; + self.canvas.program.layers.3.solutions.push((r, proposal.meta.trajectory.clone())); + } + + if let Some(conceded) = node.conceded { + if let Some(name) = self.name_map.get(&conceded) { + let name = name.clone(); + return Command::perform(async {}, move |_| Message::SelectAgent(name.clone())); + } + } + } + } + Message::StepProgress => { + self.step_progress(); + } + Message::Tick => { + if self.canvas.program.layers.3.tick() { + self.canvas.cache.clear(); + } + } + } + + Command::none() + } + + fn subscription(&self) -> iced::Subscription { + iced_native::Subscription::batch([ + iced_native::subscription::events().map(Message::EventOccurred), + iced::time::every(std::time::Duration::from_millis(50)).map(|_|{ Message::Tick }), + ]) + } + + fn view(&mut self) -> Element { + let mut content = Column::new() + .spacing(20) + .align_items(Alignment::Start) + .width(Length::Fill) + .height(Length::Fill); + + let (agent_yaw, agent_radius, agent_speed, agent_spin) = if let Some(ctx) = self.canvas.program.layers.2.selected_agent() { + (ctx.agent.yaw, ctx.agent.radius, ctx.agent.speed, ctx.agent.spin) + } else { + (0.0, default_radius(), default_speed(), default_spin()) + }; + let yaw_tick = Self::get_tick(agent_yaw, MIN_AGENT_YAW, MAX_AGENT_YAW); + let radius_tick = Self::get_tick(agent_radius, MIN_AGENT_RADIUS, MAX_AGENT_RADIUS); + let speed_tick = Self::get_tick(agent_speed, MIN_AGENT_SPEED, MAX_AGENT_SPEED); + let spin_tick = Self::get_tick(agent_spin, MIN_AGENT_SPIN, MAX_AGENT_SPIN); + + let file_row = Row::new() + .spacing(20) + .align_items(Alignment::Center) + .width(Length::Fill) + .push( + Button::new( + &mut self.select_file_button, + iced::Text::new("Save As...") + ).on_press(Message::SaveFileAs) + ) + .push({ + Button::new( + &mut self.load_button, + iced::Text::new("Load") + ).on_press(Message::LoadFile) + }) + .push({ + let button = Button::new( + &mut self.save_button, + iced::Text::new("Save") + ); + if !self.file_text_input_value.is_empty() { + button.on_press(Message::SaveFile) + } else { + button + } + }) + .push( + TextInput::new( + &mut self.file_text_input, + "Scenario File", + &mut self.file_text_input_value, + Message::ScenarioFileInput + ) + .padding(10) + ); + + let mut agent_names: Vec = self.canvas.program.layers.2.agents.iter().map(|(name, _)| name.clone()).collect(); + agent_names.sort_unstable(); + let robot_row = Row::new() + .spacing(20) + .align_items(Alignment::Center) + .push( + Button::new( + &mut self.add_agent_button, + iced::Text::new("Add Agent"), + ).on_press(Message::AddAgent) + ) + .push( + Button::new( + &mut self.remove_agent_button, + iced::Text::new("Remove Agent"), + ).on_press(Message::RemoveAgent) + ) + .push( + PickList::new( + &mut self.pick_agent_state, + agent_names, + self.canvas.program.layers.2.selected_agent.clone(), + Message::SelectAgent, + ) + ) + .push( + Column::new() + .width(Length::FillPortion(1)) + .push(Text::new(format!("Yaw: {:.2} deg", agent_yaw))) + .push( + Slider::new( + &mut self.agent_yaw_slider, + 0..=Self::TICK_COUNT, + yaw_tick, + Message::AgentYawSlide + ) + ) + ) + .push( + Column::new() + .width(Length::FillPortion(1)) + .push(Text::new(format!("Radius: {:.2} m", agent_radius))) + .push( + Slider::new( + &mut self.agent_radius_slider, + 0..=Self::TICK_COUNT, + radius_tick, + Message::AgentRadiusSlide, + ) + ) + ) + .push( + Column::new() + .width(Length::FillPortion(1)) + .push(Text::new(format!("Speed: {:.2} m/s", agent_speed))) + .push( + Slider::new( + &mut self.agent_speed_slider, + 0..=Self::TICK_COUNT, + speed_tick, + Message::AgentSpeedSlide, + ) + ) + ) + .push( + Column::new() + .width(Length::FillPortion(1)) + .push(Text::new(format!("Spin: {:.2} deg/sec", agent_spin.to_degrees()))) + .push( + Slider::new( + &mut self.agent_spin_slider, + 0..=Self::TICK_COUNT, + spin_tick, + Message::AgentSpinSlide, + ) + ) + ); + + let instruction_row = Row::::new() + .spacing(40) + .align_items(Alignment::Center) + .width(Length::Shrink) + .push( + Column::new() + .push(Text::new("Left click: Set start")) + .push(Text::new("Right click: Set goal")) + ) + .push( + Column::new() + .push(Text::new("Shift + Left click: Add occupancy")) + .push(Text::new("Shift + Right click: Remove occupancy")) + ) + .push( + Column::new() + .push(Text::new("Middle click: Pan view")) + .push(Text::new("Scroll: Zoom")) + ) + .push( + Button::new( + &mut self.reset_view_button, + iced::Text::new("Reset View") + ) + .on_press(Message::ResetView) + .height(Length::Shrink) + ) + .push({ + if let Some(t) = self.canvas.program.layers.3.now { + Text::new(format!("t: {:.2}", t.as_secs_f64())) + } else { + Text::new("t: off") + } + }) + .push( + Button::new( + &mut self.reset_time_button, + iced::Text::new("Reset Time") + ) + .on_press(Message::ResetTime) + .height(Length::Shrink) + ); + + content = content + .push(file_row) + .push(robot_row) + .push(instruction_row); + + const DEBUG_TEXT_SIZE: u16 = 15; + if self.debug_planner_on { + content = content.push( + Row::new() + .push(self.canvas.view()) + .push( + Column::new() + .push( + Row::new() + .push( + Button::new(&mut self.step_progress, Text::new("Step")) + .on_press(Message::StepProgress) + ) + .push(iced::Space::with_width(Length::Units(16))) + .push(Text::new(format!("Steps: {}", self.debug_step_count))) + .push(iced::Space::with_width(Length::Units(16))) + .push(Text::new(format!("Queue size: {}", self.search_memory.len()))) + .align_items(Alignment::Center) + ) + .push({ + let mut scroll = Scrollable::::new(&mut self.node_list_scroll); + if let Some((_, search)) = &self.search { + for (i, ticket) in self.search_memory.iter().enumerate() { + let node = search.memory().0.arena.get(ticket.node_id).unwrap(); + scroll = scroll.push(Radio::new( + i, format!( + "{i}: {:?} + {:?} = {:?}", + node.cost().0, + node.remaining_cost_estimate().0, + ticket.evaluation.0, + ), self.debug_node_selected, + Message::SelectDebugNode + )); + } + } + scroll + }.height(Length::Fill)) + .push({ + Scrollable::::new(&mut self.debug_text_scroll) + .push( + Text::new({ + if let Some(selection) = self.debug_node_selected { + if let Some(node) = self.search_memory.get(selection) { + if let Some((_, search)) = &self.search { + let node = search.memory().0.arena.get(node.node_id).unwrap(); + format!("{node:#?}") + } else { + String::new() + } + } else { + String::new() + } + } else { + String::new() + } + }) + .size(DEBUG_TEXT_SIZE) + ) + }.height(Length::Fill)) + ) + ); + } else if !self.negotiation_history.is_empty() { + content = content.push( + Row::new() + .push(self.canvas.view()) + .push( + Column::new() + .push(Text::new(format!("History size: {}", self.negotiation_history.len()))) + .push({ + let mut scroll = Scrollable::::new(&mut self.node_list_scroll); + for (i, node) in self.negotiation_history.iter().enumerate() { + scroll = scroll.push(Radio::new( + i, + format!("{}. Cost {:.2} | Keys: {} | Parent: {} | Conflicts: {}", node.id, node.cost.0, node.keys.len(), node.parent.unwrap_or(node.id), node.negotiation.conflicts.len()), + self.negotiation_node_selected, + Message::SelectNegotiationNode, + )); + } + scroll + }.height(Length::Fill)) + .push( + Scrollable::::new(&mut self.debug_text_scroll) + .push( + Text::new({ + if let Some(selection) = self.negotiation_node_selected { + if let Some(node) = self.negotiation_history.get(selection) { + let mut text = String::new(); + text += &format!("Keys: {}\n", node.keys.len()); + for key in &node.keys { + text += &format!("{key:?}\n"); + } + text += &format!("------\n{node:#?}"); + text + } else { + String::new() + } + } else { + String::new() + } + }) + .size(DEBUG_TEXT_SIZE) + ) + .height(Length::Fill) + ) + ) + ) + } else { + content = content.push(self.canvas.view()); + } + + Container::new(content) + .width(Length::Fill) + .height(Length::Fill) + .padding(5) + .center_x() + .center_y() + .into() + } +} + +#[derive(Debug, Clone)] +enum Message { + AgentYawSlide(u32), + AgentRadiusSlide(u32), + AgentSpeedSlide(u32), + AgentSpinSlide(u32), + AddAgent, + RemoveAgent, + SelectAgent(String), + SaveFileAs, + SaveFile, + LoadFile, + ScenarioFileInput(String), + ResetView, + ResetTime, + SetView(InclusionZone), + EventOccurred(iced_native::Event), + EndpointSelected(EndpointSelection), + OccupancyChanged, + SelectDebugNode(usize), + SelectNegotiationNode(usize), + StepProgress, + Tick, +} + +fn main() -> iced::Result { + let flags = Args::parse(); + App::run(iced::Settings::with_flags(flags)) +} + +mod style { + use iced::container; + pub struct Pane; + impl container::StyleSheet for Pane { + fn style(&self) -> container::Style { + container::Style{ + text_color: None, + background: None, + border_radius: 0.0, + border_width: 3.0, + border_color: iced::Color{r: 0.9, g: 0.9, b: 0.9, a: 0.9}, + } + } + } +} diff --git a/mapf-viz/examples/grid_planner.rs b/mapf-viz/examples/grid_planner.rs deleted file mode 100644 index 5df084d..0000000 --- a/mapf-viz/examples/grid_planner.rs +++ /dev/null @@ -1,1293 +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. - * -*/ -#![feature(array_windows, binary_heap_into_iter_sorted)] - -use time_point::{TimePoint, Duration}; -use iced::{ - Application, Alignment, Element, Length, Command, Column, Row, Text, Container, Radio, - slider::{self, Slider}, - scrollable::{self, Scrollable}, - text_input::{self, TextInput}, - button::{self, Button}, - canvas::{self, Event, Path, Stroke}, - executor, keyboard, mouse, -}; -use iced_native; -use mapf::{ - graph::{ - SharedGraph, - occupancy::{ - Visibility, VisibilityGraph, SparseGrid, Cell, NeighborhoodGraph, Grid - }, - }, - motion::{ - Trajectory, Motion, OverlayedDynamicEnvironment, DynamicEnvironment, - CircularProfile, DynamicCircularObstacle, TravelEffortCost, - se2::{ - Point, LinearTrajectorySE2, Vector, WaypointSE2, GoalSE2, - DifferentialDriveLineFollow, StartSE2, Orientation, - }, - }, - templates::InformedSearch, - planner::{Planner, Search}, - premade::SippSE2, - algorithm::{AStarConnect, SearchStatus, tree::NodeContainer}, -}; -use mapf_viz::{ - SparseGridOccupancyVisual, InfiniteGrid, - spatial_canvas::{SpatialCanvas, SpatialCanvasProgram, SpatialCache, InclusionZone}, - toggle::{Toggle, Toggler, KeyToggler}, -}; -use mapf_viz::spatial_layers; -use std::collections::HashMap; -use std::sync::Arc; - -type SparseVisibility = Visibility; - -pub(crate) struct Minimum std::cmp::Ordering> { - value: Option, - f: F, -} - -impl std::cmp::Ordering> Minimum { - pub(crate) fn new(f: F) -> Self { - Self{value: None, f} - } - - pub(crate) fn consider(&mut self, other: &T) -> bool { - if let Some(value) = &self.value { - if std::cmp::Ordering::Less == (self.f)(other, value) { - self.value = Some(other.clone()); - return true; - } - } else { - self.value = Some(other.clone()); - return true; - } - - return false; - } - - pub(crate) fn consider_take(&mut self, other: T) -> bool { - if let Some(value) = &self.value { - if std::cmp::Ordering::Less == (self.f)(&other, value) { - self.value = Some(other); - return true; - } - } else { - self.value = Some(other); - return true; - } - - return false; - } - - pub(crate) fn result(self) -> Option { - self.value - } - - pub(crate) fn has_value(&self) -> bool { - self.value.is_some() - } -} - - -fn draw_agent( - frame: &mut canvas::Frame, - point: Point, - angle: Option, - agent_radius: f32, - color: iced::Color, -) { - frame.with_save( - |frame| { - frame.translate([point.x as f32, point.y as f32].into()); - frame.fill(&Path::circle([0, 0].into(), agent_radius), color); - - if let Some(angle) = angle { - frame.rotate(angle as f32); - let r = 0.8 * agent_radius; - let angle_to_point = |angle: f32| { - iced::Point::new(r * f32::cos(angle), r * f32::sin(angle)) - }; - - let points: [iced::Point; 3] = [ - angle_to_point(-150_f32.to_radians()), - angle_to_point(0_f32.to_radians()), - angle_to_point(150_f32.to_radians()), - ]; - - frame.fill( - &Path::new( - |builder| { - builder.move_to(points[0]); - builder.line_to(points[1]); - builder.line_to(points[2]); - builder.line_to(points[0]); - } - ), - iced::Color::from_rgb(1.0, 1.0, 1.0) - ); - } - } - ); -} - -fn draw_trajectory( - frame: &mut canvas::Frame, - trajectory: &LinearTrajectorySE2, - color: iced::Color, -) { - for [wp0, wp1] in trajectory.array_windows() { - let p0 = wp0.position.translation; - let p1 = wp1.position.translation; - frame.stroke( - &Path::line( - [p0.x as f32, p0.y as f32].into(), - [p1.x as f32, p1.y as f32].into(), - ), - Stroke{ - color, - width: 3_f32, - ..Default::default() - } - ); - } -} - -#[derive(Debug, Clone)] -struct EndpointSelector { - pub agent_radius: f64, - pub cell_size: f64, - pub invalid_color: iced::Color, - pub start_color: iced::Color, - pub goal_color: iced::Color, - pub show_details: bool, - pub start_cell: Option, - pub start_angle: Option, - pub start_valid: bool, - pub goal_cell: Option, - pub goal_angle: Option, - pub goal_valid: bool, - start_visibility: Vec, - goal_visibility: Vec, - start_sees_goal: bool, - pressed: bool, - shift: KeyToggler, - _msg: std::marker::PhantomData, -} - -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -enum Endpoint { - Start, - Goal, -} - -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -enum EndpointSelection { - Cell(Endpoint), - Orientation(Endpoint), - Finalize(Endpoint), -} - -impl EndpointSelector { - - fn new( - agent_radius: f64, - cell_size: f64, - invalid_color: iced::Color, - start_color: iced::Color, - goal_color: iced::Color, - ) -> Self { - Self{ - agent_radius, - cell_size, - invalid_color, - start_color, - goal_color, - show_details: false, - // start_cell: None, - // start_cell: Some(Cell::new(-7, -4)), - // start_cell: Some(Cell::new(0, 0)), - start_cell: Some(Cell::new(-1, -13)), - start_angle: None, - start_valid: true, - // goal_cell: None, - // goal_cell: Some(Cell::new(18, 3)), - // goal_cell: Some(Cell::new(10, 0)), - goal_cell: Some(Cell::new(-1, 6)), - goal_angle: None, - goal_valid: true, - start_visibility: Vec::new(), - goal_visibility: Vec::new(), - start_sees_goal: false, - pressed: false, - shift: KeyToggler::for_key(keyboard::KeyCode::LShift), - _msg: Default::default(), - } - } - - fn endpoint_cell(&self, choice: Endpoint) -> &Option { - match choice { - Endpoint::Start => &self.start_cell, - Endpoint::Goal => &self.goal_cell, - } - } - - fn endpoint_cell_mut(&mut self, choice: Endpoint) -> &mut Option { - match choice { - Endpoint::Start => &mut self.start_cell, - Endpoint::Goal => &mut self.goal_cell, - } - } - - fn set_endpoint_valid(&mut self, choice: Endpoint, valid: bool) { - match choice { - Endpoint::Start => { self.start_valid = valid; }, - Endpoint::Goal => { self.goal_valid = valid; }, - } - } - - pub fn calculate_visibility(&mut self, visibility: &SparseVisibility) { - self.start_visibility = self.calculate_endpoint_visibility( - self.start_cell, self.start_valid, visibility - ); - - self.goal_visibility = self.calculate_endpoint_visibility( - self.goal_cell, self.goal_valid, visibility - ); - - self.start_sees_goal = false; - if self.start_valid && self.goal_valid { - if let (Some(cell_start), Some(cell_goal)) = (self.start_cell, self.goal_cell) { - let p_start = cell_start.to_center_point(self.cell_size); - let p_goal = cell_goal.to_center_point(self.cell_size); - self.start_sees_goal = visibility.grid().is_sweep_occupied( - p_start, p_goal, 2.0*self.agent_radius - ).is_none(); - } - } - } - - fn calculate_endpoint_visibility( - &self, - cell_opt: Option, - valid: bool, - visibility: &SparseVisibility, - ) -> Vec { - if valid { - if let Some(cell) = cell_opt { - return visibility.calculate_visibility(cell).map(|c| c).collect(); - } - } - - return Vec::new(); - } -} - -impl SpatialCanvasProgram for EndpointSelector { - fn update( - &mut self, - event: iced::canvas::Event, - cursor: iced::canvas::Cursor - ) -> (SpatialCache, iced::canvas::event::Status, Option) { - if let Some(p) = cursor.position() { - match event { - Event::Mouse(event) => { - if mouse::Event::ButtonPressed(mouse::Button::Right) == event { - self.pressed = true; - let endpoint = if self.shift.state() == Toggle::On { Endpoint::Goal } else { Endpoint::Start }; - *self.endpoint_cell_mut(endpoint) = Some( - Cell::from_point([p.x as f64, p.y as f64].into(), self.cell_size) - ); - - return ( - SpatialCache::Refresh, - canvas::event::Status::Captured, - Some(Message::EndpointSelected( - EndpointSelection::Cell(endpoint) - )) - ); - } - - if mouse::Event::ButtonReleased(mouse::Button::Right) == event { - self.pressed = false; - let endpoint = if self.shift.state() == Toggle::On { Endpoint::Goal } else { Endpoint::Start }; - return ( - SpatialCache::Unchanged, - canvas::event::Status::Captured, - Some(Message::EndpointSelected( - EndpointSelection::Finalize(endpoint) - )) - ); - } - }, - Event::Keyboard(event) => { - self.shift.key_toggle(event); - } - } - } - - return (SpatialCache::Unchanged, canvas::event::Status::Ignored, None); - } - - fn draw_in_space(&self, frame: &mut canvas::Frame, _spatial_bounds: iced::Rectangle, _spatial_cursor: canvas::Cursor) { - for (cell, angle, color, valid) in [ - (self.start_cell, self.start_angle, self.start_color, self.start_valid), - (self.goal_cell, self.goal_angle, self.goal_color, self.goal_valid) - ] { - let radius = self.agent_radius as f32; - if let Some(cell) = cell { - let p = cell.to_center_point(self.cell_size); - draw_agent(frame, p, angle, self.agent_radius as f32, color); - if !valid { - frame.stroke( - &Path::circle([p.x as f32, p.y as f32].into(), radius), - Stroke{ - color: self.invalid_color, - width: 5_f32, - ..Default::default() - }, - ); - } - } - - for (cell_opt, visible, color) in [ - (self.start_cell, &self.start_visibility, self.start_color), - (self.goal_cell, &self.goal_visibility, self.goal_color) - ] { - if let Some(cell) = cell_opt { - let p = cell.to_center_point(self.cell_size); - for v_cell in visible { - let p_v = v_cell.to_center_point(self.cell_size); - frame.stroke( - &Path::line( - [p.x as f32, p.y as f32].into(), - [p_v.x as f32, p_v.y as f32].into() - ), - Stroke{ - color, - width: 5_f32, - ..Default::default() - } - ); - } - } - } - - if self.start_sees_goal { - if let (Some(cell_s), Some(cell_g)) = (self.start_cell, self.goal_cell) { - let p_start = cell_s.to_center_point(self.cell_size); - let p_goal = cell_g.to_center_point(self.cell_size); - frame.stroke( - &Path::line( - [p_start.x as f32, p_start.y as f32].into(), - [p_goal.x as f32, p_goal.y as f32].into() - ), - Stroke{ - color: iced::Color::from_rgb(0.1, 1.0, 1.0), - width: 5_f32, - ..Default::default() - } - ); - } - } - } - } - - fn draw_on_hud<'a, 'b: 'a>( - &self, - hud: &'a mut mapf_viz::spatial_canvas::SpatialHUD<'b>, - bounds: iced::Rectangle, - _cursor: canvas::Cursor - ) { - if self.show_details { - for cell_opt in [self.start_cell, self.goal_cell] { - if let Some(cell) = cell_opt { - let p = cell.to_center_point(self.cell_size); - let r = self.agent_radius as f32; - let delta = iced::Vector::new(r, -r); - let p = iced::Point::new(p.x as f32, p.y as f32) + delta; - - if bounds.contains(p) { - hud.at( - p, - |frame| { - frame.fill_text(format!("({}, {})", cell.x, cell.y).to_string()); - } - ); - } - } - } - } - } - - fn estimate_bounds(&self) -> mapf_viz::spatial_canvas::InclusionZone { - let mut zone = InclusionZone::Empty; - for endpoint in [self.start_cell, self.goal_cell] { - if let Some(cell) = endpoint { - let p = cell.to_center_point(self.cell_size); - for v in [[1.0, 1.0], [-1.0, -1.0]] { - let v: Vector = v.into(); - let r = p + self.agent_radius*v; - zone.include([r.x as f32, r.y as f32].into()); - } - } - } - - return zone; - } -} - -#[derive(Debug, Clone)] -struct SolutionVisual { - pub cell_size: f64, - pub agent_radius: f32, - pub solution_color: iced::Color, - pub solution: Option, - pub search_color: iced::Color, - pub searches: Vec, - pub obstacles: Vec<(f64, LinearTrajectorySE2)>, - pub vertex_lookup: HashMap, - tick_start: Option, - now: Option, - _msg: std::marker::PhantomData, -} - -impl SolutionVisual { - fn new( - cell_size: f64, - agent_radius: f32, - solution_color: iced::Color, - search_color: iced::Color, - ) -> Self { - - let t_obs = Trajectory::from_iter([ - WaypointSE2::new(TimePoint::from_secs_f64(0.0), 10.0, 0.0, 180_f64.to_radians()), - // WaypointSE2::new_f64(10.0, -5.0, 0.0, 180_f64.to_radians()), - WaypointSE2::new_f64(5.0, 0.0, 0.0, 180_f64.to_radians()), - WaypointSE2::new_f64(8.0, 0.0, 0.0, -90_f64.to_radians()), - WaypointSE2::new_f64(18.0, 0.0, -10.0, -90_f64.to_radians()), - ]).unwrap(); - - Self{ - cell_size, - agent_radius, - solution_color, - solution: None, - search_color, - searches: Vec::new(), - obstacles: vec![(1.0, t_obs)], - // obstacles: Vec::new(), - vertex_lookup: HashMap::new(), - tick_start: None, - now: None, - _msg: Default::default(), - } - } - - fn reset_time(&mut self) { - self.tick_start = Some(TimePoint::from_std_instant(std::time::Instant::now())); - self.now = Some(Duration::zero()); - } - - fn time_range(&self) -> Option<(TimePoint, TimePoint)> { - let mut earliest = Minimum::new(|l: &TimePoint, r: &TimePoint| l.cmp(r)); - let mut latest = Minimum::new(|l: &TimePoint, r: &TimePoint| r.cmp(l)); - - for traj in &self.searches { - earliest.consider(&traj.initial_motion_time()); - latest.consider(&traj.finish_motion_time()); - } - - if !earliest.has_value() || !latest.has_value() { - if let Some(solution) = &self.solution { - return Some((solution.initial_motion_time(), solution.finish_motion_time())); - } - - for (_, obs) in &self.obstacles { - earliest.consider(&obs.initial_motion_time()); - latest.consider(&obs.finish_motion_time()); - } - } - - if let (Some(earliest), Some(latest)) = (earliest.result(), latest.result()) { - return Some((earliest, latest)); - } - - return None; - } - - fn tick(&mut self) -> bool { - - if let Some((start, end)) = self.time_range() { - let duration = end - start; - if let Some(start) = self.tick_start { - let dt = TimePoint::from_std_instant(std::time::Instant::now()) - start; - if dt > duration + Duration::from_secs(1) { - self.reset_time(); - } else if dt > duration { - self.now = Some(duration); - } else { - self.now = Some(dt); - } - - return true; - } else { - self.reset_time(); - } - } - - return false; - } -} - -impl SpatialCanvasProgram for SolutionVisual { - fn draw_in_space( - &self, - frame: &mut canvas::Frame, - _spatial_bounds: iced::Rectangle, - _spatial_cursor: canvas::Cursor - ) { - if let Some((t0, _)) = self.time_range() { - - for trajectory in &self.searches { - draw_trajectory(frame, trajectory, self.search_color); - } - - if let Some(trajectory) = &self.solution { - draw_trajectory(frame, trajectory, self.solution_color); - } - - for (r, obs) in &self.obstacles { - let red = iced::Color::from_rgb(1.0, 0.0, 0.0); - draw_trajectory(frame, obs, red); - - if let Some(now) = self.now { - if let Ok(p) = obs.motion().compute_position(&(t0 + now)) { - draw_agent( - frame, - Point::from(p.translation.vector), - Some(p.rotation.angle()), - *r as f32, - red, - ); - } - } - } - - if let Some(now) = self.now { - for trajectory in &self.searches { - if let Ok(p) = trajectory.motion().compute_position(&(t0 + now)) { - draw_agent( - frame, - p.translation.vector.into(), - Some(p.rotation.angle()), - self.agent_radius, - self.search_color, - ); - } - } - - if let Some(trajectory) = &self.solution { - if let Ok(p) = trajectory.motion().compute_position(&(t0 + now)) { - draw_agent( - frame, - Point::from(p.translation.vector), - Some(p.rotation.angle()), - self.agent_radius, - self.solution_color, - ); - } - } - } - } - } - - fn draw_on_hud<'a, 'b: 'a>( - &self, - hud: &'a mut mapf_viz::spatial_canvas::SpatialHUD<'b>, - bound: iced::Rectangle, - _spatial_cursor: canvas::Cursor - ) { - if let Some(trajectory) = &self.solution { - let mut sequence: HashMap> = HashMap::new(); - for (i, wp) in trajectory.iter().enumerate() { - sequence.entry( - Cell::from_point( - Point::from(wp.position.translation.vector), self.cell_size - ) - ).or_default().push(i.to_string()); - } - - let r = self.agent_radius / 2_f32.sqrt(); - let delta = iced::Vector::new(r, r); - for (cell, seq) in sequence { - let p = cell.to_center_point(self.cell_size); - let p = iced::Point::new(p.x as f32, p.y as f32) + delta; - if bound.contains(p) { - hud.at( - p, - |frame| { - frame.translate([0_f32, -16_f32].into()); - frame.fill_text(seq.join(", ")); - } - ); - } - } - } - - for (cell, v) in &self.vertex_lookup { - let p = cell.to_center_point(self.cell_size); - let p = iced::Point::new(p.x as f32, p.y as f32); - if bound.contains(p) { - hud.at( - p, - |frame| { - frame.fill_text(format!("{v}")); - } - ); - } - } - } - - fn estimate_bounds(&self) -> InclusionZone { - // This layer should always be contained within the other layers, - // so we don't need to provide any bounds for this. - return InclusionZone::Empty; - } -} - -spatial_layers!(GridLayers: InfiniteGrid, SparseGridOccupancyVisual, EndpointSelector, SolutionVisual); - -type MyAlgo = AStarConnect, VisibilityGraph>>; -type TreeTicket = mapf::algorithm::tree::TreeQueueTicket>; - -struct App { - robot_size_slider: slider::State, - max_robot_radius: f32, - input: text_input::State, - input_value: String, - reset_size_button: button::State, - canvas: SpatialCanvas, - node_list_scroll: scrollable::State, - debug_text_scroll: scrollable::State, - show_details: KeyToggler, - search: Option, ()>>, - step_progress: button::State, - debug_on: bool, - memory: Vec, - debug_step_count: u64, - debug_node_selected: Option, -} - -impl App { - const TICK_COUNT: u32 = 1000; - - fn default_invalid_color() -> iced::Color { - iced::Color::from_rgb(1.0, 0.0, 0.0) - } - - fn default_endpoint_color(endpoint: Endpoint) -> iced::Color { - match endpoint { - Endpoint::Start => iced::Color::from_rgba(0.1, 0.8, 0.1, 0.9), - Endpoint::Goal => iced::Color::from_rgba(0.1, 0.1, 1.0, 0.9), - } - } - - fn default_solution_color() -> iced::Color { - iced::Color::from_rgb(1.0, 0.1, 1.0) - } - - fn default_search_color() -> iced::Color { - iced::Color::from_rgb8(191, 148, 228) - } - - fn visibility(&self) -> &SparseVisibility { - self.canvas.program.layers.1.visibility() - } - - fn grid(&self) -> &SparseGrid { - self.canvas.program.layers.1.grid() - } - - fn endpoint_selector(&self) -> &EndpointSelector { - &self.canvas.program.layers.2 - } - - fn endpoint_selector_mut(&mut self) -> &mut EndpointSelector { - &mut self.canvas.program.layers.2 - } - - fn is_valid_endpoint(&self, endpoint: Endpoint) -> bool { - if let Some(cell) = self.endpoint_selector().endpoint_cell(endpoint) { - let p = cell.to_center_point(self.grid().cell_size()); - if self.grid().is_square_occupied(p, 2.0*self.visibility().agent_radius()).is_some() { - return false; - } else { - return true; - } - } - - return false; - } - - fn set_robot_radius(&mut self, radius: f32) { - self.canvas.program.layers.1.set_robot_radius(radius); - self.canvas.program.layers.3.agent_radius = radius; - let endpoint_selector = &mut self.canvas.program.layers.2; - endpoint_selector.agent_radius = radius as f64; - for endpoint in [Endpoint::Start, Endpoint::Goal] { - if let Some(cell) = endpoint_selector.endpoint_cell(endpoint) { - let p = cell.to_center_point(endpoint_selector.cell_size); - let valid = self.canvas.program.layers.1.grid().is_square_occupied(p, 2.0*endpoint_selector.agent_radius).is_none(); - endpoint_selector.set_endpoint_valid(endpoint, valid); - } - } - self.recalculate_visibility(); - self.canvas.cache.clear(); - } - - fn recalculate_visibility(&mut self) { - self.canvas.program.layers.2.calculate_visibility(self.canvas.program.layers.1.visibility()); - } - - fn step_progress(&mut self) { - if let Some(search) = &mut self.search { - self.debug_step_count += 1; - - self.memory = search.memory().0 - .queue.clone().into_iter_sorted() - .map(|n| n.0.clone()).collect(); - - for ticket in &self.memory { - if let Some(traj) = search.memory().0 - .arena.retrace(ticket.node_id).unwrap() - .make_trajectory().unwrap() - { - self.canvas.program.layers.3.searches.push(traj); - } - } - - if let SearchStatus::Solved(solution) = search.step().unwrap() { - println!("Solution: {:#?}", solution); - self.canvas.program.layers.3.solution = solution.make_trajectory().unwrap(); - self.debug_node_selected = None; - self.search = None; - } else { - if let Some(selection) = self.debug_node_selected { - if let Some(node) = self.memory.get(selection) { - self.canvas.program.layers.3.solution = search - .memory().0.arena - .retrace(node.node_id) - .unwrap() - .make_trajectory() - .unwrap(); - } - } - } - - self.canvas.cache.clear(); - } - } - - fn generate_plan(&mut self) { - self.canvas.program.layers.3.solution = None; - let endpoints = &self.canvas.program.layers.2; - let visibility = self.canvas.program.layers.1.visibility(); - if let (Some(start_cell), Some(goal_cell)) = (endpoints.start_cell, endpoints.goal_cell) { - if start_cell == goal_cell { - // No plan is needed - return; - } - - if endpoints.start_valid && endpoints.goal_valid { - self.canvas.program.layers.3.vertex_lookup.clear(); - - let shared_visibility = Arc::new(visibility.clone()); - let heuristic_graph = SharedGraph::new(VisibilityGraph::new( - shared_visibility.clone(), [], - )); - - let activity_graph = SharedGraph::new(NeighborhoodGraph::new( - shared_visibility, [], - )); - - let extrapolator = DifferentialDriveLineFollow::new(3.0, 1.0).expect("Bad speeds"); - let agent_radius = self.canvas.program.layers.2.agent_radius; - let profile = CircularProfile::new( - agent_radius, agent_radius, agent_radius, - ).expect("Bad profile sizes"); - - let environment = Arc::new( - OverlayedDynamicEnvironment::new( - Arc::new({ - let mut env = DynamicEnvironment::new(profile); - for (obs_size, obs_traj) in &self.canvas.program.layers.3.obstacles { - env.obstacles.push( - DynamicCircularObstacle::new( - CircularProfile::new(*obs_size, 0.0, 0.0).unwrap() - ).with_trajectory(Some(obs_traj.clone())) - ); - } - env - }) - ) - ); - - let domain = InformedSearch::new_sipp_se2( - activity_graph, - heuristic_graph, - extrapolator, - environment, - TravelEffortCost::save_one_second_with_detour_up_to( - 5.0, - 360_f64.to_radians(), - ), - ).unwrap(); - - let start = StartSE2 { - time: TimePoint::from_secs_f64(0.0), - key: start_cell, - orientation: Orientation::new(0_f64), - }; - - let goal = GoalSE2 { - key: goal_cell, - orientation: None, - }; - - println!("About to plan:\nStart: {start:#?}\nGoal: {goal:#?}"); - - let start_time = std::time::Instant::now(); - let planner = Planner::new(AStarConnect(domain)); - let mut search = planner.plan(start, goal).unwrap(); - - self.canvas.program.layers.3.searches.clear(); - self.debug_step_count = 0; - if self.debug_on { - self.search = Some(search); - self.memory = self.search.as_ref().unwrap().memory().0 - .queue.clone().into_iter_sorted() - .map(|n| n.0.clone()).collect(); - } else { - let result = search.solve().unwrap(); - let elapsed_time = start_time.elapsed(); - match result { - SearchStatus::Solved(solution) => { - - println!("Solution: {:#?}", solution); - println!(" ======================= "); - self.canvas.program.layers.3.solution = solution.make_trajectory().unwrap(); - println!(" ======================= "); - println!("Trajectory: {:#?}", self.canvas.program.layers.3.solution); - println!( - "Arrival time: {:?}, cost: {:?}", - self.canvas.program.layers.3.solution.as_ref() - .map(|t| t.motion_duration().as_secs_f64()).unwrap_or(0.0), - solution.total_cost.0, - ); - self.canvas.cache.clear(); - }, - SearchStatus::Impossible => { - println!("Impossible to solve!"); - }, - SearchStatus::Incomplete => { - println!("Planning is incomplete..?"); - } - } - println!("Planning took {:?}s", elapsed_time.as_secs_f64()); - } - self.debug_node_selected = None; - - return; - } - } - - // If the attempt to plan falls through, then clear out all these fields. - self.memory.clear(); - self.search = None; - self.canvas.program.layers.3.solution = None; - self.canvas.program.layers.3.searches.clear(); - self.canvas.cache.clear(); - } -} - -impl Application for App { - type Message = Message; - type Executor = executor::Default; - type Flags = (); - - fn new(_flags: Self::Flags) -> (Self, Command) { - let cell_size = 1.0_f32; - let robot_radius = 0.75_f32; - - let mut canvas = SpatialCanvas::new( - GridLayers{ - layers: ( - InfiniteGrid::new(cell_size), - SparseGridOccupancyVisual::new( - SparseGrid::new(cell_size as f64), - robot_radius, - None, - Some(Box::new(|| { Message::OccupancyChanged })), - ), - EndpointSelector::new( - robot_radius as f64, - cell_size as f64, - Self::default_invalid_color(), - Self::default_endpoint_color(Endpoint::Start), - Self::default_endpoint_color(Endpoint::Goal), - ), - SolutionVisual::new( - cell_size as f64, - robot_radius, - Self::default_solution_color(), - Self::default_search_color(), - ), - ) - } - ); - canvas.zoom = 20.0; - - ( - Self{ - robot_size_slider: slider::State::new(), - max_robot_radius: 10_f32*cell_size, - input: text_input::State::default(), - input_value: String::new(), - reset_size_button: button::State::new(), - canvas, - node_list_scroll: scrollable::State::new(), - debug_text_scroll: scrollable::State::new(), - show_details: KeyToggler::for_key(keyboard::KeyCode::LAlt), - search: None, - step_progress: button::State::new(), - debug_on: false, - memory: Default::default(), - debug_step_count: 0, - debug_node_selected: None, - }, - Command::none() - ) - } - - fn title(&self) -> String { - "Test App".to_owned() - } - - fn update( - &mut self, - message: Self::Message - ) -> Command { - match message { - Message::TextInputChanged(value) => { - self.input_value = value; - if let Ok(radius) = self.input_value.parse::() { - if radius > 0.0 { - self.max_robot_radius = radius; - - let current_robot_radius = self.canvas.program.layers.1.visibility().agent_radius() as f32; - if self.max_robot_radius < current_robot_radius { - self.set_robot_radius(radius); - } - } - } - }, - Message::RobotSizeSlide(value) => { - let min_size = self.canvas.program.layers.1.grid().cell_size() as f32/10_f32; - let new_robot_radius = value as f32 * (self.max_robot_radius - min_size)/(Self::TICK_COUNT as f32) + min_size; - self.set_robot_radius(new_robot_radius); - self.generate_plan(); - }, - Message::ResetView => { - self.canvas.fit_to_bounds(); - }, - Message::EventOccurred(event) => { - if let iced_native::Event::Keyboard(event) = event { - match self.show_details.key_toggle(event) { - Toggle::On => { - self.canvas.program.layers.1.show_details(true); - self.canvas.program.layers.2.show_details = true; - self.canvas.cache.clear(); - }, - Toggle::Off => { - self.canvas.program.layers.1.show_details(false); - self.canvas.program.layers.2.show_details = false; - self.canvas.cache.clear(); - }, - Toggle::NoChange => { - // Do nothing - } - } - - if let keyboard::Event::KeyPressed{key_code: keyboard::KeyCode::D, modifiers} = event { - if modifiers.shift() { - self.debug_on = false; - self.generate_plan(); - } else { - self.debug_on = true; - self.generate_plan(); - } - } - - if let keyboard::Event::KeyPressed{key_code: keyboard::KeyCode::S, ..} = event { - self.step_progress(); - } - - if let keyboard::Event::KeyPressed { key_code: keyboard::KeyCode::Down, .. } = event { - let next_debug_node = self.debug_node_selected.map(|n| n+1).unwrap_or(0); - return Command::perform(async move {}, move |_| { - Message::DebugNodeSelected(next_debug_node) - }); - } - - if let keyboard::Event::KeyPressed { key_code: keyboard::KeyCode::Up, .. } = event { - let next_debug_node = self.debug_node_selected.map(|n| if n > 0 { n-1 } else { 0 }).unwrap_or(0); - return Command::perform(async move {}, move |_| { - Message::DebugNodeSelected(next_debug_node) - }); - } - } - }, - Message::EndpointSelected(selection) => { - match selection { - EndpointSelection::Cell(endpoint) => { - let valid = self.is_valid_endpoint(endpoint); - self.endpoint_selector_mut().set_endpoint_valid( - endpoint, valid - ); - self.recalculate_visibility(); - }, - EndpointSelection::Orientation(_) => { - // Do nothing - }, - EndpointSelection::Finalize(_) => { - self.generate_plan(); - } - } - }, - Message::OccupancyChanged => { - for endpoint in [Endpoint::Start, Endpoint::Goal] { - let cell_opt = self.canvas.program.layers.2.endpoint_cell(endpoint); - if let Some(cell) = cell_opt { - let p = cell.to_center_point(self.grid().cell_size()); - let valid = self.grid().is_square_occupied(p, 2.0*self.visibility().agent_radius()).is_none(); - self.endpoint_selector_mut().set_endpoint_valid(endpoint, valid); - self.recalculate_visibility(); - } - } - - self.generate_plan(); - }, - Message::DebugNodeSelected(value) => { - if self.memory.is_empty() { - self.debug_node_selected = Some(0); - } else { - self.debug_node_selected = Some(usize::min(value, self.memory.len() - 1)); - } - - if let Some(node) = self.memory.get(value) { - if let Some(search) = &self.search { - self.canvas.program.layers.3.solution = search - .memory().0.arena - .retrace(node.node_id) - .unwrap() - .make_trajectory() - .unwrap(); - self.canvas.cache.clear(); - } - } - }, - Message::StepProgress => { - self.step_progress(); - } - Message::Tick => { - if self.canvas.program.layers.3.tick() { - self.canvas.cache.clear(); - } - } - } - - Command::none() - } - - fn subscription(&self) -> iced::Subscription { - iced_native::Subscription::batch([ - iced_native::subscription::events().map(Message::EventOccurred), - iced::time::every(std::time::Duration::from_millis(50)).map(|_|{ Message::Tick }), - ]) - } - - fn view(&mut self) -> Element { - let mut content = Column::new() - .spacing(20) - .align_items(Alignment::Start) - .width(Length::Fill) - .height(Length::Fill); - - let min_size = self.canvas.program.layers.1.grid().cell_size() as f32/10_f32; - let robot_radius = self.canvas.program.layers.1.visibility().agent_radius() as f32; - let robot_tick = ((robot_radius - min_size)/(self.max_robot_radius - min_size) * Self::TICK_COUNT as f32) as u32; - let file_row = Row::new() - .spacing(20) - .align_items(Alignment::Center) - .width(Length::Fill) - .push( - Button::new( - &mut self.reset_size_button, - iced::Text::new("Reset View") - ).on_press(Message::ResetView) - ) - .push( - Slider::new( - &mut self.robot_size_slider, - 0..=Self::TICK_COUNT, - robot_tick, - Message::RobotSizeSlide, - ) - ) - .push(Text::new(format!("{:.2}", robot_radius))) - .push( - TextInput::new( - &mut self.input, - "Max Robot Radius", - &mut self.input_value, - Message::TextInputChanged - ) - .padding(10) - .width(Length::Fill) - ); - - let instruction_row = Row::::new() - .spacing(40) - .align_items(Alignment::Start) - .width(Length::Shrink) - .push(Text::new("Left click: Add occupancy")) - .push(Text::new("Shift + Left click: Remove occupancy")) - .push(Text::new("Middle click: Pan view")) - .push(Text::new("Scroll: Zoom")); - - content = content - .push(file_row) - .push(instruction_row); - - if self.debug_on { - content = content.push( - Row::new() - .push(self.canvas.view()) - .push( - Column::new() - .push( - Row::new() - .push( - Button::new(&mut self.step_progress, Text::new("Step")) - .on_press(Message::StepProgress) - ) - .push(iced::Space::with_width(Length::Units(16))) - .push(Text::new(format!("Steps: {}", self.debug_step_count))) - .push(iced::Space::with_width(Length::Units(16))) - .push(Text::new(format!("Queue size: {}", self.memory.len()))) - .align_items(Alignment::Center) - ) - .push({ - let mut scroll = Scrollable::::new(&mut self.node_list_scroll); - if let Some(search) = &self.search { - for (i, ticket) in self.memory.iter().enumerate() { - let node = search.memory().0.arena.get(ticket.node_id).unwrap(); - scroll = scroll.push(Radio::new( - i, format!( - "{i}: {:?} + {:?} = {:?}", - node.cost().0, - node.remaining_cost_estimate().0, - ticket.evaluation.0, - ), self.debug_node_selected, - Message::DebugNodeSelected - )); - } - } - scroll - }.height(Length::Fill)) - .push({ - Scrollable::::new(&mut self.debug_text_scroll) - .push( - Text::new({ - if let Some(selection) = self.debug_node_selected { - if let Some(node) = self.memory.get(selection) { - if let Some(search) = &self.search { - let node = search.memory().0.arena.get(node.node_id).unwrap(); - format!("{node:#?}") - } else { - String::new() - } - } else { - String::new() - } - } else { - String::new() - } - }).size(10) - ) - }.height(Length::Fill)) - ) - ); - } else { - content = content.push(self.canvas.view()); - } - - Container::new(content) - .width(Length::Fill) - .height(Length::Fill) - .padding(5) - .center_x() - .center_y() - .into() - } -} - -#[derive(Debug, Clone)] -enum Message { - TextInputChanged(String), - RobotSizeSlide(u32), - ResetView, - EventOccurred(iced_native::Event), - EndpointSelected(EndpointSelection), - OccupancyChanged, - DebugNodeSelected(usize), - StepProgress, - Tick, -} - -fn main() -> iced::Result { - App::run(iced::Settings::default()) -} - -mod style { - use iced::container; - pub struct Pane; - impl container::StyleSheet for Pane { - fn style(&self) -> container::Style { - container::Style{ - text_color: None, - background: None, - border_radius: 0.0, - border_width: 3.0, - border_color: iced::Color{r: 0.9, g: 0.9, b: 0.9, a: 0.9}, - } - } - } -} diff --git a/mapf-viz/examples/occupancy.rs b/mapf-viz/examples/occupancy.rs deleted file mode 100644 index 41512a8..0000000 --- a/mapf-viz/examples/occupancy.rs +++ /dev/null @@ -1,319 +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::{ - Application, Alignment, Element, Length, Command, Column, Row, Text, Container, - slider::{self, Slider}, - scrollable::{self, Scrollable}, - text_input::{self, TextInput}, - button::{self, Button}, - executor, keyboard, -}; -use iced_native; -use mapf::graph::occupancy::{Grid, SparseGrid, Cell, CornerStatus}; -use mapf_viz::{ - SparseGridOccupancyVisual, InfiniteGrid, - spatial_canvas::SpatialCanvas, - toggle::{Toggle, KeyToggler}, -}; -use mapf_viz::spatial_layers; -use std::collections::{HashSet, HashMap}; - -spatial_layers!(GridLayers: InfiniteGrid, SparseGridOccupancyVisual); - -struct App { - robot_size_slider: slider::State, - max_robot_radius: f32, - input: text_input::State, - input_value: String, - reset_size_button: button::State, - canvas: SpatialCanvas, - scroll: scrollable::State, - show_details: KeyToggler, - debug_text: String, - debug_corners: HashSet, -} - -impl App { - const TICK_COUNT: u32 = 1000; - - fn set_debug_text(&mut self) { - self.debug_text.clear(); - let occupancy = &self.canvas.program.layers.1; - let mut corner_info_map: HashMap, CornerStatus)> = HashMap::new(); - for (corner_cell, info) in occupancy.visibility().unstable().points() { - if self.debug_corners.contains(corner_cell) { - corner_info_map.insert(*corner_cell, *info); - } - } - - let mut edge_info_map: HashMap>> = HashMap::new(); - let mut reverse_edge_info_map: HashMap>> = HashMap::new(); - for (cell, info) in occupancy.visibility().unstable().edges() { - if self.debug_corners.contains(cell) { - edge_info_map.insert(*cell, info.clone()); - } else { - for (other, _) in info { - if self.debug_corners.contains(other) { - reverse_edge_info_map.insert(*cell, info.clone()); - } - } - } - } - - self.debug_text = format!( - "Point info:\n{:#?}\n\nEdge Info:\n{:#?}\n\nReverse Edge Info:\n{:#?}", - corner_info_map, - edge_info_map, - reverse_edge_info_map, - ); - } -} - -impl Application for App { - type Message = Message; - type Executor = executor::Default; - type Flags = (); - - fn new(_flags: Self::Flags) -> (Self, Command) { - let cell_size = 1.0_f32; - let robot_radius = 0.75_f32; - - let mut canvas = SpatialCanvas::new( - GridLayers{ - layers: ( - InfiniteGrid::new(cell_size), - SparseGridOccupancyVisual::new( - SparseGrid::new(cell_size as f64), - robot_radius, - Some(Box::new(Message::CornerSelected)), - None, - ), - ) - } - ); - canvas.zoom = 20.0; - - ( - Self{ - robot_size_slider: slider::State::new(), - max_robot_radius: 10_f32*cell_size, - input: text_input::State::default(), - input_value: String::new(), - reset_size_button: button::State::new(), - canvas, - scroll: scrollable::State::new(), - show_details: KeyToggler::for_key(keyboard::KeyCode::LAlt), - debug_text: Default::default(), - debug_corners: Default::default(), - }, - Command::none() - ) - } - - fn title(&self) -> String { - "Test App".to_owned() - } - - fn update( - &mut self, - message: Self::Message - ) -> Command { - match message { - Message::TextInputChanged(value) => { - self.input_value = value; - if let Ok(radius) = self.input_value.parse::() { - if radius > 0.0 { - self.max_robot_radius = radius; - - let current_robot_radius = self.canvas.program.layers.1.visibility().agent_radius() as f32; - if self.max_robot_radius < current_robot_radius { - self.canvas.program.layers.1.set_robot_radius(radius); - self.canvas.cache.clear(); - } - } - } - }, - Message::RobotSizeSlide(value) => { - let min_size = self.canvas.program.layers.1.grid().cell_size() as f32/10_f32; - let new_robot_radius = value as f32 * (self.max_robot_radius - min_size)/(Self::TICK_COUNT as f32) + min_size; - self.canvas.program.layers.1.set_robot_radius(new_robot_radius); - self.canvas.cache.clear(); - }, - Message::ResetView => { - self.canvas.fit_to_bounds(); - }, - Message::EventOccurred(event) => { - if let iced_native::Event::Keyboard(event) = event { - if let iced_native::keyboard::Event::KeyPressed{key_code, modifiers} = event { - if keyboard::KeyCode::D == key_code { - if modifiers.shift() { - self.debug_text.clear(); - } else { - self.debug_text = format!( - "Debug visibility:\n{:#?}", - self.canvas.program.layers.1.visibility(), - ); - } - } - } - - match self.show_details.key_toggle(event) { - Toggle::On => { - self.canvas.program.layers.1.show_details(true); - self.canvas.cache.clear(); - }, - Toggle::Off => { - self.canvas.program.layers.1.show_details(false); - self.canvas.cache.clear(); - }, - Toggle::NoChange => { - // Do nothing - } - } - } - }, - Message::CornerSelected(cell, selected) => { - if selected { - if self.debug_corners.insert(cell) { - self.canvas.program.layers.1.special_visibility_color - .insert(cell, iced::Color::from_rgb(1.0, 0.0, 0.0)); - self.canvas.cache.clear(); - self.set_debug_text(); - } - } else { - if self.debug_corners.remove(&cell) { - self.canvas.program.layers.1.special_visibility_color.remove(&cell); - self.canvas.cache.clear(); - self.set_debug_text(); - } - } - } - } - - Command::none() - } - - fn subscription(&self) -> iced::Subscription { - iced_native::subscription::events().map(Message::EventOccurred) - } - - fn view(&mut self) -> Element { - let mut content = Column::new() - .spacing(20) - .align_items(Alignment::Start) - .width(Length::Fill) - .height(Length::Fill); - - let min_size = self.canvas.program.layers.1.grid().cell_size() as f32/10_f32; - let robot_radius = self.canvas.program.layers.1.visibility().agent_radius() as f32; - let robot_tick = ((robot_radius - min_size)/(self.max_robot_radius - min_size) * Self::TICK_COUNT as f32) as u32; - let file_row = Row::new() - .spacing(20) - .align_items(Alignment::Center) - .width(Length::Fill) - .push( - Button::new( - &mut self.reset_size_button, - iced::Text::new("Reset View") - ).on_press(Message::ResetView) - ) - .push( - Slider::new( - &mut self.robot_size_slider, - 0..=Self::TICK_COUNT, - robot_tick, - Message::RobotSizeSlide, - ) - ) - .push(Text::new(format!("{:.2}", robot_radius))) - .push( - TextInput::new( - &mut self.input, - "Max Robot Radius", - &mut self.input_value, - Message::TextInputChanged - ) - .padding(10) - .width(Length::Fill) - ); - - let instruction_row = Row::::new() - .spacing(40) - .align_items(Alignment::Start) - .width(Length::Shrink) - .push(Text::new("Left click: Add occupancy")) - .push(Text::new("Shift + Left click: Remove occupancy")) - .push(Text::new("Middle click: Pan view")) - .push(Text::new("Scroll: Zoom")); - - content = content - .push(file_row) - .push(instruction_row); - - if self.debug_text.is_empty() { - content = content.push(self.canvas.view()); - } else { - - content = content.push( - Row::new() - .push(self.canvas.view()) - .push(Scrollable::new(&mut self.scroll).push( - Text::new(&self.debug_text) - .size(13) - )) - ); - } - - Container::new(content) - .width(Length::Fill) - .height(Length::Fill) - .padding(5) - .center_x() - .center_y() - .into() - } -} - -#[derive(Debug, Clone)] -enum Message { - TextInputChanged(String), - RobotSizeSlide(u32), - ResetView, - EventOccurred(iced_native::Event), - CornerSelected(Cell, bool), -} - -fn main() -> iced::Result { - App::run(iced::Settings::default()) -} - -mod style { - use iced::container; - pub struct Pane; - impl container::StyleSheet for Pane { - fn style(&self) -> container::Style { - container::Style{ - text_color: None, - background: None, - border_radius: 0.0, - border_width: 3.0, - border_color: iced::Color{r: 0.9, g: 0.9, b: 0.9, a: 0.9}, - } - } - } -} diff --git a/mapf-viz/scenarios/T-crossing.yaml b/mapf-viz/scenarios/T-crossing.yaml new file mode 100644 index 0000000..994978c --- /dev/null +++ b/mapf-viz/scenarios/T-crossing.yaml @@ -0,0 +1,88 @@ +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 new file mode 100644 index 0000000..66cdb0c --- /dev/null +++ b/mapf-viz/scenarios/sizes.yaml @@ -0,0 +1,56 @@ +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 new file mode 100644 index 0000000..d3144d2 --- /dev/null +++ b/mapf-viz/scenarios/warehouse.yaml @@ -0,0 +1,868 @@ +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 new file mode 100644 index 0000000..c473880 --- /dev/null +++ b/mapf-viz/src/accessibility_visual.rs @@ -0,0 +1,243 @@ +/* + * 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/lib.rs b/mapf-viz/src/lib.rs index 5a2c0df..913c139 100644 --- a/mapf-viz/src/lib.rs +++ b/mapf-viz/src/lib.rs @@ -18,8 +18,11 @@ pub mod spatial_canvas; pub use spatial_canvas::SpatialCanvasProgram; -pub mod occupancy; -pub use occupancy::{OccupancyVisual, SparseGridOccupancyVisual}; +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; diff --git a/mapf-viz/src/spatial_canvas.rs b/mapf-viz/src/spatial_canvas.rs index a4a9e20..92ff41c 100644 --- a/mapf-viz/src/spatial_canvas.rs +++ b/mapf-viz/src/spatial_canvas.rs @@ -106,6 +106,11 @@ impl InclusionZone { } } + 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, @@ -158,16 +163,26 @@ impl> SpatialCanvas bool { + self.fit_to_zone(self.program.estimate_bounds()) } - pub fn fit_to_zone(&mut self, zone: InclusionZone) { - let bounds = if let Some(bounds) = &mut self.bounds { - bounds - } else { - return; - }; + 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); @@ -181,6 +196,8 @@ impl> SpatialCanvas(&'a mut self) -> Element<'a, Message> diff --git a/mapf-viz/src/occupancy.rs b/mapf-viz/src/visibility_visual.rs similarity index 79% rename from mapf-viz/src/occupancy.rs rename to mapf-viz/src/visibility_visual.rs index 61e12ac..b150dea 100644 --- a/mapf-viz/src/occupancy.rs +++ b/mapf-viz/src/visibility_visual.rs @@ -32,9 +32,9 @@ use std::collections::HashMap; #[derive(Derivative)] #[derivative(Debug)] -pub struct OccupancyVisual { +pub struct VisibilityVisual { #[derivative(Debug = "ignore")] - occupancy: Visibility, + 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 @@ -57,10 +57,10 @@ pub struct OccupancyVisual { #[derivative(Debug = "ignore")] pub on_occupancy_change: Option Message>>, - _msg: std::marker::PhantomData, + _ignore: std::marker::PhantomData, } -impl OccupancyVisual { +impl VisibilityVisual { pub fn new( grid: G, robot_radius: f32, @@ -68,71 +68,55 @@ impl OccupancyVisual { on_occupancy_change: Option Message>>, ) -> Self { Self { - occupancy: { - let mut vis = Visibility::new(grid, robot_radius as f64); - // vis.change_cells( - // &(-10..=-1_i64) - // .into_iter() - // .map(|y| (Cell::new(3, y), true)) - // .collect() - // ); - // vis.change_cells(&[(Cell::new(5, 0), true)].into_iter().collect()); - - // vis.change_cells( - // &(-10..10).into_iter() - // .map(|y| (Cell::new(5, y), true)) - // .collect() - // ); - - for x in [-3, 1] { - vis.change_cells( - &(-14..=-1) - .into_iter() - .map(|y| (Cell::new(x, y), true)) - .collect(), - ); - } - vis - }, + 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(Some(mouse::Button::Left), None), DragToggler::new( None, Some((keyboard::Modifiers::SHIFT, mouse::Button::Left)), ), - ))), - corner_select_toggler: Some(Box::new(FillToggler::new( - DragToggler::new(Some(mouse::Button::Right), None), 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, - _msg: Default::default(), + _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.occupancy.change_agent_radius(radius as f64); + self.visibility.change_agent_radius(radius as f64); } pub fn visibility(&self) -> &Visibility { - &self.occupancy + &self.visibility } pub fn visibility_mut(&mut self) -> &mut Visibility { - &mut self.occupancy + &mut self.visibility } pub fn grid(&self) -> &G { - &self.occupancy.grid() + &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 @@ -145,14 +129,14 @@ impl OccupancyVisual { if let Some(cell_toggler) = &self.cell_toggler { let cell = Cell::from_point( [p.x as f64, p.y as f64].into(), - self.occupancy.grid().cell_size(), + self.visibility.grid().cell_size(), ); match cell_toggler.state() { Toggle::On => { - return self.occupancy.change_cells(&[(cell, true)].into()); + return self.visibility.change_cells(&[(cell, true)].into()); } Toggle::Off => { - return self.occupancy.change_cells(&[(cell, false)].into()); + return self.visibility.change_cells(&[(cell, false)].into()); } Toggle::NoChange => { return false; @@ -165,11 +149,11 @@ impl OccupancyVisual { fn find_closest(&self, p: iced::Point) -> Option { let mut closest: Option<(Cell, f64)> = None; - let r = self.occupancy.agent_radius(); + let r = self.visibility.agent_radius(); let p = Point::new(p.x as f64, p.y as f64); - for (cell, _) in self.occupancy.iter_points() { - let p_cell = cell.to_center_point(self.grid().cell_size()); + for (cell, _) in self.visibility.iter_points() { + let p_cell = cell.center_point(self.grid().cell_size()); let dist = (p_cell - p).norm(); if dist <= r { if let Some((_, old_dist)) = closest { @@ -186,7 +170,7 @@ impl OccupancyVisual { } } -impl SpatialCanvasProgram for OccupancyVisual { +impl SpatialCanvasProgram for VisibilityVisual { fn update( &mut self, event: Event, @@ -239,10 +223,10 @@ impl SpatialCanvasProgram for OccupancyVisual SpatialCanvasProgram for OccupancyVisual SpatialCanvasProgram for OccupancyVisual SpatialCanvasProgram for OccupancyVisual SpatialCanvasProgram for OccupancyVisual InclusionZone { let mut zone = InclusionZone::Empty; - let r = self.occupancy.agent_radius() as f32; - for (cell, _) in self.occupancy.iter_points() { - let p = cell.to_center_point(self.occupancy.grid().cell_size()); + 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); @@ -387,4 +371,4 @@ impl SpatialCanvasProgram for OccupancyVisual = OccupancyVisual; +pub type SparseGridVisibilityVisual = VisibilityVisual; diff --git a/mapf/Cargo.toml b/mapf/Cargo.toml index afbe370..b14771c 100644 --- a/mapf/Cargo.toml +++ b/mapf/Cargo.toml @@ -26,6 +26,9 @@ thiserror = "*" paste = "0.1" float-ord = "*" smallvec = "1.10" +serde = { version="1.0", features = ["derive"] } +serde_yaml = "*" +slotmap = "1.0" [dev-dependencies] approx = "*" diff --git a/mapf/src/algorithm/a_star.rs b/mapf/src/algorithm/a_star.rs index b3aeb00..1143e17 100644 --- a/mapf/src/algorithm/a_star.rs +++ b/mapf/src/algorithm/a_star.rs @@ -17,7 +17,7 @@ use crate::{ algorithm::{ - tree::*, Algorithm, Coherent, Measure, MinimumCostBound, Path, SearchStatus, Solvable, + tree::*, Algorithm, Coherent, MinimumCostBound, Path, QueueLength, SearchStatus, Solvable, }, domain::{ Activity, Closable, CloseResult, ClosedSet, Configurable, Connectable, Domain, Informed, @@ -50,12 +50,12 @@ pub struct AStarConnect(pub D); #[derive(Debug)] pub struct Memory(pub Tree, Cost>); -impl Measure for Memory +impl QueueLength for Memory where Cost: Clone + Add, { - fn size(&self) -> usize { - self.0.size() + fn queue_length(&self) -> usize { + self.0.queue_length() } } @@ -338,10 +338,9 @@ where impl Configurable for AStar { type Configuration = D::Configuration; - type ConfigurationError = D::ConfigurationError; - fn configure(self, f: F) -> Result + fn configure(self, f: F) -> Result where - F: FnOnce(Self::Configuration) -> Self::Configuration, + F: FnOnce(Self::Configuration) -> Result, { Ok(AStar(self.0.configure(f)?)) } @@ -436,10 +435,9 @@ where impl Configurable for AStarConnect { type Configuration = D::Configuration; - type ConfigurationError = D::ConfigurationError; - fn configure(self, f: F) -> Result + fn configure(self, f: F) -> Result where - F: FnOnce(Self::Configuration) -> Self::Configuration, + F: FnOnce(Self::Configuration) -> Result, { Ok(AStarConnect(self.0.configure(f)?)) } @@ -461,6 +459,10 @@ impl Node { pub fn remaining_cost_estimate(&self) -> &Cost { &self.remaining_cost_estimate } + + pub fn state(&self) -> &State { + &self.state + } } impl TreeNode for Node diff --git a/mapf/src/algorithm/dijkstra/backward.rs b/mapf/src/algorithm/dijkstra/backward.rs index 8c696f4..1087238 100644 --- a/mapf/src/algorithm/dijkstra/backward.rs +++ b/mapf/src/algorithm/dijkstra/backward.rs @@ -27,7 +27,7 @@ use crate::{ Activity, ArrivalKeyring, Backtrack, Closable, ClosedStatusForKey, Configurable, Connectable, Domain, Initializable, Keyed, Keyring, Reversible, Weighted, }, - error::ThisError, + error::{Anyhow, ThisError}, }; use std::ops::Add; @@ -161,22 +161,21 @@ where + Activity + Weighted + Closable, + D::ReversalError: Into, { type Configuration = D::Configuration; - type ConfigurationError = BackwardConfigurationError; - fn configure(self, f: F) -> Result + fn configure(self, f: F) -> Result where - F: FnOnce(Self::Configuration) -> Self::Configuration, + F: FnOnce(Self::Configuration) -> Result, { let configured = self .backward .domain() .reversed() - .map_err(BackwardConfigurationError::Reverse)? - .configure(f) - .map_err(BackwardConfigurationError::Configure)?; + .map_err(Into::into)? + .configure(f)?; - Self::new(&configured).map_err(BackwardConfigurationError::Reverse) + Self::new(&configured).map_err(Into::into) } } @@ -270,7 +269,7 @@ mod tests { .unwrap(), )); - for i in 0..=8 { + for i in 0..=8usize { for angle in [0.0, 90.0, 180.0, 30.0, -140.0_f64] { let start_key = KeySE2::new(i, angle.to_radians()); let result = planner.plan(start_key, i).unwrap().solve().unwrap(); diff --git a/mapf/src/algorithm/dijkstra/forward.rs b/mapf/src/algorithm/dijkstra/forward.rs index 786e2f5..78a412e 100644 --- a/mapf/src/algorithm/dijkstra/forward.rs +++ b/mapf/src/algorithm/dijkstra/forward.rs @@ -22,7 +22,7 @@ use crate::{ ClosedStatusForKey, Configurable, Connectable, Domain, Initializable, Keyed, Keyring, Weighted, }, - error::ThisError, + error::{Anyhow, ThisError}, }; use std::{ borrow::Borrow, @@ -483,10 +483,9 @@ where + Closable, { type Configuration = D::Configuration; - type ConfigurationError = D::ConfigurationError; - fn configure(self, f: F) -> Result + fn configure(self, f: F) -> Result where - F: FnOnce(Self::Configuration) -> Self::Configuration, + F: FnOnce(Self::Configuration) -> Result, { // We have to assume that all caches are invalidated when the domain // gets configured. diff --git a/mapf/src/algorithm/mod.rs b/mapf/src/algorithm/mod.rs index 346dcdb..e7673dc 100644 --- a/mapf/src/algorithm/mod.rs +++ b/mapf/src/algorithm/mod.rs @@ -141,14 +141,13 @@ impl> Solvable for Arc { } } -/// The `Measure` trait can be implemented by `Algorithm::Memory` types to -/// provide an indication of how large their current level of effort or memory -/// footprint is. This may be used to halt search efforts that have grown -/// excessively large. -pub trait Measure { +/// The [`QueueLength`] trait can be implemented by `Algorithm::Memory` types to +/// provide an indication of how large the remaining search queue is. This may +/// be used to halt search efforts that have grown excessively large. +pub trait QueueLength { /// How "big" is the current memory footprint or level of effort. The exact /// meaning of this value may vary between algorithms. - fn size(&self) -> usize; + fn queue_length(&self) -> usize; } /// The `MinimumCostBound` trait can be implemented by `Algorithm::Memory` types diff --git a/mapf/src/algorithm/path.rs b/mapf/src/algorithm/path.rs index 7d4d16a..0e6f6d1 100644 --- a/mapf/src/algorithm/path.rs +++ b/mapf/src/algorithm/path.rs @@ -18,8 +18,9 @@ use crate::{ domain::{Backtrack, Domain}, error::ThisError, - motion::{IntegrateWaypoints, Trajectory, Waypoint}, + motion::{Duration, IntegrateWaypoints, TimePoint, Timed, Trajectory, Waypoint}, }; +use smallvec::SmallVec; #[derive(Debug, Clone)] pub struct Path { @@ -73,16 +74,33 @@ impl Path { }) } + pub fn final_state(&self) -> &S { + self.sequence + .last() + .map(|(_, s)| s) + .unwrap_or(&self.initial_state) + } + + /// Make a trajectory out of this path if possible. It might not be possible + /// to make a trajectory if the path does not involve any motion. + /// + /// To always get a trajectory out of the path (provided there are no errors), + /// then use [`make_trajectory_or_hold`]. pub fn make_trajectory( &self, ) -> Result< - Option>, + Option>, WaypointIntegrationError, > where - S: IntegrateWaypoints, - A: IntegrateWaypoints, + S: IntegrateWaypoints + Clone + std::fmt::Debug + Timed, + A: IntegrateWaypoints + std::fmt::Debug, { + let mut decision_points = Vec::new(); + decision_points.push(DecisionPoint { + index: 0, + state: self.initial_state.clone(), + }); let mut waypoints = self .initial_state .integrated_waypoints(None) @@ -91,7 +109,8 @@ impl Path { .map_err(WaypointIntegrationError::State)?; let mut initial_wp = waypoints.last().cloned(); - for (action, _) in self.sequence.iter() { + for (action, state) in self.sequence.iter() { + let wp_count = waypoints.len(); waypoints.extend( action .integrated_waypoints(initial_wp) @@ -100,10 +119,204 @@ impl Path { .map_err(WaypointIntegrationError::Action)?, ); + waypoints.sort_by_key(|w| w.time()); + waypoints.dedup_by_key(|w| w.time()); + initial_wp = waypoints.last().cloned(); + if wp_count < waypoints.len() { + // dbg!((waypoints.len(), action, state)); + assert_eq!(waypoints.last().unwrap().time(), state.time()); + decision_points.push(DecisionPoint { + index: waypoints.len() - 1, + state: state.clone(), + }); + } + } + + Ok(Trajectory::from_iter(waypoints) + .ok() + .map(|trajectory| MetaTrajectory { + trajectory, + decision_points, + initial_state: self.initial_state.clone(), + final_state: self.final_state().clone(), + })) + } + + /// Make a trajectory out of this path if possible. If producing a path is + /// not possible because no motion is necessary, then creating a trajectory + /// that holds the agent in place at the start location for the given + /// `hold_duration`. + /// + /// If an initial waypoint cannot be drawn out of the initial state, then + /// this will return an error instead of not returning a trajectory. + /// + /// If `hold_duration` is zero, 1ns will be used instead. + pub fn make_trajectory_or_hold( + &self, + hold_duration: Duration, + ) -> Result< + MetaTrajectory, + WaypointIntegrationError, + > + where + S: IntegrateWaypoints + Clone + std::fmt::Debug + Timed, + A: IntegrateWaypoints + std::fmt::Debug, + { + if let Some(mt) = self.make_trajectory()? { + return Ok(mt); } - Ok(Trajectory::from_iter(waypoints).ok()) + let wp0: W = match self + .initial_state + .integrated_waypoints(None) + .into_iter() + .last() + { + Some(r) => r.map_err(WaypointIntegrationError::State)?, + None => return Err(WaypointIntegrationError::EmptyInitialState), + }; + + let hold_duration = if hold_duration.nanos == 0 { + Duration::new(1) + } else { + hold_duration + }; + + Ok(MetaTrajectory { + trajectory: Trajectory::new(wp0.clone(), wp0.time_shifted_by(hold_duration)).unwrap(), + decision_points: Vec::from_iter([DecisionPoint { + index: 0, + state: self.initial_state.clone(), + }]), + initial_state: self.initial_state.clone(), + final_state: self.final_state().clone(), + }) + } +} + +#[derive(Debug, Clone)] +pub struct MetaTrajectory { + pub trajectory: Trajectory, + pub decision_points: Vec>, + pub initial_state: State, + pub final_state: State, +} + +#[derive(Debug, Clone, Copy)] +pub struct DecisionPoint { + pub index: usize, + pub state: State, +} + +#[derive(Debug, Clone, Copy)] +pub enum DecisionRange { + Between([DecisionPoint; 2]), + Before(S, TimePoint), + After(S, TimePoint), +} + +impl DecisionRange { + pub fn initial_state(&self) -> &S { + match self { + DecisionRange::Between([s0, _]) => &s0.state, + DecisionRange::Before(s, _) => s, + DecisionRange::After(s, _) => s, + } + } + + pub fn final_state(&self) -> &S { + match self { + DecisionRange::Between([_, s1]) => &s1.state, + DecisionRange::Before(s, _) => s, + DecisionRange::After(s, _) => s, + } + } +} + +impl MetaTrajectory { + pub fn get_decision_range(&self, trajectory_index: usize) -> DecisionRange { + // dbg!((trajectory_index, &self)); + for i in 1..self.decision_points.len() { + if trajectory_index < self.decision_points[i].index { + // dbg!(i); + // if dbg!(trajectory_index) < dbg!(self.decision_points[i].index) { + return DecisionRange::Between([ + self.decision_points[i - 1].clone(), + self.decision_points[i].clone(), + ]); + } + } + // If we couldn't find suitable decision points, then assume the conflict + // runs past the end of the trajectory + DecisionRange::After( + self.final_state.clone(), + self.trajectory.finish_motion_time() + Duration::from_secs(1), + ) + } + + pub fn get_trajectory_segment(&self, range: &DecisionRange) -> Trajectory { + match range { + DecisionRange::Before(_, t) => { + // This implies that we want an indefinite start trajectory leading + // up to the first waypoint. + let wp1 = self.trajectory.initial_motion().clone(); + let t = if *t == wp1.time() { + *t - Duration::from_secs(1) + } else { + *t + }; + let wp0 = wp1.clone().with_time(t); + Trajectory::new(wp0, wp1).unwrap() + } + DecisionRange::After(_, t) => { + let wp0 = self.trajectory.finish_motion().clone(); + let t = if *t == wp0.time() { + *t + Duration::from_secs(1) + } else { + *t + }; + let wp1 = wp0.clone().with_time(t); + Trajectory::new(wp0, wp1).unwrap() + } + DecisionRange::Between(range) => { + let i_max = self.trajectory.len() - 1; + let mut wps: SmallVec<[_; 10]> = SmallVec::new(); + for i in range[0].index..=range[1].index { + wps.push(self.trajectory[usize::min(i, i_max)].0.clone()); + } + + // Remove any possible duplicated points + wps.sort_by_key(|wp| wp.time()); + wps.dedup_by_key(|wp| wp.time()); + + if wps.len() < 2 { + // If only one waypoint was extracted then it must be the final + // waypoint. Add a holding point so we can create a valid trajectory. + let wp0 = wps.last().unwrap(); + wps.push(wp0.clone().time_shifted_by(Duration::from_secs(1))); + } + + Trajectory::from_iter(wps).unwrap() + } + } + } + + pub fn decision_start_time(&self, range: &DecisionRange) -> TimePoint { + match range { + DecisionRange::Before(..) => self.trajectory.initial_motion_time(), + DecisionRange::After(..) => self.trajectory.finish_motion_time(), + DecisionRange::Between(range) => self + .trajectory + .get(range[0].index) + .map(|wp| wp.time()) + .unwrap_or(self.trajectory.finish_motion_time()), + } + } + + pub fn with_indefinite_finish_time(mut self, value: bool) -> MetaTrajectory { + self.trajectory.set_indefinite_finish_time(value); + self } } @@ -111,4 +324,5 @@ impl Path { pub enum WaypointIntegrationError { State(S), Action(A), + EmptyInitialState, } diff --git a/mapf/src/algorithm/tree.rs b/mapf/src/algorithm/tree.rs index 63b6806..ea7e5bf 100644 --- a/mapf/src/algorithm/tree.rs +++ b/mapf/src/algorithm/tree.rs @@ -16,7 +16,7 @@ */ use crate::{ - algorithm::{Measure, MinimumCostBound, Path}, + algorithm::{MinimumCostBound, Path, QueueLength}, domain::{ClosedSet, ClosedStatus}, error::ThisError, }; @@ -138,6 +138,11 @@ impl Eq for TreeQueueTicket {} impl PartialOrd for TreeQueueTicket { fn partial_cmp(&self, other: &Self) -> Option { match self.evaluation.partial_cmp(&other.evaluation) { + // TODO(@mxgrey): Let downstream users to define criteria for + // deciding when two evaluations are close enough that the biases + // should be compared instead. Small floating point numerical + // residue could cause one evaluation to be unfairly favored over + // another that should be considered equal. Some(Ordering::Equal) => { if let (Some(l), Some(r)) = (&self.bias, &other.bias) { l.partial_cmp(r) @@ -216,9 +221,9 @@ pub enum TreeError { BrokenReference(usize), } -impl Measure for Tree { - fn size(&self) -> usize { - self.arena.len() * std::mem::size_of::() +impl QueueLength for Tree { + fn queue_length(&self) -> usize { + self.queue.len() } } diff --git a/mapf/src/domain/configurable.rs b/mapf/src/domain/configurable.rs index 1117090..5368c2c 100644 --- a/mapf/src/domain/configurable.rs +++ b/mapf/src/domain/configurable.rs @@ -15,12 +15,17 @@ * */ +use crate::error::Anyhow; + /// Configure the parameters of a domain pub trait Configurable { type Configuration; - type ConfigurationError; - fn configure(self, f: F) -> Result + fn configure(self, f: F) -> Result where - F: FnOnce(Self::Configuration) -> Self::Configuration, + F: FnOnce(Self::Configuration) -> Result, Self: Sized; + // TODO(@mxgrey): Consider whether it's possible to support custom strongly + // typed error structs. The main challenge is that callbacks used inside the + // configuration callback could have arbitrary error structs. That's why we + // roll it all into Anyhow for now. } diff --git a/mapf/src/domain/conflict.rs b/mapf/src/domain/conflict.rs index 1240f11..ea355b3 100644 --- a/mapf/src/domain/conflict.rs +++ b/mapf/src/domain/conflict.rs @@ -17,7 +17,7 @@ /// Create an action that extrapolates from an initial state to a target while /// avoiding conflicts with the environment. -pub trait ConflictAvoider { +pub trait ConflictAvoider { type AvoidanceAction; type AvoidanceError; @@ -30,6 +30,7 @@ pub trait ConflictAvoider { State: 'a, Target: 'a, Guidance: 'a, + Key: 'a, Environment: 'a; fn avoid_conflicts<'a>( @@ -37,6 +38,7 @@ pub trait ConflictAvoider { from_state: &State, to_target: &Target, with_guidance: &Guidance, + for_keys: (Option<&Key>, Option<&Key>), in_environment: &Environment, ) -> Self::AvoidanceActionIter<'a> where @@ -46,5 +48,6 @@ pub trait ConflictAvoider { State: 'a, Target: 'a, Guidance: 'a, - Environment: 'a; + Environment: 'a, + Key: 'a; } diff --git a/mapf/src/domain/extrapolator.rs b/mapf/src/domain/extrapolator.rs index fd39ca6..86ee54f 100644 --- a/mapf/src/domain/extrapolator.rs +++ b/mapf/src/domain/extrapolator.rs @@ -23,7 +23,7 @@ use crate::error::NoError; /// * [`IncrementalExtrapolator`] /// * [`crate::domain::ConflictAvoider`] /// * [`crate::domain::Connectable`] -pub trait Extrapolator { +pub trait Extrapolator { /// What kind of action is produced during extrapolation type Extrapolation; @@ -46,7 +46,8 @@ pub trait Extrapolator { Self::ExtrapolationError: 'a, State: 'a, Target: 'a, - Guidance: 'a; + Guidance: 'a, + Key: 'a; /// Extrapolate an action from a state to a target with the provided guidance. /// For each alternative action that the agent can use to reach the target, @@ -54,17 +55,21 @@ pub trait Extrapolator { /// once the target is reached. If the target cannot be reached, return an /// empty iterator. /// + /// ### Arguments + /// /// * `from_state` - The initial state to extrapolate from. /// * `to_target` - The target to extrapolate towards. This can be a different /// type than the `State`. For example, if `State` is a position in SE2, then /// the `Target` could be a position in R2. /// * `with_guidance` - Parameters to describe how the extrapolation should /// be performed. This can include constraints like speed limits. + /// * `for_keys` - The keys for the start vertex and target vertex, if applicable. fn extrapolate<'a>( &'a self, from_state: &State, to_target: &Target, with_guidance: &Guidance, + for_keys: (Option<&Key>, Option<&Key>), ) -> Self::ExtrapolationIter<'a> where Self: 'a, @@ -72,7 +77,8 @@ pub trait Extrapolator { Self::ExtrapolationError: 'a, State: 'a, Target: 'a, - Guidance: 'a; + Guidance: 'a, + Key: 'a; } /// Incrementally extrapolate an action from a state to a target with the @@ -95,7 +101,7 @@ pub trait Extrapolator { /// * [`Extrapolator`] /// * [`crate::domain::ConflictAvoider`] /// * [`crate::domain::Connectable`] -pub trait IncrementalExtrapolator { +pub trait IncrementalExtrapolator { /// What kind of action is produced during extrapolation type IncrementalExtrapolation; @@ -122,7 +128,8 @@ pub trait IncrementalExtrapolator { Self::IncrementalExtrapolationError: 'a, State: 'a, Target: 'a, - Guidance: 'a; + Guidance: 'a, + Key: 'a; /// Extrapolate an action from a state towards a target with the provided /// guidance. For each alternative action that the agent can use to move @@ -141,6 +148,7 @@ pub trait IncrementalExtrapolator { from_state: &State, to_target: &Target, with_guidance: &Guidance, + for_keys: (Option<&Key>, Option<&Key>), ) -> Self::IncrementalExtrapolationIter<'a> where Self: 'a, @@ -148,7 +156,8 @@ pub trait IncrementalExtrapolator { Self::IncrementalExtrapolationError: 'a, State: 'a, Target: 'a, - Guidance: 'a; + Guidance: 'a, + Key: 'a; } #[derive(Debug, Clone, Copy)] @@ -168,7 +177,9 @@ impl ExtrapolationProgress { } pub struct NoExtrapolation(std::marker::PhantomData); -impl Extrapolator for NoExtrapolation { +impl Extrapolator + for NoExtrapolation +{ type Extrapolation = E; type ExtrapolationError = NoError; type ExtrapolationIter<'a> = [Result<(E, State), NoError>; 0] @@ -176,25 +187,28 @@ impl Extrapolator for NoExt E: 'a, State: 'a, Target: 'a, - Guidance: 'a; + Guidance: 'a, + Key: 'a; fn extrapolate<'a>( &'a self, _: &State, _: &Target, _: &Guidance, + _: (Option<&Key>, Option<&Key>), ) -> [Result<(E, State), NoError>; 0] where E: 'a, State: 'a, Target: 'a, Guidance: 'a, + Key: 'a, { [] } } -impl IncrementalExtrapolator +impl IncrementalExtrapolator for NoExtrapolation { type IncrementalExtrapolation = E; @@ -204,19 +218,22 @@ impl IncrementalExtrapolator( &'a self, _: &State, _: &Target, _: &Guidance, + _: (Option<&Key>, Option<&Key>), ) -> [Result<(E, State, ExtrapolationProgress), NoError>; 0] where E: 'a, State: 'a, Target: 'a, Guidance: 'a, + Key: 'a, { [] } diff --git a/mapf/src/domain/reversible.rs b/mapf/src/domain/reversible.rs index 4164281..4d39041 100644 --- a/mapf/src/domain/reversible.rs +++ b/mapf/src/domain/reversible.rs @@ -94,12 +94,12 @@ pub fn flip_endpoint_times( initial_reverse_state: &State, final_reverse_state: &State, ) -> Result<(State, State), NoError> { - let delta_t = *initial_reverse_state.time() - *final_reverse_state.time(); + let delta_t = initial_reverse_state.time() - final_reverse_state.time(); let mut initial_forward_state = final_reverse_state.clone(); - initial_forward_state.set_time(*initial_forward_state.time() + delta_t); + initial_forward_state.set_time(initial_forward_state.time() + delta_t); let mut final_forward_state = initial_reverse_state.clone(); - final_forward_state.set_time(*final_forward_state.time() + delta_t); + final_forward_state.set_time(final_forward_state.time() + delta_t); Ok((initial_forward_state, final_forward_state)) } @@ -110,10 +110,10 @@ pub fn backtrack_times( reverse_action: &ArrayVec, child_reverse_state: &State, ) -> Result<(ArrayVec, State), NoError> { - let dt = *parent_forward_state.time() - *parent_reverse_state.time(); + let dt = parent_forward_state.time() - parent_reverse_state.time(); let mut child_forward_state = child_reverse_state.clone(); - child_forward_state.set_time(*child_forward_state.time() + dt); + child_forward_state.set_time(child_forward_state.time() + dt); let mut forward_action = reverse_action.clone(); // Check swap_endpoints now and save it because the length will change @@ -126,7 +126,7 @@ pub fn backtrack_times( forward_action.reverse(); for wp in &mut forward_action { // Adjust the times of each waypoint - wp.set_time(*wp.time() + dt); + wp.set_time(wp.time() + dt); } if swap_endpoints { // Add the waypoint of the parent_forward_state, which is the diff --git a/mapf/src/graph/occupancy/accessibility_graph.rs b/mapf/src/graph/occupancy/accessibility_graph.rs new file mode 100644 index 0000000..c49a7c0 --- /dev/null +++ b/mapf/src/graph/occupancy/accessibility_graph.rs @@ -0,0 +1,427 @@ +/* + * 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 crate::{ + domain::Reversible, + error::NoError, + graph::{ + occupancy::{Cell, Grid}, + Graph, + }, + motion::r2::Point, + util::ForkIter, +}; +use bitfield::{bitfield, Bit}; +use std::{ + collections::{HashMap, HashSet}, + sync::Arc, +}; + +/// From any unoccupied cell, expand towards any adjacent cells for whom the +/// expansion is valid. +pub struct AccessibilityGraph { + accessibility: Arc>, +} + +impl AccessibilityGraph { + pub fn new(accessibility: Arc>) -> Self { + Self { accessibility } + } +} + +impl Clone for AccessibilityGraph { + fn clone(&self) -> Self { + Self { + accessibility: self.accessibility.clone(), + } + } +} + +impl Graph for AccessibilityGraph { + type Key = Cell; + type Vertex = Point; + type EdgeAttributes = (); + + type VertexRef<'a> = Point where G: 'a; + type Edge<'a> = (Cell, Cell) where G: 'a; + type EdgeIter<'a> = impl Iterator + 'a where Self: 'a; + + fn vertex<'a>(&'a self, key: &Cell) -> Option { + if self.accessibility.is_inaccessible(key) { + return None; + } + + Some(key.center_point(self.accessibility.grid.cell_size())) + } + + fn edges_from_vertex<'a>(&'a self, key: &Self::Key) -> Self::EdgeIter<'a> + where + Self: 'a, + Self::Vertex: 'a, + Self::Key: 'a, + Self::EdgeAttributes: 'a, + { + if self.accessibility.grid.is_occupied(key) { + return ForkIter::Left(None.into_iter()); + } + + let directions = match self.accessibility.constraints.get(key) { + Some(constraints) => match constraints { + CellAccessibility::Accessible(constraints) => *constraints, + CellAccessibility::Inaccessible => return ForkIter::Left(None.into_iter()), + }, + None => CellDirections::all(), + }; + + let from_cell = *key; + ForkIter::Right( + directions + .iter_from(from_cell) + .map(move |to_cell: Cell| (from_cell, to_cell)), + ) + } + + type LazyEdgeIter<'a> = [(Cell, Cell); 0] where G: 'a; + + fn lazy_edges_between<'a>(&'a self, _: &Self::Key, _: &Self::Key) -> Self::LazyEdgeIter<'a> + where + Self: 'a, + Self::Vertex: 'a, + Self::Key: 'a, + Self::EdgeAttributes: 'a, + { + [] + } +} + +impl Reversible for AccessibilityGraph { + type ReversalError = NoError; + fn reversed(&self) -> Result + where + Self: Sized, + { + // Accessibility is always symmetric/bidirectional, so we can just clone + // the graph in order to reverse it. + Ok(self.clone()) + } +} + +bitfield! { + #[derive(Clone, Copy, PartialEq, Eq)] + pub struct CellDirections(u8); + impl Debug; + u8; + pub north, set_north: 0; + pub northeast, set_northeast: 1; + pub east, set_east: 2; + pub southeast, set_southeast: 3; + pub south, set_south: 4; + pub southwest, set_southwest: 5; + pub west, set_west: 6; + pub northwest, set_northwest: 7; +} + +impl CellDirections { + pub fn iter_from(self, cell: Cell) -> impl Iterator { + self.iter().map(move |[i, j]| cell.shifted(i, j)) + } + + /// Iterate over the directions that are accessible + pub fn iter(self) -> CellDirectionsIter { + CellDirectionsIter { + next_dir: 0, + directions: self, + accessibility: true, + } + } + + /// Iterate over the directions that are inaccessible + pub fn iter_inaccessible(self) -> CellDirectionsIter { + CellDirectionsIter { + next_dir: 0, + directions: self, + accessibility: false, + } + } +} + +impl IntoIterator for CellDirections { + type Item = [i64; 2]; + type IntoIter = CellDirectionsIter; + fn into_iter(self) -> Self::IntoIter { + self.iter() + } +} + +pub struct CellDirectionsIter { + // NOTE: We need to allow next_dir to increment beyond 255 so we don't get + // an integer overflow while iterating + next_dir: u16, + directions: CellDirections, + /// Do we want to iterate on directions that are accessible (true) or + /// inaccessible (false)? + accessibility: bool, +} + +impl Iterator for CellDirectionsIter { + type Item = [i64; 2]; + fn next(&mut self) -> Option { + if self.next_dir > u8::MAX as u16 { + return None; + } + + while self.directions.bit(self.next_dir as usize) != self.accessibility { + self.next_dir += 1; + if self.next_dir > u8::MAX as u16 { + return None; + } + } + + let shift = match self.next_dir { + 0 => [0, 1], + 1 => [1, 1], + 2 => [1, 0], + 3 => [1, -1], + 4 => [0, -1], + 5 => [-1, -1], + 6 => [-1, 0], + 7 => [-1, 1], + _ => return None, + }; + + self.next_dir += 1; + Some(shift) + } +} + +impl CellDirections { + pub fn all() -> Self { + Self(u8::MAX) + } + + pub fn is_all(&self) -> bool { + self.0 == u8::MAX + } + + pub fn set_direction(&mut self, i: i8, j: i8, value: bool) -> Result<(), [i8; 2]> { + match [i, j] { + [0, 1] => self.set_north(value), + [1, 1] => self.set_northeast(value), + [1, 0] => self.set_east(value), + [1, -1] => self.set_southeast(value), + [0, -1] => self.set_south(value), + [-1, -1] => self.set_southwest(value), + [-1, 0] => self.set_west(value), + [-1, 1] => self.set_northwest(value), + _ => return Err([i, j]), + } + + Ok(()) + } +} + +#[derive(Clone, Debug)] +pub enum CellAccessibility { + /// The cell is accessible but it has some constraints on where the agent + /// can move from it. + Accessible(CellDirections), + /// The cell is entirely inaccessible. The agent cannot be centered at this + /// cell. + Inaccessible, +} + +impl CellAccessibility { + pub fn is_inaccessible(&self) -> bool { + matches!(self, CellAccessibility::Inaccessible) + } +} + +#[derive(Clone, Debug)] +pub struct Accessibility { + grid: G, + agent_radius: f64, + cell_shift: i64, + constraints: HashMap, +} + +impl Accessibility { + pub fn new(grid: G, agent_radius: f64) -> Self { + let cell_size = grid.cell_size(); + let mut output = Self { + grid, + agent_radius, + cell_shift: Self::calculate_cell_shift(agent_radius, cell_size), + constraints: HashMap::new(), + }; + + Self::update_constraints( + output.grid.occupied_cells(), + &output.grid, + output.agent_radius, + output.cell_shift, + &mut output.constraints, + ); + + output + } + + pub fn iter_accessibility<'a>( + &'a self, + ) -> impl Iterator { + self.constraints.iter() + } + + pub fn grid(&self) -> &G { + &self.grid + } + + pub fn agent_radius(&self) -> f64 { + self.agent_radius + } + + pub fn is_inaccessible(&self, cell: &Cell) -> bool { + self.grid.is_occupied(cell) + || self + .constraints + .get(cell) + .filter(|c| c.is_inaccessible()) + .is_some() + } + + pub fn change_cells(&mut self, mut changes: HashMap) -> bool { + changes.retain(|cell, value| self.grid.is_occupied(cell) != *value); + if changes.is_empty() { + return false; + } + + self.grid.change_cells(&changes); + Self::update_constraints( + changes.keys(), + &self.grid, + self.agent_radius, + self.cell_shift, + &mut self.constraints, + ); + + return true; + } + + pub fn change_agent_radius(&mut self, value: f64) { + self.agent_radius = value; + self.cell_shift = Self::calculate_cell_shift(self.agent_radius, self.grid.cell_size()); + self.constraints.clear(); + + Self::update_constraints( + self.grid.occupied_cells(), + &self.grid, + self.agent_radius, + self.cell_shift, + &mut self.constraints, + ); + } + + fn update_constraints<'a>( + changed_cells: impl IntoIterator, + grid: &G, + agent_radius: f64, + cell_shift: i64, + constraints: &mut HashMap, + ) { + let mut inspect_cells: HashSet = HashSet::new(); + for cell in changed_cells { + let cell: Cell = *cell; + for x in -cell_shift..=cell_shift { + for y in -cell_shift..=cell_shift { + inspect_cells.insert(cell.shifted(x, y)); + } + } + } + + // For each cell, determine if it is occupied or unavailable + for cell in &inspect_cells { + if grid.is_occupied(cell) { + // If the cell is occupied there's no need to refer to store it + // in the constraints because we know that it has no adjacency + // by virtue of being occupied. + constraints.remove(cell); + continue; + } + + let p = cell.center_point(grid.cell_size()); + if grid.is_circle_occupied(p, agent_radius).is_some() { + constraints.insert(*cell, CellAccessibility::Inaccessible); + continue; + } + + // Remove any constraint that might exist on this cell for now. We + // will put it back if needed in the next loop. + constraints.remove(cell); + } + + // For each cell, determine which of its neighbors it can travel to + for from_cell in &inspect_cells { + if grid.is_occupied(from_cell) { + // If the cell is occupied, there's no need to check for + // expansions out of it + continue; + } + + if constraints.contains_key(from_cell) { + // If it was set as Unavailable in the previous loop, there's + // no need to check for expansions out of it. + continue; + } + + let from_p = from_cell.center_point(grid.cell_size()); + let mut cell_directions = CellDirections::all(); + for i in [-1, 0, 1] { + for j in [-1, 0, 1] { + if i == 0 && j == 0 { + continue; + } + + let to_cell: Cell = from_cell.shifted(i, j); + if grid.is_occupied(&to_cell) + || constraints + .get(&to_cell) + .filter(|c| c.is_inaccessible()) + .is_some() + { + cell_directions.set_direction(i as i8, j as i8, false).ok(); + continue; + } + + let to_p = to_cell.center_point(grid.cell_size()); + if grid + .is_sweep_occupied(from_p, to_p, 2.0 * agent_radius) + .is_some() + { + cell_directions.set_direction(i as i8, j as i8, false).ok(); + } + } + } + + if !cell_directions.is_all() { + constraints.insert(*from_cell, CellAccessibility::Accessible(cell_directions)); + } + } + } + + fn calculate_cell_shift(agent_radius: f64, cell_size: f64) -> i64 { + (agent_radius / cell_size + 0.5).ceil() as i64 + } +} diff --git a/mapf/src/graph/occupancy/mod.rs b/mapf/src/graph/occupancy/mod.rs index 93f218e..a13f62a 100644 --- a/mapf/src/graph/occupancy/mod.rs +++ b/mapf/src/graph/occupancy/mod.rs @@ -15,7 +15,10 @@ * */ -use crate::util::triangular_for; +use crate::{ + motion::{MaybeTimed, TimePoint}, + util::triangular_for, +}; use bitfield::{bitfield, Bit, BitMut}; use std::collections::{hash_map, HashMap, HashSet}; use std::ops::Sub; @@ -48,12 +51,24 @@ impl Cell { /// Get the point on the "bottom left" (lowest coordinate values) corner of /// the cell. - pub fn to_bottom_left_point(&self, cell_size: f64) -> Point { + pub fn bottom_left_point(&self, cell_size: f64) -> Point { Point::new(cell_size * self.x as f64, cell_size * self.y as f64) } + pub fn bottom_right_point(&self, cell_size: f64) -> Point { + self.shifted(1, 0).bottom_left_point(cell_size) + } + + pub fn top_left_point(&self, cell_size: f64) -> Point { + self.shifted(0, 1).bottom_left_point(cell_size) + } + + pub fn top_right_point(&self, cell_size: f64) -> Point { + self.shifted(1, 1).bottom_left_point(cell_size) + } + /// Get the point in the center of the cell. - pub fn to_center_point(&self, cell_size: f64) -> Point { + pub fn center_point(&self, cell_size: f64) -> Point { Point::new( cell_size * (self.x as f64 + 0.5), cell_size * (self.y as f64 + 0.5), @@ -80,6 +95,30 @@ impl Cell { } } +impl From<[i64; 2]> for Cell { + fn from(value: [i64; 2]) -> Self { + Cell::new(value[0], value[1]) + } +} + +impl From<(i64, i64)> for Cell { + fn from((x, y): (i64, i64)) -> Self { + Cell::new(x, y) + } +} + +impl From for [i64; 2] { + fn from(value: Cell) -> Self { + [value.x, value.y] + } +} + +impl From for (i64, i64) { + fn from(value: Cell) -> Self { + (value.x, value.y) + } +} + impl Sub for Cell { type Output = (i64, i64); fn sub(self, other: Self) -> Self::Output { @@ -87,6 +126,12 @@ impl Sub for Cell { } } +impl MaybeTimed for Cell { + fn maybe_time(&self) -> Option { + None + } +} + /// Indicates what type of type of corner a cell is. Each value in the tuple can /// be +1 or -1. The pair of values gives an (x, y) direction indicating where /// the corner is. @@ -130,7 +175,7 @@ bitfield! { /// Indicates what type of type of corner a cell is. This is implemented as /// a bitfield, so you can call northwest(), southeast(), etc to get a /// boolean which indicates whether this is a corner in that direction. - #[derive(Clone, Copy, PartialEq)] + #[derive(Clone, Copy, PartialEq, Eq)] pub struct CornerStatus(u8); impl Debug; u8; @@ -207,6 +252,8 @@ impl IntoIterator for &CornerStatus { type ConfirmedChanges = Vec<(Cell, bool)>; type ChangedCorners = Vec<(Cell, CornerStatus)>; +// TODO(@mxgrey): Consider splitting the corner-related functions into a separate +// trait since they are not necessarily needed by all grid users. pub trait Grid: std::fmt::Debug { type OccupiedIterator<'a>: IntoIterator where @@ -243,6 +290,10 @@ pub trait Grid: std::fmt::Debug { /// the space will be returned. fn is_square_occupied(&self, p: Point, width: f64) -> Option; + /// Check if a circle has any occupancy. The first cell found that occupies + /// the circle will be returned. + fn is_circle_occupied(&self, p: Point, radius: f64) -> Option; + /// Check if a rectangular sweep from p0 to p1 with the specified width has /// any occupancy. The first cell found that occupies the space will be /// returned. @@ -365,8 +416,8 @@ impl Visibility { } let cell_size = self.grid.cell_size(); - let p0 = cell.to_center_point(cell_size); - let p1 = v_cell.to_center_point(cell_size); + let p0 = cell.center_point(cell_size); + let p1 = v_cell.center_point(cell_size); return self .grid .is_sweep_occupied(p0, p1, 2.0 * self.agent_radius) @@ -383,7 +434,7 @@ impl Visibility { .filter(|of_cell| { self.grid() .is_square_occupied( - of_cell.to_center_point(self.grid().cell_size()), + of_cell.center_point(self.grid().cell_size()), 2.0 * self.agent_radius, ) .is_none() @@ -399,8 +450,8 @@ impl Visibility { if self .grid() .is_sweep_occupied( - of_cell.to_center_point(cell_size), - neighbor.to_center_point(cell_size), + of_cell.center_point(cell_size), + neighbor.center_point(cell_size), 2.0 * self.agent_radius(), ) .is_none() @@ -473,7 +524,7 @@ impl Visibility { // If this corner point is currently vacant, then we // need to check whether it has any blockers. let blocked_by = grid.is_square_occupied( - cell.to_center_point(grid.cell_size()), + cell.center_point(grid.cell_size()), 2.0 * agent_radius, ); @@ -559,7 +610,7 @@ impl Visibility { // visibility point, then the visibility point might // be unoccupied now, but we need to test that. *point_blocked_by = grid.is_square_occupied( - point_cell.to_center_point(grid.cell_size()), + point_cell.center_point(grid.cell_size()), 2.0 * agent_radius, ); @@ -601,8 +652,8 @@ impl Visibility { } let blocked_by = grid.is_sweep_occupied( - cell.to_center_point(grid.cell_size()), - other.to_center_point(grid.cell_size()), + cell.center_point(grid.cell_size()), + other.center_point(grid.cell_size()), 2.0 * agent_radius, ); @@ -643,8 +694,8 @@ impl Visibility { { if *occupied { let line = LineSegment::new( - cell_i.to_center_point(grid.cell_size()), - cell_j.to_center_point(grid.cell_size()), + cell_i.center_point(grid.cell_size()), + cell_j.center_point(grid.cell_size()), ); if line.passes_near_cell(changed_cell, grid.cell_size(), agent_radius) { @@ -657,8 +708,8 @@ impl Visibility { if let Some(blocked_by) = *entry.get() { if blocked_by == *changed_cell { let new_blocker = grid.is_sweep_occupied( - cell_i.to_center_point(grid.cell_size()), - cell_j.to_center_point(grid.cell_size()), + cell_i.center_point(grid.cell_size()), + cell_j.center_point(grid.cell_size()), 2.0 * agent_radius, ); @@ -787,4 +838,6 @@ pub mod sparse_grid; pub use sparse_grid::SparseGrid; pub mod visibility_graph; pub use visibility_graph::{NeighborhoodGraph, VisibilityGraph}; +pub mod accessibility_graph; +pub use accessibility_graph::{Accessibility, AccessibilityGraph}; mod util; diff --git a/mapf/src/graph/occupancy/sparse_grid.rs b/mapf/src/graph/occupancy/sparse_grid.rs index 5cadd35..07a1f80 100644 --- a/mapf/src/graph/occupancy/sparse_grid.rs +++ b/mapf/src/graph/occupancy/sparse_grid.rs @@ -149,9 +149,9 @@ impl Grid for SparseGrid { let mut confirmed_changes = Vec::new(); confirmed_changes.reserve(changes.len()); - for change in changes { - if self.update_cell(change.0, *change.1) { - confirmed_changes.push((*change.0, *change.1)); + for (cell, occupied) in changes { + if self.update_cell(cell, *occupied) { + confirmed_changes.push((*cell, *occupied)); } } @@ -216,6 +216,106 @@ impl Grid for SparseGrid { return None; } + fn is_circle_occupied(&self, p: Point, radius: f64) -> Option { + let r_squared = radius.powi(2); + let min_center_dist_squared = (radius + self.cell_size / 2.0).powi(2); + let max_center_dist_squared = + (radius + std::f64::consts::SQRT_2 * self.cell_size / 2.0).powi(2); + let delta = Vector::new(radius, radius); + let min_p = p - delta; + let max_p = p + delta; + let min_cell = Cell::from_point(min_p, self.cell_size); + let max_cell = Cell::new( + (max_p.x / self.cell_size).ceil() as i64, + (max_p.y / self.cell_size).ceil() as i64, + ); + + let point_inside = |p0: Point| { + let dp = p - p0; + return dp.dot(&dp) < r_squared; + }; + + let line_inside = |p0: Point, p1: Point| { + if point_inside(p0) { + return true; + } + + if point_inside(p1) { + return true; + } + + let n = p1 - p0; + let length = n.norm(); + let n = n / length; + let s = (p - p0).dot(&n); + if s <= 0.0 || length <= s { + return false; + } + + let pc = p0 + s * n; + let dp = pc - p; + return dp.dot(&dp) < r_squared; + }; + + for (i, column) in self.occupancy_map.range(min_cell.x..max_cell.x) { + for j in column.range(min_cell.y..max_cell.y) { + // The fact that this cell is occupied does not guarantee that + // it intersects the circle. We still need to test if the square + // intersects the radius. + let cell = Cell::new(*i, *j); + let p_cell = cell.center_point(self.cell_size); + let dp = p - p_cell; + let center_dist_squared = dp.dot(&dp); + if center_dist_squared < min_center_dist_squared { + return Some(cell); + } + + if max_center_dist_squared <= center_dist_squared { + // Cannot have an intersection for this cell because it's + // definitely too far + continue; + } + + if dp.x > 0.0 { + if line_inside( + cell.bottom_right_point(self.cell_size), + cell.top_right_point(self.cell_size), + ) { + return Some(cell); + } + } else if dp.x < 0.0 { + if line_inside( + cell.bottom_left_point(self.cell_size), + cell.top_left_point(self.cell_size), + ) { + return Some(cell); + } + } + + if dp.y > 0.0 { + if line_inside( + cell.top_left_point(self.cell_size), + cell.top_right_point(self.cell_size), + ) { + return Some(cell); + } + } else if dp.y < 0.0 { + if line_inside( + cell.bottom_left_point(self.cell_size), + cell.bottom_right_point(self.cell_size), + ) { + return Some(cell); + } + } + + // If we reach this point then the circle does not intersect + // this cell. + } + } + + None + } + fn is_sweep_occupied(&self, p0: Point, p1: Point, width: f64) -> Option { let d = width / 2.0; let dist = (p1 - p0).norm(); diff --git a/mapf/src/graph/occupancy/util.rs b/mapf/src/graph/occupancy/util.rs index d9ad444..8ad3409 100644 --- a/mapf/src/graph/occupancy/util.rs +++ b/mapf/src/graph/occupancy/util.rs @@ -129,7 +129,7 @@ impl LineSegment { let bound_distance = proximity + 1.4143 * cell_size; for m in [0, 1] { for n in [0, 1] { - let p = cell.shifted(m, n).to_bottom_left_point(cell_size); + let p = cell.shifted(m, n).bottom_left_point(cell_size); let dist = self.distance_from_point(p); if dist < proximity { return true; @@ -141,8 +141,8 @@ impl LineSegment { } } - let p_min = cell.to_bottom_left_point(cell_size); - let p_max = cell.shifted(1, 1).to_bottom_left_point(cell_size); + let p_min = cell.bottom_left_point(cell_size); + let p_max = cell.shifted(1, 1).bottom_left_point(cell_size); // We need to check if either line segment endpoint is inside the cell for p in [&self.p0, &self.p1] { diff --git a/mapf/src/graph/occupancy/visibility_graph.rs b/mapf/src/graph/occupancy/visibility_graph.rs index ee70d71..98e0693 100644 --- a/mapf/src/graph/occupancy/visibility_graph.rs +++ b/mapf/src/graph/occupancy/visibility_graph.rs @@ -56,7 +56,7 @@ fn gather_points_of_interest( if visibility .grid() .is_square_occupied( - interest.to_center_point(visibility.grid().cell_size()), + interest.center_point(visibility.grid().cell_size()), visibility.agent_radius(), ) .is_some() @@ -81,8 +81,8 @@ fn gather_points_of_interest( if visibility .grid() .is_sweep_occupied( - interest_i.to_center_point(cell_size), - interest_j.to_center_point(cell_size), + interest_i.center_point(cell_size), + interest_j.center_point(cell_size), 2.0 * visibility.agent_radius(), ) .is_none() @@ -131,7 +131,7 @@ impl Graph for VisibilityGraph { // We don't bother to filter out occupied cells because those cells will // not generate any valid edges anyway. If we filtered them out here we // would be frequently doing redundant occupancy checking. - Some(cell.to_center_point(self.visibility.grid().cell_size())) + Some(cell.center_point(self.visibility.grid().cell_size())) } fn edges_from_vertex<'a, 'b>(&'a self, from_cell: &'b Self::Key) -> Self::EdgeIter<'a> @@ -145,7 +145,7 @@ impl Graph for VisibilityGraph { self.visibility .grid() .is_square_occupied( - from_cell.to_center_point(self.visibility.grid().cell_size()), + from_cell.center_point(self.visibility.grid().cell_size()), 2.0 * self.visibility.agent_radius(), ) .is_none() @@ -171,9 +171,9 @@ impl Graph for VisibilityGraph { .iter() .filter(move |poi| { let to_p = - poi.to_center_point(self.visibility.grid().cell_size()); + poi.center_point(self.visibility.grid().cell_size()); let from_p = from_cell - .to_center_point(self.visibility.grid().cell_size()); + .center_point(self.visibility.grid().cell_size()); self.visibility .grid() .is_sweep_occupied( @@ -207,8 +207,8 @@ impl Graph for VisibilityGraph { return None; } - let from_p = from_key.to_center_point(self.visibility.grid().cell_size()); - let to_p = to_key.to_center_point(self.visibility.grid().cell_size()); + let from_p = from_key.center_point(self.visibility.grid().cell_size()); + let to_p = to_key.center_point(self.visibility.grid().cell_size()); for p in [from_p, to_p] { if self .visibility @@ -283,6 +283,9 @@ impl Clone for NeighborhoodGraph { /// From any unoccupied cell, expand towards both its adjacent cells and the /// visibility graph. +/// +/// Similar to [`VisibilityGraph`] except it also expands to adjacent cells like +/// [`super::AdjacencyGraph`]. impl Graph for NeighborhoodGraph { type Key = Cell; type Vertex = Point; @@ -296,7 +299,7 @@ impl Graph for NeighborhoodGraph { if self.visibility.grid().is_occupied(&cell) { None } else { - Some(cell.to_center_point(self.visibility.grid().cell_size())) + Some(cell.center_point(self.visibility.grid().cell_size())) } } @@ -306,7 +309,7 @@ impl Graph for NeighborhoodGraph { { // dbg!("neighborhood graph"); let from_cell = *from_cell; - let from_p = from_cell.to_center_point(self.visibility.grid().cell_size()); + let from_p = from_cell.center_point(self.visibility.grid().cell_size()); [from_cell] .into_iter() .filter(move |_| { @@ -349,9 +352,8 @@ impl Graph for NeighborhoodGraph { self.points_of_interest .iter() .filter(move |poi| { - let to_p = poi.to_center_point( - self.visibility.grid().cell_size(), - ); + let to_p = poi + .center_point(self.visibility.grid().cell_size()); self.visibility .grid() .is_sweep_occupied( @@ -387,8 +389,8 @@ impl Graph for NeighborhoodGraph { return None; } - let from_p = from_key.to_center_point(self.visibility.grid().cell_size()); - let to_p = to_key.to_center_point(self.visibility.grid().cell_size()); + let from_p = from_key.center_point(self.visibility.grid().cell_size()); + let to_p = to_key.center_point(self.visibility.grid().cell_size()); for p in [from_p, to_p] { if self .visibility diff --git a/mapf/src/graph/shared_graph.rs b/mapf/src/graph/shared_graph.rs index e14fe1f..557669c 100644 --- a/mapf/src/graph/shared_graph.rs +++ b/mapf/src/graph/shared_graph.rs @@ -54,6 +54,21 @@ impl SharedGraph { reverse: Arc::new(RwLock::new(Some(reverse))), } } + + pub fn modify(self, f: F) -> Result + where + F: FnOnce(G) -> Result, + G: Clone, + { + // If this is the only instance that shares a reference to this + // graph, then we will not have to make a clone of the graph. + let graph = match Arc::try_unwrap(self.graph) { + Ok(graph) => graph, + Err(arc_graph) => (*arc_graph).clone(), + }; + + f(graph).map(|graph| Self::new(graph)) + } } impl Graph for SharedGraph { diff --git a/mapf/src/lib.rs b/mapf/src/lib.rs index 8750085..25af328 100644 --- a/mapf/src/lib.rs +++ b/mapf/src/lib.rs @@ -35,6 +35,8 @@ pub mod templates; pub mod motion; +pub mod negotiation; + pub mod error; pub mod premade; diff --git a/mapf/src/motion/conflict.rs b/mapf/src/motion/conflict.rs index bb21f6d..1e1413f 100644 --- a/mapf/src/motion/conflict.rs +++ b/mapf/src/motion/conflict.rs @@ -102,7 +102,13 @@ where for action in self { let wp: W = match action { SafeAction::Move(wp) => wp.clone().into(), - SafeAction::Wait(wait) => initial_waypoint.with_time(wait.time_estimate), + SafeAction::Wait(wait) => { + if initial_waypoint.time() < wait.time_estimate { + initial_waypoint.with_time(wait.time_estimate) + } else { + continue; + } + } }; waypoints.push(Ok(wp.clone())); @@ -256,7 +262,7 @@ where }); } - ranked_hints.sort_by(|a, b| match a.contour.cmp(&b.contour) { + ranked_hints.sort_unstable_by(|a, b| match a.contour.cmp(&b.contour) { Ordering::Equal => { if a.reach <= b.reach { Ordering::Less @@ -503,8 +509,14 @@ where // Test if the start waypoint can arrive at this hint. If it can // then we have found our path. let hint_arrival_wp = make_parent_arrival(from_point, hint_wp); + // TODO(@mxgrey): We have seen cases where hint_arrival_wp.time + // is larger than hint_wp.time. We should consider whether this is + // a bug or is acceptable. In the worst case it means we have some + // unhelpful hints which we should be ignoring to reduce the search + // effort. Maybe we can filter based on the contour. let safe_hint_arrival = is_safe_segment((&from_point, &hint_arrival_wp), None, in_environment) + && hint_arrival_wp.time <= hint_wp.time && is_safe_segment((&hint_arrival_wp, &hint_wp), None, in_environment); if safe_hint_arrival { // We have found the desired path @@ -529,7 +541,7 @@ where path.push(SafeAction::Move(final_wp)); return Some(path); } - }; + } } None @@ -717,6 +729,7 @@ fn detect_proximity( }; } +#[derive(Debug)] struct CandidateAdjustment { pushed: bool, result: Option, @@ -736,7 +749,7 @@ where let mut this_obs_violated = false; let mut new_candidate_time: Option = None; for [wp0, wp1] in obs_traj.iter_from(candidate_time).pairs() { - if candidate_time < *wp0.time() && !this_obs_violated { + if candidate_time < wp0.time() && !this_obs_violated { break; } @@ -866,7 +879,7 @@ pub fn is_safe_segment( in_environment: &Env, ) -> bool where - W: Into + Waypoint, + W: Into + Waypoint + std::fmt::Debug, Env: Environment>, { let profile = in_environment.agent_profile(); @@ -887,7 +900,7 @@ where } for [wp0_b, wp1_b] in obs_traj.iter_range(line_a.0.time, line_a.1.time).pairs() { - if line_a.1.time < *wp0_b.time() { + if line_a.1.time < wp0_b.time() { // The trajectories are no longer overlapping in time so there // is no longer a risk. return true; @@ -896,45 +909,78 @@ where let wp0_b: WaypointR2 = wp0_b.into(); let wp1_b: WaypointR2 = wp1_b.into(); let line_b = (&wp0_b, &wp1_b); - - if !bb.overlaps(Some( - BoundingBox::for_line(obs.profile(), &wp0_b, &wp1_b).inflated_by(1e-3), - )) { - continue; + if have_conflict( + line_a, + Some(bb), + in_environment.agent_profile(), + line_b, + None, + obs.profile(), + conflict_distance_squared, + ) { + return false; } + } + } - let in_time_range = |t: &TimePoint| -> bool { - line_a.0.time < *t && *t < line_a.1.time && line_b.0.time < *t && *t < line_b.1.time - }; + true +} - let proximity = detect_proximity(conflict_distance_squared, line_a, line_b); - - if let Some(t) = proximity.enter.filter(in_time_range) { - let t_range = compute_t_range(line_a, line_b); - let t = (t - t_range.0).as_secs_f64(); - let (dp0, dv) = compute_dp0_dv(line_a, line_b, &t_range); - let a = dv.dot(&dv); - let b = 2.0 * dv.dot(&dp0); - let deriv = 2.0 * a * t + b; - // Allow for a little floating point error. When the derivative - // is very very close to zero (which is a perfectly acceptable - // value), floating point calculation errors can cause its - // calculated value to dip into the negative. - // - // TODO(@mxgrey): Consider if there are more robust ways (not - // sensitive to floating point error) to determine whether the - // segment is safe. - if deriv < -1e-6 { - // The distance between the agents is reducing while they - // are already within an unsafe proximity, so we will call - // this situation unsafe. - return false; - } - } +#[inline] +pub fn have_conflict( + line_a: (&WaypointR2, &WaypointR2), + bb_a: Option, + profile_a: &CircularProfile, + line_b: (&WaypointR2, &WaypointR2), + bb_b: Option, + profile_b: &CircularProfile, + conflict_distance_squared: f64, +) -> bool { + let bb_a = match bb_a { + Some(bb_a) => bb_a, + None => BoundingBox::for_line(profile_a, line_a.0, line_a.1), + }; + + let bb_b = match bb_b { + Some(bb_b) => bb_b, + None => BoundingBox::for_line(profile_b, line_b.0, line_b.1), + }; + + if !bb_a.overlaps(Some(bb_b)) { + return false; + } + + let in_time_range = |t: &TimePoint| -> bool { + line_a.0.time < *t && *t < line_a.1.time && line_b.0.time < *t && *t < line_b.1.time + }; + + let proximity = detect_proximity(conflict_distance_squared, line_a, line_b); + + if let Some(t) = proximity.enter.filter(in_time_range) { + let t_range = compute_t_range(line_a, line_b); + let t = (t - t_range.0).as_secs_f64(); + let (dp0, dv) = compute_dp0_dv(line_a, line_b, &t_range); + let a = dv.dot(&dv); + let b = 2.0 * dv.dot(&dp0); + // let deriv = 2.0 * a * t + b; + let deriv = 2.0 * a * t + b; + // Allow for a little floating point error. When the derivative + // is very very close to zero (which is a perfectly acceptable + // value), floating point calculation errors can cause its + // calculated value to dip into the negative. + // + // TODO(@mxgrey): Consider if there are more robust ways (not + // sensitive to floating point error) to determine whether the + // segment is safe. + if deriv < -1e-6 { + // The distance between the agents is reducing while they + // are already within an unsafe proximity, so we will call + // this situation unsafe. + return true; } } - true + return false; } #[inline] @@ -963,6 +1009,7 @@ where let mut wait_hints = SmallVec::new(); for (for_obstacle, obs) in in_environment.obstacles().into_iter().enumerate() { + // dbg!((for_obstacle, obs)); if !bb.overlaps(obs.bounding_box().cloned()) { continue; } @@ -1277,7 +1324,7 @@ fn compute_quadratic_wait_hints( } #[inline] -fn compute_stationary_proximity( +pub fn compute_stationary_proximity( r: Point, p0: Point, v: Vector2, @@ -1607,4 +1654,26 @@ mod tests { } } } + + #[test] + fn test_indefinite_finish_safety() { + for expect_safe in [true, false] { + let obstacle = Trajectory::from_iter([ + WaypointR2::new_f64(0.0, 0.0, 0.0), + WaypointR2::new_f64(1.0, 0.0, 0.0), + ]) + .unwrap() + .with_indefinite_finish_time(!expect_safe); + + let profile = CircularProfile::new(1.0, 0.0, 0.0).unwrap(); + let mut environment = DynamicEnvironment::new(profile); + environment + .obstacles + .push(DynamicCircularObstacle::new(profile).with_trajectory(Some(obstacle))); + + let p0 = WaypointR2::new_f64(0.0, -5.0, 0.0); + let p1 = WaypointR2::new_f64(20.0, 5.0, 0.0); + assert_eq!(expect_safe, is_safe_segment((&p0, &p1), None, &environment)); + } + } } diff --git a/mapf/src/motion/environment.rs b/mapf/src/motion/environment.rs index 241ff94..7551e72 100644 --- a/mapf/src/motion/environment.rs +++ b/mapf/src/motion/environment.rs @@ -15,9 +15,12 @@ * */ -use crate::motion::{ - r2::{Point, WaypointR2}, - Trajectory, Waypoint, +use crate::{ + domain::Key, + motion::{ + r2::{Point, WaypointR2}, + Trajectory, Waypoint, + }, }; use std::{ collections::{hash_map::Entry, HashMap}, @@ -84,20 +87,95 @@ impl Default for DynamicEnvironmentOverlay { } } +pub type CcbsKey = (K, K); + +#[derive(Debug, Clone)] +pub struct CcbsConstraint { + pub obstacle: DynamicCircularObstacle, + pub mask: usize, +} + +/// A dynamic environment for performing Continuous-time Conflict Based Search. #[derive(Debug, Clone)] -pub struct OverlayedDynamicEnvironment { +pub struct CcbsEnvironment { base: Arc>, overlay: DynamicEnvironmentOverlay, + constraints: HashMap, Vec>>, + mask: Option, } -impl OverlayedDynamicEnvironment { +impl CcbsEnvironment { pub fn new(base: Arc>) -> Self { Self { base, overlay: Default::default(), + constraints: Default::default(), + mask: None, + } + } + + pub fn view_for<'a>(&'a self, key: Option<&CcbsKey>) -> CcbsEnvironmentView<'a, W, K> + where + K: Key, + { + CcbsEnvironmentView { + view: self, + constraints: key.map(|key| self.constraints.get(key)).flatten(), } } + /// Iterate over all obstacles that might be visible for this environment, + /// including all constraints for all masks, but not including any base + /// obstacles that are hidden by the overlay. + pub fn iter_all_obstacles(&self) -> impl Iterator> { + self.base + .obstacles + .iter() + .enumerate() + .map(|(i, obs)| self.overlay.obstacles.get(&i).unwrap_or(obs)) + .chain( + self.constraints + .values() + .flat_map(|x| x) + .map(|x| &x.obstacle), + ) + } + + pub fn iter_obstacles_from( + &self, + key: K, + mask: usize, + ) -> impl Iterator> + where + K: Key + Clone, + { + self.base + .obstacles + .iter() + .enumerate() + .map(|(i, obs)| self.overlay.obstacles.get(&i).unwrap_or(obs)) + .chain( + self.constraints + .iter() + .filter(move |((k, _), _)| *k == key) + .flat_map(|(_, constraints)| constraints) + .filter_map(move |x| { + if x.mask == mask { + return None; + } + Some(&x.obstacle) + }), + ) + } + + pub fn overlay_profile(&mut self, profile: CircularProfile) { + self.overlay.profile = Some(profile); + } + + pub fn revert_profile(&mut self) { + self.overlay.profile = None; + } + /// Overlay an obstacle. If there was already an overlay for this obstacle /// then get back the previous overlay. /// @@ -107,6 +185,9 @@ impl OverlayedDynamicEnvironment { /// However, the overlaid data will be retained and will become applicable /// if the base environment is changed to one that does contain the obstacle /// entry. + /// + /// To add an entirely new obstacle that does not exist in the underlay, use + /// `insert_obstacle`. pub fn overlay_obstacle( &mut self, index: usize, @@ -204,25 +285,73 @@ impl OverlayedDynamicEnvironment { self.base = Arc::new(env); self } + + pub fn insert_constraint(&mut self, key: CcbsKey, constraint: CcbsConstraint) + where + K: Key, + { + self.constraints.entry(key).or_default().push(constraint); + } + + pub fn set_mask(&mut self, mask: Option) { + self.mask = mask; + } } -impl Environment> - for OverlayedDynamicEnvironment +// #[derive(Clone, Copy)] +pub struct CcbsEnvironmentView<'a, W: Waypoint, K> { + view: &'a CcbsEnvironment, + constraints: Option<&'a Vec>>, +} + +impl<'a, W: Waypoint, K> Clone for CcbsEnvironmentView<'a, W, K> { + fn clone(&self) -> Self { + Self { + view: self.view, + constraints: self.constraints, + } + } +} + +impl<'a, W: Waypoint, K> Copy for CcbsEnvironmentView<'a, W, K> {} + +impl<'e, W: Waypoint, K: Key> Environment> + for CcbsEnvironmentView<'e, W, K> { type Obstacles<'a> = impl Iterator> where - W: 'a; + W: 'a, + K: 'a, + 'e: 'a; fn agent_profile(&self) -> &CircularProfile { - self.overlay.profile.as_ref().unwrap_or(&self.base.profile) + self.view + .overlay + .profile + .as_ref() + .unwrap_or(&self.view.base.profile) } fn obstacles<'a>(&'a self) -> Self::Obstacles<'a> { - self.base + self.view + .base .obstacles .iter() .enumerate() - .map(|(i, obs)| self.overlay.obstacles.get(&i).unwrap_or(obs)) + .map(|(i, obs)| self.view.overlay.obstacles.get(&i).unwrap_or(obs)) + .chain( + self.constraints + .iter() + .flat_map(|x| *x) + .filter_map(|constraint| { + if let Some(mask) = self.view.mask { + if constraint.mask == mask { + return None; + } + } + Some(&constraint.obstacle) + }), + ) } } diff --git a/mapf/src/motion/mod.rs b/mapf/src/motion/mod.rs index 88193ac..3df1115 100644 --- a/mapf/src/motion/mod.rs +++ b/mapf/src/motion/mod.rs @@ -36,9 +36,11 @@ pub use travel_effort_cost::*; pub mod travel_time_cost; pub use travel_time_cost::*; +pub mod safe_interval; +pub use safe_interval::*; + pub mod speed_limit; pub use speed_limit::*; -pub mod reach; pub mod conflict; pub use conflict::*; diff --git a/mapf/src/motion/r2/direct_travel.rs b/mapf/src/motion/r2/direct_travel.rs index aed800c..4f3d61d 100644 --- a/mapf/src/motion/r2/direct_travel.rs +++ b/mapf/src/motion/r2/direct_travel.rs @@ -57,7 +57,12 @@ where }; self.extrapolator - .extrapolate(&from_state.waypoint, p_target.borrow().borrow(), &()) + .extrapolate( + &from_state.waypoint, + p_target.borrow().borrow(), + &(), + (Some(&from_state.key), Some(to_goal.key().borrow())), + ) .transpose() .map_err(DirectTravelError::Extrapolator) .map(|action| { diff --git a/mapf/src/motion/r2/line_follow.rs b/mapf/src/motion/r2/line_follow.rs index 4470b20..151ab22 100644 --- a/mapf/src/motion/r2/line_follow.rs +++ b/mapf/src/motion/r2/line_follow.rs @@ -19,13 +19,17 @@ use super::{Position, Positioned, WaypointR2}; use crate::{ domain::{ backtrack_times, flip_endpoint_times, Backtrack, ConflictAvoider, ExtrapolationProgress, - Extrapolator, IncrementalExtrapolator, Reversible, + Extrapolator, IncrementalExtrapolator, Key, Reversible, }, error::{NoError, ThisError}, + graph::Graph, motion::{ self, - conflict::{compute_safe_linear_paths, SafeAction, WaitForObstacle}, - se2, Duration, DynamicEnvironment, SpeedLimiter, + conflict::{ + compute_safe_arrival_path, compute_safe_linear_path_wait_hints, SafeAction, + WaitForObstacle, + }, + se2, Duration, SafeArrivalTimes, SafeIntervalCache, SafeIntervalMotionError, SpeedLimiter, }, util::ForkIter, }; @@ -95,7 +99,7 @@ impl LineFollow { } } -impl Extrapolator for LineFollow +impl Extrapolator for LineFollow where Target: Positioned, Guidance: SpeedLimiter, @@ -105,23 +109,27 @@ where type ExtrapolationIter<'a> = Option, WaypointR2), LineFollowError>> where Target: 'a, - Guidance: 'a; + Guidance: 'a, + Key: 'a; fn extrapolate<'a>( &'a self, from_state: &WaypointR2, to_target: &Target, with_guidance: &Guidance, + _: (Option<&Key>, Option<&Key>), ) -> Self::ExtrapolationIter<'a> where Target: 'a, Guidance: 'a, + Key: 'a, { Some(self.extrapolate_impl(from_state, &to_target.point(), with_guidance.speed_limit())) } } -impl IncrementalExtrapolator for LineFollow +impl IncrementalExtrapolator + for LineFollow where Target: Positioned, Guidance: SpeedLimiter, @@ -131,19 +139,22 @@ where type IncrementalExtrapolationIter<'a> = Option, WaypointR2, ExtrapolationProgress), LineFollowError>> where Target: 'a, - Guidance: 'a; + Guidance: 'a, + Key: 'a; fn incremental_extrapolate<'a>( &'a self, from_state: &WaypointR2, to_target: &Target, with_guidance: &Guidance, + for_keys: (Option<&Key>, Option<&Key>), ) -> Self::IncrementalExtrapolationIter<'a> where Target: 'a, Guidance: 'a, + Key: 'a, { - self.extrapolate(from_state, to_target, with_guidance) + self.extrapolate(from_state, to_target, with_guidance, for_keys) .map(|r| r.map(|(action, state)| (action, state, ExtrapolationProgress::Arrived))) } } @@ -174,28 +185,31 @@ impl Backtrack> for LineFoll } } -impl ConflictAvoider> - for LineFollow +impl> + ConflictAvoider> for LineFollow where + G::Vertex: Positioned, Target: Positioned, Guidance: SpeedLimiter, - W: motion::Waypoint + Into, + K: Key + Clone, { type AvoidanceAction = SmallVec<[SafeAction; 5]>; - type AvoidanceActionIter<'a> = impl IntoIterator> + 'a + type AvoidanceActionIter<'a> = impl IntoIterator> + 'a where Target: 'a, Guidance: 'a, - W: 'a; + K: 'a, + G: 'a; - type AvoidanceError = LineFollowError; + type AvoidanceError = SafeIntervalMotionError; fn avoid_conflicts<'a>( &'a self, from_point: &WaypointR2, to_target: &Target, with_guidance: &Guidance, - in_environment: &DynamicEnvironment, + (from_key, target_key): (Option<&K>, Option<&K>), + safe_intervals: &SafeIntervalCache, ) -> Self::AvoidanceActionIter<'a> where Self: 'a, @@ -203,7 +217,8 @@ where Self::AvoidanceError: 'a, Target: 'a, Guidance: 'a, - DynamicEnvironment: 'a, + K: 'a, + G: 'a, { let from_point = *from_point; let to_point = match self.extrapolate_impl( @@ -212,35 +227,60 @@ where with_guidance.speed_limit(), ) { Ok(extrapolation) => extrapolation.1, - Err(err) => return ForkIter::Left(Some(Err(err)).into_iter()), + Err(err) => { + return ForkIter::Left( + Some(Err(SafeIntervalMotionError::Extrapolator(err))).into_iter(), + ) + } }; - let paths = compute_safe_linear_paths(from_point, to_point, in_environment); - ForkIter::Right( - paths - .into_iter() - // TODO(@mxgrey): Should we pass an error if the last - // element in the action is not a movement? That should - // never happen, so being quiet about it might not be a - // good thing. - // .filter_map(move |action| { - // let wp = action - // .last() - // .map(|m| m.movement()) - // .flatten() - // .copied(); - // wp.map(move |wp| (action, wp)) - // }) - .map(move |action| { - // TODO(@mxgrey): Remove these unwraps before targeting - // production. Either use the map technique that's commented - // out above or return an error. I'm temporarily using - // unwrap to violently catch internal mistakes. - let wp = *action.last().unwrap().movement().unwrap(); - - Ok((action, wp)) - }), - ) + let mut safe_arrival_times = match target_key { + Some(target_key) => match safe_intervals.safe_intervals_for(&target_key) { + Ok(r) => r, + Err(err) => { + return ForkIter::Left( + Some(Err(SafeIntervalMotionError::Cache(err))).into_iter(), + ) + } + }, + None => SafeArrivalTimes::new(), + }; + + let motion_key = if let (Some(from_key), Some(target_key)) = (from_key, target_key) { + Some((from_key.clone(), target_key.clone())) + } else { + None + }; + let environment_view = safe_intervals.environment().view_for(motion_key.as_ref()); + + safe_arrival_times.retain(|t| *t >= to_point.time); + // Add the time when the agent would normally arrive at the vertex. + safe_arrival_times.insert(0, to_point.time); + + let ranked_hints = + compute_safe_linear_path_wait_hints((&from_point, &to_point), None, &environment_view); + + let paths: SmallVec<[_; 5]> = safe_arrival_times + .into_iter() + .filter_map(move |arrival_time| { + compute_safe_arrival_path( + from_point, + to_point, + arrival_time, + &ranked_hints, + &environment_view, + ) + }) + .map(move |action| { + // TODO(@mxgrey): Remove these unwraps before targeting + // production, possibly by returning an error. We're temporarily + // using unwrap to violently catch internal mistakes. + let wp = *action.last().unwrap().movement().unwrap(); + Ok((action, wp)) + }) + .collect(); + + ForkIter::Right(paths.into_iter()) } } @@ -282,7 +322,7 @@ mod tests { let movement = LineFollow::new(2.0).expect("Failed to make LineFollow"); let p_target = Position::new(1.0, 3.0); let (waypoints, _) = movement - .extrapolate(&wp0, &p_target, &()) + .extrapolate(&wp0, &p_target, &(), (Some(&0), Some(&1))) .expect("Failed to extrapolate") .expect("Missing extrapolation result"); assert_eq!(waypoints.len(), 1); diff --git a/mapf/src/motion/r2/space.rs b/mapf/src/motion/r2/space.rs index 066df20..9219d2d 100644 --- a/mapf/src/motion/r2/space.rs +++ b/mapf/src/motion/r2/space.rs @@ -22,7 +22,7 @@ use crate::{ motion::{ r2::*, se2::{MaybeOriented, Orientation, StateSE2}, - IntegrateWaypoints, TimePoint, Timed, + IntegrateWaypoints, MaybeTimed, TimePoint, Timed, }, }; use std::borrow::Borrow; @@ -158,11 +158,17 @@ impl Timed for StateR2 { self.waypoint.set_time(new_time); } - fn time(&self) -> &TimePoint { + fn time(&self) -> TimePoint { self.waypoint.time() } } +impl MaybeTimed for StateR2 { + fn maybe_time(&self) -> Option { + Some(self.waypoint.time()) + } +} + impl Keyed for StateR2 { type Key = K; } diff --git a/mapf/src/motion/r2/timed_position.rs b/mapf/src/motion/r2/timed_position.rs index 1c0c5f6..02dfdc1 100644 --- a/mapf/src/motion/r2/timed_position.rs +++ b/mapf/src/motion/r2/timed_position.rs @@ -21,7 +21,7 @@ use crate::{ motion::{ self, se2::{MaybeOriented, WaypointSE2}, - IntegrateWaypoints, InterpError, Interpolation, TimePoint, Timed, + IntegrateWaypoints, InterpError, Interpolation, MaybeTimed, TimePoint, Timed, }, }; use arrayvec::ArrayVec; @@ -62,8 +62,8 @@ impl WaypointR2 { } impl Timed for WaypointR2 { - fn time(&self) -> &TimePoint { - return &self.time; + fn time(&self) -> TimePoint { + return self.time; } fn set_time(&mut self, new_time: TimePoint) { @@ -71,6 +71,12 @@ impl Timed for WaypointR2 { } } +impl MaybeTimed for WaypointR2 { + fn maybe_time(&self) -> Option { + Some(self.time) + } +} + impl Positioned for WaypointR2 { fn point(&self) -> super::Point { self.position.point() @@ -102,6 +108,13 @@ impl From for WaypointR2 { impl motion::Waypoint for WaypointR2 { type Position = Position; type Velocity = Velocity; + fn position(&self) -> Self::Position { + self.position + } + + fn zero_velocity() -> Self::Velocity { + Velocity::zeros() + } } pub struct Motion { diff --git a/mapf/src/motion/reach.rs b/mapf/src/motion/reach.rs deleted file mode 100644 index 6886a8d..0000000 --- a/mapf/src/motion/reach.rs +++ /dev/null @@ -1,47 +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 crate::{ - error::{NoError, StdError}, - motion::{Trajectory, Waypoint}, -}; - -/// Reachable is a trait to describe attempts to reach directly from a node -/// towards its goal if the goal can be reached from the node with a single -/// motion decision. This returns an iterator in case there are multiple options -/// for how to reach the goal with a single motion decision. -pub trait Reachable { - type ReachError: StdError; - type Reaching<'a>: IntoIterator, Self::ReachError>> - where - Self: 'a, - W: 'a, - Node: 'a, - Goal: 'a; - - fn reach_for<'a>(&'a self, parent: &'a Node, goal: &'a Goal) -> Self::Reaching<'a>; -} - -pub struct NoReach; -impl Reachable for NoReach { - type ReachError = NoError; - type Reaching<'a> = impl Iterator, NoError>> where N: 'a, G: 'a, W: 'a ; - - fn reach_for<'a>(&'a self, _: &'a N, _: &'a G) -> Self::Reaching<'a> { - [].into_iter() - } -} diff --git a/mapf/src/premade/safe_interval_motion.rs b/mapf/src/motion/safe_interval.rs similarity index 50% rename from mapf/src/premade/safe_interval_motion.rs rename to mapf/src/motion/safe_interval.rs index 90e8994..f883b57 100644 --- a/mapf/src/premade/safe_interval_motion.rs +++ b/mapf/src/motion/safe_interval.rs @@ -16,23 +16,16 @@ */ use crate::{ - domain::{ - Activity, Closable, CloseResult, ClosedSet, ClosedStatus, Domain, Key, Keyed, Keyring, - }, + domain::{Closable, CloseResult, ClosedSet, ClosedStatus, Key, Keyed, Keyring}, error::ThisError, - graph::{Edge, Graph}, + graph::Graph, motion::{ - compute_safe_arrival_path, compute_safe_arrival_times, compute_safe_linear_path_wait_hints, - is_safe_segment, + compute_safe_arrival_times, r2::{Positioned, WaypointR2}, - se2::{ - DifferentialDriveLineFollow, DifferentialDriveLineFollowError, KeySE2, MaybeOriented, - Position, StateSE2, WaypointSE2, - }, - Duration, Environment, OverlayedDynamicEnvironment, SafeAction, SafeArrivalTimes, - SpeedLimiter, TimePoint, Timed, WaitForObstacle, + se2::WaypointSE2, + CcbsEnvironment, SafeArrivalTimes, TimePoint, Timed, }, - util::{FlatResultMapTrait, ForkIter, Minimum}, + util::Minimum, }; use smallvec::SmallVec; use std::{ @@ -42,16 +35,19 @@ use std::{ }; pub struct SafeIntervalCache { - pub environment: Arc>, - pub graph: G, + graph: G, + environment: Arc>, earliest_time: Option, safe_intervals: RwLock>, } impl SafeIntervalCache { - pub fn new(environment: Arc>, graph: G) -> Self { + pub fn new(environment: Arc>, graph: G) -> Self + where + G::Key: Key, + { let mut earliest_time = Minimum::new(|a: &TimePoint, b: &TimePoint| a.cmp(b)); - for obs in environment.obstacles() { + for obs in environment.iter_all_obstacles() { if let Some(traj) = obs.trajectory() { earliest_time.consider(&traj.initial_motion_time()); } @@ -66,6 +62,14 @@ impl SafeIntervalCache { } } + pub fn graph(&self) -> &G { + &self.graph + } + + pub fn environment(&self) -> &Arc> { + &self.environment + } + pub fn safe_intervals_for( &self, key: &G::Key, @@ -98,8 +102,12 @@ impl SafeIntervalCache { .borrow() .point(); + let wp = WaypointR2::new(earliest_time, p.x, p.y); + let ccbs_key = (key.clone(), key.clone()); let safe_arrivals = - compute_safe_arrival_times(WaypointR2::new(earliest_time, p.x, p.y), &self.environment); + compute_safe_arrival_times(wp, &self.environment.view_for(Some(&ccbs_key))); + // let safe_arrivals = + // compute_safe_arrival_times(WaypointR2::new(earliest_time, p.x, p.y), &self.environment); match self.safe_intervals.write() { Ok(mut guard) => { guard.insert(key.clone(), safe_arrivals.clone()); @@ -118,252 +126,6 @@ pub enum SafeIntervalCacheError { MissingVertex(K), } -pub struct SafeIntervalMotion { - pub extrapolator: DifferentialDriveLineFollow, - pub safe_intervals: Arc>, -} - -impl Clone for SafeIntervalMotion { - fn clone(&self) -> Self { - Self { - extrapolator: self.extrapolator, - safe_intervals: self.safe_intervals.clone(), - } - } -} - -impl SafeIntervalMotion { - fn compute_safe_arrivals( - &self, - from_state: &StateSE2, - target_key: G::Key, - to_target: &G::Vertex, - with_guidance: &G::EdgeAttributes, - ) -> impl Iterator>> - where - G::Key: Key + Clone, - G::Vertex: Positioned + MaybeOriented, - G::EdgeAttributes: SpeedLimiter, - { - let mut safe_intervals = match self.safe_intervals.safe_intervals_for(&target_key) { - Ok(r) => r, - Err(err) => { - return ForkIter::Left(Some(Err(SafeIntervalMotionError::Cache(err))).into_iter()) - } - }; - - let target_point = to_target.point(); - let mut arrival = match self.extrapolator.move_towards_target( - &from_state.waypoint, - &target_point, - with_guidance, - ) { - Ok(arrival) => arrival, - Err(err) => { - return ForkIter::Left( - Some(Err(SafeIntervalMotionError::Extrapolator(err))).into_iter(), - ) - } - }; - - if arrival.waypoints.len() > 1 { - assert!(arrival.waypoints.len() < 3); - let wp0 = arrival.waypoints[0].clone().into(); - // Make sure the act of rotating to face the target is valid - if !is_safe_segment( - (&from_state.waypoint.into(), &wp0), - None, - &self.safe_intervals.environment, - ) { - // We cannot rotate to face the target, so there is no way to - // avoid conflicts from the start state. - return ForkIter::Left(None.into_iter()); - } - } - - let to_position = match arrival.waypoints.last() { - Some(p) => *p, - // No motion is needed, the agent is already on the target - None => { - return ForkIter::Left(Some(Ok((SmallVec::new(), from_state.waypoint))).into_iter()) - } - }; - - let maybe_oriented = to_target.maybe_oriented(); - let from_point: WaypointR2 = arrival.facing_target.into(); - let to_point: WaypointR2 = to_position.into(); - let yaw = arrival.yaw.angle(); - let ranked_hints = compute_safe_linear_path_wait_hints( - (&from_point, &to_point), - None, - &self.safe_intervals.environment, - ); - - safe_intervals.retain(|t| *t >= to_position.time); - // Add the time when the agent would normally arrive at the vertex. - safe_intervals.insert(0, to_position.time); - - let paths: SmallVec<[_; 5]> = safe_intervals - .into_iter() - .filter_map(|arrival_time| { - compute_safe_arrival_path( - from_point, - to_point, - arrival_time, - &ranked_hints, - &self.safe_intervals.environment, - ) - }) - .filter_map(move |action| { - let mut action: SmallVec<[SafeAction; 5]> = action - .into_iter() - .map(|a| a.map_movement(|wp| wp.with_yaw(yaw))) - .collect(); - - if arrival.waypoints.len() > 1 { - // Add the initial rotation to the safe actions - action.insert(0, SafeAction::Move(arrival.waypoints[0])); - } - - // TODO(@mxgrey): Remove these unwraps before targeting production. - let arrival_wp = *action.last().unwrap().movement().unwrap(); - if let Some(target_yaw) = maybe_oriented { - // TODO(@mxgrey): Consider how to de-duplicate this block - // from the Extrapolator impl. - let delta_yaw_abs = (target_yaw / arrival.yaw).angle().abs(); - if delta_yaw_abs > self.extrapolator.rotational_threshold() { - arrival.time += Duration::from_secs_f64( - self.extrapolator.direction() * delta_yaw_abs - / self.extrapolator.rotational_speed(), - ); - let final_wp = WaypointSE2 { - time: arrival.time, - position: Position::new(target_point.coords, target_yaw.angle()), - }; - - if !is_safe_segment( - (&arrival_wp.into(), &final_wp.into()), - None, - &self.safe_intervals.environment, - ) { - // We cannot rotate to face the target orientation - // so this is not a valid action. - return None; - } - action.push(SafeAction::Move(final_wp)); - } - } - - let wp = *action.last().unwrap().movement().unwrap(); - Some(Ok((action, wp))) - }) - .collect(); - - ForkIter::Right(paths.into_iter()) - } -} - -impl Domain for SafeIntervalMotion { - type State = StateSE2; - type Error = SafeIntervalMotionError; -} - -pub type SafePathSE2 = SmallVec<[SafeAction; 5]>; - -impl Activity> for SafeIntervalMotion -where - G::Key: Key + Clone, - G::Vertex: Positioned + MaybeOriented, - G::EdgeAttributes: Clone + SpeedLimiter, -{ - type ActivityAction = SafePathSE2; - type ActivityError = SafeIntervalMotionError; - type Choices<'a> = impl Iterator), Self::ActivityError>> + 'a - where - G: 'a; - - fn choices<'a>(&'a self, from_state: StateSE2) -> Self::Choices<'a> - where - G: 'a, - G::EdgeAttributes: 'a, - { - // TODO(@mxgrey): We could simplify this implementation considerably if - // we tweak the definition of Extrapolator to also take in the vertex key. - // Then we could implement SafeIntervalMotion as an Extrapolator instead - // of an Activity. Then possibly take advantage of the ConflictAvoidance - // template to implement an Extrapolator. This is all being hard-coded - // for now as a proof of concept. - let vertex = from_state.key.vertex.clone(); - self.safe_intervals - .graph - .edges_from_vertex(&vertex) - .into_iter() - .flat_map(move |edge| { - let from_state = from_state.clone(); - let to_vertex = edge.to_vertex().clone(); - let edge = edge.attributes().clone(); - self.safe_intervals - .graph - .vertex(&to_vertex) - .ok_or_else(|| SafeIntervalMotionError::MissingVertex(to_vertex.clone())) - .flat_result_map(move |v| { - let from_state = from_state.clone(); - let to_vertex = to_vertex.clone(); - self.compute_safe_arrivals( - &from_state, - to_vertex.clone(), - v.borrow(), - &edge, - ) - .map(move |r| { - let to_vertex = to_vertex.clone(); - r.map(move |(action, wp)| { - let state = StateSE2::new(to_vertex.clone(), wp); - (action, state) - }) - }) - }) - .map(|x| x.flatten()) - }) - } -} - -impl Keyed for SafeIntervalMotion -where - G::Key: Key, -{ - type Key = KeySE2; -} - -impl Keyring> for SafeIntervalMotion -where - G::Key: Key, -{ - type KeyRef<'a> = &'a KeySE2 - where - Self: 'a, - StateSE2: 'a; - - fn key_for<'a>(&'a self, state: &'a StateSE2) -> Self::KeyRef<'a> - where - Self: 'a, - StateSE2: 'a, - { - &state.key - } -} - -#[derive(Debug, ThisError)] -pub enum SafeIntervalMotionError { - #[error("The safe interval cache experienced an error:\n{0:?}")] - Cache(SafeIntervalCacheError), - #[error("The vertex {0:?} does not exist in the graph")] - MissingVertex(K), - // Give something - #[error("An error occurred in the extrapolator:\n{0:?}")] - Extrapolator(DifferentialDriveLineFollowError), -} - #[derive(Clone)] pub struct SafeIntervalCloser { keyring: Ring, @@ -452,21 +214,21 @@ where { fn close<'a>(&'a mut self, state: &State, value: T) -> CloseResult<'a, T> { match self.get_closed_intervals(state) { - Some(closed_intervals) => closed_intervals.close(*state.time(), value), + Some(closed_intervals) => closed_intervals.close(state.time(), value), None => CloseResult::Accepted, } } fn replace(&mut self, state: &State, value: T) -> Option { match self.get_closed_intervals(state) { - Some(closed_intervals) => closed_intervals.replace(*state.time(), value), + Some(closed_intervals) => closed_intervals.replace(state.time(), value), None => Some(value), } } fn status<'a>(&'a self, state: &State) -> ClosedStatus<'a, T> { let key = self.keyring.key_for(state); - let time = *state.time(); + let time = state.time(); match self.container.get(key.borrow().borrow()) { Some(closed_intervals) => closed_intervals.status(time), None => ClosedStatus::Open, @@ -501,7 +263,7 @@ impl ClosedIntervals { let mut intervals: SmallVec<[_; 5]> = safe_intervals.into_iter().map(|t| (t, None)).collect(); - intervals.sort_by(|a, b| a.0.cmp(&b.0)); + intervals.sort_unstable_by(|a, b| a.0.cmp(&b.0)); Self { indefinite_start: None, @@ -589,3 +351,14 @@ impl ClosedIntervals { ) } } + +#[derive(Debug, ThisError)] +pub enum SafeIntervalMotionError { + #[error("The safe interval cache experienced an error:\n{0:?}")] + Cache(SafeIntervalCacheError), + #[error("The vertex {0:?} does not exist in the graph")] + MissingVertex(K), + // Give something + #[error("An error occurred in the extrapolator:\n{0:?}")] + Extrapolator(E), +} diff --git a/mapf/src/motion/se2/differential_drive_line_follow.rs b/mapf/src/motion/se2/differential_drive_line_follow.rs index 9631cbe..5438e16 100644 --- a/mapf/src/motion/se2/differential_drive_line_follow.rs +++ b/mapf/src/motion/se2/differential_drive_line_follow.rs @@ -15,19 +15,23 @@ * */ -use super::{MaybeOriented, Point, Position, StateSE2, WaypointSE2}; use crate::{ domain::{ backtrack_times, flip_endpoint_times, Backtrack, ConflictAvoider, Connectable, - ExtrapolationProgress, Extrapolator, IncrementalExtrapolator, Reversible, + ExtrapolationProgress, Extrapolator, IncrementalExtrapolator, Key, Reversible, }, error::{NoError, ThisError}, + graph::Graph, motion::{ self, - conflict::{compute_safe_linear_paths, is_safe_segment, SafeAction, WaitForObstacle}, - r2::{self, MaybePositioned}, - CircularProfile, Duration, DynamicCircularObstacle, Environment, - OverlayedDynamicEnvironment, SpeedLimiter, + conflict::{ + compute_safe_arrival_path, compute_safe_linear_path_wait_hints, is_safe_segment, + SafeAction, WaitForObstacle, + }, + r2::{MaybePositioned, Positioned, WaypointR2}, + se2::{MaybeOriented, Orientation, Point, Position, StateSE2, WaypointSE2}, + CcbsEnvironment, Duration, MaybeTimed, SafeArrivalTimes, SafeIntervalCache, + SafeIntervalMotionError, SpeedLimiter, Timed, }, util::ForkIter, }; @@ -144,7 +148,7 @@ impl DifferentialDriveLineFollow { ) -> Result { // NOTE: We trust that all properties in self have values greater // than zero because we enforce that for all inputs. - let mut output: ArrayVec = ArrayVec::new(); + let mut output: DifferentialDriveLineFollowMotion = ArrayVec::new(); let mut current_time = from_waypoint.time; let mut current_yaw = from_waypoint.position.rotation; let mut facing_target = *from_waypoint; @@ -159,8 +163,7 @@ impl DifferentialDriveLineFollow { let delta_p = self.direction * (*p1 - p0); let distance = delta_p.norm(); if distance > self.translational_threshold { - let approach_yaw = - nalgebra::UnitComplex::from_angle(f64::atan2(delta_p[1], delta_p[0])); + let approach_yaw = Orientation::from_angle(f64::atan2(delta_p[1], delta_p[0])); let delta_yaw_abs = (approach_yaw / from_waypoint.position.rotation) .angle() .abs(); @@ -198,33 +201,41 @@ impl DifferentialDriveLineFollow { } pub(crate) struct ReachedTarget { - pub(crate) waypoints: ArrayVec, + pub(crate) waypoints: DifferentialDriveLineFollowMotion, pub(crate) time: TimePoint, + /// Yaw of the agent as it arrives at the target, regardless of the final + /// yaw that the target might ask for. pub(crate) yaw: nalgebra::UnitComplex, pub(crate) facing_target: WaypointSE2, } -impl Extrapolator for DifferentialDriveLineFollow +pub type DifferentialDriveLineFollowMotion = ArrayVec; + +impl Extrapolator + for DifferentialDriveLineFollow where - Target: r2::Positioned + MaybeOriented, + Target: Positioned + MaybeOriented, Guidance: SpeedLimiter, { - type Extrapolation = ArrayVec; + type Extrapolation = DifferentialDriveLineFollowMotion; type ExtrapolationError = DifferentialDriveLineFollowError; type ExtrapolationIter<'a> = Option> where Target: 'a, - Guidance: 'a; + Guidance: 'a, + Key: 'a; fn extrapolate<'a>( &'a self, from_state: &WaypointSE2, to_target: &Target, with_guidance: &Guidance, + _: (Option<&Key>, Option<&Key>), ) -> Self::ExtrapolationIter<'a> where Target: 'a, Guidance: 'a, + Key: 'a, { let target_point = to_target.point(); let mut arrival = match self.move_towards_target(from_state, &target_point, with_guidance) { @@ -251,10 +262,10 @@ where } } -impl IncrementalExtrapolator +impl IncrementalExtrapolator for DifferentialDriveLineFollow where - Target: r2::Positioned + MaybeOriented, + Target: Positioned + MaybeOriented, Guidance: SpeedLimiter, { type IncrementalExtrapolation = ArrayVec; @@ -265,17 +276,20 @@ where >> where Target: 'a, - Guidance: 'a; + Guidance: 'a, + Key: 'a; fn incremental_extrapolate<'a>( &'a self, from_state: &WaypointSE2, to_target: &Target, with_guidance: &Guidance, + _: (Option<&Key>, Option<&Key>), ) -> Self::IncrementalExtrapolationIter<'a> where Target: 'a, Guidance: 'a, + Key: 'a, { let target_point = to_target.point(); let mut arrival = match self.move_towards_target(from_state, &target_point, with_guidance) { @@ -351,28 +365,32 @@ impl Backtrack> } } -impl ConflictAvoider +impl> + ConflictAvoider> for DifferentialDriveLineFollow where - Target: r2::Positioned + MaybeOriented, + Target: Positioned + MaybeOriented + std::fmt::Debug, Guidance: SpeedLimiter, - Env: Environment>, + K: Key + Clone, + G::Vertex: Positioned, { type AvoidanceAction = SmallVec<[SafeAction; 5]>; type AvoidanceActionIter<'a> = impl IntoIterator> + 'a where Target: 'a, Guidance: 'a, - Env: 'a; + K: 'a, + G: 'a; - type AvoidanceError = DifferentialDriveLineFollowError; + type AvoidanceError = SafeIntervalMotionError; fn avoid_conflicts<'a>( &'a self, from_state: &WaypointSE2, to_target: &Target, with_guidance: &Guidance, - in_environment: &Env, + (from_key, target_key): (Option<&K>, Option<&K>), + safe_intervals: &SafeIntervalCache, ) -> Self::AvoidanceActionIter<'a> where Self: 'a, @@ -381,19 +399,45 @@ where WaypointSE2: 'a, Target: 'a, Guidance: 'a, - Env: 'a, + K: 'a, + G: 'a, { + // println!(" ============= {from_state:?} -> {to_target:?}"); + let mut safe_arrival_times = match target_key { + Some(target_key) => match safe_intervals.safe_intervals_for(&target_key) { + Ok(r) => r, + Err(err) => { + return ForkIter::Left( + Some(Err(SafeIntervalMotionError::Cache(err))).into_iter(), + ) + } + }, + None => SafeArrivalTimes::new(), + }; + + let motion_key = if let (Some(from_key), Some(target_key)) = (from_key, target_key) { + Some((from_key.clone(), target_key.clone())) + } else { + None + }; + let environment_view = safe_intervals.environment().view_for(motion_key.as_ref()); + let target_point = to_target.point(); - let mut arrival = match self.move_towards_target(from_state, &target_point, with_guidance) { + let mut arrival = match self.move_towards_target(&from_state, &target_point, with_guidance) + { Ok(arrival) => arrival, - Err(err) => return ForkIter::Left(Some(Err(err)).into_iter()), + Err(err) => { + return ForkIter::Left( + Some(Err(SafeIntervalMotionError::Extrapolator(err))).into_iter(), + ) + } }; if arrival.waypoints.len() > 1 { assert!(arrival.waypoints.len() < 3); let wp0 = arrival.waypoints[0].clone().into(); // Make sure the act of rotating to face the target is valid - if !is_safe_segment((&from_state.clone().into(), &wp0), None, in_environment) { + if !is_safe_segment((&from_state.clone().into(), &wp0), None, &environment_view) { // We cannot rotate to face the target, so there is no way to // avoid conflicts from the start state. return ForkIter::Left(None.into_iter()); @@ -407,56 +451,71 @@ where }; let maybe_oriented = to_target.maybe_oriented(); - let from_point: r2::WaypointR2 = arrival.facing_target.into(); - let to_point: r2::WaypointR2 = to_position.into(); + let from_point: WaypointR2 = arrival.facing_target.into(); + let to_point: WaypointR2 = to_position.into(); let yaw = arrival.yaw.angle(); - let paths: SmallVec<[_; 3]> = - compute_safe_linear_paths(from_point, to_point, in_environment) - .into_iter() - .filter_map(move |action| { - let mut action: SmallVec<[SafeAction; 5]> = - action - .into_iter() - .map(|a| a.map_movement(|wp| wp.with_yaw(yaw))) - .collect(); - - if arrival.waypoints.len() > 1 { - // Add the initial rotation to the safe actions - action.insert(0, SafeAction::Move(arrival.waypoints[0])); - } + let ranked_hints = + compute_safe_linear_path_wait_hints((&from_point, &to_point), None, &environment_view); + + safe_arrival_times.retain(|t| *t >= to_position.time); + // Add the time when the agent would normally arrive at the vertex. + safe_arrival_times.insert(0, to_position.time); + + let paths: SmallVec<[_; 5]> = safe_arrival_times + .into_iter() + .filter_map(move |arrival_time| { + compute_safe_arrival_path( + from_point, + to_point, + arrival_time, + &ranked_hints, + &environment_view, + ) + }) + .filter_map(move |action| { + let mut action: SmallVec<[SafeAction; 5]> = action + .into_iter() + .map(|a| a.map_movement(|wp| wp.with_yaw(yaw))) + .collect(); + + if arrival.waypoints.len() > 1 { + // Add the initial rotation to the safe actions + action.insert(0, SafeAction::Move(arrival.waypoints[0])); + } - // TODO(@mxgrey): Remove these unwraps before targeting production. - let arrival_wp = *action.last().unwrap().movement().unwrap(); - if let Some(target_yaw) = maybe_oriented { - // TODO(@mxgrey): Consider how to de-duplicate this block - // from the Extrapolator impl. - let delta_yaw_abs = (target_yaw / arrival.yaw).angle().abs(); - if delta_yaw_abs > self.rotational_threshold { - arrival.time += Duration::from_secs_f64( - self.direction * delta_yaw_abs / self.rotational_speed, - ); - let final_wp = WaypointSE2 { - time: arrival.time, - position: Position::new(target_point.coords, target_yaw.angle()), - }; - - if !is_safe_segment( - (&arrival_wp.into(), &final_wp.into()), - None, - in_environment, - ) { - // We cannot rotate to face the target orientation - // so this is not a valid action. - return None; - } - action.push(SafeAction::Move(final_wp)); + // TODO(@mxgrey): Remove these unwraps before targeting production. + let arrival_wp = *action.last().unwrap().movement().unwrap(); + if let Some(target_yaw) = maybe_oriented { + // TODO(@mxgrey): Consider how to de-duplicate this block + // from the Extrapolator impl. + let delta_yaw_abs = (target_yaw / arrival.yaw).angle().abs(); + if delta_yaw_abs > self.rotational_threshold() { + arrival.time += Duration::from_secs_f64( + self.direction() * delta_yaw_abs / self.rotational_speed(), + ); + let final_wp = WaypointSE2 { + time: arrival.time, + position: Position::new(target_point.coords, target_yaw.angle()), + }; + + if !is_safe_segment( + (&arrival_wp.into(), &final_wp.into()), + None, + // &safe_intervals.environment().view_for_hold(target_key), + &environment_view, + ) { + // We cannot rotate to face the target orientation + // so this is not a valid action. + return None; } + action.push(SafeAction::Move(final_wp)); } + } - let wp = *action.last().unwrap().movement().unwrap(); - Some(Ok((action, wp))) - }) - .collect(); + let wp = *action.last().unwrap().movement().unwrap(); + Some(Ok((action, wp))) + }) + .collect(); ForkIter::Right(paths.into_iter()) } @@ -484,9 +543,9 @@ pub struct MergeIntoGoal(pub DifferentialDriveLineFollow); impl Connectable, Action, Target> for MergeIntoGoal where - Action: FromIterator, - Target: MaybePositioned + MaybeOriented + Borrow, - K: PartialEq, + Action: FromIterator + std::fmt::Debug, + Target: MaybePositioned + MaybeOriented + MaybeTimed + Borrow, + K: PartialEq + std::fmt::Debug, { type ConnectionError = DifferentialDriveLineFollowError; type Connections<'a> = Option), Self::ConnectionError>> @@ -514,10 +573,11 @@ where let target_pos = to_target.maybe_point(); let target_orientation = to_target.maybe_oriented(); - if target_pos.is_none() && target_orientation.is_none() { - // If there isn't a position or orientation specified for the goal, - // then simply return. We don't need to do anything special to reach - // the goal. + let target_time = to_target.maybe_time(); + if target_pos.is_none() && target_orientation.is_none() && target_time.is_none() { + // If there isn't a position, orientation, or time specified for the + // goal, then simply return. We don't need to do anything special to + // reach the goal. return None; } @@ -529,9 +589,20 @@ where ); self.0 - .extrapolate(&from_state.waypoint, &target_pos, &()) + .extrapolate( + &from_state.waypoint, + &target_pos, + &(), + (Some(&from_state.key.vertex), Some(goal_key)), + ) .map(|r| { - r.map(|(action, wp)| { + r.map(|(mut action, mut wp)| { + if let Some(t) = to_target.maybe_time() { + if wp.time < t { + wp.set_time(t); + action.push(wp); + } + } let output_action: Action = action.into_iter().collect(); (output_action, StateSE2::new(from_state.key.vertex, wp)) }) @@ -550,16 +621,16 @@ impl Reversible for MergeIntoGoal { } #[derive(Clone)] -pub struct SafeMergeIntoGoal { +pub struct SafeMergeIntoGoal { pub motion: DifferentialDriveLineFollow, // TODO(@mxgrey): Think about how to generalize this - pub environment: Arc>, + pub environment: Arc>, } -impl SafeMergeIntoGoal { +impl SafeMergeIntoGoal { pub fn new( motion: DifferentialDriveLineFollow, - environment: Arc>, + environment: Arc>, ) -> Self { Self { motion, @@ -569,11 +640,11 @@ impl SafeMergeIntoGoal { } impl Connectable, Action, Target> - for SafeMergeIntoGoal + for SafeMergeIntoGoal where Action: FromIterator>, - Target: MaybePositioned + MaybeOriented + Borrow, - K: PartialEq, + Target: MaybePositioned + MaybeOriented + MaybeTimed + Borrow, + K: Clone + Key, { type ConnectionError = DifferentialDriveLineFollowError; type Connections<'a> = Option), Self::ConnectionError>> @@ -595,17 +666,18 @@ where Target: 'a, { let mut prev_wp = from_state.waypoint; - let (action, finish_state): (ArrayVec, _) = - match MergeIntoGoal(self.motion).connect(from_state, to_target)? { + let (action, finish_state): (DifferentialDriveLineFollowMotion, _) = + match MergeIntoGoal(self.motion).connect(from_state.clone(), to_target)? { Ok(connection) => connection, Err(err) => return Some(Err(err)), }; for wp in &action { + let key = (from_state.key.vertex.clone(), from_state.key.vertex.clone()); if !is_safe_segment( (&prev_wp.into(), &wp.clone().into()), None, - &self.environment, + &self.environment.view_for(Some(&key)), ) { return None; } @@ -633,7 +705,7 @@ mod tests { .expect("Failed to make DifferentialLineFollow"); let p_target = Position::new(Vector::new(1.0, 3.0), 60f64.to_radians()); let (waypoints, end) = movement - .extrapolate(&wp0, &p_target, &()) + .extrapolate(&wp0, &p_target, &(), (Some(&0), Some(&1))) .expect("Failed to extrapolate") .expect("The extrapolation should have produced a path"); assert_eq!(waypoints.len(), 3); diff --git a/mapf/src/motion/se2/mod.rs b/mapf/src/motion/se2/mod.rs index 668abbd..a2cc0ba 100644 --- a/mapf/src/motion/se2/mod.rs +++ b/mapf/src/motion/se2/mod.rs @@ -27,6 +27,15 @@ pub struct Velocity { pub rotational: f64, } +impl Velocity { + pub fn zero() -> Self { + Velocity { + translational: Vector::zeros(), + rotational: 0.0, + } + } +} + pub mod timed_position; pub use timed_position::*; @@ -39,7 +48,7 @@ pub use oriented::*; pub type LinearTrajectorySE2 = super::Trajectory; pub mod quickest_path; -pub use quickest_path::QuickestPathHeuristic; +pub use quickest_path::{QuickestPathHeuristic, QuickestPathPlanner}; pub mod differential_drive_line_follow; pub use differential_drive_line_follow::*; diff --git a/mapf/src/motion/se2/quickest_path.rs b/mapf/src/motion/se2/quickest_path.rs index 95c13fb..ebf1331 100644 --- a/mapf/src/motion/se2/quickest_path.rs +++ b/mapf/src/motion/se2/quickest_path.rs @@ -17,13 +17,20 @@ use crate::{ algorithm::{BackwardDijkstra, Path}, - domain::{Extrapolator, Informed, Key, KeyedCloser, Reversible, Weighted}, + domain::{Connectable, Extrapolator, Informed, Key, KeyedCloser, Reversible, Weighted}, error::{Anyhow, ThisError}, motion::{ - r2::{DiscreteSpaceTimeR2, InitializeR2, LineFollow, Positioned, StateR2, WaypointR2}, - se2::{DifferentialDriveLineFollow, KeySE2, MaybeOriented, StateSE2, WaypointSE2}, - SpeedLimiter, + r2::{ + DiscreteSpaceTimeR2, InitializeR2, LineFollow, MaybePositioned, Positioned, StateR2, + WaypointR2, + }, + se2::{ + DifferentialDriveLineFollow, DifferentialDriveLineFollowError, + DifferentialDriveLineFollowMotion, KeySE2, MaybeOriented, MergeIntoGoal, StateSE2, + }, + MaybeTimed, SpeedLimiter, TimePoint, Timed, }, + planner::halt::StepLimit, templates::{GraphMotion, LazyGraphMotion, UninformedSearch}, Graph, Planner, }; @@ -44,7 +51,8 @@ pub type QuickestPathSearch = UninformedSearch< LazyGraphMotion::Key>, G, LineFollow, (), ()>, >; -pub type QuickestPathPlanner = Planner>>>; +pub type QuickestPathPlanner = + Planner>>, StepLimit>; #[derive(Clone)] pub struct QuickestPathHeuristic @@ -52,7 +60,7 @@ where WeightR2: Reversible, WeightR2: Weighted, ArrayVec>, WeightR2::WeightedError: Into, - WeightSE2: Weighted, ArrayVec>, + WeightSE2: Weighted, DifferentialDriveLineFollowMotion>, G: Graph + Reversible + Clone, G::Key: Key + Clone, G::Vertex: Positioned + MaybeOriented, @@ -61,7 +69,15 @@ where planner: QuickestPathPlanner, extrapolator: DifferentialDriveLineFollow, weight_se2: WeightSE2, - cost_cache: Arc, Option>>>, + cost_cache: Arc< + RwLock, Option>>>, + >, +} + +#[derive(Clone, Copy)] +struct CostCache { + cost: Cost, + arrival_state: StateSE2, } type HeuristicKey = (KeySE2, K); @@ -71,7 +87,7 @@ where WeightR2: Reversible, WeightR2: Weighted, ArrayVec>, WeightR2::WeightedError: Into, - WeightSE2: Weighted, ArrayVec>, + WeightSE2: Weighted, DifferentialDriveLineFollowMotion>, G: Graph + Reversible + Clone, G::Key: Key + Clone, G::Vertex: Positioned + MaybeOriented, @@ -89,6 +105,7 @@ where graph: graph.clone(), extrapolator: line_follow, }; + let step_limit = StepLimit::new(Some(10000)); Ok(Self { planner: Planner::new(Arc::new(BackwardDijkstra::new( @@ -103,7 +120,8 @@ where keyring: (), chain: (), }), - )?)), + )?)) + .with_halting(step_limit), extrapolator, weight_se2, cost_cache: Arc::new(RwLock::new(HashMap::new())), @@ -113,31 +131,30 @@ where pub fn planner(&self) -> &QuickestPathPlanner { &self.planner } -} -impl Informed - for QuickestPathHeuristic -where - WeightR2: Reversible + Weighted, ArrayVec>, - WeightR2::Cost: Clone + Ord + Add, - WeightR2::WeightedError: Into, - WeightSE2: Weighted, ArrayVec>, - WeightSE2::Cost: Clone + Add, - WeightSE2::WeightedError: Into, - G: Graph + Reversible + Clone, - G::Key: Key + Clone, - G::Vertex: Positioned + MaybeOriented, - G::EdgeAttributes: SpeedLimiter + Clone, - State: Borrow>, - Goal: Borrow, -{ - type CostEstimate = WeightSE2::Cost; - type InformedError = QuickestPathHeuristicError; - fn estimate_remaining_cost( + pub fn set_step_limit(&mut self, limit: StepLimit) { + self.planner.set_default_halting(limit); + } + + fn invariant_cost( &self, from_state: &State, to_goal: &Goal, - ) -> Result, Self::InformedError> { + ) -> Result>, QuickestPathHeuristicError> + where + WeightR2: Reversible + Weighted, ArrayVec>, + WeightR2::Cost: Clone + Ord + Add, + WeightR2::WeightedError: Into, + WeightSE2: Weighted, DifferentialDriveLineFollowMotion>, + WeightSE2::Cost: Clone + Add, + WeightSE2::WeightedError: Into, + G: Graph + Reversible + Clone, + G::Key: Key + Clone, + G::Vertex: Positioned + MaybeOriented, + G::EdgeAttributes: SpeedLimiter + Clone, + State: Borrow>, + Goal: Borrow + MaybePositioned + MaybeOriented + MaybeTimed, + { let start: &StateSE2 = from_state.borrow(); let goal: &G::Key = to_goal.borrow(); @@ -151,13 +168,13 @@ where } // The cost wasn't in the cache, so let's use the planner to find it. - let cost = 'cost: { + let invariant = 'cost: { let solution: Path<_, _, _> = match self .planner .plan(start.key.vertex.clone(), goal.clone()) - .map_err(|_| QuickestPathHeuristicError::PlannerError)? + .map_err(|err| QuickestPathHeuristicError::PlannerError(err.into()))? .solve() - .map_err(|_| QuickestPathHeuristicError::PlannerError)? + .map_err(|err| QuickestPathHeuristicError::PlannerError(err.into()))? .solution() { Some(solution) => solution, @@ -167,7 +184,7 @@ where let mut cost = match self .weight_se2 .initial_cost(start) - .map_err(|_| QuickestPathHeuristicError::BrokenWeight)? + .map_err(|err| QuickestPathHeuristicError::BrokenWeight(err.into()))? { Some(cost) => cost, None => break 'cost None, @@ -181,44 +198,118 @@ where &previous_state.waypoint, &child_state.waypoint.position, &(), + (Some(&start.key.vertex), Some(goal)), ) { - Some(wp) => wp.map_err(|_| QuickestPathHeuristicError::Extrapolation)?, + Some(wp) => wp.map_err(QuickestPathHeuristicError::Extrapolation)?, None => break 'cost None, }; let child_state = StateSE2::new(child_state.key.clone(), child_wp); - let child_cost = match self .weight_se2 .cost(&previous_state, &action, &child_state) - .map_err(|_| QuickestPathHeuristicError::BrokenWeight)? + .map_err(|err| QuickestPathHeuristicError::BrokenWeight(err.into()))? { Some(cost) => cost, None => break 'cost None, }; cost = cost + child_cost; - previous_state = child_state; } - Some(cost) + + // Shift the time of the final state to what it would be if the + // start time had been zero. + previous_state.waypoint.time = + TimePoint::zero() + (previous_state.waypoint.time - start.waypoint.time); + Some(CostCache { + cost, + arrival_state: previous_state, + }) }; - Ok(cost) + match self.cost_cache.write() { + Ok(mut cache) => { + cache.insert((start.key.clone(), goal.clone()), invariant.clone()); + } + Err(_) => return Err(QuickestPathHeuristicError::PoisonedMutex), + } + + Ok(invariant) + } +} + +impl Informed + for QuickestPathHeuristic +where + WeightR2: Reversible + Weighted, ArrayVec>, + WeightR2::Cost: Clone + Ord + Add, + WeightR2::WeightedError: Into, + WeightSE2: Weighted, DifferentialDriveLineFollowMotion>, + WeightSE2::Cost: Clone + Add, + WeightSE2::WeightedError: Into, + G: Graph + Reversible + Clone, + G::Key: Key + Clone, + G::Vertex: Positioned + MaybeOriented, + G::EdgeAttributes: SpeedLimiter + Clone, + State: Borrow>, + Goal: Borrow + MaybePositioned + MaybeOriented + MaybeTimed, +{ + type CostEstimate = WeightSE2::Cost; + type InformedError = QuickestPathHeuristicError; + fn estimate_remaining_cost( + &self, + from_state: &State, + to_goal: &Goal, + ) -> Result, Self::InformedError> { + // Calculate an invariant for getting to this goal from the start, no + // matter what time it is being done. + let invariant = match self.invariant_cost(from_state, to_goal)? { + Some(r) => r, + None => return Ok(None), + }; + + let start: &StateSE2 = from_state.borrow(); + // Shift the time of the arrival state so that it it makes sense for the + // starting time. + // TODO(@mxgrey): Write unit tests specifically to make sure this time + // shifting is working correctly. + let arrival_state = invariant + .arrival_state + .time_shifted_by(start.time() - TimePoint::zero()); + + // Now add the cost of the final connection, which may vary based on time + let cost = match MergeIntoGoal(self.extrapolator).connect(arrival_state.clone(), to_goal) { + Some(r) => { + let (connect, final_state): (DifferentialDriveLineFollowMotion, _) = + r.map_err(QuickestPathHeuristicError::Extrapolation)?; + + match self + .weight_se2 + .cost(&arrival_state, &connect, &final_state) + .map_err(|err| QuickestPathHeuristicError::BrokenWeight(err.into()))? + { + Some(connection_cost) => invariant.cost + connection_cost, + None => return Ok(None), + } + } + None => invariant.cost, + }; + Ok(Some(cost)) } } // TODO(@mxgrey): Put actual error information inside of here. #[derive(Debug, ThisError)] pub enum QuickestPathHeuristicError { - #[error("An error occurred in the planner")] - PlannerError, + #[error("An error occurred in the planner:\n{0}")] + PlannerError(Anyhow), #[error("The mutex was poisoned")] PoisonedMutex, - #[error("An error occurred during SE2 extrapolation")] - Extrapolation, - #[error("An error occurred while calculating SE2 cost")] - BrokenWeight, + #[error("An error occurred during SE2 extrapolation:\n{0}")] + Extrapolation(DifferentialDriveLineFollowError), + #[error("An error occurred while calculating SE2 cost:\n{0}")] + BrokenWeight(Anyhow), } #[cfg(test)] @@ -230,7 +321,7 @@ mod tests { SharedGraph, SimpleGraph, }, motion::{ - se2::{GoalSE2, Point}, + se2::{GoalSE2, Point, WaypointSE2}, CircularProfile, TimePoint, TravelTimeCost, }, }; @@ -320,18 +411,15 @@ mod tests { .unwrap(); let state_cell = Cell::new(0, 0); - let state_p = state_cell.to_center_point(cell_size); + let state_p = state_cell.center_point(cell_size); let goal_cell = Cell::new(10, 0); - let goal_p = goal_cell.to_center_point(cell_size); + let goal_p = goal_cell.center_point(cell_size); let from_state = StateSE2::new( state_cell, WaypointSE2::new_f64(0.0, state_p.x, state_p.y, 0.0), ); - let goal = GoalSE2 { - key: goal_cell, - orientation: None, - }; + let goal = GoalSE2::new(goal_cell); let remaining_cost_estimate = heuristic .estimate_remaining_cost(&from_state, &goal) @@ -373,10 +461,7 @@ mod tests { WaypointSE2::new_f64(1.256802684, -5.5, -2.5, 45_f64.to_radians()), ); - let goal = GoalSE2 { - key: Cell::new(18, 3), - orientation: None, - }; + let goal = GoalSE2::new(Cell::new(18, 3)); heuristic .estimate_remaining_cost(&from_state, &goal) diff --git a/mapf/src/motion/se2/space.rs b/mapf/src/motion/se2/space.rs index 746d2df..3e20593 100644 --- a/mapf/src/motion/se2/space.rs +++ b/mapf/src/motion/se2/space.rs @@ -25,7 +25,7 @@ use crate::{ motion::{ r2::{MaybePositioned, Point, Positioned}, se2::*, - IntegrateWaypoints, TimePoint, Timed, DEFAULT_ROTATIONAL_THRESHOLD, + IntegrateWaypoints, MaybeTimed, TimePoint, Timed, DEFAULT_ROTATIONAL_THRESHOLD, }, util::{wrap_to_pi, FlatResultMapTrait}, }; @@ -170,11 +170,17 @@ impl Timed for StateSE2 { self.waypoint.set_time(new_time); } - fn time(&self) -> &TimePoint { + fn time(&self) -> TimePoint { self.waypoint.time() } } +impl MaybeTimed for StateSE2 { + fn maybe_time(&self) -> Option { + Some(self.waypoint.time()) + } +} + impl Keyed for StateSE2 { type Key = KeySE2; } @@ -242,6 +248,12 @@ impl MaybePositioned for KeySE2 { } } +impl MaybeTimed for KeySE2 { + fn maybe_time(&self) -> Option { + None + } +} + #[derive(Debug, Clone, Copy, PartialEq)] pub struct StartSE2 { pub time: TimePoint, @@ -733,7 +745,7 @@ impl From for SatisfySE2 { impl Satisfiable, Goal> for SatisfySE2 where - Goal: SelfKey + MaybeOriented, + Goal: SelfKey + MaybeOriented + MaybeTimed, K: PartialEq, { type SatisfactionError = NoError; @@ -755,6 +767,12 @@ where } } + if let Some(target_time) = for_goal.maybe_time() { + if by_state.time() < target_time { + return Ok(false); + } + } + Ok(true) } } @@ -763,6 +781,32 @@ where pub struct GoalSE2 { pub key: K, pub orientation: Option, + pub minimum_time: Option, +} + +impl GoalSE2 { + pub fn new(key: K) -> Self { + Self { + key, + orientation: None, + minimum_time: None, + } + } + + pub fn with_key(mut self, key: K) -> Self { + self.key = key; + self + } + + pub fn with_orientation(mut self, orientation: Option) -> Self { + self.orientation = orientation; + self + } + + pub fn with_minimum_time(mut self, minimum_time: Option) -> Self { + self.minimum_time = minimum_time; + self + } } impl Keyed for GoalSE2 { @@ -791,6 +835,12 @@ impl MaybePositioned for GoalSE2 { } } +impl MaybeTimed for GoalSE2 { + fn maybe_time(&self) -> Option { + self.minimum_time + } +} + impl Borrow for GoalSE2 { fn borrow(&self) -> &K { &self.key diff --git a/mapf/src/motion/se2/timed_position.rs b/mapf/src/motion/se2/timed_position.rs index e862d08..9042a3a 100644 --- a/mapf/src/motion/se2/timed_position.rs +++ b/mapf/src/motion/se2/timed_position.rs @@ -22,7 +22,7 @@ use crate::{ self, r2::{MaybePositioned, Positioned}, se2::{MaybeOriented, Orientation, Oriented}, - timed, IntegrateWaypoints, InterpError, Interpolation, TimePoint, + IntegrateWaypoints, InterpError, Interpolation, MaybeTimed, TimePoint, Timed, }, }; use arrayvec::ArrayVec; @@ -33,9 +33,9 @@ pub struct WaypointSE2 { pub position: Position, } -impl timed::Timed for WaypointSE2 { - fn time(&self) -> &TimePoint { - return &self.time; +impl Timed for WaypointSE2 { + fn time(&self) -> TimePoint { + self.time } fn set_time(&mut self, new_time: TimePoint) { @@ -43,6 +43,12 @@ impl timed::Timed for WaypointSE2 { } } +impl MaybeTimed for WaypointSE2 { + fn maybe_time(&self) -> Option { + Some(self.time) + } +} + impl WaypointSE2 { pub fn new(time: TimePoint, x: f64, y: f64, yaw: f64) -> Self { return WaypointSE2 { @@ -118,6 +124,13 @@ impl From for DebugPositionSE2 { impl motion::Waypoint for WaypointSE2 { type Position = Position; type Velocity = Velocity; + fn position(&self) -> Self::Position { + self.position + } + + fn zero_velocity() -> Self::Velocity { + Velocity::zero() + } } #[derive(Clone, Copy, Debug, PartialEq)] diff --git a/mapf/src/motion/timed.rs b/mapf/src/motion/timed.rs index becb8e2..4e2a07b 100644 --- a/mapf/src/motion/timed.rs +++ b/mapf/src/motion/timed.rs @@ -15,10 +15,10 @@ * */ -use time_point::TimePoint; +use time_point::{Duration, TimePoint}; -pub trait Timed { - fn time(&self) -> &TimePoint; +pub trait Timed: MaybeTimed { + fn time(&self) -> TimePoint; fn set_time(&mut self, new_time: TimePoint); fn with_time(mut self, new_time: TimePoint) -> Self @@ -28,6 +28,24 @@ pub trait Timed { self.set_time(new_time); self } + + fn time_shifted_by(mut self, delta_t: Duration) -> Self + where + Self: Sized, + { + self.set_time(self.time() + delta_t); + self + } +} + +pub trait MaybeTimed { + fn maybe_time(&self) -> Option; +} + +impl MaybeTimed for usize { + fn maybe_time(&self) -> Option { + None + } } #[repr(transparent)] diff --git a/mapf/src/motion/trajectory.rs b/mapf/src/motion/trajectory.rs index 707bc24..363e14f 100644 --- a/mapf/src/motion/trajectory.rs +++ b/mapf/src/motion/trajectory.rs @@ -21,7 +21,7 @@ use sorted_vec::{FindOrInsert, SortedSet}; use std::cell::RefCell; use std::rc::Rc; -#[derive(Clone, Copy, PartialEq, Eq)] +#[derive(Debug, Clone, Copy, PartialEq, Eq)] pub enum FindWaypoint { /// The requested time is exactly on the waypoint at this index Exact(usize), @@ -91,6 +91,10 @@ impl Trajectory { self } + pub fn set_indefinite_initial_time(&mut self, value: bool) { + self.indefinite_initial_time = value; + } + pub fn has_indefinite_initial_time(&self) -> bool { self.indefinite_initial_time } @@ -100,6 +104,10 @@ impl Trajectory { self } + pub fn set_indefinite_finish_time(&mut self, value: bool) { + self.indefinite_finish_time = value; + } + pub fn has_indefinite_finish_time(&self) -> bool { self.indefinite_finish_time } @@ -108,7 +116,7 @@ impl Trajectory { /// If the finish time is equal to the start time, then this will return an /// Err. pub fn hold(from: W, until: TimePoint) -> Result { - if *from.time() == until { + if from.time() == until { return Result::Err(()); } @@ -196,7 +204,7 @@ impl Trajectory { unsafe { let vec = self.waypoints.get_unchecked_mut_vec(); for element in vec.iter_mut() { - let new_time = *element.0.time() + by; + let new_time = element.0.time() + by; element.0.set_time(new_time); } } @@ -220,7 +228,7 @@ impl Trajectory { } pub fn motion_duration(&self) -> Duration { - *self.finish_motion().time() - *self.initial_motion().time() + self.finish_motion().time() - self.initial_motion().time() } /// Trajectories always have at least two values, so we can always get the @@ -243,7 +251,7 @@ impl Trajectory { } pub fn initial_motion_time(&self) -> TimePoint { - *self.initial_motion().time() + self.initial_motion().time() } /// Get the time that the trajectory finishes. @@ -256,7 +264,7 @@ impl Trajectory { } pub fn finish_motion_time(&self) -> TimePoint { - *self.finish_motion().time() + self.finish_motion().time() } /// Make changes to the waypoint at a specified index. If a change is made @@ -294,14 +302,14 @@ impl Trajectory { f(&mut wp.0); if let Some(lower_bound) = lower_bound_opt { - if *wp.0.time() <= lower_bound { + if wp.0.time() <= lower_bound { wp.0.set_time(original_time); return Result::Err(MutateError::InvalidTimeChange); } } if let Some(upper_bound) = upper_bound_opt { - if *wp.0.time() >= upper_bound { + if wp.0.time() >= upper_bound { wp.0.set_time(original_time); return Result::Err(MutateError::InvalidTimeChange); } @@ -373,13 +381,23 @@ impl Trajectory { } impl std::fmt::Debug for Trajectory { - fn fmt(&self, fmt: &mut std::fmt::Formatter) -> std::fmt::Result { - let mut builder = fmt.debug_list(); - for wp in self.waypoints.iter() { + fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result { + f.debug_struct("Trajectory") + .field("indefinite_initial_time", &self.indefinite_initial_time) + .field("indefinite_finish_time", &self.indefinite_finish_time) + .field("waypoints", &DebugWaypoints(&self.waypoints)) + .finish() + } +} + +struct DebugWaypoints<'a, W: Waypoint>(&'a SortedSet>); +impl<'a, W: Waypoint> std::fmt::Debug for DebugWaypoints<'a, W> { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + let mut builder = f.debug_list(); + for wp in self.0.iter() { builder.entry(&wp.0); } - - return builder.finish(); + builder.finish() } } @@ -393,34 +411,47 @@ impl std::ops::Deref for Trajectory { pub struct TrajectoryMotion<'a, W: Waypoint> { trajectory: &'a Trajectory, + // TODO(@mxgrey): We could probably use references with explicit lifetimes + // instead of Rc. motion_cache: RefCell>>, } impl<'a, W: Waypoint> TrajectoryMotion<'a, W> { - fn find_motion_segment(&self, time: &TimePoint) -> Result, InterpError> { + fn find_motion_segment(&self, time: &TimePoint) -> Result, InterpError> { match self.trajectory.find(time) { FindWaypoint::Exact(index) => { if index == 0 { - return Ok(self.get_motion_segment(index + 1)); + return Ok(MotionSegment::Interp(self.get_motion_segment(index + 1))); } else { - return Ok(self.get_motion_segment(index)); + return Ok(MotionSegment::Interp(self.get_motion_segment(index))); } } FindWaypoint::Approaching(index) => { - return Ok(self.get_motion_segment(index)); + return Ok(MotionSegment::Interp(self.get_motion_segment(index))); + } + FindWaypoint::BeforeStart => { + if self.trajectory.has_indefinite_initial_time() { + return Ok(MotionSegment::Holding( + self.trajectory.initial_motion().position(), + )); + } } - FindWaypoint::BeforeStart | FindWaypoint::AfterFinish => { - // TODO(@mxgrey): Handle this differently for trajectories with - // indefinite initial/final times. - return Err(InterpError::OutOfBounds { - range: [ - self.trajectory.initial_motion_time(), - self.trajectory.finish_motion_time(), - ], - request: *time, - }); + FindWaypoint::AfterFinish => { + if self.trajectory.has_indefinite_finish_time() { + return Ok(MotionSegment::Holding( + self.trajectory.finish_motion().position(), + )); + } } } + + Err(InterpError::OutOfBounds { + range: [ + self.trajectory.initial_motion_time(), + self.trajectory.finish_motion_time(), + ], + request: *time, + }) } fn get_motion_segment(&self, index: usize) -> Rc { @@ -441,13 +472,24 @@ impl<'a, W: Waypoint> TrajectoryMotion<'a, W> { } } +enum MotionSegment { + Interp(Rc), + Holding(W::Position), +} + impl<'a, W: Waypoint> Motion for TrajectoryMotion<'a, W> { fn compute_position(&self, time: &TimePoint) -> Result { - return self.find_motion_segment(time)?.compute_position(time); + match self.find_motion_segment(time)? { + MotionSegment::Interp(motion) => motion.compute_position(time), + MotionSegment::Holding(p) => Ok(p), + } } fn compute_velocity(&self, time: &TimePoint) -> Result { - return self.find_motion_segment(time)?.compute_velocity(time); + match self.find_motion_segment(time)? { + MotionSegment::Interp(motion) => motion.compute_velocity(time), + MotionSegment::Holding(_) => Ok(W::zero_velocity()), + } } } @@ -477,7 +519,7 @@ impl<'a, W: Waypoint> TrajectoryIter<'a, W> { } FindWaypoint::Exact(index) => TrajectoryIterNext::Index(index), FindWaypoint::Approaching(index) => TrajectoryIterNext::Index(index - 1), - FindWaypoint::AfterFinish => TrajectoryIterNext::Depleted, + FindWaypoint::AfterFinish => TrajectoryIterNext::StartPostFinish(begin), }; Self { @@ -492,7 +534,8 @@ impl<'a, W: Waypoint> TrajectoryIter<'a, W> { enum TrajectoryIterNext { PreInitial(TimePoint), Index(usize), - PostFinish(TimePoint), + StartPostFinish(TimePoint), + ReachedPostFinish(TimePoint), Depleted, } @@ -517,11 +560,11 @@ impl<'a, W: Waypoint> Iterator for TrajectoryIter<'a, W> { TrajectoryIterNext::Index(index) => { let wp = self.trajectory.get(index).map(|wp| wp.clone()); if let (Some(t_f), Some(wp)) = (self.until, &wp) { - if t_f <= *wp.time() { + if t_f <= wp.time() { // We only include the first element that exceeds the // finish time. self.next_element = TrajectoryIterNext::Depleted; - if *wp.time() < self.begin { + if wp.time() < self.begin { // This element comes before the begin time. // This is not supposed to happen, but let's handle // it gracefully anyway. @@ -531,11 +574,11 @@ impl<'a, W: Waypoint> Iterator for TrajectoryIter<'a, W> { } } - self.next_element = if wp.is_some() { + self.next_element = if index + 1 < self.trajectory.len() { TrajectoryIterNext::Index(index + 1) } else { if let Some(t_f) = self.until { - TrajectoryIterNext::PostFinish(t_f) + TrajectoryIterNext::ReachedPostFinish(t_f) } else { TrajectoryIterNext::Depleted } @@ -543,18 +586,31 @@ impl<'a, W: Waypoint> Iterator for TrajectoryIter<'a, W> { wp } - TrajectoryIterNext::PostFinish(t) => { + TrajectoryIterNext::StartPostFinish(t) => { if !self.trajectory.has_indefinite_finish_time() { self.next_element = TrajectoryIterNext::Depleted; return None; } let mut wp = self.trajectory.finish_motion().clone(); - if *wp.time() < t { - wp.set_time(t); - return Some(wp); + wp.set_time(t); + if let Some(t_f) = self.until { + self.next_element = TrajectoryIterNext::ReachedPostFinish(t_f); + } else { + self.next_element = TrajectoryIterNext::Depleted; } - None + return Some(wp); + } + TrajectoryIterNext::ReachedPostFinish(t) => { + if !self.trajectory.has_indefinite_finish_time() { + self.next_element = TrajectoryIterNext::Depleted; + return None; + } + + let mut wp = self.trajectory.finish_motion().clone(); + wp.set_time(t); + self.next_element = TrajectoryIterNext::Depleted; + return Some(wp); } TrajectoryIterNext::Depleted => None, } diff --git a/mapf/src/motion/travel_effort_cost.rs b/mapf/src/motion/travel_effort_cost.rs index b37fa8e..e2639c4 100644 --- a/mapf/src/motion/travel_effort_cost.rs +++ b/mapf/src/motion/travel_effort_cost.rs @@ -96,7 +96,7 @@ impl> Weighted for Travel // implementation to work both forwards and backwards in time. We are // assuming that any case where time is decreasing in the child state, // a reverse search is being performed. - let duration = (*to_state.time() - *from_state.time()).as_secs_f64().abs(); + let duration = (to_state.time() - from_state.time()).as_secs_f64().abs(); let arclength = action.arclength(from_state, to_state); let cost = self.time * duration + self.translation * arclength.translational diff --git a/mapf/src/motion/travel_time_cost.rs b/mapf/src/motion/travel_time_cost.rs index a15b6ba..722235f 100644 --- a/mapf/src/motion/travel_time_cost.rs +++ b/mapf/src/motion/travel_time_cost.rs @@ -44,7 +44,7 @@ impl Weighted for TravelTimeCost { // implementation to work both forwards and backwards in time. We are // assuming that any case where time is decreasing in the child state, // a reverse search is being performed. - let duration = (*to_state.time() - *from_state.time()).as_secs_f64().abs(); + let duration = (to_state.time() - from_state.time()).as_secs_f64().abs(); Ok(Some(Cost(duration * self.0))) } diff --git a/mapf/src/motion/waypoint.rs b/mapf/src/motion/waypoint.rs index 42bc4c2..c62f251 100644 --- a/mapf/src/motion/waypoint.rs +++ b/mapf/src/motion/waypoint.rs @@ -26,6 +26,12 @@ pub trait Waypoint: /// How does the waypoint represent the time derivative of its position type Velocity; + + /// Get the position of a waypoint + fn position(&self) -> Self::Position; + + /// Provide a zero-valued velocity vector for your waypoint type. + fn zero_velocity() -> Self::Velocity; } /// A trait for states and actions to yield their waypoints diff --git a/mapf/src/negotiation/mod.rs b/mapf/src/negotiation/mod.rs new file mode 100644 index 0000000..6f93d3c --- /dev/null +++ b/mapf/src/negotiation/mod.rs @@ -0,0 +1,1198 @@ +/* + * 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. + * +*/ + +pub mod scenario; +pub use scenario::*; + +use crate::{ + algorithm::{ + path::{DecisionRange, MetaTrajectory}, + AStarConnect, QueueLength, SearchStatus, + }, + domain::{ClosedStatus, Configurable, Cost}, + error::ThisError, + graph::{occupancy::*, SharedGraph}, + motion::{ + have_conflict, + r2::{Positioned, WaypointR2}, + se2::{DifferentialDriveLineFollow, WaypointSE2}, + trajectory::TrajectoryIter, + BoundingBox, CcbsConstraint, CcbsEnvironment, CircularProfile, Duration, + DynamicCircularObstacle, DynamicEnvironment, TimePoint, Timed, TravelEffortCost, + }, + planner::{halt::QueueLengthLimit, Planner}, + premade::{SippSE2, StateSippSE2}, + util::triangular_for, +}; +use std::{ + cmp::Reverse, + collections::{BinaryHeap, HashMap, HashSet}, + sync::Arc, +}; + +#[derive(Debug, ThisError)] +pub enum NegotiationError { + #[error("Some endpoints have a conflict:\n{0:?}")] + ConflictingEndpoints(HashMap), + #[error("It was impossible to find a basic plan for {0}")] + PlanningImpossible(String), + #[error("A solution might have been possible, but we failed to find it")] + PlanningFailed((Vec, HashMap)), +} + +pub fn negotiate( + scenario: &Scenario, + queue_length_limit: Option, +) -> Result< + ( + NegotiationNode, + Vec, + HashMap, + ), + NegotiationError, +> { + let cs = scenario.cell_size; + let mut conflicts = HashMap::new(); + triangular_for(scenario.agents.iter(), |(n_a, a), (n_b, b)| { + for (cell_a, cell_b) in [ + (a.start_cell(), b.start_cell()), + (a.goal_cell(), b.goal_cell()), + ] { + let pa = cell_a.center_point(cs); + let pb = cell_b.center_point(cs); + let dist = (pa - pb).norm(); + let min_dist = a.radius + b.radius; + if dist < min_dist { + conflicts.insert( + (**n_a).clone().min((*n_b).clone()), + (**n_a).clone().max((*n_b).clone()), + ); + } + } + }); + if !conflicts.is_empty() { + return Err(NegotiationError::ConflictingEndpoints(conflicts)); + } + + let (name_map, agents) = { + let mut name_map = HashMap::new(); + let mut agents = Vec::new(); + for (name, agent) in &scenario.agents { + name_map.insert(agents.len(), name.clone()); + agents.push(agent.clone()); + } + + (name_map, agents) + }; + + let grid = { + let mut grid = SparseGrid::new(scenario.cell_size); + let changes: HashMap<_, _> = scenario + .occupancy + .iter() + .flat_map(|(y, row)| row.iter().map(|x| (Cell::new(*x, *y), true))) + .collect(); + grid.change_cells(&changes); + grid + }; + + let profiles: Vec<_> = agents + .iter() + .map(|a| CircularProfile::new(a.radius, 0.0, 0.0).unwrap()) + .collect(); + + let planners = agents + .iter() + .map(|a| { + let profile = CircularProfile::new(a.radius, 0.0, 0.0).unwrap(); + let accessibility = Arc::new(Accessibility::new(grid.clone(), a.radius)); + // let visibility = Arc::new(Visibility::new(grid.clone(), a.radius)); + // let heuristic = SharedGraph::new(VisibilityGraph::new(visibility.clone(), [])); + // let activity = SharedGraph::new(NeighborhoodGraph::new(visibility.clone(), [])); + let activity = SharedGraph::new(AccessibilityGraph::new(accessibility)); + let heuristic = activity.clone(); + let environment = Arc::new(CcbsEnvironment::new(Arc::new(DynamicEnvironment::new( + profile, + )))); + let extrapolator = DifferentialDriveLineFollow::new(a.speed, a.spin).unwrap(); + let weight = TravelEffortCost::default(); + + Planner::new(AStarConnect( + SippSE2::new_sipp_se2(activity, heuristic, extrapolator, environment, weight) + .unwrap(), + )) + .with_halting(QueueLengthLimit(queue_length_limit)) + }) + .collect::>(); + + let mut ideal: Vec = Vec::new(); + for (i, (agent, planner)) in agents.iter().zip(planners.iter()).enumerate() { + let start = agent.make_start(); + let goal = agent.make_goal(); + let s = match match planner.plan(start, goal) { + Ok(search) => search, + Err(err) => { + return Err(NegotiationError::PlanningImpossible( + format!("{err:?}").to_owned(), + )) + } + } + .solve() + .unwrap() + .solution() + { + Some(s) => s, + None => { + return Err(NegotiationError::PlanningImpossible( + name_map.get(&i).unwrap().clone(), + )); + } + }; + let mt = s + .make_trajectory_or_hold(Duration::from_secs(1)) + .unwrap() + .with_indefinite_finish_time(true); + ideal.push(Proposal { + meta: mt, + cost: s.total_cost, + }); + } + + let (mut negotiation_of_agent, mut negotiations) = organize_negotiations(&ideal, &profiles); + + let mut closer = NegotiationCloser::new(); + let mut culled = 0; + let mut count = 0; + let mut solution_node: Option = None; + let mut arena = Vec::new(); + while !negotiations.is_empty() { + dbg!(count); + count += 1; + + let base_env = { + let mut base_env = + DynamicEnvironment::new(CircularProfile::new(0.0, 0.0, 0.0).unwrap()); + for i in 0..agents.len() { + if !negotiation_of_agent.contains_key(&i) { + base_env.obstacles.push( + DynamicCircularObstacle::new(profiles[i]) + .with_trajectory(Some(ideal[i].meta.trajectory.clone())), + ); + } + } + Arc::new(base_env) + }; + + for root in negotiations.values() { + let mut queue: BinaryHeap = BinaryHeap::new(); + let root = NegotiationNode::from_root(root, &ideal, base_env.clone(), arena.len()); + arena.push(root.clone()); + queue.push(QueueEntry::new(root)); + + let mut solution = None; + let mut iters = 0; + while let Some(mut top) = queue.pop() { + iters += 1; + if iters % 10 == 0 { + dbg!(iters); + } + if iters > 1000 { + println!("Too many iterations"); + + // Dump the remaining queue into the node history + println!("Queue begins at {}", arena.len() + 1); + while let Some(remainder) = queue.pop() { + arena.push(remainder.node); + } + + break; + } + + if !closer.close(&top.node) { + culled += 1; + // println!("REDUNDANT NODE: {:?}", top.node.keys); + continue; + } + + // Sort the conflicts such that we pop the the earliest conflict. + // Using Reverse will put the conflicts in descending order, and + // then popping the last element will grab the lowest time. + top.node + .negotiation + .conflicts + .sort_unstable_by_key(|c| Reverse(c.time)); + let next_conflict = match top.node.negotiation.conflicts.pop() { + Some(c) => c, + None => { + // There are no conflicts left, so we have found the + // solution for this negotiation. + // solution = Some(top.node.proposals); + solution = Some(top.node); + + // Dump the remaining queue into the node history + // println!("Queue begins at {}", arena.len() + 1); + // while let Some(remainder) = queue.pop() { + // arena.push(remainder.node); + // } + + break; + } + }; + + let finish_time = top + .node + .proposals + .values() + .max_by_key(|t| t.meta.trajectory.finish_motion_time()) + .unwrap() + .meta + .trajectory + .finish_motion_time(); + + let segments = next_conflict.segments; + for (concede, constraint) in + [(segments[0], segments[1]), (segments[1], segments[0])] + { + let mut environment = top.node.environment.clone(); + // Insert the new constraint on top of the previous + // environment + let env_constraint = CcbsConstraint { + obstacle: DynamicCircularObstacle::new(profiles[constraint.agent]) + .with_trajectory(Some( + top.node + .proposals + .get(&constraint.agent) + .unwrap() + .meta + .get_trajectory_segment(&constraint.range), + )), + mask: constraint.agent, + }; + + match concede.range { + DecisionRange::Before(s, _) | DecisionRange::After(s, _) => { + // dbg!((concede, constraint)); + environment.insert_constraint( + (s.key.vertex, s.key.vertex), + env_constraint.clone(), + ); + + // let mut test_env = DynamicEnvironment::new(profiles[concede.agent]); + // test_env.obstacles.push(env_constraint.obstacle); + // let arrivals = compute_safe_arrival_times(s.waypoint.into(), &test_env); + // assert!(arrivals.len() > 1, "The obstacle should have given us an altered arrival time"); + // let t0 = s.waypoint.time; + // let tf = arrivals.last().cloned(); + // NegotiationKey::new((s.key.vertex, s.key.vertex), (t0, tf), constraint.agent) + } + DecisionRange::Between(range) => { + environment.insert_constraint( + (range[0].state.key.vertex, range[1].state.key.vertex), + env_constraint.clone(), + ); + // let mut test_env = DynamicEnvironment::new(profiles[concede.agent]); + // test_env.obstacles.push(env_constraint.obstacle); + // let paths = compute_safe_linear_paths( + // range[0].state.waypoint.into(), + // range[1].state.waypoint.into(), + // &test_env, + // ); + // assert!(paths.len() <= 1, "We should only get one safe path against a conflict constraint"); + // let t0 = range[0].state.waypoint.time; + // let tf = paths.first().map(|p| p.last().unwrap().movement().unwrap().time); + // NegotiationKey::new((range[0].state.key.vertex, range[1].state.key.vertex), (t0, tf), constraint.agent) + } + }; + + let key = + NegotiationKey::new(&concede.range, &constraint.range, constraint.agent); + + // Set the environment to be suitable for the conceding agent + environment.overlay_profile(profiles[concede.agent]); + environment.set_mask(Some(concede.agent)); + + let agent = &agents[concede.agent]; + // Replan for the conceding agent with this constraint added + let mut search = planners[concede.agent] + .clone() + .configure(|config| config.modify_environment(|_| Ok(environment.clone()))) + .unwrap() + .plan( + agent.make_start(), + agent.make_goal().with_minimum_time(Some(finish_time)), + ) + .unwrap(); + + let solution = match search.solve().unwrap() { + SearchStatus::Solved(solution) => solution, + SearchStatus::Impossible => { + println!( + "The search is IMPOSSIBLE for {}! node: {}:{}, queue: {}, arena: {}", + name_map[&concede.agent], + top.node.id, + concede.agent, + search.memory().0.queue.len(), + search.memory().0.arena.len(), + ); + let mut failed_node = top.node.clone(); + failed_node.conceded = Some(concede.agent); + failed_node.environment = environment; + failed_node.outcome = NodeOutcome::Impossible; + arena.push(failed_node); + continue; + } + SearchStatus::Incomplete => { + println!( + "The search is INCOMPLETE for {}! node: {}:{} measure: {}, queue: {}, arena: {}", + name_map[&concede.agent], + top.node.id, + concede.agent, + search.memory().queue_length(), + search.memory().0.queue.len(), + search.memory().0.arena.len(), + ); + let mut failed_node = top.node.clone(); + failed_node.conceded = Some(concede.agent); + failed_node.environment = environment; + failed_node.outcome = NodeOutcome::Incomplete; + arena.push(failed_node); + continue; + } + }; + + let mt = solution + .make_trajectory_or_hold::(Duration::from_secs(1)) + .unwrap() + .with_indefinite_finish_time(true); + let cost = solution.total_cost; + + let mut proposals = top.node.proposals.clone(); + proposals.insert(concede.agent, Proposal { meta: mt, cost }); + let conflicts = reasses_conflicts(&proposals, &profiles); + + let node = top.node.fork( + conflicts, + proposals, + environment, + key, + Some(concede.agent), + arena.len(), + ); + arena.push(node.clone()); + queue.push(QueueEntry::new(node)); + } + } + + if let Some(solution) = solution { + for (i, mt) in &solution.proposals { + ideal[*i] = mt.clone(); + } + solution_node = Some(solution); + } else { + // TODO(@mxgrey): Consider re-running the negotiation but + // without the base environment in order to identify which other + // agents need to be pulled into the negotiation. + // Even better would be to queue up those nodes as backup nodes + // in a lower priority queue running in parallel, then use the + // outcome if a solution cannot be found. + println!("Culled {culled}"); + return Err(NegotiationError::PlanningFailed((arena, name_map))); + } + } + + (negotiation_of_agent, negotiations) = + reconsider_negotiations(&ideal, &profiles, negotiation_of_agent, negotiations); + } + + if solution_node.is_none() { + let fake = NegotiationNode::from_root( + &Negotiation { + conflicts: vec![], + participants: vec![], + }, + &ideal, + Arc::new(DynamicEnvironment::new( + CircularProfile::new(0.0, 0.0, 0.0).unwrap(), + )), + 0, + ); + + solution_node = Some(fake); + } + + if let Some(node) = &mut solution_node { + for (i, proposal) in ideal.iter().enumerate() { + match node.proposals.entry(i) { + std::collections::hash_map::Entry::Vacant(vacant) => { + vacant.insert(proposal.clone()); + } + _ => {} + } + } + } + + // dbg!(closer); + + println!("Culled {culled}"); + Ok((solution_node.unwrap(), arena, name_map)) + // Ok(ideal.into_iter().enumerate().map(|(i, proposal)| + // (name_map.get(&i).unwrap().clone(), proposal) + // ).collect()) +} + +#[derive(Debug, Clone)] +pub struct Proposal { + pub meta: MetaTrajectory>, + pub cost: Cost, +} + +#[derive(Debug, Clone, Copy, Hash, Eq, PartialEq)] +pub struct NegotiationKey { + pub concede: (Cell, Cell, i64), + pub constraint: (Cell, Cell, i64), + pub mask: usize, +} + +impl NegotiationKey { + pub fn new( + // concede: (Cell, Cell, TimePoint), + // constraint: (Cell, Cell, TimePoint), + concede: &DecisionRange>, + constraint: &DecisionRange>, + mask: usize, + ) -> Self { + let res = 1e3 as i64; + // let res = 1e7 as i64; + let concede = ( + concede.initial_state().key.vertex, + concede.final_state().key.vertex, + concede.initial_state().waypoint.time, + ); + let constraint = ( + constraint.initial_state().key.vertex, + constraint.final_state().key.vertex, + constraint.initial_state().waypoint.time, + ); + Self { + concede: (concede.0, concede.1, concede.2.nanos_since_zero / res), + constraint: ( + constraint.0, + constraint.1, + constraint.2.nanos_since_zero / res, + ), + mask, + } + } +} + +#[derive(Debug, Clone)] +pub struct NegotiationCloser { + pub closed_set: HashMap>, +} + +impl NegotiationCloser { + pub fn new() -> Self { + Self { + closed_set: Default::default(), + } + } + + pub fn status<'a>(&'a self, node: &NegotiationNode) -> ClosedStatus<'a, ()> { + let mut key_iter = node.keys.iter(); + let Some(first_key) = key_iter.next() else { return ClosedStatus::Open }; + + let mut candidates: HashSet = HashSet::from_iter( + self.closed_set + .get(first_key) + .iter() + .flat_map(|x| *x) + .cloned(), + ); + + while let Some(next_key) = key_iter.next() { + if candidates.is_empty() { + break; + } + + let Some(new_candidates) = self.closed_set.get(next_key) else { return ClosedStatus::Open }; + candidates.retain(|c| new_candidates.contains(c)); + } + + if candidates.is_empty() { + ClosedStatus::Open + } else { + ClosedStatus::Closed(&()) + } + } + + pub fn close<'a>(&'a mut self, node: &NegotiationNode) -> bool { + if self.status(node).is_closed() { + return false; + } + + for key in &node.keys { + self.closed_set.entry(*key).or_default().insert(node.id); + } + + true + } +} + +#[derive(Debug, Clone)] +pub struct NegotiationNode { + /// Basic info about this negotiation + pub negotiation: Negotiation, + /// What participant trajectories have been completed. When these are all + /// conflict-free then we have found a solution. + pub proposals: HashMap, + /// The environment that was used to reach this node. The overlay contains + /// the accumulated constraints for this node, inserted as obstacles in the + /// overlay. + pub environment: CcbsEnvironment, + pub keys: HashSet, + pub conceded: Option, + pub cost: Cost, + pub depth: usize, + pub outcome: NodeOutcome, + pub id: usize, + pub parent: Option, +} + +#[derive(Clone, Copy, Debug)] +pub enum NodeOutcome { + Success, + Impossible, + Incomplete, +} + +impl NegotiationNode { + fn from_root( + root: &Negotiation, + ideal: &Vec, + base_env: Arc>, + id: usize, + ) -> Self { + let cost = ideal + .iter() + .fold(Cost(0.0), |cost, proposal| cost + proposal.cost); + Self { + negotiation: root.clone(), + proposals: root + .participants + .iter() + .map(|i| (*i, ideal[*i].clone())) + .collect(), + environment: { + let mut env = CcbsEnvironment::new(base_env); + for i in &root.participants { + env.overlay_trajectory(*i, None).ok(); + } + env + }, + conceded: None, + keys: HashSet::new(), + cost, + depth: 0, + outcome: NodeOutcome::Success, + id, + parent: None, + } + } + + fn fork( + &self, + conflicts: Vec, + proposals: HashMap, + environment: CcbsEnvironment, + key: NegotiationKey, + conceded: Option, + id: usize, + ) -> Self { + let cost = proposals + .values() + .fold(Cost(0.0), |cost, proposal| cost + proposal.cost); + let mut keys = self.keys.clone(); + if !keys.insert(key) { + println!("REDUNDANT KEY IN {}: {key:?}", self.id); + } + NegotiationNode { + negotiation: Negotiation { + conflicts, + participants: self.negotiation.participants.clone(), + }, + proposals, + environment, + conceded, + cost, + keys, + depth: self.depth + 1, + outcome: self.outcome, + id, + parent: Some(self.id), + } + } +} + +#[derive(Clone)] +struct QueueEntry { + node: NegotiationNode, +} + +impl PartialOrd for QueueEntry { + fn partial_cmp(&self, other: &Self) -> Option { + if f64::abs(self.node.cost.0 - other.node.cost.0) < 0.1 { + Reverse(self.node.depth).partial_cmp(&Reverse(other.node.depth)) + } else { + Reverse(self.node.cost).partial_cmp(&Reverse(other.node.cost)) + } + } +} + +impl PartialEq for QueueEntry { + fn eq(&self, other: &Self) -> bool { + self.node.cost.eq(&other.node.cost) + } +} + +impl Ord for QueueEntry { + fn cmp(&self, other: &Self) -> std::cmp::Ordering { + if f64::abs(self.node.cost.0 - other.node.cost.0) < 0.1 { + self.node.depth.cmp(&self.node.depth) + } else { + Reverse(self.node.cost).cmp(&Reverse(other.node.cost)) + } + } +} +impl Eq for QueueEntry {} + +impl QueueEntry { + fn new(node: NegotiationNode) -> Self { + Self { node } + } +} + +#[derive(Debug, Default, Clone)] +pub struct Negotiation { + /// Conflicts that were identified for this state of the negotiation + pub conflicts: Vec, + /// All agents that need to participate in the negotiation + pub participants: Vec, +} + +#[derive(Clone, Copy)] +pub struct Conflict { + time: TimePoint, + segments: [Segment; 2], +} + +impl std::fmt::Debug for Conflict { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.debug_struct("Conflict") + .field("time", &self.time.as_secs_f64()) + .field("segments", &self.segments) + .finish() + } +} + +pub type SippDecisionRange = DecisionRange>; +pub type DecisionRangePair = (SippDecisionRange, SippDecisionRange); + +#[derive(Debug, Clone, Copy)] +pub struct Segment { + agent: usize, + range: SippDecisionRange, +} + +fn reasses_conflicts( + proposals: &HashMap, + profiles: &Vec, +) -> Vec { + let mut conflicts = Vec::new(); + triangular_for( + proposals.iter().map(|(i, p)| (i, &p.meta)), + |(i_a, mt_a), (i_b, mt_b)| { + let profile_a = profiles.get(**i_a).unwrap(); + let profile_b = profiles.get(*i_b).unwrap(); + let (range_a, range_b) = match find_first_conflict(mt_a, profile_a, mt_b, profile_b) { + Some(r) => r, + None => return, + }; + + conflicts.push(Conflict { + time: TimePoint::min( + mt_a.decision_start_time(&range_a), + mt_b.decision_start_time(&range_b), + ), + segments: [ + Segment { + agent: **i_a, + range: range_a, + }, + Segment { + agent: *i_b, + range: range_b, + }, + ], + }); + }, + ); + + conflicts +} + +fn organize_negotiations( + ideal: &Vec, + profiles: &Vec, +) -> (HashMap, HashMap) { + let mut next_conflict_id = 0; + let mut negotiation_of_agent: HashMap = HashMap::new(); + let mut negotiations: HashMap = HashMap::new(); + triangular_for( + ideal + .iter() + .map(|p| &p.meta) + .zip(profiles.iter()) + .enumerate(), + |(i_a, (mt_a, profile_a)), (i_b, (mt_b, profile_b))| { + let (range_a, range_b) = match find_first_conflict(*mt_a, *profile_a, mt_b, profile_b) { + Some(r) => r, + None => return, + }; + + // Check if either agent is already in a conflict + let conflict_id_a = negotiation_of_agent.get(i_a).cloned(); + let conflict_id_b = negotiation_of_agent.get(&i_b).cloned(); + let conflict_id = if let (Some(conflict_id_a), Some(conflict_id_b)) = + (conflict_id_a, conflict_id_b) + { + // Pick the smaller id to merge both into. + let conflict_id = usize::min(conflict_id_a, conflict_id_b); + let acquire_id = usize::max(conflict_id_a, conflict_id_b); + let info = negotiations.remove(&acquire_id).unwrap(); + for c in &info.conflicts { + for w in &c.segments { + negotiation_of_agent.insert(w.agent, conflict_id); + } + } + negotiations + .entry(conflict_id) + .or_default() + .conflicts + .extend(info.conflicts); + + conflict_id + } else if let Some(conflict_id_a) = conflict_id_a { + conflict_id_a + } else if let Some(conflict_id_b) = conflict_id_b { + conflict_id_b + } else { + let conflict_id = next_conflict_id; + next_conflict_id += 1; + conflict_id + }; + + negotiation_of_agent.insert(*i_a, conflict_id); + negotiation_of_agent.insert(i_b, conflict_id); + negotiations + .entry(conflict_id) + .or_default() + .conflicts + .push(Conflict { + time: TimePoint::min( + mt_a.decision_start_time(&range_a), + mt_b.decision_start_time(&range_b), + ), + segments: [ + Segment { + agent: *i_a, + range: range_a, + }, + Segment { + agent: i_b, + range: range_b, + }, + ], + }); + }, + ); + + for negotiation in negotiations.values_mut() { + negotiation.participants = negotiation + .conflicts + .iter() + .flat_map(|c| c.segments.iter().map(|s| s.agent)) + .collect(); + negotiation.participants.sort_unstable(); + negotiation.participants.dedup(); + } + + (negotiation_of_agent, negotiations) +} + +fn reconsider_negotiations( + base: &Vec, + profiles: &Vec, + previous_negotiation_of_agent: HashMap, + previous_negotiations: HashMap, +) -> (HashMap, HashMap) { + let (mut new_negotiation_of_agent, mut new_negotiations) = + organize_negotiations(base, profiles); + + // Now that we've negotiated away some conflicts, check if any new conflicts + // have been formed and pull all newly conflicting agents together into a + // larger negotiation. + + // Key: ID of an old negotiation + // Value: IDs of the new negotiations that ought to contain the participants + // of the old negotiations. + let mut merge_old_negotiation_into_new: HashMap> = HashMap::new(); + for (i, negotiation) in &new_negotiations { + for agent in &negotiation.participants { + let Some(n_prev) = previous_negotiation_of_agent.get(agent).cloned() else { continue }; + merge_old_negotiation_into_new + .entry(n_prev) + .or_default() + .insert(*i); + } + } + + let mut merge_new_negotiation_into: HashMap = HashMap::new(); + for overlapping in merge_old_negotiation_into_new.values() { + let merge_all_into = 'merge: { + // Find if one of them is already supposed to merge into another + for i in overlapping { + if let Some(i_into) = merge_new_negotiation_into.get(i) { + break 'merge *i_into; + } + } + + // None of them is being merged into a new negotiation yet, so pick + // the smallest value. + overlapping.iter().min().cloned().unwrap() + }; + + for i in overlapping { + dbg!(i); + if let Some(prev_parent) = merge_new_negotiation_into.get(i).copied() { + // assert!(!merge_new_negotiation_into.contains_key(&prev_parent)); + if merge_new_negotiation_into.contains_key(&prev_parent) { + dbg!(prev_parent); + dbg!(&merge_new_negotiation_into); + assert!(false); + } + + dbg!((prev_parent, merge_all_into)); + if prev_parent != merge_all_into { + merge_new_negotiation_into.insert(prev_parent, merge_all_into); + } + } + + if *i == merge_all_into { + dbg!(merge_all_into); + merge_new_negotiation_into.remove(&merge_all_into); + } else { + dbg!((*i, merge_all_into)); + merge_new_negotiation_into.insert(*i, merge_all_into); + } + } + } + + let mut count = 0; + let mut inconsistent = true; + while inconsistent { + // TODO(@mxgrey): Remove the counting and assertion after testing + count += 1; + // assert!(count < 1_000_000); + if count > 1_000_000 { + println!("Unable to achieve a consistent renegotiation"); + dbg!(&merge_new_negotiation_into); + assert!(false); + } + + inconsistent = false; + let mut redirect = Vec::new(); + for (from_i, to_i) in &merge_new_negotiation_into { + if let Some(redirect_i) = merge_new_negotiation_into.get(to_i) { + inconsistent = true; + redirect.push((*from_i, *redirect_i)); + } + } + + for (from_i, redirect_i) in redirect { + merge_new_negotiation_into.insert(from_i, redirect_i); + } + } + + for (merge_from, merge_into) in &merge_new_negotiation_into { + let merge_from_n = new_negotiations.remove(merge_from).unwrap(); + let merge_into_n = new_negotiations.get_mut(merge_into).unwrap(); + merge_into_n.conflicts.extend(merge_from_n.conflicts); + merge_into_n.participants.extend(merge_from_n.participants); + merge_into_n.participants.sort_unstable(); + merge_into_n.participants.dedup(); + } + + for (old_i, into_negotiations) in &merge_old_negotiation_into_new { + if let Some(new_n) = into_negotiations.iter().next() { + let merge_into = if let Some(redirect_n) = merge_new_negotiation_into.get(new_n) { + *redirect_n + } else { + *new_n + }; + + let old_n = previous_negotiations.get(old_i).unwrap(); + let merge_into_n = new_negotiations.get_mut(&merge_into).unwrap(); + merge_into_n + .participants + .extend(old_n.participants.iter().copied()); + merge_into_n.participants.sort_unstable(); + merge_into_n.participants.dedup(); + } + } + + for (i, n) in &new_negotiations { + for p in &n.participants { + new_negotiation_of_agent.insert(*p, *i); + } + } + + (new_negotiation_of_agent, new_negotiations) +} + +pub fn find_first_conflict( + mt_a: &MetaTrajectory>, + profile_a: &CircularProfile, + mt_b: &MetaTrajectory>, + profile_b: &CircularProfile, +) -> Option { + let (traj_a, traj_b) = (&mt_a.trajectory, &mt_b.trajectory); + + if !BoundingBox::for_trajectory(profile_a, traj_a) + .overlaps(Some(BoundingBox::for_trajectory(profile_b, traj_b))) + { + return None; + } + + if let Some(r) = find_pre_initial_conflict(mt_a, profile_a, mt_b, profile_b) { + return Some(r); + } + + let mut iter_a = traj_a.iter().pairs().enumerate(); + let mut iter_b = traj_b.iter().pairs().enumerate(); + let mut next_a = iter_a.next(); + let mut next_b = iter_b.next(); + let mut bb_a: Option = None; + let mut bb_b: Option = None; + let conflict_distance_squared = profile_a.conflict_distance_squared_for(profile_b); + + while let (Some((i_a, [wp0_a, wp1_a])), Some((i_b, [wp0_b, wp1_b]))) = (next_a, next_b) { + if wp1_a.time <= wp0_b.time { + bb_a = None; + next_a = iter_a.next(); + continue; + } + + if wp1_b.time <= wp0_a.time { + bb_b = None; + next_b = iter_b.next(); + continue; + } + + let wp0_a: WaypointR2 = wp0_a.into(); + let wp1_a: WaypointR2 = wp1_a.into(); + let wp0_b: WaypointR2 = wp0_b.into(); + let wp1_b: WaypointR2 = wp1_b.into(); + + if bb_a.is_none() { + bb_a = Some(BoundingBox::for_line(profile_a, &wp0_a, &wp1_a)); + } + + if bb_b.is_none() { + bb_b = Some(BoundingBox::for_line(profile_b, &wp0_b, &wp1_b)); + } + + if have_conflict( + (&wp0_a, &wp1_a), + bb_a, + profile_a, + (&wp0_b, &wp1_b), + bb_b, + profile_b, + conflict_distance_squared, + ) { + return Some((mt_a.get_decision_range(i_a), mt_b.get_decision_range(i_b))); + // return dbg!(Some(( + // mt_a.get_decision_range(i_a), + // mt_b.get_decision_range(i_b), + // ))); + } + + if wp1_a.time < wp1_b.time { + bb_a = None; + next_a = iter_a.next(); + } else { + bb_b = None; + next_b = iter_b.next(); + } + } + + if let Some(r) = find_post_finish_conflict(mt_a, profile_a, mt_b, profile_b) { + return Some(r); + } + + None +} + +fn find_pre_initial_conflict( + mt_a: &MetaTrajectory>, + profile_a: &CircularProfile, + mt_b: &MetaTrajectory>, + profile_b: &CircularProfile, +) -> Option { + let (mt_a, profile_a, wp_b, profile_b, swapped) = { + if mt_a.trajectory.initial_motion_time() < mt_b.trajectory.initial_motion_time() { + ( + mt_a, + profile_a, + mt_b.trajectory.initial_motion().clone(), + profile_b, + false, + ) + } else if mt_b.trajectory.initial_motion_time() < mt_a.trajectory.initial_motion_time() { + ( + mt_b, + profile_b, + mt_a.trajectory.initial_motion().clone(), + profile_a, + true, + ) + } else { + return None; + } + }; + + let (i_a, t0) = + match find_spillover_conflict(mt_a.trajectory.iter(), profile_a, wp_b, profile_b, false) { + Some(i_a) => i_a, + None => return None, + }; + + let mut range_a = mt_a.get_decision_range(i_a); + let mut range_b = DecisionRange::Before(mt_b.initial_state.clone(), t0); + if swapped { + std::mem::swap(&mut range_a, &mut range_b); + } + + Some((range_a, range_b)) + // dbg!(Some((range_a, range_b))) +} + +fn find_post_finish_conflict( + mt_a: &MetaTrajectory>, + profile_a: &CircularProfile, + mt_b: &MetaTrajectory>, + profile_b: &CircularProfile, +) -> Option { + let (mt_a, profile_a, mt_b, profile_b, swapped) = { + if mt_b.trajectory.finish_motion_time() < mt_a.trajectory.finish_motion_time() { + (mt_a, profile_a, mt_b, profile_b, false) + } else if mt_a.trajectory.finish_motion_time() < mt_b.trajectory.finish_motion_time() { + (mt_b, profile_b, mt_a, profile_a, true) + } else { + return None; + } + }; + + let wp_b = mt_b.trajectory.finish_motion().clone(); + + let (i_a, tf) = + match find_spillover_conflict(mt_a.trajectory.iter(), profile_a, wp_b, profile_b, true) { + Some(i_a) => i_a, + None => return None, + }; + + // dbg!((relative_i_a, i_a)); + let mut range_a = mt_a.get_decision_range(i_a); + // let mut range_b = mt_b.get_decision_range(mt_b.trajectory.len()); + let mut range_b = DecisionRange::After(mt_b.final_state, tf); + if swapped { + std::mem::swap(&mut range_a, &mut range_b); + } + + Some((range_a, range_b)) + // dbg!(Some((range_a, range_b))) +} + +fn find_spillover_conflict( + iter_a: TrajectoryIter, + profile_a: &CircularProfile, + wp_b: WaypointSE2, + profile_b: &CircularProfile, + // If trailing is true that means we're looking at the indefinite ending of a trajectory. + // If trailing is false that means we're looking at the indefinite beginning of a trajectory. + trailing: bool, +) -> Option<(usize, TimePoint)> { + let bb_b = BoundingBox::for_point(wp_b.point()).inflated_by(profile_b.footprint_radius()); + let wp_b: WaypointR2 = wp_b.into(); + let conflict_distance_squared = profile_a.conflict_distance_squared_for(profile_b); + + for (i_a, [wp0_a, wp1_a]) in iter_a.pairs().enumerate() { + let wp0_a: WaypointR2 = wp0_a.into(); + let wp1_a: WaypointR2 = wp1_a.into(); + + if trailing { + if wp1_a.time < wp_b.time { + continue; + } + } else { + if wp_b.time < wp0_a.time { + break; + } + } + + let (wp0_b, wp1_b) = if trailing { + (wp_b, wp_b.with_time(wp1_a.time)) + } else { + (wp_b.with_time(wp0_a.time), wp_b) + }; + + let t = if trailing { wp1_a.time } else { wp0_a.time }; + + if have_conflict( + (&wp0_a, &wp1_a), + None, + profile_a, + (&wp0_b, &wp1_b), + Some(bb_b), + profile_b, + conflict_distance_squared, + ) { + return Some((i_a, t)); + } + // if have_conflict( + // dbg!((&wp0_a, &wp1_a)), None, profile_a, + // dbg!((&wp0_b, &wp1_b)), Some(bb_b), profile_b, + // conflict_distance_squared, + // ) { + // dbg!(); + // return Some(i_a); + // } + // dbg!(); + } + + None +} diff --git a/mapf/src/negotiation/scenario.rs b/mapf/src/negotiation/scenario.rs new file mode 100644 index 0000000..69073d4 --- /dev/null +++ b/mapf/src/negotiation/scenario.rs @@ -0,0 +1,136 @@ +/* + * 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 crate::{ + graph::occupancy::Cell, + motion::{ + se2::{GoalSE2, Orientation, StartSE2, WaypointSE2}, + TimePoint, Trajectory, + }, +}; +use serde::{Deserialize, Serialize}; +use std::collections::{BTreeMap, HashMap}; + +pub type LinearTrajectorySE2 = Trajectory; + +#[derive(Serialize, Deserialize, Clone, Debug, Copy)] +pub struct Agent { + /// Start cell + pub start: [i64; 2], + /// Initial yaw of the robot + pub yaw: f64, + /// Goal cell + pub goal: [i64; 2], + /// Radius of the robot's footprint (meters) + #[serde(default = " default_radius")] + pub radius: f64, + /// Translational speed of the robot (meters/sec) + #[serde(default = "default_speed")] + pub speed: f64, + /// How fast the robot can spin (radians/sec) + #[serde(default = "default_spin")] + pub spin: f64, + // TODO(@mxgrey): Allow parameters for travel effort cost +} + +impl Agent { + pub fn start_cell(&self) -> Cell { + self.start.into() + } + + pub fn goal_cell(&self) -> Cell { + self.goal.into() + } + + pub fn make_start(&self) -> StartSE2 { + StartSE2 { + time: TimePoint::zero(), + key: Cell::from(self.start), + orientation: Orientation::from_angle(self.yaw), + } + } + + pub fn make_goal(&self) -> GoalSE2 { + GoalSE2::new(Cell::from(self.goal)) + } +} + +#[derive(Serialize, Deserialize)] +pub struct Obstacle { + /// Trajectory of the obstacle in terms of (time (s), x cell, y cell) + pub trajectory: Vec<(f64, i64, i64)>, + /// Radius of the obstacle + #[serde(default = "default_radius")] + pub radius: f64, + #[serde(default = "bool_false", skip_serializing_if = "is_false")] + pub indefinite_start: bool, + #[serde(default = "bool_false", skip_serializing_if = "is_false")] + pub indefinite_finish: bool, +} + +impl Obstacle { + pub fn new(radius: f64, trajectory: &LinearTrajectorySE2, cell_size: f64) -> Obstacle { + Obstacle { + trajectory: trajectory + .iter() + .map(|wp| { + let cell = Cell::from_point(wp.position.translation.vector.into(), cell_size); + (wp.time.as_secs_f64(), cell.x, cell.y) + }) + .collect(), + radius, + indefinite_start: trajectory.has_indefinite_initial_time(), + indefinite_finish: trajectory.has_indefinite_finish_time(), + } + } +} + +#[derive(Serialize, Deserialize)] +pub struct Scenario { + pub agents: BTreeMap, + pub obstacles: Vec, + // y -> [..x..] + pub occupancy: HashMap>, + #[serde(default = "default_cell_size")] + pub cell_size: f64, + #[serde(skip_serializing_if = "Option::is_none", default)] + pub camera_bounds: Option<[[f32; 2]; 2]>, +} + +pub fn default_radius() -> f64 { + 0.45 +} + +pub fn default_speed() -> f64 { + 0.75 +} + +pub fn default_spin() -> f64 { + 60_f64.to_radians() +} + +pub fn default_cell_size() -> f64 { + 1.0 +} + +pub fn bool_false() -> bool { + false +} + +pub fn is_false(b: &bool) -> bool { + !b +} diff --git a/mapf/src/planner/halt.rs b/mapf/src/planner/halt.rs index 7e3a1e8..49e86d5 100644 --- a/mapf/src/planner/halt.rs +++ b/mapf/src/planner/halt.rs @@ -15,7 +15,7 @@ * */ -use crate::algorithm::{Measure, MinimumCostBound}; +use crate::algorithm::{MinimumCostBound, QueueLength}; use std::{ops::Fn, sync::Arc}; /// A trait to define conditions in which a search should be halted. The @@ -72,7 +72,7 @@ impl Halt for Interruptible { /// This option sets a maximum number of steps that can be taken before the /// planner is told to halt. -#[derive(Debug, Clone)] +#[derive(Debug, Clone, Copy)] pub struct StepLimit { steps: usize, pub limit: Option, @@ -103,15 +103,15 @@ impl Halt for StepLimit { /// solve attempt quits. For example, this might put a limit on how large /// the search queue can get. #[derive(Default, Clone)] -pub struct MeasureLimit(pub Option); +pub struct QueueLengthLimit(pub Option); -impl Halt for MeasureLimit +impl Halt for QueueLengthLimit where - Mem: Measure, + Mem: QueueLength, { fn halt(&mut self, memory: &Mem) -> bool { if let Some(limit) = self.0 { - return memory.size() > limit; + return memory.queue_length() > limit; } false @@ -174,8 +174,8 @@ mod tests { struct FakeMem; - impl Measure for FakeMem { - fn size(&self) -> usize { + impl QueueLength for FakeMem { + fn queue_length(&self) -> usize { 0 } } @@ -192,7 +192,7 @@ mod tests { let mut halting = ( Interruptible::new(|| Interruption::Continue), StepLimit::new(Some(5)), - MeasureLimit(Some(100)), + QueueLengthLimit(Some(100)), ); for _ in 0..5 { diff --git a/mapf/src/planner/mod.rs b/mapf/src/planner/mod.rs index 37a62f4..b328bf1 100644 --- a/mapf/src/planner/mod.rs +++ b/mapf/src/planner/mod.rs @@ -75,6 +75,10 @@ impl Planner { } } + pub fn set_default_halting(&mut self, halting: Halting) { + self.default_halting = halting; + } + /// Consume this Planner, modify its Algorithm, and return a new planner /// with the modified algorithm. /// @@ -171,10 +175,9 @@ impl Planner { impl Configurable for Planner { type Configuration = A::Configuration; - type ConfigurationError = A::ConfigurationError; - fn configure(self, f: F) -> Result + fn configure(self, f: F) -> Result where - F: FnOnce(Self::Configuration) -> Self::Configuration, + F: FnOnce(Self::Configuration) -> Result, { Ok(Self { algorithm: self.algorithm.configure(f)?, diff --git a/mapf/src/premade/mod.rs b/mapf/src/premade/mod.rs index 327fe8e..3855e7a 100644 --- a/mapf/src/premade/mod.rs +++ b/mapf/src/premade/mod.rs @@ -21,8 +21,5 @@ pub use search_r2::*; pub mod search_se2; pub use search_se2::*; -pub mod safe_interval_motion; -pub use safe_interval_motion::*; - pub mod sipp_se2; pub use sipp_se2::*; diff --git a/mapf/src/premade/search_se2.rs b/mapf/src/premade/search_se2.rs index 5fe7983..913f95f 100644 --- a/mapf/src/premade/search_se2.rs +++ b/mapf/src/premade/search_se2.rs @@ -154,10 +154,8 @@ mod tests { let solution = planner .plan( (0usize, 20_f64.to_radians()), - GoalSE2 { - key: 8usize, - orientation: Some(Orientation::from_angle(90_f64.to_radians())), - }, + GoalSE2::new(8usize) + .with_orientation(Some(Orientation::from_angle(90_f64.to_radians()))), ) .unwrap() .solve() @@ -190,10 +188,7 @@ mod tests { let solution = planner .plan( (Cell::new(-3, -3), 20_f64.to_radians()), - GoalSE2 { - key: Cell::new(10, 10), - orientation: None, - }, + GoalSE2::new(Cell::new(10, 10)), ) .unwrap() .solve() @@ -204,10 +199,8 @@ mod tests { let solution = planner .plan( (Cell::new(-3, -3), 20_f64.to_radians()), - GoalSE2 { - key: Cell::new(10, 10), - orientation: Some(Orientation::from_angle(-60_f64.to_radians())), - }, + GoalSE2::new(Cell::new(10, 10)) + .with_orientation(Some(Orientation::from_angle(-60_f64.to_radians()))), ) .unwrap() .solve() diff --git a/mapf/src/premade/sipp_se2.rs b/mapf/src/premade/sipp_se2.rs index 5197089..ff57ea7 100644 --- a/mapf/src/premade/sipp_se2.rs +++ b/mapf/src/premade/sipp_se2.rs @@ -17,18 +17,18 @@ use crate::{ domain::{Configurable, Key, Reversible}, + error::{Anyhow, StdError}, graph::{Graph, SharedGraph}, motion::{ r2::Positioned, se2::{quickest_path::QuickestPathSearch, *}, - OverlayedDynamicEnvironment, SpeedLimiter, TravelEffortCost, + CcbsEnvironment, SafeIntervalCache, SafeIntervalCloser, SpeedLimiter, TravelEffortCost, }, - premade::{SafeIntervalCache, SafeIntervalCloser, SafeIntervalMotion}, templates::{ConflictAvoidance, GraphMotion, InformedSearch, LazyGraphMotion}, }; use std::sync::Arc; -const DEFAULT_RES: u32 = 360; +pub const DEFAULT_SIPP_RES: u32 = 360; /// A specialization of InformedSearch for performing Safe Interval Path /// Planning (SIPP) through the SE(2) space (Special Euclidean Group, dimension @@ -43,24 +43,28 @@ const DEFAULT_RES: u32 = 360; /// [`crate::algorithm::AStar`], otherwise the planner will fail to reach the /// goal. pub type SippSE2 = InformedSearch< - SafeIntervalMotion, DEFAULT_RES>, + // SafeIntervalMotion, DEFAULT_SIPP_RES>, + GraphMotion< + DiscreteSpaceTimeSE2<::Key, DEFAULT_SIPP_RES>, + SharedGraph, + ConflictAvoidance>>, + >, TravelEffortCost, - QuickestPathHeuristic, TravelEffortCost, TravelEffortCost, DEFAULT_RES>, - SafeIntervalCloser::Key, DEFAULT_RES>, SharedGraph>, - InitializeSE2, DEFAULT_RES>, + QuickestPathHeuristic, TravelEffortCost, TravelEffortCost, DEFAULT_SIPP_RES>, + SafeIntervalCloser::Key, DEFAULT_SIPP_RES>, SharedGraph>, + InitializeSE2, DEFAULT_SIPP_RES>, SatisfySE2, LazyGraphMotion< - DiscreteSpaceTimeSE2<::Key, DEFAULT_RES>, + DiscreteSpaceTimeSE2<::Key, DEFAULT_SIPP_RES>, SharedGraph, - ConflictAvoidance< - DifferentialDriveLineFollow, - Arc>, - >, + ConflictAvoidance>>, (), - SafeMergeIntoGoal, + SafeMergeIntoGoal<::Key, DEFAULT_SIPP_RES>, >, >; +pub type StateSippSE2 = StateSE2; + pub type NewSippSE2Error = , TravelEffortCost> as Reversible>::ReversalError; @@ -78,7 +82,7 @@ where activity_graph: SharedGraph, heuristic_graph: SharedGraph, extrapolator: DifferentialDriveLineFollow, - environment: Arc>, + environment: Arc>, weight: TravelEffortCost, ) -> Result> { let cache = Arc::new(SafeIntervalCache::new( @@ -86,112 +90,302 @@ where activity_graph.clone(), )); - let activity_motion = SafeIntervalMotion { - extrapolator, - safe_intervals: cache.clone(), - }; - - let connect_motion = GraphMotion { - space: DiscreteSpaceTimeSE2::::new(), + let activity_motion = GraphMotion { + space: DiscreteSpaceTimeSE2::::new(), graph: activity_graph.clone(), extrapolator: ConflictAvoidance { avoider: extrapolator, - environment: environment.clone(), + environment: cache.clone(), }, }; Ok(InformedSearch::new( - activity_motion, + activity_motion.clone(), weight, QuickestPathHeuristic::new(heuristic_graph, weight, weight, extrapolator)?, SafeIntervalCloser::new( - DiscreteSpaceTimeSE2::::new(), + DiscreteSpaceTimeSE2::::new(), cache.clone(), ), ) .with_initializer(InitializeSE2(activity_graph)) .with_satisfier(SatisfySE2::from(extrapolator)) .with_connector(LazyGraphMotion { - motion: connect_motion, + motion: activity_motion, keyring: (), chain: SafeMergeIntoGoal::new(extrapolator, environment), })) } } -pub struct SippSE2Configuration { - pub activity_graph: SharedGraph, - pub heuristic_graph: SharedGraph, - pub motion: DifferentialDriveLineFollow, - pub environment: Arc>, - pub weight: TravelEffortCost, +pub struct SippSE2Configuration +where + G: Graph, + H: Graph + Reversible, + H::Key: Key + Clone, + H::Vertex: Positioned + MaybeOriented, + H::EdgeAttributes: SpeedLimiter + Clone, +{ + safe_intervals: Arc>>, + cache: SippSE2ManageCache, } -impl SippSE2Configuration { - pub fn modify_environment(mut self, f: F) -> Self +impl SippSE2Configuration +where + G: Graph + Clone, + G::Key: Key + Clone, + H: Graph + Reversible, + H::Key: Key + Clone, + H::Vertex: Positioned + MaybeOriented, + H::EdgeAttributes: SpeedLimiter + Clone, + H::ReversalError: StdError + Send + Sync, +{ + /// Modify the environment of the domain. This allows dynamic obstacles to + /// be modified. + /// + /// ### Warning + /// + /// Using this function will invalidate the [`SafeIntervalCache`] so safe + /// intervals will be recalculated during the next search. This is a + /// relatively inexpensive cache to repopulate, but it's still a hit to + /// performance, so you should not call this function needlessly. + pub fn modify_environment(mut self, f: F) -> Result where F: FnOnce( - OverlayedDynamicEnvironment, - ) -> OverlayedDynamicEnvironment, + CcbsEnvironment, + ) -> Result, Anyhow>, + { + let (env, graph) = { + let (environment, graph) = { + let scoped = self.safe_intervals; + (scoped.environment().clone(), scoped.graph().clone()) + }; + + // Attempt to reclaim the data held by the Arc, as long as it's not + // being shared anywhere else. Otherwise we have to make a clone of it. + match Arc::try_unwrap(environment) { + Ok(env) => (env, graph), + Err(arc_env) => ((*arc_env).clone(), graph), + } + }; + + match f(env) { + Ok(env) => { + self.safe_intervals = Arc::new(SafeIntervalCache::new(Arc::new(env), graph)); + Ok(self) + } + Err(err) => Err(err), + } + } + + /// Modify the graph used to define the activity. This allows you to close + /// or open lanes, toggle occupancy of cells, etc. + /// + /// ### Warning + /// + /// This does not modify the graph used to calculate the heuristic. To + /// access that graph, use [`discard_cache`]. If the activity graph and the + /// heuristic graph are too far out of sync then you may get sub-optimal + /// results or worse performance. + /// + /// Using this function will invalidate the [`SafeIntervalCache`] so safe + /// intervals will be recalculated during the next search. This is a + /// relatively inexpensive cache to repopulate, but it's still a hit to + /// performance, so you should not call this function needlessly. + pub fn modify_activity_graph(mut self, f: F) -> Result + where + F: FnOnce(G) -> Result, { - // Attempt to reclaim the data held by the Arc, as long as it's not - // being shared anywhere else. Otherwise we have to make a clone of it. - let env = match Arc::try_unwrap(self.environment) { - Ok(env) => env, - Err(arc_env) => (*arc_env).clone(), + let (env, graph) = { + // Move the values out of self so that they are not owned elsewhere. + let scoped = self.safe_intervals; + (scoped.environment().clone(), scoped.graph().clone()) }; - self.environment = Arc::new(f(env)); - self + match graph.modify(f) { + Ok(graph) => { + self.safe_intervals = Arc::new(SafeIntervalCache::new(env, graph)); + Ok(self) + } + Err(err) => Err(err), + } + } + + /// Replace the graph used to define the activity. With this function, you + /// can provide a graph that is shared with other stakeholders. In contrast, + /// `modify_activity_graph` will allow you to change the graph `G` instance + /// itself, either by cloning a new one or by handing over sole ownership if + /// the graph was not being shared. + /// + /// ### Warning + /// + /// This does not modify the graph used to calculate the heuristic. To + /// access that graph, use [`discard_cache`]. If the activity graph and the + /// heuristic graph are too far out of sync then you may get sub-optimal + /// results or worse performance. + /// + /// Using this function will invalidate the [`SafeIntervalCache`] so safe + /// intervals will be recalculated during the next search. This is a + /// relatively inexpensive cache to repopulate, but it's still a hit to + /// performance, so you should not call this function needlessly. + pub fn replace_activity_graph(mut self, graph: SharedGraph) -> Result { + self.safe_intervals = Arc::new(SafeIntervalCache::new( + self.safe_intervals.environment().clone(), + graph, + )); + Ok(self) + } + + /// Get access to parts of the configuration that cannot be modified without + /// discarding the whole heuristic cache. Recalculating the heuristic cache + /// can incur a substantial performance cost, potentially doubling the + /// planning time or worse. + /// + /// Before using this function in production software, it is recommended + /// that you run benchmarks to test your usage strategy, unless maximum + /// performance is not a prevailing concern for your use case. + pub fn discard_cache(mut self, f: F) -> Result + where + F: FnOnce(SippSE2DiscardCache) -> Result, Anyhow>, + H: Reversible, + H::ReversalError: Into + 'static, + { + let discard = match self.cache { + SippSE2ManageCache::Preserve(cache) => { + let backward_domain = cache.heuristic.planner().algorithm().backward().domain(); + SippSE2DiscardCache { + motion: cache.motion, + weight: cache.weight, + heuristic_graph: backward_domain + .activity + .graph + .reversed() + .map_err(|e| Anyhow::from(e))?, + } + } + SippSE2ManageCache::Discard(discard) => discard, + }; + + match f(discard) { + Ok(discard) => { + self.cache = SippSE2ManageCache::Discard(discard); + Ok(self) + } + Err(err) => Err(err), + } } } +pub enum SippSE2ManageCache +where + H: Graph + Reversible, + H::Key: Key + Clone, + H::Vertex: Positioned + MaybeOriented, + H::EdgeAttributes: SpeedLimiter + Clone, +{ + Preserve(SippSE2PreserveCache), + Discard(SippSE2DiscardCache), +} + +pub struct SippSE2PreserveCache +where + H: Graph + Reversible, + H::Key: Key + Clone, + H::Vertex: Positioned + MaybeOriented, + H::EdgeAttributes: SpeedLimiter + Clone, +{ + motion: DifferentialDriveLineFollow, + weight: TravelEffortCost, + heuristic: + QuickestPathHeuristic, TravelEffortCost, TravelEffortCost, DEFAULT_SIPP_RES>, +} + +pub struct SippSE2DiscardCache { + pub motion: DifferentialDriveLineFollow, + pub heuristic_graph: SharedGraph, + pub weight: TravelEffortCost, +} + impl Configurable for SippSE2 where - G: Graph, + G: Graph + Clone, G::Key: Key + Clone, G::Vertex: Positioned + MaybeOriented, G::EdgeAttributes: SpeedLimiter + Clone, H: Graph + Reversible, H::Vertex: Positioned + MaybeOriented, H::EdgeAttributes: SpeedLimiter + Clone, + H::ReversalError: StdError + Send + Sync + 'static, { type Configuration = SippSE2Configuration; - type ConfigurationError = NewSippSE2Error; - fn configure(self, f: F) -> Result + fn configure(self, f: F) -> Result where - F: FnOnce(Self::Configuration) -> Self::Configuration, + F: FnOnce(Self::Configuration) -> Result, { let config = { // Move from the self variable into a temporary that will die at the // end of this scope, allowing us to potentially transfer all shared // data ownership into the config. let scoped_self = self; - SippSE2Configuration { - activity_graph: scoped_self.activity.safe_intervals.graph.clone(), - heuristic_graph: scoped_self - .heuristic - .planner() - .algorithm() - .backward() - .domain() - .activity - .graph - .clone(), + let preserve = SippSE2PreserveCache { motion: scoped_self.connector.motion.extrapolator.avoider, - environment: scoped_self.activity.safe_intervals.environment.clone(), weight: scoped_self.weight, + heuristic: scoped_self.heuristic.clone(), + }; + + SippSE2Configuration { + safe_intervals: scoped_self.activity.extrapolator.environment, + cache: SippSE2ManageCache::Preserve(preserve), } }; - let config = f(config); - Self::new_sipp_se2( - config.activity_graph, - config.heuristic_graph, - config.motion, - config.environment, - config.weight, - ) + let config = match f(config) { + Ok(config) => config, + Err(err) => return Err(err), + }; + + let domain = match config.cache { + SippSE2ManageCache::Preserve(preserve) => { + let activity_graph = config.safe_intervals.graph().clone(); + let environment = config.safe_intervals.environment().clone(); + let activity_motion = GraphMotion { + space: DiscreteSpaceTimeSE2::::new(), + graph: activity_graph.clone(), + extrapolator: ConflictAvoidance { + avoider: preserve.motion, + environment: config.safe_intervals.clone(), + }, + }; + + let closer = SafeIntervalCloser::new( + DiscreteSpaceTimeSE2::::new(), + config.safe_intervals.clone(), + ); + + InformedSearch::new( + activity_motion.clone(), + preserve.weight, + preserve.heuristic, + closer, + ) + .with_initializer(InitializeSE2(activity_graph.clone())) + .with_satisfier(SatisfySE2::from(preserve.motion)) + .with_connector(LazyGraphMotion { + motion: activity_motion, + keyring: (), + chain: SafeMergeIntoGoal::new(preserve.motion, environment), + }) + } + SippSE2ManageCache::Discard(discard) => Self::new_sipp_se2( + config.safe_intervals.graph().clone(), + discard.heuristic_graph, + discard.motion, + config.safe_intervals.environment().clone(), + discard.weight, + ) + .map_err(|e| Anyhow::new(e))?, + }; + Ok(domain) } } @@ -256,7 +450,7 @@ mod tests { ); let profile = CircularProfile::new(0.1, 0.25, 1.0).unwrap(); - let environment = Arc::new(OverlayedDynamicEnvironment::new(Arc::new({ + let environment = Arc::new(CcbsEnvironment::new(Arc::new({ let mut env = DynamicEnvironment::new(profile); env.obstacles.push( DynamicCircularObstacle::new(profile).with_trajectory(Some( @@ -306,7 +500,8 @@ mod tests { .as_secs_f64(); assert_relative_eq!(arrival_time, expected_arrival, max_relative = 0.1); - let trajectory: Trajectory = solution.make_trajectory().unwrap().unwrap(); + let trajectory: Trajectory = + solution.make_trajectory().unwrap().unwrap().trajectory; assert!(trajectory.len() >= 11); } @@ -317,7 +512,7 @@ mod tests { .configure(|domain| { domain.modify_environment(|mut env| { env.overlay_trajectory(0, None).unwrap(); - env + Ok(env) }) }) .unwrap(); @@ -345,16 +540,22 @@ mod tests { assert_relative_eq!(arrival_time, expected_arrival, max_relative = 0.01); let planner = planner - .configure(|mut domain| { - domain - .motion - .set_translational_speed(2.0 * domain.motion.translational_speed()) - .unwrap(); - domain - .motion - .set_rotational_speed(2.0 * domain.motion.rotational_speed()) - .unwrap(); - domain + .configure(|domain| { + Ok(domain + .discard_cache(|mut params| { + params + .motion + .set_translational_speed(2.0 * params.motion.translational_speed()) + .unwrap(); + + params + .motion + .set_rotational_speed(2.0 * params.motion.rotational_speed()) + .unwrap(); + + Ok(params) + }) + .unwrap()) }) .unwrap(); @@ -383,7 +584,7 @@ mod tests { let planner = planner .configure(|domain| { domain.modify_environment(|env| { - env.modify_base(|base| { + Ok(env.modify_base(|base| { base.obstacles.push( DynamicCircularObstacle::new( CircularProfile::new(0.1, 1.0, 1.0).unwrap(), @@ -396,7 +597,7 @@ mod tests { .unwrap(), )), ); - }) + })) }) }) .unwrap(); @@ -419,7 +620,6 @@ mod tests { .waypoint .time .as_secs_f64(); - // TODO(@mxgrey): Make a more specific expectation. assert_relative_eq!(arrival_time, expected_arrival, epsilon = 0.5); } @@ -436,9 +636,9 @@ mod tests { SharedGraph::new(NeighborhoodGraph::new(visibility.clone(), [])), SharedGraph::new(VisibilityGraph::new(visibility, [])), DifferentialDriveLineFollow::new(3.0, 1.0).unwrap(), - Arc::new(OverlayedDynamicEnvironment::new(Arc::new( - DynamicEnvironment::new(profile), - ))), + Arc::new(CcbsEnvironment::new(Arc::new(DynamicEnvironment::new( + profile, + )))), TravelEffortCost::default(), ) .unwrap(), @@ -451,10 +651,7 @@ mod tests { key: Cell::new(0, 0), orientation: Orientation::new(0.0), }, - GoalSE2 { - key: Cell::new(10, 1), - orientation: None, - }, + GoalSE2::new(Cell::new(10, 1)), ) .unwrap() .solve() @@ -462,7 +659,11 @@ mod tests { .solution() .unwrap(); - let trajectory = solution.make_trajectory::().unwrap().unwrap(); + let trajectory = solution + .make_trajectory::() + .unwrap() + .unwrap() + .trajectory; assert_eq!(3, trajectory.len()); } @@ -480,9 +681,9 @@ mod tests { SharedGraph::new(NeighborhoodGraph::new(visibility.clone(), [])), SharedGraph::new(VisibilityGraph::new(visibility, [])), DifferentialDriveLineFollow::new(3.0, 1.0).unwrap(), - Arc::new(OverlayedDynamicEnvironment::new(Arc::new( - DynamicEnvironment::new(profile), - ))), + Arc::new(CcbsEnvironment::new(Arc::new(DynamicEnvironment::new( + profile, + )))), TravelEffortCost::default(), ) .unwrap(), @@ -495,10 +696,7 @@ mod tests { key: Cell::new(0, 0), orientation: Orientation::new(0.0), }, - GoalSE2 { - key: Cell::new(10, 0), - orientation: None, - }, + GoalSE2::new(Cell::new(10, 0)), ) .unwrap() .solve() @@ -506,7 +704,11 @@ mod tests { .solution() .unwrap(); - let trajectory = solution.make_trajectory::().unwrap().unwrap(); + let trajectory = solution + .make_trajectory::() + .unwrap() + .unwrap() + .trajectory; assert_eq!(5, trajectory.len()); } } diff --git a/mapf/src/templates/conflict_avoidance.rs b/mapf/src/templates/conflict_avoidance.rs index 01406e3..2a6bee4 100644 --- a/mapf/src/templates/conflict_avoidance.rs +++ b/mapf/src/templates/conflict_avoidance.rs @@ -16,17 +16,27 @@ */ use crate::domain::{ConflictAvoider, Extrapolator}; +use std::sync::Arc; -#[derive(Debug, Clone)] +#[derive(Debug)] pub struct ConflictAvoidance { pub avoider: Avoider, - pub environment: Environment, + pub environment: Arc, } -impl Extrapolator +impl Clone for ConflictAvoidance { + fn clone(&self) -> Self { + Self { + avoider: self.avoider.clone(), + environment: self.environment.clone(), + } + } +} + +impl Extrapolator for ConflictAvoidance where - Avoider: ConflictAvoider, + Avoider: ConflictAvoider, { type Extrapolation = Avoider::AvoidanceAction; type ExtrapolationError = Avoider::AvoidanceError; @@ -37,13 +47,15 @@ where Self::ExtrapolationError: 'a, State: 'a, Target: 'a, - Guidance: 'a; + Guidance: 'a, + Key: 'a; fn extrapolate<'a>( &'a self, from_state: &State, to_target: &Target, with_guidance: &Guidance, + for_keys: (Option<&Key>, Option<&Key>), ) -> Self::ExtrapolationIter<'a> where Self: 'a, @@ -52,9 +64,16 @@ where State: 'a, Target: 'a, Guidance: 'a, + Key: 'a, { self.avoider - .avoid_conflicts(from_state, to_target, with_guidance, &self.environment) + .avoid_conflicts( + from_state, + to_target, + with_guidance, + for_keys, + &*self.environment, + ) .into_iter() } } diff --git a/mapf/src/templates/graph_motion.rs b/mapf/src/templates/graph_motion.rs index 012e638..715404a 100644 --- a/mapf/src/templates/graph_motion.rs +++ b/mapf/src/templates/graph_motion.rs @@ -64,7 +64,7 @@ impl Domain for GraphMotion where S: KeyedSpace, G: Graph, - E: Extrapolator, + E: Extrapolator, { type State = S::State; type Error = GraphMotionError; @@ -78,7 +78,7 @@ where G: Graph, G::Key: Clone, G::EdgeAttributes: Clone, - E: Extrapolator, + E: Extrapolator, E::ExtrapolationError: StdError, { type ActivityAction = E::Extrapolation; @@ -90,6 +90,7 @@ where Self::ActivityError: 'a, S::State: 'a, G::EdgeAttributes: 'a, + G::Key: 'a, S::State: 'a; fn choices<'a>(&'a self, from_state: S::State) -> Self::Choices<'a> @@ -98,6 +99,7 @@ where Self::ActivityAction: 'a, Self::ActivityError: 'a, S::State: 'a, + G::Key: 'a, { self.graph .edges_from_vertex(self.space.key_for(&from_state.clone()).borrow().borrow()) @@ -116,6 +118,10 @@ where self.space.waypoint(&from_state).borrow(), v.borrow(), &edge, + ( + Some(self.space.key_for(&from_state).borrow().borrow()), + Some(&to_vertex), + ), ); extrapolations.into_iter().map(move |r| { @@ -200,7 +206,7 @@ where G: Graph, G::Key: Clone, G::EdgeAttributes: Clone, - E: Extrapolator + E: Extrapolator + Backtrack, { type BacktrackError = E::BacktrackError; diff --git a/mapf/src/templates/incremental_graph_motion.rs b/mapf/src/templates/incremental_graph_motion.rs index 8016728..642e805 100644 --- a/mapf/src/templates/incremental_graph_motion.rs +++ b/mapf/src/templates/incremental_graph_motion.rs @@ -22,7 +22,7 @@ use crate::{ }, error::StdError, graph::{Edge, Graph}, - motion::Timed, + motion::{MaybeTimed, TimePoint, Timed}, templates::graph_motion::{GraphMotionError, GraphMotionReversalError}, util::{FlatResultMapTrait, ForkIter}, }; @@ -73,7 +73,7 @@ impl Domain for IncrementalGraphMotion where S: KeyedSpace, G: Graph, - E: IncrementalExtrapolator, + E: IncrementalExtrapolator, { type State = IncrementalState; type Error = GraphMotionError; @@ -87,7 +87,7 @@ where G: Graph, G::Key: Clone, G::EdgeAttributes: Clone, - E: IncrementalExtrapolator, + E: IncrementalExtrapolator, E::IncrementalExtrapolationError: StdError, { type ActivityAction = E::IncrementalExtrapolation; @@ -123,6 +123,10 @@ where self.space.waypoint(&from_state.base_state).borrow(), to_v_ref.borrow(), &with_guidance, + ( + Some(self.space.key_for(&from_state.base_state).borrow().borrow()), + Some(&to_key), + ), ); let from_state = from_state.clone(); @@ -189,6 +193,15 @@ where self.space.waypoint(&from_state.base_state).borrow(), v.borrow(), &with_guidance, + ( + Some( + self.space + .key_for(&from_state.base_state) + .borrow() + .borrow(), + ), + Some(&to_key), + ), ); let from_state = from_state.clone(); @@ -302,7 +315,7 @@ where G: Graph, G::Key: Clone, G::EdgeAttributes: Clone, - E: IncrementalExtrapolator + E: IncrementalExtrapolator + Backtrack, { type BacktrackError = E::BacktrackError; @@ -419,15 +432,21 @@ impl SelfKey for IncrementalState { } impl Timed for IncrementalState { - fn set_time(&mut self, new_time: time_point::TimePoint) { + fn set_time(&mut self, new_time: TimePoint) { self.base_state.set_time(new_time) } - fn time(&self) -> &time_point::TimePoint { + fn time(&self) -> TimePoint { self.base_state.time() } } +impl MaybeTimed for IncrementalState { + fn maybe_time(&self) -> Option { + Some(self.base_state.time()) + } +} + #[cfg(test)] mod tests { use super::*; diff --git a/mapf/src/templates/lazy_graph_motion.rs b/mapf/src/templates/lazy_graph_motion.rs index 797d58a..da77c63 100644 --- a/mapf/src/templates/lazy_graph_motion.rs +++ b/mapf/src/templates/lazy_graph_motion.rs @@ -49,7 +49,7 @@ where G: Graph, G::Key: Clone, G::EdgeAttributes: Clone, - E: Extrapolator, + E: Extrapolator, E::Extrapolation: Into, Action: Into, R: ArrivalKeyring, @@ -89,12 +89,14 @@ where let from_key = from_key.clone(); r.map_err(LazyGraphMotionError::Keyring) .flat_result_map(move |to_vertex: G::Key| { + let from_key = from_key.clone(); let from_spatial_state = from_spatial_state.clone(); self.motion .graph .lazy_edges_between(&from_key, &to_vertex) .into_iter() .flat_map(move |edge| { + let from_key = from_key.clone(); let from_state = from_spatial_state.clone(); let to_vertex = to_vertex.clone(); let edge: G::EdgeAttributes = edge.attributes().clone(); @@ -106,12 +108,14 @@ where LazyGraphMotionError::MissingVertex(to_vertex.clone()) }) .flat_result_map(move |v| { + let from_key = from_key.clone(); let from_state = from_state.clone(); let to_vertex = to_vertex.clone(); let extrapolations = self.motion.extrapolator.extrapolate( self.motion.space.waypoint(&from_state).borrow(), v.borrow(), &edge, + (Some(&from_key), Some(&to_vertex)), ); extrapolations.into_iter().map(move |r| { @@ -204,7 +208,6 @@ pub enum LazyGraphMotionReversalError { mod tests { use super::*; use crate::{graph::occupancy::*, motion::se2::*}; - use arrayvec::ArrayVec; use std::sync::Arc; #[test] @@ -227,7 +230,7 @@ mod tests { }; let from_cell = Cell::new(2, 3); - let from_p = from_cell.to_center_point(cell_size); + let from_p = from_cell.center_point(cell_size); let from_state = StateSE2::new( from_cell, WaypointSE2::new_f64(0.0, from_p.x, from_p.y, 0.0), @@ -241,7 +244,7 @@ mod tests { (Cell::new(-30, -30), 1), (Cell::new(-60, 20), 1), ] { - let r: Result, _)>, _> = + let r: Result, _> = lazy.connect(from_state, &to_cell).into_iter().collect(); let connections = r.unwrap();