Add a few more properties to legacy navgraphs

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<String, NavLevel>,
+    pub doors: HashMap<String, NavDoor>,
+    pub lifts: HashMap<String, NavLift>,
+}
+
+// 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 [p1-p2] and [p3-p4]
+    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 {
@@ -25,36 +46,120 @@ impl NavGraph {
             };
 
             let lanes_with_anchor = {
-                let mut lanes_with_anchor = HashMap::new();
+                let mut lanes_with_anchor: HashMap<u32, Vec<u32>> = 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
             };
 
-            // 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 {
-                    let (lane, _) = match lanes_with_anchor.get(id) {
-                        Some(v) => v,
-                        None => continue,
+                // 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;
+                    };
+
+                    for lane in lanes.iter() {
+                        lanes_to_include.insert(*lane);
+                    }
 
-                    lanes_to_include.insert(*lane);
                     anchor_to_vertex.insert(*id, vertices.len());
                     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 (
+                        site.get_anchor(door.anchors.start()),
+                        site.get_anchor(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,21 +174,35 @@ 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
                         }
                     }
                 }
 
+                doors.extend(level_doors);
                 levels.insert(
                     level.properties.name.clone().0,
                     NavLevel { lanes, vertices },
@@ -95,6 +214,8 @@ impl NavGraph {
                 Self {
                     building_name: site.properties.name.clone().0,
                     levels,
+                    doors,
+                    lifts,
                 },
             ))
         }
@@ -117,17 +238,34 @@ pub struct NavLaneProperties {
     pub speed_limit: f32,
     #[serde(skip_serializing_if = "Option::is_none")]
     pub dock_name: Option<String>,
-    // TODO(MXG): Add other lane properties
-    // door_name,
-    // orientation_constraint,
+    #[serde(skip_serializing_if = "Option::is_none")]
+    pub door_name: Option<String>,
+    #[serde(skip_serializing_if = "Option::is_none")]
+    pub orientation_constraint: Option<String>,
+    // TODO(luca): Add other lane properties
     // demo_mock_floor_name
+    // mutex
 }
 
 impl NavLaneProperties {
-    fn from_motion(motion: &Motion) -> Self {
+    fn from_motion(motion: &Motion, door_name: Option<String>) -> 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,8 +280,9 @@ impl NavVertex {
     }
 }
 
-#[derive(Serialize, Deserialize, Clone)]
+#[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<String>,
     #[serde(skip_serializing_if = "is_false")]
@@ -152,21 +291,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<f32>,
     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<u32>>) -> Self {
         let mut props = Self::default();
@@ -196,3 +325,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],
+}

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("<Unnamed>".to_string())),
+                name: NameInSite(name.unwrap_or_default()),
                 graphs: AssociatedGraphs::All,
             });
         }

rmf_site_format/src/lift.rs

@@ -115,6 +115,60 @@ pub struct LiftProperties<T: RefTrait> {
     pub initial_level: InitialLevel<T>,
 }
 
+impl LiftProperties<u32> {
+    /// Returns the pose of the lift cabin center in global coordinates.
+    pub fn center(&self, site: &Site) -> Option<Pose> {
+        // 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[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,
+        ];
+        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))]

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};

rmf_site_format/src/site.rs

@@ -183,6 +183,12 @@ impl Site {
     pub fn from_bytes<'a>(s: &'a [u8]) -> ron::error::SpannedResult<Self> {
         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 {}