From 614a9a3619cbc929a689c6364a4b4866113bac7f Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 30 Jan 2024 17:25:57 +0800 Subject: [PATCH 1/8] Add orientation constraint, doors and lifts Signed-off-by: Luca Della Vedova --- rmf_site_format/src/legacy/nav_graph.rs | 148 ++++++++++++++++++++---- rmf_site_format/src/lift.rs | 54 +++++++++ rmf_site_format/src/misc.rs | 2 +- rmf_site_format/src/site.rs | 6 + 4 files changed, 189 insertions(+), 21 deletions(-) diff --git a/rmf_site_format/src/legacy/nav_graph.rs b/rmf_site_format/src/legacy/nav_graph.rs index 17316410..fe0e806b 100644 --- a/rmf_site_format/src/legacy/nav_graph.rs +++ b/rmf_site_format/src/legacy/nav_graph.rs @@ -6,6 +6,27 @@ use std::collections::{HashMap, HashSet}; pub struct NavGraph { pub building_name: String, pub levels: HashMap, + pub doors: HashMap, + pub lifts: HashMap, +} + +// Readapted from legacy traffic editor implementation +fn segments_intersect(p1: [f32; 2], p2: [f32; 2], p3: [f32; 2], p4: [f32; 2]) -> bool { + // line segments are (x1,y1),(x2,y2) and (x3,y3),(x4,y4) + let [x1, y1] = p1; + let [x2, y2] = p2; + let [x3, y3] = p3; + let [x4, y4] = p4; + let det = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4); + if det.abs() < 0.01 { + return false; + } + let t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / det; + let u = -((x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)) / det; + if u < 0.0 || t < 0.0 || u > 1.0 || t > 1.0 { + return false; + } + true } impl NavGraph { @@ -39,6 +60,7 @@ impl NavGraph { // TODO(MXG): Make this work for lifts + let mut doors = HashMap::new(); let mut levels = HashMap::new(); for (_, level) in &site.levels { let mut anchor_to_vertex = HashMap::new(); @@ -55,6 +77,33 @@ impl NavGraph { vertices.push(NavVertex::from_anchor(anchor, location_at_anchor.get(id))); } + let mut level_doors = HashMap::new(); + for (_, door) in &level.doors { + let door_name = &door.name.0; + let (v0, v1) = match ( + level.anchors.get(&door.anchors.start()), + level.anchors.get(&door.anchors.end()), + ) { + (Some(v0), Some(v1)) => ( + v0.translation_for_category(Category::Level), + v1.translation_for_category(Category::Level), + ), + _ => { + println!( + "ERROR: Skipping door {door_name} due to broken anchor reference" + ); + continue; + } + }; + level_doors.insert( + door_name.clone(), + NavDoor { + map: level.properties.name.0.clone(), + endpoints: [*v0, *v1], + }, + ); + } + let mut lanes = Vec::new(); for lane_id in &lanes_to_include { let lane = site.navigation.guided.lanes.get(lane_id).unwrap(); @@ -69,14 +118,27 @@ impl NavGraph { } }; - let props = NavLaneProperties::from_motion(&lane.forward); + let mut door_name = None; + let l0 = [vertices[v0].0, vertices[v0].1]; + let l1 = [vertices[v1].0, vertices[v1].1]; + for (name, door) in &level_doors { + if segments_intersect(l0, l1, door.endpoints[0], door.endpoints[1]) { + door_name = Some(name); + } + } + + let props = NavLaneProperties::from_motion(&lane.forward, door_name.cloned()); lanes.push(NavLane(v0, v1, props.clone())); match &lane.reverse { ReverseLane::Same => { lanes.push(NavLane(v1, v0, props)); } ReverseLane::Different(motion) => { - lanes.push(NavLane(v1, v0, NavLaneProperties::from_motion(motion))); + lanes.push(NavLane( + v1, + v0, + NavLaneProperties::from_motion(motion, door_name.cloned()), + )); } ReverseLane::Disable => { // Do nothing @@ -84,17 +146,46 @@ impl NavGraph { } } + doors.extend(level_doors); levels.insert( level.properties.name.clone().0, NavLevel { lanes, vertices }, ); } + let mut lifts = HashMap::new(); + for (_, lift) in &site.lifts { + let lift_name = &lift.properties.name.0; + let Some(pose) = lift.properties.center(site) else { + println!("ERROR: Skipping lift {lift_name} due to broken anchor reference"); + continue; + }; + let Rotation::Yaw(yaw) = pose.rot else { + println!("ERROR: Skipping lift {lift_name} due to rotation not being pure yaw"); + continue; + }; + match &lift.properties.cabin { + LiftCabin::Rect(params) => { + lifts.insert( + lift_name.clone(), + NavLift { + position: [pose.trans[0], pose.trans[1], yaw.radians()], + // Note depth and width are inverted between legacy and site editor + dims: [params.depth, params.width], + }, + ); + } + } + // TODO(luca) lift property for vertices + } + graphs.push(( graph.name.0.clone(), Self { building_name: site.properties.name.clone().0, levels, + doors, + lifts, }, )) } @@ -117,17 +208,32 @@ pub struct NavLaneProperties { pub speed_limit: f32, #[serde(skip_serializing_if = "Option::is_none")] pub dock_name: Option, - // TODO(MXG): Add other lane properties - // door_name, - // orientation_constraint, - // demo_mock_floor_name + #[serde(skip_serializing_if = "Option::is_none")] + pub door_name: Option, + #[serde(skip_serializing_if = "Option::is_none")] + pub orientation_constraint: Option, // TODO(MXG): Add other lane properties + // demo_mock_floor_name } impl NavLaneProperties { - fn from_motion(motion: &Motion) -> Self { + fn from_motion(motion: &Motion, door_name: Option) -> Self { + let orientation_constraint = match &motion.orientation_constraint { + OrientationConstraint::None => None, + OrientationConstraint::Forwards => Some("forward".to_owned()), + OrientationConstraint::Backwards => Some("backward".to_owned()), + OrientationConstraint::RelativeYaw(_) | OrientationConstraint::AbsoluteYaw(_) => { + println!( + "Skipping orientation constraint [{:?}] because of incompatibility", + motion.orientation_constraint + ); + None + } + }; Self { speed_limit: motion.speed_limit.unwrap_or(0.0), dock_name: motion.dock.as_ref().map(|d| d.name.clone()), + orientation_constraint, + door_name, } } } @@ -142,7 +248,7 @@ impl NavVertex { } } -#[derive(Serialize, Deserialize, Clone)] +#[derive(Serialize, Deserialize, Clone, Default)] pub struct NavVertexProperties { #[serde(skip_serializing_if = "Option::is_none")] pub lift: Option, @@ -152,21 +258,11 @@ pub struct NavVertexProperties { pub is_holding_point: bool, #[serde(skip_serializing_if = "is_false")] pub is_parking_spot: bool, + #[serde(skip_serializing_if = "Option::is_none")] + pub merge_radius: Option, pub name: String, } -impl Default for NavVertexProperties { - fn default() -> Self { - Self { - lift: None, - is_charger: false, - is_holding_point: false, - is_parking_spot: false, - name: "".to_owned(), - } - } -} - impl NavVertexProperties { fn from_location(location: Option<&Location>) -> Self { let mut props = Self::default(); @@ -196,3 +292,15 @@ impl NavVertexProperties { fn is_false(b: &bool) -> bool { !b } + +#[derive(Serialize, Deserialize, Clone)] +pub struct NavDoor { + pub endpoints: [[f32; 2]; 2], + pub map: String, +} + +#[derive(Serialize, Deserialize, Clone)] +pub struct NavLift { + pub position: [f32; 3], + pub dims: [f32; 2], +} diff --git a/rmf_site_format/src/lift.rs b/rmf_site_format/src/lift.rs index 97d22d38..90d16040 100644 --- a/rmf_site_format/src/lift.rs +++ b/rmf_site_format/src/lift.rs @@ -115,6 +115,60 @@ pub struct LiftProperties { pub initial_level: InitialLevel, } +impl LiftProperties { + /// Returns the pose of the lift cabin center in global coordinates. + pub fn center(&self, site: &Site) -> Option { + // Center of the aabb + let center = match &self.cabin { + LiftCabin::Rect(params) => { + let front_door_t = params + .front_door + .as_ref() + .map(|d| d.thickness()) + .unwrap_or(DEFAULT_CABIN_DOOR_THICKNESS); + + [ + -params.depth / 2.0 - params.thickness() - params.gap() - front_door_t / 2.0, + params.shift(), + DEFAULT_LEVEL_HEIGHT / 2.0, + ] + } + }; + // Get the vector between the reference anchors + let left_anchor = site.get_anchor(self.reference_anchors.left())?; + let right_anchor = site.get_anchor(self.reference_anchors.right())?; + let left_trans = left_anchor.translation_for_category(Category::Level); + let right_trans = right_anchor.translation_for_category(Category::Level); + let yaw = (left_trans[1] - right_trans[1]).atan2(left_trans[0] - right_trans[0]); + let midpoint = [ + (left_trans[0] + right_trans[0]) / 2.0, + (left_trans[1] + right_trans[1]) / 2.0, + ]; + let elevation = match &self.initial_level.0 { + Some(l) => site + .levels + .get(l) + .map(|level| level.properties.elevation.0)?, + None => { + let mut min_elevation = site + .levels + .first_key_value() + .map(|(_, l)| l.properties.elevation.0)?; + for l in site.levels.values().skip(1) { + if l.properties.elevation.0 < min_elevation { + min_elevation = l.properties.elevation.0; + } + } + min_elevation + } + }; + Some(Pose { + trans: [midpoint[0] + center[0], midpoint[1] + center[1], elevation], + rot: Rotation::Yaw(Angle::Rad(yaw)), + }) + } +} + #[derive(Serialize, Deserialize, Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] #[serde(transparent)] #[cfg_attr(feature = "bevy", derive(Component, Deref, DerefMut))] diff --git a/rmf_site_format/src/misc.rs b/rmf_site_format/src/misc.rs index 236065e1..85d4d175 100644 --- a/rmf_site_format/src/misc.rs +++ b/rmf_site_format/src/misc.rs @@ -15,7 +15,7 @@ * */ -use crate::{Recall, RefTrait}; +use crate::RefTrait; #[cfg(feature = "bevy")] use bevy::prelude::*; use glam::{Quat, Vec2, Vec3}; diff --git a/rmf_site_format/src/site.rs b/rmf_site_format/src/site.rs index b5b6f17b..9c6e8c0b 100644 --- a/rmf_site_format/src/site.rs +++ b/rmf_site_format/src/site.rs @@ -183,6 +183,12 @@ impl Site { pub fn from_bytes<'a>(s: &'a [u8]) -> ron::error::SpannedResult { ron::de::from_bytes(s) } + + pub fn get_anchor(&self, id: u32) -> Option<&Anchor> { + self.anchors + .get(&id) + .or_else(|| self.levels.values().find_map(|l| l.anchors.get(&id))) + } } pub trait RefTrait: Ord + Eq + Copy + Send + Sync + Hash + 'static {} From 287356599f5b7e84a4014f74889ec32af2188c55 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 31 Jan 2024 10:43:26 +0800 Subject: [PATCH 2/8] Add TODOs for documentation Signed-off-by: Luca Della Vedova --- rmf_site_format/src/legacy/nav_graph.rs | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/rmf_site_format/src/legacy/nav_graph.rs b/rmf_site_format/src/legacy/nav_graph.rs index fe0e806b..93d24360 100644 --- a/rmf_site_format/src/legacy/nav_graph.rs +++ b/rmf_site_format/src/legacy/nav_graph.rs @@ -12,7 +12,7 @@ pub struct NavGraph { // Readapted from legacy traffic editor implementation fn segments_intersect(p1: [f32; 2], p2: [f32; 2], p3: [f32; 2], p4: [f32; 2]) -> bool { - // line segments are (x1,y1),(x2,y2) and (x3,y3),(x4,y4) + // line segments are [p1-p2] and [p3-p4] let [x1, y1] = p1; let [x2, y2] = p2; let [x3, y3] = p3; @@ -164,6 +164,7 @@ impl NavGraph { println!("ERROR: Skipping lift {lift_name} due to rotation not being pure yaw"); continue; }; + // TODO(luca) check that the lift position is correct when doing end to end testing match &lift.properties.cabin { LiftCabin::Rect(params) => { lifts.insert( @@ -176,7 +177,7 @@ impl NavGraph { ); } } - // TODO(luca) lift property for vertices + // TODO(luca) lift property for vertices contained in lifts } graphs.push(( @@ -211,8 +212,10 @@ pub struct NavLaneProperties { #[serde(skip_serializing_if = "Option::is_none")] pub door_name: Option, #[serde(skip_serializing_if = "Option::is_none")] - pub orientation_constraint: Option, // TODO(MXG): Add other lane properties - // demo_mock_floor_name + pub orientation_constraint: Option, + // TODO(luca): Add other lane properties + // demo_mock_floor_name + // mutex } impl NavLaneProperties { @@ -250,6 +253,7 @@ impl NavVertex { #[derive(Serialize, Deserialize, Clone, Default)] pub struct NavVertexProperties { + // TODO(luca) serialize lift and merge_radius, they are currently skipped #[serde(skip_serializing_if = "Option::is_none")] pub lift: Option, #[serde(skip_serializing_if = "is_false")] From d76f218b70ebdf9aa1c9bcf0259a9a1e6ce2652e Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 31 Jan 2024 13:53:04 +0800 Subject: [PATCH 3/8] Fix lanes being skipped when they share an anchor Signed-off-by: Luca Della Vedova --- rmf_site_format/src/legacy/nav_graph.rs | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/rmf_site_format/src/legacy/nav_graph.rs b/rmf_site_format/src/legacy/nav_graph.rs index 93d24360..1da3b9ac 100644 --- a/rmf_site_format/src/legacy/nav_graph.rs +++ b/rmf_site_format/src/legacy/nav_graph.rs @@ -46,13 +46,14 @@ impl NavGraph { }; let lanes_with_anchor = { - let mut lanes_with_anchor = HashMap::new(); + let mut lanes_with_anchor: HashMap> = HashMap::new(); for (lane_id, lane) in &site.navigation.guided.lanes { if !lane.graphs.includes(graph_id) { continue; } for a in lane.anchors.array() { - lanes_with_anchor.insert(a, (*lane_id, lane)); + let entry = lanes_with_anchor.entry(a).or_default(); + entry.push(*lane_id); } } lanes_with_anchor @@ -67,12 +68,14 @@ impl NavGraph { let mut vertices = Vec::new(); let mut lanes_to_include = HashSet::new(); for (id, anchor) in &level.anchors { - let (lane, _) = match lanes_with_anchor.get(id) { - Some(v) => v, - None => continue, + let Some(lanes) = lanes_with_anchor.get(id) else { + continue; }; - lanes_to_include.insert(*lane); + for lane in lanes.iter() { + lanes_to_include.insert(*lane); + } + anchor_to_vertex.insert(*id, vertices.len()); vertices.push(NavVertex::from_anchor(anchor, location_at_anchor.get(id))); } From 3e9497484ca3d0ec9d9098800ad7810d0456d753 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 31 Jan 2024 13:55:31 +0800 Subject: [PATCH 4/8] Fix duplicated unnamed waypoints when exporting Signed-off-by: Luca Della Vedova --- rmf_site_format/src/legacy/vertex.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_site_format/src/legacy/vertex.rs b/rmf_site_format/src/legacy/vertex.rs index fa1ea453..3e0d7897 100644 --- a/rmf_site_format/src/legacy/vertex.rs +++ b/rmf_site_format/src/legacy/vertex.rs @@ -80,7 +80,7 @@ impl Vertex { return Some(Location { anchor: anchor.into(), tags: LocationTags(tags), - name: NameInSite(name.unwrap_or("".to_string())), + name: NameInSite(name.unwrap_or_default()), graphs: AssociatedGraphs::All, }); } From b4d0644cc2cf52c2e4aa9ab3d2d735c18843d2ac Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 1 Feb 2024 18:12:10 +0800 Subject: [PATCH 5/8] Add anchors in lifts, fix lift center rotation Signed-off-by: Luca Della Vedova --- rmf_site_format/src/legacy/nav_graph.rs | 90 ++++++++++++++++--------- rmf_site_format/src/lift.rs | 2 +- 2 files changed, 59 insertions(+), 33 deletions(-) diff --git a/rmf_site_format/src/legacy/nav_graph.rs b/rmf_site_format/src/legacy/nav_graph.rs index 1da3b9ac..f0dd1801 100644 --- a/rmf_site_format/src/legacy/nav_graph.rs +++ b/rmf_site_format/src/legacy/nav_graph.rs @@ -59,15 +59,68 @@ impl NavGraph { lanes_with_anchor }; - // TODO(MXG): Make this work for lifts - let mut doors = HashMap::new(); let mut levels = HashMap::new(); + let mut lifts = HashMap::new(); for (_, level) in &site.levels { let mut anchor_to_vertex = HashMap::new(); let mut vertices = Vec::new(); let mut lanes_to_include = HashSet::new(); - for (id, anchor) in &level.anchors { + // Add vertices for anchors that are in lifts + for lift in site.lifts.values() { + let lift_name = &lift.properties.name.0; + let Some(center) = lift.properties.center(site) else { + println!("ERROR: Skipping lift {lift_name} due to broken anchor reference"); + continue; + }; + let Rotation::Yaw(yaw) = center.rot else { + println!( + "ERROR: Skipping lift {lift_name} due to rotation not being pure yaw" + ); + continue; + }; + let yaw = yaw.radians(); + // Note this will overwrite the entry in the map but that is OK + // TODO(luca) check that the lift position is correct when doing end to end testing + match &lift.properties.cabin { + LiftCabin::Rect(params) => { + lifts.insert( + lift_name.clone(), + NavLift { + position: [center.trans[0], center.trans[1], yaw], + // Note depth and width are inverted between legacy and site editor + dims: [params.depth, params.width], + }, + ); + } + } + for (id, anchor) in &lift.cabin_anchors { + let Some(lanes) = lanes_with_anchor.get(id) else { + continue; + }; + + for lane in lanes.iter() { + lanes_to_include.insert(*lane); + } + + // The anchor is in lift coordinates, make it in global coordinates + let trans = anchor.translation_for_category(Category::General); + // TODO(luca) check that this translation is correct, most cases just have + // an anchor in the origin. + let anchor = Anchor::Translate2D([ + center.trans[0] + yaw.cos() * trans[0], + center.trans[1] + yaw.sin() * trans[1], + ]); + + anchor_to_vertex.insert(*id, vertices.len()); + let mut vertex = + NavVertex::from_anchor(&anchor, location_at_anchor.get(id)); + vertex.2.lift = Some(lift_name.clone()); + vertices.push(vertex); + } + } + // Add site and level anchors + for (id, anchor) in site.anchors.iter().chain(level.anchors.iter()) { let Some(lanes) = lanes_with_anchor.get(id) else { continue; }; @@ -84,8 +137,8 @@ impl NavGraph { for (_, door) in &level.doors { let door_name = &door.name.0; let (v0, v1) = match ( - level.anchors.get(&door.anchors.start()), - level.anchors.get(&door.anchors.end()), + site.get_anchor(door.anchors.start()), + site.get_anchor(door.anchors.end()), ) { (Some(v0), Some(v1)) => ( v0.translation_for_category(Category::Level), @@ -156,33 +209,6 @@ impl NavGraph { ); } - let mut lifts = HashMap::new(); - for (_, lift) in &site.lifts { - let lift_name = &lift.properties.name.0; - let Some(pose) = lift.properties.center(site) else { - println!("ERROR: Skipping lift {lift_name} due to broken anchor reference"); - continue; - }; - let Rotation::Yaw(yaw) = pose.rot else { - println!("ERROR: Skipping lift {lift_name} due to rotation not being pure yaw"); - continue; - }; - // TODO(luca) check that the lift position is correct when doing end to end testing - match &lift.properties.cabin { - LiftCabin::Rect(params) => { - lifts.insert( - lift_name.clone(), - NavLift { - position: [pose.trans[0], pose.trans[1], yaw.radians()], - // Note depth and width are inverted between legacy and site editor - dims: [params.depth, params.width], - }, - ); - } - } - // TODO(luca) lift property for vertices contained in lifts - } - graphs.push(( graph.name.0.clone(), Self { diff --git a/rmf_site_format/src/lift.rs b/rmf_site_format/src/lift.rs index 90d16040..5644403a 100644 --- a/rmf_site_format/src/lift.rs +++ b/rmf_site_format/src/lift.rs @@ -139,7 +139,7 @@ impl LiftProperties { let right_anchor = site.get_anchor(self.reference_anchors.right())?; let left_trans = left_anchor.translation_for_category(Category::Level); let right_trans = right_anchor.translation_for_category(Category::Level); - let yaw = (left_trans[1] - right_trans[1]).atan2(left_trans[0] - right_trans[0]); + let yaw = (left_trans[0] - right_trans[0]).atan2(left_trans[1] - right_trans[1]); let midpoint = [ (left_trans[0] + right_trans[0]) / 2.0, (left_trans[1] + right_trans[1]) / 2.0, From 877694366834090af439093f722f39cba03c5e58 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 7 Feb 2024 17:42:18 +0800 Subject: [PATCH 6/8] Address feedback Signed-off-by: Luca Della Vedova --- rmf_site_format/src/legacy/nav_graph.rs | 29 ++++++++++++++----------- rmf_site_format/src/lift.rs | 4 ++-- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/rmf_site_format/src/legacy/nav_graph.rs b/rmf_site_format/src/legacy/nav_graph.rs index f0dd1801..ba620cdb 100644 --- a/rmf_site_format/src/legacy/nav_graph.rs +++ b/rmf_site_format/src/legacy/nav_graph.rs @@ -1,4 +1,5 @@ use crate::*; +use glam::Affine2; use serde::{Deserialize, Serialize}; use std::collections::{HashMap, HashSet}; @@ -70,11 +71,13 @@ impl NavGraph { for lift in site.lifts.values() { let lift_name = &lift.properties.name.0; let Some(center) = lift.properties.center(site) else { - println!("ERROR: Skipping lift {lift_name} due to broken anchor reference"); + eprintln!( + "ERROR: Skipping lift {lift_name} due to broken anchor reference" + ); continue; }; let Rotation::Yaw(yaw) = center.rot else { - println!( + eprintln!( "ERROR: Skipping lift {lift_name} due to rotation not being pure yaw" ); continue; @@ -105,12 +108,12 @@ impl NavGraph { // The anchor is in lift coordinates, make it in global coordinates let trans = anchor.translation_for_category(Category::General); - // TODO(luca) check that this translation is correct, most cases just have - // an anchor in the origin. - let anchor = Anchor::Translate2D([ - center.trans[0] + yaw.cos() * trans[0], - center.trans[1] + yaw.sin() * trans[1], - ]); + let lift_tf = Affine2::from_angle_translation( + yaw, + [center.trans[0], center.trans[1]].into(), + ); + let trans = lift_tf.transform_point2((*trans).into()); + let anchor = Anchor::Translate2D([trans[0], trans[1]]); anchor_to_vertex.insert(*id, vertices.len()); let mut vertex = @@ -120,7 +123,7 @@ impl NavGraph { } } // Add site and level anchors - for (id, anchor) in site.anchors.iter().chain(level.anchors.iter()) { + for (id, anchor) in level.anchors.iter() { let Some(lanes) = lanes_with_anchor.get(id) else { continue; }; @@ -141,11 +144,11 @@ impl NavGraph { site.get_anchor(door.anchors.end()), ) { (Some(v0), Some(v1)) => ( - v0.translation_for_category(Category::Level), - v1.translation_for_category(Category::Level), + v0.translation_for_category(Category::Door), + v1.translation_for_category(Category::Door), ), _ => { - println!( + eprintln!( "ERROR: Skipping door {door_name} due to broken anchor reference" ); continue; @@ -169,7 +172,7 @@ impl NavGraph { ) { (Some(v0), Some(v1)) => (*v0, *v1), _ => { - println!("ERROR: Skipping lane {lane_id} due to incompatibility"); + eprintln!("ERROR: Lane {lane_id} is using a site anchor. This is not supported, the lane will be skipped."); continue; } }; diff --git a/rmf_site_format/src/lift.rs b/rmf_site_format/src/lift.rs index 5644403a..397d4db1 100644 --- a/rmf_site_format/src/lift.rs +++ b/rmf_site_format/src/lift.rs @@ -137,8 +137,8 @@ impl LiftProperties { // Get the vector between the reference anchors let left_anchor = site.get_anchor(self.reference_anchors.left())?; let right_anchor = site.get_anchor(self.reference_anchors.right())?; - let left_trans = left_anchor.translation_for_category(Category::Level); - let right_trans = right_anchor.translation_for_category(Category::Level); + let left_trans = left_anchor.translation_for_category(Category::Lift); + let right_trans = right_anchor.translation_for_category(Category::Lift); let yaw = (left_trans[0] - right_trans[0]).atan2(left_trans[1] - right_trans[1]); let midpoint = [ (left_trans[0] + right_trans[0]) / 2.0, From 3860cc4af1429a721f2b3b788682ba753c618a98 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 5 Mar 2024 14:39:57 +0000 Subject: [PATCH 7/8] Provide a reference for line segment inntersection calculation Signed-off-by: Michael X. Grey --- rmf_site_format/src/legacy/nav_graph.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_site_format/src/legacy/nav_graph.rs b/rmf_site_format/src/legacy/nav_graph.rs index ba620cdb..2794f1cc 100644 --- a/rmf_site_format/src/legacy/nav_graph.rs +++ b/rmf_site_format/src/legacy/nav_graph.rs @@ -11,7 +11,7 @@ pub struct NavGraph { pub lifts: HashMap, } -// Readapted from legacy traffic editor implementation +// Reference: https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line_segment fn segments_intersect(p1: [f32; 2], p2: [f32; 2], p3: [f32; 2], p4: [f32; 2]) -> bool { // line segments are [p1-p2] and [p3-p4] let [x1, y1] = p1; From e73011a2e0ca7aec76c1af331476f75a64787bea Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 5 Mar 2024 14:55:05 +0000 Subject: [PATCH 8/8] Switch to eprintln Signed-off-by: Michael X. Grey --- rmf_site_format/src/legacy/nav_graph.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_site_format/src/legacy/nav_graph.rs b/rmf_site_format/src/legacy/nav_graph.rs index 2794f1cc..907c1390 100644 --- a/rmf_site_format/src/legacy/nav_graph.rs +++ b/rmf_site_format/src/legacy/nav_graph.rs @@ -257,7 +257,7 @@ impl NavLaneProperties { OrientationConstraint::Forwards => Some("forward".to_owned()), OrientationConstraint::Backwards => Some("backward".to_owned()), OrientationConstraint::RelativeYaw(_) | OrientationConstraint::AbsoluteYaw(_) => { - println!( + eprintln!( "Skipping orientation constraint [{:?}] because of incompatibility", motion.orientation_constraint );