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..065733c --- /dev/null +++ b/mapf-derive/src/lib.rs @@ -0,0 +1,220 @@ +/* + * 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 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("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(); + + expanded.extend(quote! { + impl #impl_generics ::mapf::domain::Domain for #name #ty_generics #where_clause { + type State = #state_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") { + expanded.extend(quote! { + impl #impl_generics ::mapf::domain::Activity<#state_type> for #name #ty_generics #where_clause { + type Action = <#field_ty as ::mapf::domain::Activity<#state_type>>::Action; + type ActivityError = <#field_ty as ::mapf::domain::Activity<#state_type>>::ActivityError; + type Choices<'a> = <#field_ty as ::mapf::domain::Activity<#state_type>>::Choices<'a> + where + Self: 'a, + Self::Action: 'a, + Self::ActivityError: 'a, + #state_type: 'a; + + fn choices<'a>(&'a self, from_state: #state_type) -> Self::Choices<'a> + where + Self: 'a, + Self::Action: 'a, + Self::ActivityError: 'a, + #state_type: 'a + { + self.#field_name.choices(from_state) + } + } + }); + } else if attr.path().is_ident("weight") { + 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") { + 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..8c59967 --- /dev/null +++ b/mapf/examples/astar_derive.rs @@ -0,0 +1,185 @@ +/* + * 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; + +/// 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, 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 Action = char; + type ActivityError = NoError; + type Choices<'a> + = std::vec::IntoIter> + where + Self: 'a, + Point: 'a; + + fn choices<'a>(&'a self, from_state: Point) -> Self::Choices<'a> + where + Self: 'a, + Point: 'a, + { + vec![ + Ok(( + 'N', + Point { + x: from_state.x, + y: from_state.y + 1, + }, + )), + Ok(( + 'S', + Point { + x: from_state.x, + y: from_state.y - 1, + }, + )), + Ok(( + 'E', + Point { + x: from_state.x + 1, + y: from_state.y, + }, + )), + Ok(( + 'W', + 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, _: &char, _: &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..218abdc --- /dev/null +++ b/mapf/examples/domain_derive.rs @@ -0,0 +1,128 @@ +/* + * 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, 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 Action = f64; + type ActivityError = NoError; + type Choices<'a> + = std::vec::IntoIter> + where + Self: 'a, + Self::Action: 'a, + Self::ActivityError: 'a, + f64: 'a; + + fn choices<'a>(&'a self, from_state: f64) -> Self::Choices<'a> + where + Self: 'a, + Self::Action: 'a, + Self::ActivityError: '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..e2b8967 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())); @@ -197,8 +197,8 @@ where 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>; } @@ -275,14 +275,14 @@ where + Initializable + Closable + Activity - + Weighted - + Informed, + + 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; @@ -296,8 +296,8 @@ where D: Domain + Closable + Activity - + Weighted - + Informed + + 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>; } @@ -356,14 +356,14 @@ where + Initializable + Closable + Activity - + Weighted - + Informed, + + 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; @@ -377,8 +377,8 @@ where D: Domain + Closable + Activity - + Weighted - + Informed + + 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..ac848ff 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,14 @@ 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 +55,14 @@ 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, } @@ -73,7 +73,7 @@ where + Keyring + Initializable + Activity - + Weighted + + Weight + Closable + Connectable + ArrivalKeyring, @@ -81,7 +81,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, @@ -102,7 +102,7 @@ where D: Domain + Reversible + Activity - + Weighted + + Weight + Keyring + Closable + Connectable @@ -112,7 +112,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, { @@ -143,7 +143,7 @@ where + Reversible + Keyed + Activity - + Weighted + + Weight + Closable, D::ReversalError: Into, { diff --git a/mapf/src/algorithm/dijkstra/forward.rs b/mapf/src/algorithm/dijkstra/forward.rs index ce6aeff..7a27eca 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,7 @@ use std::{ pub struct Dijkstra where - D: Domain + Keyed + Activity + Weighted + Closable, + D: Domain + Keyed + Activity + Weight + Closable, { domain: D, cache: Arc>>, @@ -41,14 +41,14 @@ 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 { @@ -87,7 +87,7 @@ where + Keyring + Initializable + Activity - + Weighted + + Weight + Closable + Connectable + ArrivalKeyring, @@ -95,7 +95,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, @@ -224,7 +224,7 @@ where D: Domain + Keyring + Activity - + Weighted + + Weight + Closable + Connectable, D::State: Clone, @@ -232,7 +232,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 +463,7 @@ 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 +503,14 @@ 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 +527,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 +536,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 +555,7 @@ 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 +575,7 @@ 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 +590,7 @@ 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 +604,7 @@ 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..10fa4dc 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. /// @@ -33,7 +33,7 @@ pub trait Domain { type Error; } -// pub use mapf_derive::Domain; +pub use mapf_derive::Domain; pub mod action_map; pub use action_map::*; @@ -65,8 +65,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 +86,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/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/informed.rs b/mapf/src/domain/heuristic.rs similarity index 79% rename from mapf/src/domain/informed.rs rename to mapf/src/domain/heuristic.rs index ba12a4b..48657b3 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); } diff --git a/mapf/src/domain/weighted.rs b/mapf/src/domain/weight.rs similarity index 83% rename from mapf/src/domain/weighted.rs rename to mapf/src/domain/weight.rs index 08a0d9c..72a1f06 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,7 +389,7 @@ pub(crate) mod tests { )) } - fn initial_cost(&self, _: &State) -> Result, Self::WeightedError> { + fn initial_cost(&self, _: &State) -> Result, Self::WeightError> { Ok(Some(0.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/templates/informed_search.rs b/mapf/src/templates/informed_search.rs index fb216ed..ceaf3a5 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`]. @@ -314,44 +314,41 @@ where } } -impl Weighted for InformedSearch +impl Weight for InformedSearch where A: Domain + Activity, - W: Weighted, - W::WeightedError: Into, + 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) } }