Fix lanes being skipped when they share an anchor

Signed-off-by: Luca Della Vedova <[email protected]>

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