Skip to content

Commit

Permalink
Add a few more properties to legacy navgraphs (#201)
Browse files Browse the repository at this point in the history 
Signed-off-by: Luca Della Vedova <[email protected]>
Signed-off-by: Michael X. Grey <[email protected]>
Co-authored-by: Michael X. Grey <[email protected]>
  • Loading branch information
@luca-della-vedova @mxgrey
luca-della-vedova and mxgrey authored Mar 6, 2024
1 parent f30aabe commit b63a9fe
Show file tree
Hide file tree
Showing 5 changed files with 235 additions and 31 deletions.
202 changes: 173 additions & 29 deletions rmf_site_format/src/legacy/nav_graph.rs
View file
Original file line number Diff line number Diff line change
@@ -1,11 +1,33 @@
use crate::*;
use glam::Affine2;
use serde::{Deserialize, Serialize};
use std::collections::{HashMap, HashSet};

#[derive(Serialize, Deserialize, Clone)]
pub struct NavGraph {
pub building_name: String,
pub levels: HashMap<String, NavLevel>,
pub doors: HashMap<String, NavDoor>,
pub lifts: HashMap<String, NavLift>,
}

// 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;
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 {
Expand All @@ -25,36 +47,122 @@ 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 {
eprintln!(
"ERROR: Skipping lift {lift_name} due to broken anchor reference"
);
continue;
};
let Rotation::Yaw(yaw) = center.rot else {
eprintln!(
"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);
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 =
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 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::Door),
v1.translation_for_category(Category::Door),
),
_ => {
eprintln!(
"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();
Expand All @@ -64,26 +172,40 @@ 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;
}
};

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 },
Expand All @@ -95,6 +217,8 @@ impl NavGraph {
Self {
building_name: site.properties.name.clone().0,
levels,
doors,
lifts,
},
))
}
Expand All @@ -117,17 +241,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(_) => {
eprintln!(
"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,
}
}
}
Expand All @@ -142,8 +283,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")]
Expand All @@ -152,21 +294,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();
Expand Down Expand Up @@ -196,3 +328,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],
}
2 changes: 1 addition & 1 deletion rmf_site_format/src/legacy/vertex.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
});
}
Expand Down
54 changes: 54 additions & 0 deletions rmf_site_format/src/lift.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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::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,
(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))]
Expand Down
2 changes: 1 addition & 1 deletion rmf_site_format/src/misc.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
*
*/

use crate::{Recall, RefTrait};
use crate::RefTrait;
#[cfg(feature = "bevy")]
use bevy::prelude::*;
use glam::{Quat, Vec2, Vec3};
Expand Down
Loading

0 comments on commit b63a9fe

Please sign in to comment.