diff --git a/Cargo.toml b/Cargo.toml index 8dd3043..22a9a65 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [workspace] members = [ - "mapf", + "mapf", "mapf-derive", "mapf-viz", ] diff --git a/mapf-derive/Cargo.toml b/mapf-derive/Cargo.toml new file mode 100644 index 0000000..0e5de75 --- /dev/null +++ b/mapf-derive/Cargo.toml @@ -0,0 +1,15 @@ +[package] +name = "mapf-derive" +version = "0.1.0" +edition = "2021" +description = "Procedural macros for the mapf library" +license = "Apache-2.0" + +[lib] +proc-macro = true + +[dependencies] +syn = { version = "2.0", features = ["full", "extra-traits"] } +quote = "1.0" +proc-macro2 = "1.0" +paste = "1.0" diff --git a/mapf-derive/src/lib.rs b/mapf-derive/src/lib.rs new file mode 100644 index 0000000..805e629 --- /dev/null +++ b/mapf-derive/src/lib.rs @@ -0,0 +1,230 @@ +/* + * Copyright (C) 2025 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 proc_macro::TokenStream; +use quote::quote; +use syn::{parse_macro_input, Data, DeriveInput, Fields}; + +#[proc_macro_derive( + Domain, + attributes( + domain, + activity, + weight, + heuristic, + closer, + satisfier, + initializer, + connector, + arrival_keyring + ) +)] +pub fn derive_domain(input: TokenStream) -> TokenStream { + let input = parse_macro_input!(input as DeriveInput); + let name = &input.ident; + + let mut state_type = None; + let mut action_type = None; + let mut error_type = None; + + for attr in &input.attrs { + if attr.path().is_ident("domain") { + let _ = attr.parse_nested_meta(|meta| { + if meta.path.is_ident("state") { + let value = meta.value()?; + state_type = Some(value.parse::()?); + Ok(()) + } else if meta.path.is_ident("action") { + let value = meta.value()?; + action_type = Some(value.parse::()?); + Ok(()) + } else if meta.path.is_ident("error") { + let value = meta.value()?; + error_type = Some(value.parse::()?); + Ok(()) + } else { + Err(meta.error("unsupported domain attribute")) + } + }); + } + } + + let state_type = + state_type.expect("Domain derive requires a 'state' attribute: #[domain(state = ...)]"); + let error_type = error_type.unwrap_or_else(|| syn::parse_quote!(anyhow::Error)); + + let mut expanded = quote! {}; + + let (impl_generics, ty_generics, where_clause) = input.generics.split_for_impl(); + + let action_type = + action_type.expect("Domain derive requires an 'action' attribute: #[domain(action = ...)]"); + expanded.extend(quote! { + impl #impl_generics ::mapf::domain::Domain for #name #ty_generics #where_clause { + type State = #state_type; + type Action = #action_type; + type Error = #error_type; + } + }); + + if let Data::Struct(data) = &input.data { + if let Fields::Named(fields) = &data.fields { + for field in &fields.named { + let field_name = &field.ident; + let field_ty = &field.ty; + for attr in &field.attrs { + if attr.path().is_ident("activity") { + let action = &action_type; + expanded.extend(quote! { + impl #impl_generics ::mapf::domain::Activity<#state_type, #action> for #name #ty_generics #where_clause { + type ActivityError = <#field_ty as ::mapf::domain::Activity<#state_type, #action>>::ActivityError; + type Choices<'a> = <#field_ty as ::mapf::domain::Activity<#state_type, #action>>::Choices<'a> + where + Self: 'a, + #action: 'a, + Self::ActivityError: 'a, + #state_type: 'a; + + fn choices<'a>(&'a self, from_state: #state_type) -> Self::Choices<'a> + where + Self: 'a, + #action: 'a, + Self::ActivityError: 'a, + #state_type: 'a + { + self.#field_name.choices(from_state) + } + } + }); + } else if attr.path().is_ident("weight") { + let action = &action_type; + expanded.extend(quote! { + impl #impl_generics ::mapf::domain::Weight<#state_type, #action> for #name #ty_generics #where_clause { + type Cost = <#field_ty as ::mapf::domain::Weight<#state_type, #action>>::Cost; + type WeightError = <#field_ty as ::mapf::domain::Weight<#state_type, #action>>::WeightError; + fn cost(&self, from_state: &#state_type, action: &#action, to_state: &#state_type) -> Result, Self::WeightError> { + self.#field_name.cost(from_state, action, to_state) + } + fn initial_cost(&self, for_state: &#state_type) -> Result, Self::WeightError> { + self.#field_name.initial_cost(for_state) + } + } + }); + } else if attr.path().is_ident("heuristic") { + // Assuming Goal is #state_type by default + expanded.extend(quote! { + impl #impl_generics ::mapf::domain::Heuristic<#state_type, #state_type> for #name #ty_generics #where_clause { + type CostEstimate = <#field_ty as ::mapf::domain::Heuristic<#state_type, #state_type>>::CostEstimate; + type HeuristicError = <#field_ty as ::mapf::domain::Heuristic<#state_type, #state_type>>::HeuristicError; + fn estimate_remaining_cost(&self, from_state: &#state_type, to_goal: &#state_type) -> Result, Self::HeuristicError> { + self.#field_name.estimate_remaining_cost(from_state, to_goal) + } + } + }); + } else if attr.path().is_ident("closer") { + expanded.extend(quote! { + impl #impl_generics ::mapf::domain::Closable<#state_type> for #name #ty_generics #where_clause { + type ClosedSet = <#field_ty as ::mapf::domain::Closable<#state_type>>::ClosedSet; + fn new_closed_set(&self) -> Self::ClosedSet { + self.#field_name.new_closed_set() + } + } + }); + } else if attr.path().is_ident("satisfier") { + // Assuming Goal is #state_type + expanded.extend(quote! { + impl #impl_generics ::mapf::domain::Satisfiable<#state_type, #state_type> for #name #ty_generics #where_clause { + type SatisfactionError = <#field_ty as ::mapf::domain::Satisfiable<#state_type, #state_type>>::SatisfactionError; + fn is_satisfied(&self, by_state: &#state_type, for_goal: &#state_type) -> Result { + self.#field_name.is_satisfied(by_state, for_goal) + } + } + }); + } else if attr.path().is_ident("initializer") { + // Assuming Start and Goal are #state_type + expanded.extend(quote! { + impl #impl_generics ::mapf::domain::Initializable<#state_type, #state_type, #state_type> for #name #ty_generics #where_clause { + type InitialError = <#field_ty as ::mapf::domain::Initializable<#state_type, #state_type, #state_type>>::InitialError; + type InitialStates<'a> = <#field_ty as ::mapf::domain::Initializable<#state_type, #state_type, #state_type>>::InitialStates<'a> + where + Self: 'a, + Self::InitialError: 'a, + #state_type: 'a; + + fn initialize<'a>(&'a self, from_start: #state_type, to_goal: &#state_type) -> Self::InitialStates<'a> + where + Self: 'a, + Self::InitialError: 'a, + #state_type: 'a + { + self.#field_name.initialize(from_start, to_goal) + } + } + }); + } else if attr.path().is_ident("connector") { + let action = &action_type; + expanded.extend(quote! { + impl #impl_generics ::mapf::domain::Connectable<#state_type, #action, #state_type> for #name #ty_generics #where_clause { + type ConnectionError = <#field_ty as ::mapf::domain::Connectable<#state_type, #action, #state_type>>::ConnectionError; + type Connections<'a> = <#field_ty as ::mapf::domain::Connectable<#state_type, #action, #state_type>>::Connections<'a> + where + Self: 'a, + Self::ConnectionError: 'a, + #state_type: 'a, + #action: 'a; + + fn connect<'a>(&'a self, from_state: #state_type, to_target: &'a #state_type) -> Self::Connections<'a> + where + Self: 'a, + Self::ConnectionError: 'a, + #state_type: 'a, + #action: 'a + { + self.#field_name.connect(from_state, to_target) + } + } + }); + } else if attr.path().is_ident("arrival_keyring") { + expanded.extend(quote! { + impl #impl_generics ::mapf::domain::ArrivalKeyring<<#field_ty as ::mapf::domain::Keyed>::Key, #state_type, #state_type> for #name #ty_generics #where_clause { + type ArrivalKeyError = <#field_ty as ::mapf::domain::ArrivalKeyring<<#field_ty as ::mapf::domain::Keyed>::Key, #state_type, #state_type>>::ArrivalKeyError; + type ArrivalKeys<'a> = <#field_ty as ::mapf::domain::ArrivalKeyring<<#field_ty as ::mapf::domain::Keyed>::Key, #state_type, #state_type>>::ArrivalKeys<'a> + where + Self: 'a, + Self::ArrivalKeyError: 'a, + <#field_ty as ::mapf::domain::Keyed>::Key: 'a, + #state_type: 'a; + + fn get_arrival_keys<'a>(&'a self, start: &#state_type, goal: &#state_type) -> Self::ArrivalKeys<'a> + where + Self: 'a, + Self::ArrivalKeyError: 'a, + <#field_ty as ::mapf::domain::Keyed>::Key: 'a, + #state_type: 'a + { + self.#field_name.get_arrival_keys(start, goal) + } + } + }); + } + } + } + } + } + + TokenStream::from(expanded) +} diff --git a/mapf-viz/examples/grid.rs b/mapf-viz/examples/grid.rs index f2fdba5..079cd7e 100644 --- a/mapf-viz/examples/grid.rs +++ b/mapf-viz/examples/grid.rs @@ -1009,7 +1009,11 @@ impl App { self.canvas.program.layers.3.searches.clear(); - for ticket in self.search_memory.iter().take(self.debug_ticket_size as usize) { + for ticket in self + .search_memory + .iter() + .take(self.debug_ticket_size as usize) + { if let Some(mt) = search .memory() .0 @@ -1990,7 +1994,10 @@ impl Application for App { .push(iced::Space::with_width(Length::Units(16))) .push( Column::new() - .push(Text::new(format!("Debug Paths: {}", &self.debug_ticket_size))) + .push(Text::new(format!( + "Debug Paths: {}", + &self.debug_ticket_size + ))) .push(iced::Space::with_height(Length::Units(2))) .push( Slider::new( diff --git a/mapf/Cargo.toml b/mapf/Cargo.toml index 6920707..363a6cb 100644 --- a/mapf/Cargo.toml +++ b/mapf/Cargo.toml @@ -27,6 +27,7 @@ smallvec = "1.10" serde = { version="1.0", features = ["derive"] } serde_yaml = "0.9" slotmap = "1.0" +mapf-derive = { path = "../mapf-derive", version = "0.1.0" } [dev-dependencies] approx = "0.5" diff --git a/mapf/examples/astar_derive.rs b/mapf/examples/astar_derive.rs new file mode 100644 index 0000000..a6002c8 --- /dev/null +++ b/mapf/examples/astar_derive.rs @@ -0,0 +1,195 @@ +/* + * Copyright (C) 2025 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 mapf::algorithm::{AStar, SearchStatus}; +use mapf::domain::{Activity, Cost, Domain, Heuristic, Keyed, KeyedCloser, Keyring, Weight}; +use mapf::error::NoError; +use mapf::Planner; + +/// Actions that can be performed in the grid. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +enum GridAction { + North, + South, + East, + West, +} + +/// A simple 2D point state. +#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] +struct Point { + x: i32, + y: i32, +} + +/// Define a domain using the new derive macro. +/// Adding Clone to GridDomain because AStar needs to be Clone for the planner. +#[derive(Domain, Clone)] +#[domain(state = Point, action = GridAction, error = NoError)] +struct GridDomain { + #[activity] + motion: GridMotion, + + #[weight] + cost: ConstantCost, + + #[heuristic] + heuristic: ManhattanHeuristic, + + #[closer] + closer: KeyedCloser, + + #[satisfier] + satisfier: (), // Default satisfier uses PartialEq + + #[initializer] + initializer: (), // Default initializer accepts Start: Into +} + +/// Move in 4 directions. +#[derive(Clone)] +struct GridMotion; +impl Activity for GridMotion { + type ActivityError = NoError; + type Choices<'a> + = std::vec::IntoIter> + where + Self: 'a, + GridAction: 'a, + Point: 'a; + + fn choices<'a>(&'a self, from_state: Point) -> Self::Choices<'a> + where + Self: 'a, + GridAction: 'a, + Point: 'a, + { + vec![ + Ok(( + GridAction::North, + Point { + x: from_state.x, + y: from_state.y + 1, + }, + )), + Ok(( + GridAction::South, + Point { + x: from_state.x, + y: from_state.y - 1, + }, + )), + Ok(( + GridAction::East, + Point { + x: from_state.x + 1, + y: from_state.y, + }, + )), + Ok(( + GridAction::West, + Point { + x: from_state.x - 1, + y: from_state.y, + }, + )), + ] + .into_iter() + } +} + +/// Every move costs 1.0. We use mapf::domain::Cost to get Ord for floats. +#[derive(Clone)] +struct ConstantCost; +impl Weight for ConstantCost { + type Cost = Cost; + type WeightError = NoError; + fn cost(&self, _: &Point, _: &GridAction, _: &Point) -> Result>, NoError> { + Ok(Some(Cost(1.0))) + } + fn initial_cost(&self, _: &Point) -> Result>, NoError> { + Ok(Some(Cost(0.0))) + } +} + +/// Manhattan distance heuristic. +#[derive(Clone)] +struct ManhattanHeuristic; +impl Heuristic for ManhattanHeuristic { + type CostEstimate = Cost; + type HeuristicError = NoError; + fn estimate_remaining_cost( + &self, + from: &Point, + to: &Point, + ) -> Result>, NoError> { + Ok(Some(Cost( + ((from.x - to.x).abs() + (from.y - to.y).abs()) as f64, + ))) + } +} + +/// Keyring for Point state. +#[derive(Clone, Default)] +struct PointRing; +impl Keyed for PointRing { + type Key = Point; +} +impl Keyring for PointRing { + type KeyRef<'a> + = &'a Point + where + Self: 'a, + Point: 'a; + fn key_for<'a>(&'a self, state: &'a Point) -> Self::KeyRef<'a> + where + Self: 'a, + Point: 'a, + { + state + } +} + +fn main() { + let domain = GridDomain { + motion: GridMotion, + cost: ConstantCost, + heuristic: ManhattanHeuristic, + closer: KeyedCloser(PointRing), + satisfier: (), + initializer: (), + }; + + let start = Point { x: 0, y: 0 }; + let goal = Point { x: 5, y: 5 }; + + // Create a planner using AStar with our derived domain. + let planner = Planner::new(AStar(domain)); + + // Plan and solve. + let result = planner.plan(start, goal).unwrap().solve().unwrap(); + + if let SearchStatus::Solved(path) = result { + println!("Goal reached!"); + println!("Initial state: {:?}", path.initial_state); + for (action, state) in path.sequence { + println!(" {:?} -> {:?}", action, state); + } + } else { + println!("Failed to find path: {:?}", result); + } +} diff --git a/mapf/examples/domain_derive.rs b/mapf/examples/domain_derive.rs new file mode 100644 index 0000000..c25d945 --- /dev/null +++ b/mapf/examples/domain_derive.rs @@ -0,0 +1,123 @@ +/* + * Copyright (C) 2025 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 mapf::domain::{Activity, Domain, Heuristic, Keyed, KeyedCloser, Keyring, Weight}; +use mapf::error::NoError; + +#[derive(Domain)] +#[domain(state = f64, action = f64, error = NoError)] +struct RobotDomain { + #[activity] + motion: MyActivity, + + #[weight] + cost: MyWeight, + + #[heuristic] + heuristic: MyHeuristic, + + #[closer] + closer: KeyedCloser, +} + +#[derive(Clone)] +struct MyActivity; +impl Activity for MyActivity { + type ActivityError = NoError; + type Choices<'a> + = std::vec::IntoIter> + where + Self: 'a, + f64: 'a; + + fn choices<'a>(&'a self, from_state: f64) -> Self::Choices<'a> + where + Self: 'a, + f64: 'a, + { + vec![Ok((1.0, from_state + 1.0))].into_iter() + } +} + +struct MyWeight; +impl Weight for MyWeight { + type Cost = f64; + type WeightError = NoError; + fn cost(&self, _: &f64, _: &f64, _: &f64) -> Result, NoError> { + Ok(Some(1.0)) + } + fn initial_cost(&self, _: &f64) -> Result, NoError> { + Ok(Some(0.0)) + } +} + +struct MyHeuristic; +impl Heuristic for MyHeuristic { + type CostEstimate = f64; + type HeuristicError = NoError; + fn estimate_remaining_cost( + &self, + from_state: &f64, + to_goal: &f64, + ) -> Result, NoError> { + Ok(Some((to_goal - from_state).abs())) + } +} + +#[derive(Clone, Default)] +struct MyRing; +impl Keyed for MyRing { + type Key = u64; +} +impl Keyring for MyRing { + type KeyRef<'a> + = u64 + where + Self: 'a, + f64: 'a; + fn key_for<'a>(&'a self, state: &'a f64) -> Self::KeyRef<'a> + where + Self: 'a, + f64: 'a, + { + *state as u64 + } +} + +fn main() { + let domain = RobotDomain { + motion: MyActivity, + cost: MyWeight, + heuristic: MyHeuristic, + closer: KeyedCloser(MyRing), + }; + + // Test delegation + let choices: Vec<_> = domain.choices(0.0).collect(); + assert_eq!(choices[0].as_ref().unwrap().1, 1.0); + + let cost = domain.cost(&0.0, &1.0, &1.0).unwrap().unwrap(); + assert_eq!(cost, 1.0); + + let estimate = domain + .estimate_remaining_cost(&0.0, &10.0) + .unwrap() + .unwrap(); + assert_eq!(estimate, 10.0); + + println!("Delegation worked!"); +} diff --git a/mapf/src/algorithm/a_star.rs b/mapf/src/algorithm/a_star.rs index db9c98b..960dd32 100644 --- a/mapf/src/algorithm/a_star.rs +++ b/mapf/src/algorithm/a_star.rs @@ -20,8 +20,8 @@ use crate::{ tree::*, Algorithm, Coherent, MinimumCostBound, Path, QueueLength, SearchStatus, Solvable, }, domain::{ - Activity, Closable, CloseResult, ClosedSet, Configurable, Connectable, Domain, Informed, - Initializable, Satisfiable, Weighted, + Activity, Closable, CloseResult, ClosedSet, Configurable, Connectable, Domain, Heuristic, + Initializable, Satisfiable, Weight, }, error::{Anyhow, ThisError}, }; @@ -31,8 +31,8 @@ use std::ops::Add; /// * [`Initializable`] /// * [`Closable`] /// * [`Activity`] -/// * [`Weighted`] -/// * [`Informed`] +/// * [`Weight`] +/// * [`Heuristic`] /// * [`Satisfiable`] /// /// The following templates implement these traits: @@ -78,14 +78,14 @@ pub enum AStarSearchError { } impl AStar { - fn domain_err(err: impl Into) -> AStarSearchError + pub fn domain_err(err: impl Into) -> AStarSearchError where D: Domain, { AStarSearchError::Domain(err.into()) } - fn algo_err(err: TreeError) -> AStarSearchError + pub fn algo_err(err: TreeError) -> AStarSearchError where D: Domain, { @@ -95,10 +95,10 @@ impl AStar { impl AStar where - D: Domain + Closable + Activity + Weighted, + D: Domain + Closable + Activity + Weight, D::State: Clone, D::Action: Clone, - D::WeightedError: Into, + D::WeightError: Into, D::Cost: Ord + Add + Clone, { #[inline] @@ -108,9 +108,9 @@ where goal: &Goal, ) -> Result<::Memory, AStarSearchError> where - D: Initializable + Informed, + D: Initializable + Heuristic, D::InitialError: Into, - D::InformedError: Into, + D::HeuristicError: Into, { let mut memory = Memory(Tree::new(domain.new_closed_set())); @@ -151,7 +151,7 @@ where goal: &Goal, ) -> Result), D>, AStarSearchError> where - D: Satisfiable + Activity, + D: Satisfiable + Activity, D::SatisfactionError: Into, { let top_id = match queue.pop() { @@ -194,11 +194,11 @@ where goal: &Goal, ) -> Result<(), AStarSearchError> where - D: Activity, + D: Activity, D::Action: Into, D::ActivityError: Into, - D: Informed, - D::InformedError: Into, + D: Heuristic, + D::HeuristicError: Into, { for next in domain.choices(parent.state.clone()) { let (action, child_state) = next.map_err(Self::domain_err)?; @@ -229,8 +229,8 @@ where goal: &Goal, ) -> Result<(), AStarSearchError> where - D: Informed, - D::InformedError: Into, + D: Heuristic, + D::HeuristicError: Into, { let cost = match domain .cost(parent_state, &action, &child_state) @@ -264,7 +264,7 @@ where impl Algorithm for AStar where - D: Domain + Closable + Activity + Weighted, + D: Domain + Closable + Activity + Weight, { type Memory = Memory, D::State, D::Action, D::Cost>; } @@ -274,15 +274,15 @@ where D: Domain + Initializable + Closable - + Activity - + Weighted - + Informed, + + Activity + + Weight + + Heuristic, D::State: Clone, D::Action: Clone, D::Cost: Ord + Add + Clone, D::InitialError: Into, - D::WeightedError: Into, - D::InformedError: Into, + D::WeightError: Into, + D::HeuristicError: Into, { type InitError = AStarSearchError; @@ -295,9 +295,9 @@ impl Solvable for AStar where D: Domain + Closable - + Activity - + Weighted - + Informed + + Activity + + Weight + + Heuristic + Satisfiable, D::State: Clone, D::Action: Clone, @@ -305,8 +305,8 @@ where D::Cost: Ord + Add + Clone, D::SatisfactionError: Into, D::ActivityError: Into, - D::WeightedError: Into, - D::InformedError: Into, + D::WeightError: Into, + D::HeuristicError: Into, { type Solution = Path; type StepError = AStarSearchError; @@ -345,7 +345,7 @@ impl Configurable for AStar { impl Algorithm for AStarConnect where - D: Domain + Closable + Activity + Weighted, + D: Domain + Closable + Activity + Weight, { type Memory = Memory, D::State, D::Action, D::Cost>; } @@ -355,15 +355,15 @@ where D: Domain + Initializable + Closable - + Activity - + Weighted - + Informed, + + Activity + + Weight + + Heuristic, D::State: Clone, D::Action: Clone, D::Cost: Ord + Add + Clone, D::InitialError: Into, - D::WeightedError: Into, - D::InformedError: Into, + D::WeightError: Into, + D::HeuristicError: Into, { type InitError = AStarSearchError; @@ -376,9 +376,9 @@ impl Solvable for AStarConnect where D: Domain + Closable - + Activity - + Weighted - + Informed + + Activity + + Weight + + Heuristic + Satisfiable + Connectable, D::State: Clone, @@ -386,8 +386,8 @@ where D::Cost: Ord + Add + Clone, D::SatisfactionError: Into, D::ActivityError: Into, - D::WeightedError: Into, - D::InformedError: Into, + D::WeightError: Into, + D::HeuristicError: Into, D::ConnectionError: Into, { type Solution = Path; @@ -442,10 +442,10 @@ impl Configurable for AStarConnect { #[derive(Debug, Clone)] pub struct Node { - state: State, - cost: Cost, - remaining_cost_estimate: Cost, - parent: Option<(usize, Action)>, + pub state: State, + pub cost: Cost, + pub remaining_cost_estimate: Cost, + pub parent: Option<(usize, Action)>, } impl Node { @@ -494,7 +494,7 @@ where /// Control flow return value for functions that constitute step() enum Flow where - D: Domain + Activity + Weighted, + D: Domain + Activity + Weight, // D::Error: StdError, { Proceed(T), diff --git a/mapf/src/algorithm/dijkstra/backward.rs b/mapf/src/algorithm/dijkstra/backward.rs index cc8b04d..ee4298e 100644 --- a/mapf/src/algorithm/dijkstra/backward.rs +++ b/mapf/src/algorithm/dijkstra/backward.rs @@ -25,7 +25,7 @@ use crate::{ }, domain::{ Activity, ArrivalKeyring, Backtrack, Closable, ClosedStatusForKey, Configurable, - Connectable, Domain, Initializable, Keyed, Keyring, Reversible, Weighted, + Connectable, Domain, Initializable, Keyed, Keyring, Reversible, Weight, }, error::{Anyhow, ThisError}, }; @@ -33,14 +33,22 @@ use std::ops::Add; pub struct BackwardDijkstra where - D: Domain + Keyed + Activity + Weighted + Closable, + D: Domain + + Keyed + + Activity + + Weight + + Closable, { backward: Dijkstra, } impl BackwardDijkstra where - D: Domain + Keyed + Activity + Weighted + Closable, + D: Domain + + Keyed + + Activity + + Weight + + Closable, { pub fn new(domain: &D) -> Result { Ok(Self { @@ -55,14 +63,22 @@ where impl Algorithm for BackwardDijkstra where - D: Domain + Keyed + Activity + Weighted + Closable, + D: Domain + + Keyed + + Activity + + Weight + + Closable, { type Memory = BackwardMemory; } pub struct BackwardMemory where - D: Domain + Keyed + Activity + Weighted + Closable, + D: Domain + + Keyed + + Activity + + Weight + + Closable, { backward: Memory, } @@ -72,8 +88,8 @@ where D: Domain + Keyring + Initializable - + Activity - + Weighted + + Activity + + Weight + Closable + Connectable + ArrivalKeyring, @@ -81,7 +97,7 @@ where D::Action: Clone, D::InitialError: Into, D::ArrivalKeyError: Into, - D::WeightedError: Into, + D::WeightError: Into, D::ConnectionError: Into, D::State: Clone, D::Cost: Clone + Ord + Add, @@ -101,8 +117,8 @@ impl Solvable for BackwardDijkstra where D: Domain + Reversible - + Activity - + Weighted + + Activity + + Weight + Keyring + Closable + Connectable @@ -112,7 +128,7 @@ where D::Cost: Clone + Ord + Add, D::ClosedSet: ClosedStatusForKey, D::ActivityError: Into, - D::WeightedError: Into, + D::WeightError: Into, D::ConnectionError: Into, D::BacktrackError: Into, { @@ -142,8 +158,8 @@ where D: Domain + Reversible + Keyed - + Activity - + Weighted + + Activity + + Weight + Closable, D::ReversalError: Into, { diff --git a/mapf/src/algorithm/dijkstra/forward.rs b/mapf/src/algorithm/dijkstra/forward.rs index ce6aeff..69491e1 100644 --- a/mapf/src/algorithm/dijkstra/forward.rs +++ b/mapf/src/algorithm/dijkstra/forward.rs @@ -20,7 +20,7 @@ use crate::{ domain::{ Activity, ArrivalKeyring, Closable, CloseResult, ClosedSet, ClosedStatus, ClosedStatusForKey, Configurable, Connectable, Domain, Initializable, Keyed, Keyring, - Weighted, + Weight, }, error::{Anyhow, ThisError}, }; @@ -33,7 +33,11 @@ use std::{ pub struct Dijkstra where - D: Domain + Keyed + Activity + Weighted + Closable, + D: Domain + + Keyed + + Activity + + Weight + + Closable, { domain: D, cache: Arc>>, @@ -41,14 +45,22 @@ where impl Algorithm for Dijkstra where - D: Domain + Keyed + Activity + Weighted + Closable, + D: Domain + + Keyed + + Activity + + Weight + + Closable, { type Memory = Memory; } impl Dijkstra where - D: Domain + Keyed + Activity + Weighted + Closable, + D: Domain + + Keyed + + Activity + + Weight + + Closable, { pub fn new(domain: D) -> Self { Self { @@ -86,8 +98,8 @@ where D: Domain + Keyring + Initializable - + Activity - + Weighted + + Activity + + Weight + Closable + Connectable + ArrivalKeyring, @@ -95,7 +107,7 @@ where D::Action: Clone, D::InitialError: Into, D::ArrivalKeyError: Into, - D::WeightedError: Into, + D::WeightError: Into, D::ConnectionError: Into, D::State: Clone, D::Cost: Clone + Ord + Add, @@ -223,8 +235,8 @@ impl Solvable for Dijkstra where D: Domain + Keyring - + Activity - + Weighted + + Activity + + Weight + Closable + Connectable, D::State: Clone, @@ -232,7 +244,7 @@ where D::Cost: Clone + Ord + Add, D::ClosedSet: ClosedStatusForKey, D::ActivityError: Into, - D::WeightedError: Into, + D::WeightError: Into, D::ConnectionError: Into, { type Solution = Path; @@ -463,7 +475,11 @@ where /// previous search remains valid. impl Configurable for Dijkstra where - D: Domain + Keyed + Activity + Weighted + Closable, + D: Domain + + Keyed + + Activity + + Weight + + Closable, { type Configuration = D::Configuration; fn configure(self, f: F) -> Result @@ -503,14 +519,22 @@ impl From for DijkstraImplError { #[derive(Clone)] struct Cache where - D: Domain + Keyed + Activity + Weighted + Closable, + D: Domain + + Keyed + + Activity + + Weight + + Closable, { trees: HashMap>, } impl Default for Cache where - D: Domain + Keyed + Activity + Weighted + Closable, + D: Domain + + Keyed + + Activity + + Weight + + Closable, { fn default() -> Self { Self { @@ -527,7 +551,7 @@ type SharedCachedTree = Arc>>; pub struct CachedTree where - D: Domain + Activity + Weighted + Closable, + D: Domain + Activity + Weight + Closable, { /// The tree data that has been cached tree: Tree, Node, D::Cost>, @@ -536,7 +560,7 @@ where impl CachedTree where - D: Domain + Activity + Weighted + Closable, + D: Domain + Activity + Weight + Closable, { fn new(closed_set: D::ClosedSet) -> Self where @@ -555,7 +579,11 @@ where pub struct Memory where - D: Domain + Keyed + Activity + Weighted + Closable, + D: Domain + + Keyed + + Activity + + Weight + + Closable, { /// Trees that are being grown for this search. // TODO(@mxgrey): Consider using a SmallVec here to avoid heap allocation @@ -575,7 +603,11 @@ where impl Memory where - D: Domain + Keyed + Activity + Weighted + Closable, + D: Domain + + Keyed + + Activity + + Weight + + Closable, { fn new(trees: Vec>, goal_keys: Vec) -> Self { Self { @@ -590,7 +622,11 @@ where pub struct TreeMemory where - D: Domain + Keyed + Activity + Weighted + Closable, + D: Domain + + Keyed + + Activity + + Weight + + Closable, { /// A reference to the cache entry that is being searched. tree: SharedCachedTree, @@ -604,7 +640,11 @@ where impl TreeMemory where - D: Domain + Keyed + Activity + Weighted + Closable, + D: Domain + + Keyed + + Activity + + Weight + + Closable, { fn new(tree: SharedCachedTree) -> Self { Self { diff --git a/mapf/src/domain.rs b/mapf/src/domain.rs index 1810a26..1beb9ea 100644 --- a/mapf/src/domain.rs +++ b/mapf/src/domain.rs @@ -19,8 +19,8 @@ /// its state representation and a type for errors that may occur while using /// the domain. /// -/// Domains may also implement various traits like [`Activity`], [`Weighted`], -/// and [`Informed`] which can be used by planners to search the domain. You can +/// Domains may also implement various traits like [`Activity`], [`Weight`], +/// and [`Heuristic`] which can be used by planners to search the domain. You can /// easily gather implementations for these traits into a domain using the /// `#[derive(Domain)]` macro. /// @@ -29,11 +29,14 @@ pub trait Domain { /// Data structure that represents a state within this domain. type State; + /// Data structure that represents an action within this domain. + type Action; + /// The error type that this domain may produce from its various operations. type Error; } -// pub use mapf_derive::Domain; +pub use mapf_derive::Domain; pub mod action_map; pub use action_map::*; @@ -65,8 +68,8 @@ pub use domain_map::*; pub mod extrapolator; pub use extrapolator::*; -pub mod informed; -pub use informed::*; +pub mod heuristic; +pub use heuristic::*; pub mod initializable; pub use initializable::*; @@ -86,5 +89,5 @@ pub use space::*; pub mod state_map; pub use state_map::*; -pub mod weighted; -pub use weighted::*; +pub mod weight; +pub use weight::*; diff --git a/mapf/src/domain/activity.rs b/mapf/src/domain/activity.rs index f2f4c23..eb1949e 100644 --- a/mapf/src/domain/activity.rs +++ b/mapf/src/domain/activity.rs @@ -27,18 +27,15 @@ use crate::error::NoError; /// In graph theory terms, an action an edge of a graph. An Activity is a /// function that maps a graph vertex (agent state) to the outgoing edges from /// that vertex. -pub trait Activity { - /// What kind of action is produced by this activity - type Action; - +pub trait Activity { /// What kind of error can happen if a bad state is provided type ActivityError; /// Concrete type for the returned container of choices - type Choices<'a>: IntoIterator> + 'a + type Choices<'a>: IntoIterator> + 'a where Self: 'a, - Self::Action: 'a, + Action: 'a, Self::ActivityError: 'a, State: 'a; @@ -47,7 +44,7 @@ pub trait Activity { fn choices<'a>(&'a self, from_state: State) -> Self::Choices<'a> where Self: 'a, - Self::Action: 'a, + Action: 'a, Self::ActivityError: 'a, State: 'a; } @@ -55,21 +52,20 @@ pub trait Activity { /// [`NoActivity`] can be used as a placeholder where an Activity is required /// but it doesn't need to do anything. pub struct NoActivity(std::marker::PhantomData); -impl Activity for NoActivity { - type Action = A; +impl Activity for NoActivity { type ActivityError = NoError; type Choices<'a> = [Result<(A, State), NoError>; 0] where Self: 'a, - Self::Action: 'a, + A: 'a, Self::ActivityError: 'a, State: 'a; fn choices<'a>(&'a self, _: State) -> Self::Choices<'a> where Self: 'a, - Self::Action: 'a, + A: 'a, Self::ActivityError: 'a, State: 'a, { @@ -132,28 +128,27 @@ impl ActivityModifier for () { } } -impl Activity for Incorporated +impl Activity for Incorporated where Base: Domain, - Prop: Activity, + Prop: Activity, Prop::ActivityError: Into, { - type Action = Prop::Action; type ActivityError = Base::Error; type Choices<'a> - = IncorporatedChoices<'a, Base, Prop> + = IncorporatedChoices<'a, Base, Prop, Action> where Self: 'a, Base: 'a, Prop: 'a, - Self::Action: 'a, + Action: 'a, Self::ActivityError: 'a, Base::State: 'a; fn choices<'a>(&'a self, from_state: Base::State) -> Self::Choices<'a> where Self: 'a, - Self::Action: 'a, + Action: 'a, Self::ActivityError: 'a, Base::State: 'a, { @@ -162,60 +157,59 @@ where } } -pub struct IncorporatedChoices<'a, Base, Prop> +pub struct IncorporatedChoices<'a, Base, Prop, Action> where Base: Domain, Base::State: 'a, - Prop: Activity + 'a, - Prop::Action: 'a, + Prop: Activity + 'a, + Action: 'a, Prop::ActivityError: Into + 'a, { choices: as IntoIterator>::IntoIter, } -impl<'a, Base, Prop> Iterator for IncorporatedChoices<'a, Base, Prop> +impl<'a, Base, Prop, Action> Iterator for IncorporatedChoices<'a, Base, Prop, Action> where Base: Domain, - Prop: Activity, + Prop: Activity, Prop::ActivityError: Into, { - type Item = Result<(Prop::Action, Base::State), Base::Error>; + type Item = Result<(Action, Base::State), Base::Error>; fn next(&mut self) -> Option { Some( self.choices .next()? - .map(|(a, s)| (a.into(), s.into())) + .map(|(a, s)| (a, s)) .map_err(Into::into), ) } } -impl Activity for Chained +impl Activity for Chained where - Base: Domain + Activity, + Base: Domain + Activity, Base::State: Clone, - Base::Action: Into, + Base::Action: Into, Base::ActivityError: Into, - Prop: Activity, + Prop: Activity, Prop::ActivityError: Into, { - type Action = Prop::Action; type ActivityError = Base::Error; type Choices<'a> - = ChainedChoices<'a, Base, Prop> + = ChainedChoices<'a, Base, Prop, PropAction> where Self: 'a, Base: 'a, Prop: 'a, - Self::Action: 'a, + PropAction: 'a, Self::ActivityError: 'a, Base::State: 'a; fn choices<'a>(&'a self, from_state: Base::State) -> Self::Choices<'a> where Self: 'a, - Self::Action: 'a, + PropAction: 'a, Self::ActivityError: 'a, Base::State: 'a, { @@ -226,29 +220,30 @@ where } } -pub struct ChainedChoices<'a, Base, Prop> +pub struct ChainedChoices<'a, Base, Prop, PropAction> where - Base: Domain + Activity + 'a, + Base: Domain + Activity + 'a, Base::State: Clone, - Base::Action: Into, + Base::Action: Into + 'a, Base::ActivityError: Into + 'a, - Prop: Activity + 'a, + Prop: Activity + 'a, Prop::ActivityError: Into, + PropAction: 'a, { base_choices: as IntoIterator>::IntoIter, prop_choices: as IntoIterator>::IntoIter, } -impl<'a, Base, Prop> Iterator for ChainedChoices<'a, Base, Prop> +impl<'a, Base, Prop, PropAction> Iterator for ChainedChoices<'a, Base, Prop, PropAction> where - Base: Domain + Activity, + Base: Domain + Activity, Base::State: Clone, - Base::Action: Into, + Base::Action: Into, Base::ActivityError: Into, - Prop: Activity, + Prop: Activity, Prop::ActivityError: Into, { - type Item = Result<(Prop::Action, Base::State), Base::Error>; + type Item = Result<(PropAction, Base::State), Base::Error>; fn next(&mut self) -> Option { loop { @@ -259,20 +254,19 @@ where return self .prop_choices .next() - .map(|r| r.map(|(a, s)| (a, s.into())).map_err(Into::into)); + .map(|r| r.map(|(a, s)| (a, s)).map_err(Into::into)); } } } -impl Activity for Mapped +impl Activity for Mapped where - Base: Domain + Activity, + Base: Domain + Activity, Base::State: Clone, Base::ActivityError: Into, Prop: ActivityModifier, Prop::ModifiedActionError: Into, { - type Action = Prop::ModifiedAction; type ActivityError = Base::Error; type Choices<'a> = MappedChoices<'a, Base, Prop> @@ -280,14 +274,12 @@ where Self: 'a, Base: 'a, Prop: 'a, - Self::Action: 'a, Self::ActivityError: 'a, Base::State: 'a; fn choices<'a>(&'a self, from_state: Base::State) -> Self::Choices<'a> where Self: 'a, - Self::Action: 'a, Self::ActivityError: 'a, Base::State: 'a, { @@ -303,7 +295,7 @@ where pub struct MappedChoices<'a, Base, Prop> where - Base: Domain + Activity + 'a, + Base: Domain + Activity + 'a, Base::State: Clone, Base::ActivityError: Into, Prop: ActivityModifier + 'a, @@ -317,7 +309,7 @@ where impl<'a, Base, Prop> Iterator for MappedChoices<'a, Base, Prop> where - Base: Domain + Activity, + Base: Domain + Activity, Base::State: Clone, Base::ActivityError: Into, Prop: ActivityModifier, @@ -349,20 +341,20 @@ where } } -impl Activity for Lifted +impl Activity for Lifted where Base: Domain, + Prop: Domain + Activity, Lifter: ProjectState + LiftState + ActionMap, Lifter::ActionMapError: Into, Lifter::ProjectionError: Into, Lifter::LiftError: Into, - Prop: Activity, Base::State: Clone, Prop::Action: Clone, Prop::ActivityError: Into, + Lifter::ToAction: Clone, { - type Action = Lifter::ToAction; type ActivityError = Base::Error; type Choices<'a> = LiftedActivityChoices<'a, Base, Lifter, Prop> @@ -370,14 +362,12 @@ where Self: 'a, Base: 'a, Prop: 'a, - Self::Action: 'a, Self::ActivityError: 'a, Base::State: 'a; fn choices<'a>(&'a self, from_state: Base::State) -> Self::Choices<'a> where Self: 'a, - Self::Action: 'a, Self::ActivityError: 'a, Base::State: 'a, { @@ -408,13 +398,15 @@ where pub enum LiftedActivityChoices<'a, Base, Lifter, Prop> where Base: Domain, - Lifter: - ProjectState + LiftState + ActionMap, + Lifter: StateSubspace + + ProjectState + + LiftState + + ActionMap, + Prop: Domain + Activity<::ProjectedState, Prop::Action> + 'a, Lifter::ActionMapError: Into + 'a, Lifter::ProjectionError: Into, - Lifter::ProjectedState: 'a, + ::ProjectedState: 'a, Lifter::LiftError: Into, - Prop: Activity + 'a, Base::State: Clone + 'a, Prop::Action: Clone + 'a, Prop::ActivityError: Into + 'a, @@ -434,13 +426,15 @@ where impl<'a, Base, Lifter, Prop> Iterator for LiftedActivityChoices<'a, Base, Lifter, Prop> where Base: Domain, - Lifter: - ProjectState + LiftState + ActionMap, + Lifter: StateSubspace + + ProjectState + + LiftState + + ActionMap, + Prop: Domain + Activity<::ProjectedState, Prop::Action> + 'a, Lifter::ActionMapError: Into, Lifter::ProjectionError: Into, - Lifter::ProjectedState: 'a, + ::ProjectedState: 'a, Lifter::LiftError: Into, - Prop: Activity + 'a, Base::State: Clone + 'a, Prop::Action: Clone + 'a, Prop::ActivityError: Into + 'a, @@ -472,8 +466,10 @@ where } *lifted_actions = None; - let next: Result<(Prop::Action, Lifter::ProjectedState), Prop::ActivityError> = - choices.next()?; + let next: Result< + (Prop::Action, ::ProjectedState), + Prop::ActivityError, + > = choices.next()?; let (action, state) = match next { Ok(ok) => ok, @@ -511,15 +507,14 @@ mod tests { #[derive(Debug, PartialEq, Eq)] struct Interval(u64); - impl Activity for Count { - type Action = Interval; + impl Activity for Count { type ActivityError = NoError; type Choices<'a> = Vec>; fn choices<'a>(&'a self, s: u64) -> Self::Choices<'a> where Self: 'a, - Self::Action: 'a, + Interval: 'a, Self::ActivityError: 'a, u64: 'a, { @@ -597,7 +592,7 @@ mod tests { #[test] fn test_activity_chain_map() { - let domain = DefineTrait::::new().with(Count { + let domain = DefineTrait::::new().with(Count { by_interval: vec![1, 2, 3], }); @@ -644,7 +639,7 @@ mod tests { #[test] fn test_state_dependent_activity_map() { - let domain = DefineTrait::::new() + let domain = DefineTrait::::new() .with(Count { by_interval: vec![2, 3, 4, 5], }) @@ -694,14 +689,23 @@ mod tests { } } - #[derive(Clone, Copy, Debug, Hash, PartialEq, Eq)] + #[derive(Clone, Copy, Debug, Hash, PartialEq, Eq, Domain)] + #[domain(state = Item, action = Buy, error = NoError)] struct Buy(u64 /* price per unit */); - impl Activity for Buy { - type Action = Buy; + impl Activity for Buy { type ActivityError = NoError; - type Choices<'a> = Option>; - fn choices<'a>(&'a self, mut from_state: Item) -> Option> + type Choices<'a> + = Option> where + Self: 'a, + Buy: 'a, + Self::ActivityError: 'a, + Item: 'a; + fn choices<'a>(&'a self, mut from_state: Item) -> Self::Choices<'a> + where + Self: 'a, + Buy: 'a, + Self::ActivityError: 'a, Item: 'a, { if from_state.budget < self.0 { @@ -713,14 +717,23 @@ mod tests { } } - #[derive(Clone, Copy, Debug, Hash, PartialEq, Eq)] + #[derive(Clone, Copy, Debug, Hash, PartialEq, Eq, Domain)] + #[domain(state = Item, action = Sell, error = NoError)] struct Sell(u64 /* price per unit */); - impl Activity for Sell { - type Action = Sell; + impl Activity for Sell { type ActivityError = NoError; - type Choices<'a> = Option>; + type Choices<'a> + = Option> + where + Self: 'a, + Sell: 'a, + Self::ActivityError: 'a, + Item: 'a; fn choices<'a>(&'a self, mut from_state: Item) -> Self::Choices<'a> where + Self: 'a, + Sell: 'a, + Self::ActivityError: 'a, Item: 'a, { if from_state.count <= 0 { @@ -850,7 +863,7 @@ mod tests { #[test] fn test_lifted_activity() { - let domain = DefineTrait::::new() + let domain = DefineTrait::::new() .lift(JustApples, Buy(20)) .chain_lift(JustApples, Sell(60)) .chain_lift(JustBananas, Buy(30)) diff --git a/mapf/src/domain/cost.rs b/mapf/src/domain/cost.rs index 90f259f..b5177ea 100644 --- a/mapf/src/domain/cost.rs +++ b/mapf/src/domain/cost.rs @@ -101,6 +101,12 @@ macro_rules! cost_impl { self.0 /= rhs.0; } } + impl Mul<$f> for Cost<$f> { + type Output = Self; + fn mul(self, rhs: $f) -> Self { + Cost(self.0 * rhs) + } + } impl Zero for Cost<$f> { fn zero() -> Self { Cost(0.0) diff --git a/mapf/src/domain/define_trait.rs b/mapf/src/domain/define_trait.rs index b30c58e..3da02e6 100644 --- a/mapf/src/domain/define_trait.rs +++ b/mapf/src/domain/define_trait.rs @@ -20,11 +20,11 @@ use super::*; use anyhow::Error as AnyError; /// To begin defining a trait for a domain, use [`DefineTrait::new()`]. -pub struct DefineTrait { - _ignore: std::marker::PhantomData<(State, Error)>, +pub struct DefineTrait { + _ignore: std::marker::PhantomData<(State, Action, Error)>, } -impl DefineTrait { +impl DefineTrait { /// This function begins defining a domain by indicating what the top-level /// state and action representation is. /// @@ -42,18 +42,19 @@ impl DefineTrait { } } -impl Domain for DefineTrait { +impl Domain for DefineTrait { type State = State; + type Action = Action; type Error = Error; } -impl Clone for DefineTrait { +impl Clone for DefineTrait { fn clone(&self) -> Self { Self::new() } } -impl Default for DefineTrait { +impl Default for DefineTrait { fn default() -> Self { Self::new() } @@ -67,6 +68,7 @@ pub struct Incorporated { } impl Domain for Incorporated { type State = Base::State; + type Action = Base::Action; type Error = Base::Error; } @@ -106,6 +108,7 @@ pub struct Chained { } impl Domain for Chained { type State = Base::State; + type Action = Base::Action; type Error = Base::Error; } @@ -138,6 +141,7 @@ pub struct Mapped { } impl Domain for Mapped { type State = Base::State; + type Action = Base::Action; type Error = Base::Error; } @@ -172,6 +176,7 @@ pub struct Lifted { } impl Domain for Lifted { type State = Base::State; + type Action = Base::Action; type Error = Base::Error; } @@ -196,7 +201,11 @@ pub trait Lift { type ChainedLift = Chained< Base, - Lifted::State, ::Error>, Lifter, Prop>, + Lifted< + DefineTrait<::State, ::Action, ::Error>, + Lifter, + Prop, + >, >; impl Lift for D { @@ -215,6 +224,6 @@ impl Lift for D { where Self: Sized, { - self.chain(DefineTrait::::new().lift(lifter, prop)) + self.chain(DefineTrait::::new().lift(lifter, prop)) } } diff --git a/mapf/src/domain/informed.rs b/mapf/src/domain/heuristic.rs similarity index 77% rename from mapf/src/domain/informed.rs rename to mapf/src/domain/heuristic.rs index ba12a4b..620d306 100644 --- a/mapf/src/domain/informed.rs +++ b/mapf/src/domain/heuristic.rs @@ -18,12 +18,12 @@ use super::*; use crate::error::NoError; -pub trait Informed { +pub trait Heuristic { /// How is cost represented. E.g. f32, f64, or u64 type CostEstimate; /// What kind of error can happen if a bad state is provided - type InformedError; + type HeuristicError; /// Calculate an estimate for what it will cost to move from `from_state` to /// `to_goal_state`. If it is know to be impossible to reach the goal from @@ -33,7 +33,7 @@ pub trait Informed { &self, from_state: &State, to_goal: &Goal, - ) -> Result, Self::InformedError>; + ) -> Result, Self::HeuristicError>; } pub trait EstimateModifier { @@ -63,40 +63,40 @@ where } } -impl Informed for Incorporated +impl Heuristic for Incorporated where Base: Domain, - Prop: Informed, - Prop::InformedError: Into, + Prop: Heuristic, + Prop::HeuristicError: Into, { type CostEstimate = Prop::CostEstimate; - type InformedError = Base::Error; + type HeuristicError = Base::Error; fn estimate_remaining_cost( &self, from_state: &Base::State, to_goal: &Goal, - ) -> Result, Self::InformedError> { + ) -> Result, Self::HeuristicError> { self.prop .estimate_remaining_cost(from_state, to_goal) .map_err(Into::into) } } -impl Informed for Chained +impl Heuristic for Chained where - Base: Domain + Informed, - Base::InformedError: Into, - Prop: Informed, - Prop::InformedError: Into, + Base: Domain + Heuristic, + Base::HeuristicError: Into, + Prop: Heuristic, + Prop::HeuristicError: Into, Base::CostEstimate: std::ops::Add, { type CostEstimate = Base::CostEstimate; - type InformedError = Base::Error; + type HeuristicError = Base::Error; fn estimate_remaining_cost( &self, from_state: &Base::State, to_goal: &Goal, - ) -> Result, Self::InformedError> { + ) -> Result, Self::HeuristicError> { let base_cost_estimate = self .base .estimate_remaining_cost(from_state, to_goal) @@ -119,20 +119,20 @@ where } } -impl Informed for Mapped +impl Heuristic for Mapped where - Base: Domain + Informed, - Base::InformedError: Into, + Base: Domain + Heuristic, + Base::HeuristicError: Into, Prop: EstimateModifier, Prop::EstimateModifierError: Into, { type CostEstimate = Base::CostEstimate; - type InformedError = Base::Error; + type HeuristicError = Base::Error; fn estimate_remaining_cost( &self, from_state: &Base::State, to_goal: &Goal, - ) -> Result, Self::InformedError> { + ) -> Result, Self::HeuristicError> { let original_estimate = match self .base .estimate_remaining_cost(from_state, to_goal) @@ -148,23 +148,23 @@ where } } -impl Informed for Lifted +impl Heuristic for Lifted where Base: Domain, Base::State: Clone, Lifter: ProjectState, Lifter::ProjectionError: Into, - Prop: Informed, - Prop::InformedError: Into, + Prop: Heuristic, + Prop::HeuristicError: Into, Prop::CostEstimate: std::ops::Add, { type CostEstimate = Prop::CostEstimate; - type InformedError = Base::Error; + type HeuristicError = Base::Error; fn estimate_remaining_cost( &self, from_state: &Base::State, to_goal: &Goal, - ) -> Result, Self::InformedError> { + ) -> Result, Self::HeuristicError> { let from_state_proj = match self.lifter.project(from_state).map_err(Into::into)? { Some(s) => s, None => return Ok(None), @@ -178,33 +178,33 @@ where #[cfg(test)] mod tests { - use super::weighted::tests::*; + use super::weight::tests::*; use super::*; use crate::error::NoError; use approx::assert_relative_eq; struct EuclideanDistanceEstimate; - impl Informed for EuclideanDistanceEstimate { + impl Heuristic for EuclideanDistanceEstimate { type CostEstimate = f64; - type InformedError = NoError; + type HeuristicError = NoError; fn estimate_remaining_cost( &self, from_state: &State, to_goal: &Goal, - ) -> Result, Self::InformedError> { + ) -> Result, Self::HeuristicError> { Ok(Some((from_state.position() - to_goal.position()).norm())) } } struct BatteryLevelCostEstimate; - impl Informed for BatteryLevelCostEstimate { + impl Heuristic for BatteryLevelCostEstimate { type CostEstimate = f64; - type InformedError = NoError; + type HeuristicError = NoError; fn estimate_remaining_cost( &self, from_state: &State, _: &Goal, - ) -> Result, Self::InformedError> { + ) -> Result, Self::HeuristicError> { if from_state.battery_level() <= 0.0 { return Ok(None); } @@ -216,7 +216,7 @@ mod tests { #[test] fn test_cost_estimate() { - let domain = DefineTrait::::new() + let domain = DefineTrait::::new() .with(EuclideanDistanceEstimate) .map(ScaleWeight(0.1)); @@ -238,16 +238,16 @@ mod tests { #[test] fn test_lifted_cost_estimate() { - let domain = DefineTrait::::new() + let domain = DefineTrait::::new() .lift( DefineDomainMap::for_subspace(StateInto::::new()), - DefineTrait::::new() + DefineTrait::::new() .with(EuclideanDistanceEstimate) .map(ScaleWeight(0.1)), ) .chain_lift( DefineDomainMap::for_subspace(StateInto::::new()), - DefineTrait::::new() + DefineTrait::::new() .with(BatteryLevelCostEstimate) .map(ScaleWeight(0.2)), ); diff --git a/mapf/src/domain/initializable.rs b/mapf/src/domain/initializable.rs index 369434b..7c3d96a 100644 --- a/mapf/src/domain/initializable.rs +++ b/mapf/src/domain/initializable.rs @@ -314,7 +314,7 @@ mod tests { #[test] fn test_single_start() { - let domain = DefineTrait::::new().with(InitFrom); + let domain = DefineTrait::::new().with(InitFrom); let initial_state: Result, _> = domain.initialize(5, &()).into_iter().collect(); let initial_state = initial_state.unwrap(); @@ -324,7 +324,7 @@ mod tests { #[test] fn test_multi_start() { - let domain = DefineTrait::::new().with(ManyInit(InitFrom)); + let domain = DefineTrait::::new().with(ManyInit(InitFrom)); let initial_states: Result, _> = domain .initialize([Point::new(0.1, 0.2), Point::new(3.0, 4.0)], &()) diff --git a/mapf/src/domain/weighted.rs b/mapf/src/domain/weight.rs similarity index 81% rename from mapf/src/domain/weighted.rs rename to mapf/src/domain/weight.rs index 08a0d9c..9f86824 100644 --- a/mapf/src/domain/weighted.rs +++ b/mapf/src/domain/weight.rs @@ -19,13 +19,13 @@ use super::*; use crate::error::NoError; use num::traits::Zero; -/// The `Weighted` trait describes how to calculate the cost of an action. -pub trait Weighted { +/// The `Weight` trait describes how to calculate the cost of an action. +pub trait Weight { /// How is cost represented. E.g. f32, f64, or u64 type Cost; /// What kind of error can happen if a bad state or activity is provided - type WeightedError; + type WeightError; /// Calculate the cost for performing `action` which transitions /// `from_state` to `to_state`. @@ -37,14 +37,14 @@ pub trait Weighted { from_state: &State, action: &Action, to_state: &State, - ) -> Result, Self::WeightedError>; + ) -> Result, Self::WeightError>; /// Calculate the cost of an initial state. - fn initial_cost(&self, for_state: &State) -> Result, Self::WeightedError>; + fn initial_cost(&self, for_state: &State) -> Result, Self::WeightError>; } /// The `CostModifier` trait can be used with `.map` to modify the output of a -/// `Weighted` trait. +/// `Weight` trait. pub trait CostModifier { /// What kind of error can happen if a bad input is provided type CostModifierError; @@ -73,7 +73,7 @@ pub trait CostModifier { /// Implements CostModifier for simple proportional scaling of cost calculation. /// -/// Apply this to a Weighted property using `.map(ScaleWeight(scale))`. +/// Apply this to a Weight property using `.map(ScaleWeight(scale))`. pub struct ScaleWeight + Clone>(pub Cost); impl CostModifier for ScaleWeight where @@ -91,20 +91,20 @@ where } } -impl Weighted for Incorporated +impl Weight for Incorporated where Base: Domain, - Prop: Weighted, - Prop::WeightedError: Into, + Prop: Weight, + Prop::WeightError: Into, { type Cost = Prop::Cost; - type WeightedError = Base::Error; + type WeightError = Base::Error; fn cost( &self, from_state: &Base::State, action: &Action, to_state: &Base::State, - ) -> Result, Self::WeightedError> { + ) -> Result, Self::WeightError> { self.prop .cost(from_state, action, to_state) .map_err(Into::into) @@ -113,27 +113,27 @@ where fn initial_cost( &self, for_state: &Base::State, - ) -> Result, Self::WeightedError> { + ) -> Result, Self::WeightError> { self.prop.initial_cost(for_state).map_err(Into::into) } } -impl Weighted for Chained +impl Weight for Chained where - Base: Domain + Weighted, - Base::WeightedError: Into, - Prop: Weighted, - Prop::WeightedError: Into, + Base: Domain + Weight, + Base::WeightError: Into, + Prop: Weight, + Prop::WeightError: Into, Base::Cost: std::ops::Add, { type Cost = Base::Cost; - type WeightedError = Base::Error; + type WeightError = Base::Error; fn cost( &self, from_state: &Base::State, action: &Action, to_state: &Base::State, - ) -> Result, Self::WeightedError> { + ) -> Result, Self::WeightError> { let base_cost = self .base .cost(from_state, action, to_state) @@ -158,7 +158,7 @@ where fn initial_cost( &self, for_state: &Base::State, - ) -> Result, Self::WeightedError> { + ) -> Result, Self::WeightError> { let base_cost = self.base.initial_cost(for_state).map_err(Into::into)?; let prop_cost = self.prop.initial_cost(for_state).map_err(Into::into)?; @@ -175,21 +175,21 @@ where } } -impl Weighted for Mapped +impl Weight for Mapped where - Base: Domain + Weighted, - Base::WeightedError: Into, + Base: Domain + Weight, + Base::WeightError: Into, Prop: CostModifier, Prop::CostModifierError: Into, { type Cost = Base::Cost; - type WeightedError = Base::Error; + type WeightError = Base::Error; fn cost( &self, from_state: &Base::State, action: &Action, to_state: &Base::State, - ) -> Result, Self::WeightedError> { + ) -> Result, Self::WeightError> { let base_cost = match self .base .cost(from_state, action, to_state) @@ -207,7 +207,7 @@ where fn initial_cost( &self, for_state: &Base::State, - ) -> Result, Self::WeightedError> { + ) -> Result, Self::WeightError> { let base_cost = match self.base.initial_cost(for_state).map_err(Into::into)? { Some(base_cost) => base_cost, None => return Ok(None), @@ -219,7 +219,7 @@ where } } -impl Weighted for Lifted +impl Weight for Lifted where Base: Domain, Base::State: Clone, @@ -227,18 +227,18 @@ where Lifter: ProjectState + ActionMap, Lifter::ActionMapError: Into, Lifter::ProjectionError: Into, - Prop: Weighted, - Prop::WeightedError: Into, + Prop: Weight, + Prop::WeightError: Into, Prop::Cost: std::ops::Add + Zero, { type Cost = Prop::Cost; - type WeightedError = Base::Error; + type WeightError = Base::Error; fn cost( &self, from_state: &Base::State, action: &Action, to_state: &Base::State, - ) -> Result, Self::WeightedError> { + ) -> Result, Self::WeightError> { let from_state_proj = match self.lifter.project(from_state).map_err(Into::into)? { Some(s) => s, None => return Ok(None), @@ -274,7 +274,7 @@ where fn initial_cost( &self, for_state: &Base::State, - ) -> Result, Self::WeightedError> { + ) -> Result, Self::WeightError> { let for_state_proj = match self.lifter.project(for_state).map_err(Into::into)? { Some(s) => s, None => return Ok(None), @@ -353,33 +353,33 @@ pub(crate) mod tests { } struct DistanceWeight(f64 /* cost per meter */); - impl Weighted for DistanceWeight { + impl Weight for DistanceWeight { type Cost = f64; - type WeightedError = NoError; + type WeightError = NoError; fn cost( &self, from_state: &State, _: &Action, to_state: &State, - ) -> Result, Self::WeightedError> { + ) -> Result, Self::WeightError> { Ok(Some(to_state.distance_traveled(from_state) * self.0)) } - fn initial_cost(&self, _: &State) -> Result, Self::WeightedError> { + fn initial_cost(&self, _: &State) -> Result, Self::WeightError> { Ok(Some(0.0)) } } struct BatteryLossWeight(f64 /* cost per battery loss */); - impl Weighted for BatteryLossWeight { + impl Weight for BatteryLossWeight { type Cost = f64; - type WeightedError = NoError; + type WeightError = NoError; fn cost( &self, from_state: &State, _: &Action, to_state: &State, - ) -> Result, Self::WeightedError> { + ) -> Result, Self::WeightError> { if to_state.battery_level() < 0.0 { return Ok(None); } @@ -389,21 +389,21 @@ pub(crate) mod tests { )) } - fn initial_cost(&self, _: &State) -> Result, Self::WeightedError> { + fn initial_cost(&self, _: &State) -> Result, Self::WeightError> { Ok(Some(0.0)) } } #[test] fn test_cost_calculation() { - let domain = DefineTrait::::new() + let domain = DefineTrait::::new() .with( - DefineTrait::::new() + DefineTrait::::new() .with(DistanceWeight(0.1)) .map(ScaleWeight(2.0)), ) .chain( - DefineTrait::::new() + DefineTrait::::new() .with(BatteryLossWeight(10.0)) .map(ScaleWeight(3.0)), ); @@ -423,16 +423,16 @@ pub(crate) mod tests { #[test] fn test_lifted_weight_calculation() { - let domain = DefineTrait::::new() + let domain = DefineTrait::::new() .lift( DefineDomainMap::for_subspace(StateInto::::new()), - DefineTrait::::new() + DefineTrait::::new() .with(DistanceWeight(0.1)) .map(ScaleWeight(2.0)), ) .chain_lift( DefineDomainMap::for_subspace(StateInto::::new()), - DefineTrait::::new() + DefineTrait::::new() .with(BatteryLossWeight(10.0)) .map(ScaleWeight(3.0)), ); diff --git a/mapf/src/lib.rs b/mapf/src/lib.rs index 3309e9c..0122bfa 100644 --- a/mapf/src/lib.rs +++ b/mapf/src/lib.rs @@ -40,8 +40,6 @@ pub mod premade; mod util; pub mod prelude { - pub use super::algorithm::*; - pub use super::domain::*; pub use super::graph::*; pub use super::planner::*; pub use super::premade::*; diff --git a/mapf/src/motion/r2/direct_travel.rs b/mapf/src/motion/r2/direct_travel.rs index 6ec3649..8d485d2 100644 --- a/mapf/src/motion/r2/direct_travel.rs +++ b/mapf/src/motion/r2/direct_travel.rs @@ -16,7 +16,7 @@ */ use crate::{ - domain::{Extrapolator, Informed, Key, KeyedSpace, Reversible, SelfKey, Weighted}, + domain::{Extrapolator, Heuristic, Key, KeyedSpace, Reversible, SelfKey, Weight}, graph::Graph, motion::r2::{DiscreteSpaceTimeR2, LineFollow, LineFollowError, Position, StateR2, WaypointR2}, }; @@ -32,22 +32,22 @@ pub struct DirectTravelHeuristic { pub extrapolator: LineFollow, } -impl Informed, Goal> for DirectTravelHeuristic +impl Heuristic, Goal> for DirectTravelHeuristic where G: Graph, G::Key: Key + Clone, G::Vertex: Borrow, - W: Weighted, ArrayVec>, + W: Weight, ArrayVec>, Goal: SelfKey, { type CostEstimate = W::Cost; - type InformedError = DirectTravelError; + type HeuristicError = DirectTravelError; fn estimate_remaining_cost( &self, from_state: &StateR2, to_goal: &Goal, - ) -> Result, Self::InformedError> { + ) -> Result, Self::HeuristicError> { let p_target = { if let Some(p) = self.graph.vertex(to_goal.key().borrow()) { p @@ -73,7 +73,7 @@ where .make_keyed_state(to_goal.key().borrow().clone(), child_wp); self.weight .cost(from_state, &action, &child_state) - .map_err(DirectTravelError::Weighted) + .map_err(DirectTravelError::Weight) }) .transpose() .map(|x| x.flatten()) @@ -102,7 +102,7 @@ impl Reversible for DirectTravelHeuristic< weight: self .weight .reversed() - .map_err(DirectTravelReversalError::Weighted)?, + .map_err(DirectTravelReversalError::Weight)?, extrapolator: self .extrapolator .reversed() @@ -114,7 +114,7 @@ impl Reversible for DirectTravelHeuristic< #[derive(ThisError, Debug, Clone)] pub enum DirectTravelError { #[error("The cost calculator had an error:\n{0}")] - Weighted(W), + Weight(W), #[error("The extrapolator had an error:\n{0}")] Extrapolator(LineFollowError), } @@ -124,7 +124,7 @@ pub enum DirectTravelReversalError { #[error("The graph had an error while reversing:\n{0}")] Graph(G), #[error("The cost calculator had an error while reversing:\n{0}")] - Weighted(W), + Weight(W), #[error("The extrapolator had an error while reversing:\n{0}")] Extrapolator(E), } diff --git a/mapf/src/motion/se2/quickest_path.rs b/mapf/src/motion/se2/quickest_path.rs index ebf1331..21e8a62 100644 --- a/mapf/src/motion/se2/quickest_path.rs +++ b/mapf/src/motion/se2/quickest_path.rs @@ -17,7 +17,7 @@ use crate::{ algorithm::{BackwardDijkstra, Path}, - domain::{Connectable, Extrapolator, Informed, Key, KeyedCloser, Reversible, Weighted}, + domain::{Connectable, Extrapolator, Heuristic, Key, KeyedCloser, Reversible, Weight}, error::{Anyhow, ThisError}, motion::{ r2::{ @@ -58,9 +58,9 @@ pub type QuickestPathPlanner = pub struct QuickestPathHeuristic where WeightR2: Reversible, - WeightR2: Weighted, ArrayVec>, - WeightR2::WeightedError: Into, - WeightSE2: Weighted, DifferentialDriveLineFollowMotion>, + WeightR2: Weight, ArrayVec>, + WeightR2::WeightError: Into, + WeightSE2: Weight, DifferentialDriveLineFollowMotion>, G: Graph + Reversible + Clone, G::Key: Key + Clone, G::Vertex: Positioned + MaybeOriented, @@ -85,9 +85,9 @@ type HeuristicKey = (KeySE2, K); impl QuickestPathHeuristic where WeightR2: Reversible, - WeightR2: Weighted, ArrayVec>, - WeightR2::WeightedError: Into, - WeightSE2: Weighted, DifferentialDriveLineFollowMotion>, + WeightR2: Weight, ArrayVec>, + WeightR2::WeightError: Into, + WeightSE2: Weight, DifferentialDriveLineFollowMotion>, G: Graph + Reversible + Clone, G::Key: Key + Clone, G::Vertex: Positioned + MaybeOriented, @@ -142,12 +142,12 @@ where to_goal: &Goal, ) -> Result>, QuickestPathHeuristicError> where - WeightR2: Reversible + Weighted, ArrayVec>, + WeightR2: Reversible + Weight, ArrayVec>, WeightR2::Cost: Clone + Ord + Add, - WeightR2::WeightedError: Into, - WeightSE2: Weighted, DifferentialDriveLineFollowMotion>, + WeightR2::WeightError: Into, + WeightSE2: Weight, DifferentialDriveLineFollowMotion>, WeightSE2::Cost: Clone + Add, - WeightSE2::WeightedError: Into, + WeightSE2::WeightError: Into, G: Graph + Reversible + Clone, G::Key: Key + Clone, G::Vertex: Positioned + MaybeOriented, @@ -239,15 +239,15 @@ where } } -impl Informed +impl Heuristic for QuickestPathHeuristic where - WeightR2: Reversible + Weighted, ArrayVec>, + WeightR2: Reversible + Weight, ArrayVec>, WeightR2::Cost: Clone + Ord + Add, - WeightR2::WeightedError: Into, - WeightSE2: Weighted, DifferentialDriveLineFollowMotion>, + WeightR2::WeightError: Into, + WeightSE2: Weight, DifferentialDriveLineFollowMotion>, WeightSE2::Cost: Clone + Add, - WeightSE2::WeightedError: Into, + WeightSE2::WeightError: Into, G: Graph + Reversible + Clone, G::Key: Key + Clone, G::Vertex: Positioned + MaybeOriented, @@ -256,12 +256,12 @@ where Goal: Borrow + MaybePositioned + MaybeOriented + MaybeTimed, { type CostEstimate = WeightSE2::Cost; - type InformedError = QuickestPathHeuristicError; + type HeuristicError = QuickestPathHeuristicError; fn estimate_remaining_cost( &self, from_state: &State, to_goal: &Goal, - ) -> Result, Self::InformedError> { + ) -> Result, Self::HeuristicError> { // 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)? { diff --git a/mapf/src/motion/travel_effort_cost.rs b/mapf/src/motion/travel_effort_cost.rs index e2639c4..2a975df 100644 --- a/mapf/src/motion/travel_effort_cost.rs +++ b/mapf/src/motion/travel_effort_cost.rs @@ -16,7 +16,7 @@ */ use crate::{ - domain::{Cost, Reversible, Weighted}, + domain::{Cost, Reversible, Weight}, error::NoError, motion::{Measurable, Timed}, }; @@ -82,16 +82,16 @@ impl Default for TravelEffortCost { } } -impl> Weighted for TravelEffortCost { +impl> Weight for TravelEffortCost { type Cost = Cost; - type WeightedError = NoError; + type WeightError = NoError; fn cost( &self, from_state: &State, action: &Action, to_state: &State, - ) -> Result, Self::WeightedError> { + ) -> Result, Self::WeightError> { // Using the absolute value of the difference allows this same // implementation to work both forwards and backwards in time. We are // assuming that any case where time is decreasing in the child state, @@ -105,7 +105,7 @@ impl> Weighted for Travel Ok(Some(Cost(cost))) } - fn initial_cost(&self, _: &State) -> Result, Self::WeightedError> { + fn initial_cost(&self, _: &State) -> Result, Self::WeightError> { Ok(Some(Cost(0.0))) } } diff --git a/mapf/src/motion/travel_time_cost.rs b/mapf/src/motion/travel_time_cost.rs index 722235f..bcf3af1 100644 --- a/mapf/src/motion/travel_time_cost.rs +++ b/mapf/src/motion/travel_time_cost.rs @@ -16,7 +16,7 @@ */ use crate::{ - domain::{Cost, Reversible, Weighted}, + domain::{Cost, Reversible, Weight}, error::NoError, motion::Timed, }; @@ -30,16 +30,16 @@ impl Default for TravelTimeCost { } } -impl Weighted for TravelTimeCost { +impl Weight for TravelTimeCost { type Cost = Cost; - type WeightedError = NoError; + type WeightError = NoError; fn cost( &self, from_state: &State, _: &Action, to_state: &State, - ) -> Result, Self::WeightedError> { + ) -> Result, Self::WeightError> { // Using the absolute value of the difference allows this same // implementation to work both forwards and backwards in time. We are // assuming that any case where time is decreasing in the child state, @@ -48,7 +48,7 @@ impl Weighted for TravelTimeCost { Ok(Some(Cost(duration * self.0))) } - fn initial_cost(&self, _: &State) -> Result, Self::WeightedError> { + fn initial_cost(&self, _: &State) -> Result, Self::WeightError> { Ok(Some(Cost(0.0))) } } diff --git a/mapf/src/premade/search_se2.rs b/mapf/src/premade/search_se2.rs index 913f95f..e13f21c 100644 --- a/mapf/src/premade/search_se2.rs +++ b/mapf/src/premade/search_se2.rs @@ -45,7 +45,11 @@ pub type SearchSE2 = InformedSearch< >, TravelTimeCost, Lifted< - DefineTrait::Key, DEFAULT_RES>, Anyhow>, + DefineTrait< + StateSE2<::Key, DEFAULT_RES>, + DifferentialDriveLineFollowMotion, + Anyhow, + >, StateInto::Key>>, DirectTravelHeuristic, TravelTimeCost>, >, @@ -70,15 +74,16 @@ where extrapolator: motion, }, TravelTimeCost(1.0), - DefineTrait::>::new().lift( - StateInto::>::new(), - DirectTravelHeuristic { - space: DiscreteSpaceTimeR2::::new(), - graph: graph.clone(), - weight: TravelTimeCost(1.0), - extrapolator: motion.into(), - }, - ), + DefineTrait::, DifferentialDriveLineFollowMotion>::new() + .lift( + StateInto::>::new(), + DirectTravelHeuristic { + space: DiscreteSpaceTimeR2::::new(), + graph: graph.clone(), + weight: TravelTimeCost(1.0), + extrapolator: motion.into(), + }, + ), KeyedCloser(DiscreteSpaceTimeSE2::::new()), ) .with_initializer(InitializeSE2(graph)) diff --git a/mapf/src/templates/graph_motion.rs b/mapf/src/templates/graph_motion.rs index 628e403..1672387 100644 --- a/mapf/src/templates/graph_motion.rs +++ b/mapf/src/templates/graph_motion.rs @@ -66,10 +66,11 @@ where E: Extrapolator, { type State = S::State; + type Action = E::Extrapolation; type Error = GraphMotionError; } -impl Activity for GraphMotion +impl Activity for GraphMotion where S: KeyedSpace, S::Key: Borrow, @@ -80,13 +81,12 @@ where E: Extrapolator, E::ExtrapolationError: StdError, { - type Action = E::Extrapolation; type ActivityError = GraphMotionError; type Choices<'a> = GraphMotionChoices<'a, S, G, E> where Self: 'a, - Self::Action: 'a, + E::Extrapolation: 'a, Self::ActivityError: 'a, S::State: 'a, G::EdgeAttributes: 'a, @@ -96,7 +96,7 @@ where fn choices<'a>(&'a self, from_state: S::State) -> Self::Choices<'a> where Self: 'a, - Self::Action: 'a, + E::Extrapolation: 'a, Self::ActivityError: 'a, S::State: 'a, G::Key: 'a, diff --git a/mapf/src/templates/incremental_graph_motion.rs b/mapf/src/templates/incremental_graph_motion.rs index d359b28..8a123b0 100644 --- a/mapf/src/templates/incremental_graph_motion.rs +++ b/mapf/src/templates/incremental_graph_motion.rs @@ -75,10 +75,12 @@ where E: IncrementalExtrapolator, { type State = IncrementalState; + type Action = E::IncrementalExtrapolation; type Error = GraphMotionError; } -impl Activity> for IncrementalGraphMotion +impl Activity, E::IncrementalExtrapolation> + for IncrementalGraphMotion where S: KeyedSpace, S::Key: Borrow, @@ -89,13 +91,12 @@ where E: IncrementalExtrapolator, E::IncrementalExtrapolationError: StdError, { - type Action = E::IncrementalExtrapolation; type ActivityError = GraphMotionError; type Choices<'a> = IncrementalGraphMotionChoices<'a, S, G, E> where Self: 'a, - Self::Action: 'a, + E::IncrementalExtrapolation: 'a, Self::ActivityError: 'a, S::State: 'a, G::EdgeAttributes: 'a; @@ -103,7 +104,7 @@ where fn choices<'a>(&'a self, from_state: IncrementalState) -> Self::Choices<'a> where Self: 'a, - Self::Action: 'a, + E::IncrementalExtrapolation: 'a, Self::ActivityError: 'a, IncrementalState: 'a, { diff --git a/mapf/src/templates/informed_search.rs b/mapf/src/templates/informed_search.rs index fb216ed..f56187a 100644 --- a/mapf/src/templates/informed_search.rs +++ b/mapf/src/templates/informed_search.rs @@ -18,8 +18,8 @@ use crate::{ domain::{ Activity, ArrivalKeyring, AsTimeInvariant, AsTimeVariant, Backtrack, Chained, Closable, - Connectable, Domain, Informed, Initializable, Keyed, Keyring, PartialKeyed, Reversible, - Satisfiable, Weighted, + Connectable, Domain, Heuristic, Initializable, Keyed, Keyring, PartialKeyed, Reversible, + Satisfiable, Weight, }, error::{Anyhow, ThisError}, }; @@ -38,10 +38,10 @@ pub struct InformedSearch { /// * [`crate::templates::graph_motion::GraphMotion`] pub activity: A, /// Calculate the cost of each choice that is searched. This field must - /// implement [`Weighted`]. + /// implement [`Weight`]. pub weight: W, /// Calculate an estimate of the remaining cost from a state. This field - /// must implemented [`Informed`]. + /// must implement [`Heuristic`]. pub heuristic: H, /// Provide the [`ClosedSet`] that should be used during the search. This /// field must implement [`Closable`]. @@ -246,6 +246,7 @@ impl InformedSearch { impl Domain for InformedSearch { type State = A::State; + type Action = A::Action; type Error = anyhow::Error; } @@ -289,24 +290,23 @@ where } } -impl Activity for InformedSearch +impl Activity for InformedSearch where - A: Domain + Activity, + A: Domain + Activity, { - type Action = A::Action; type ActivityError = A::ActivityError; type Choices<'a> = A::Choices<'a> where Self: 'a, - Self::Action: 'a, + A::Action: 'a, Self::ActivityError: 'a, A::State: 'a; fn choices<'a>(&'a self, from_state: A::State) -> Self::Choices<'a> where Self: 'a, - Self::Action: 'a, + A::Action: 'a, Self::ActivityError: 'a, A::State: 'a, { @@ -314,44 +314,41 @@ where } } -impl Weighted for InformedSearch +impl Weight for InformedSearch where - A: Domain + Activity, - W: Weighted, - W::WeightedError: Into, + A: Domain + Activity, + W: Weight, + W::WeightError: Into, { type Cost = W::Cost; - type WeightedError = W::WeightedError; + type WeightError = W::WeightError; fn cost( &self, from_state: &A::State, action: &A::Action, to_state: &A::State, - ) -> Result, Self::WeightedError> { + ) -> Result, Self::WeightError> { self.weight.cost(from_state, action, to_state) } - fn initial_cost( - &self, - for_state: &A::State, - ) -> Result, Self::WeightedError> { + fn initial_cost(&self, for_state: &A::State) -> Result, Self::WeightError> { self.weight.initial_cost(for_state) } } -impl Informed for InformedSearch +impl Heuristic for InformedSearch where A: Domain, - H: Informed, - H::InformedError: Into, + H: Heuristic, + H::HeuristicError: Into, { type CostEstimate = H::CostEstimate; - type InformedError = H::InformedError; + type HeuristicError = H::HeuristicError; fn estimate_remaining_cost( &self, from_state: &A::State, to_goal: &Goal, - ) -> Result, Self::InformedError> { + ) -> Result, Self::HeuristicError> { self.heuristic.estimate_remaining_cost(from_state, to_goal) } } @@ -375,7 +372,7 @@ where impl Connectable for InformedSearch where - A: Domain + Activity, + A: Domain + Activity, A::State: Clone, C: Connectable, C::ConnectionError: Into, @@ -512,7 +509,7 @@ where impl Backtrack for InformedSearch where - A: Domain + Activity + Backtrack, + A: Domain + Activity + Backtrack, { type BacktrackError = A::BacktrackError; fn flip_endpoints(