Add support for mutex groups (#350)

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Michael X. Grey <greyxmike@gmail.com>
Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>
Co-authored-by: Grey <mxgrey@intrinsic.ai>
Co-authored-by: Xiyu <xiyu@openrobotics.org>
Co-authored-by: Grey <greyxmike@gmail.com>

Author: 3 people

Cargo.lock

@@ -690,6 +690,7 @@
             66,
             67
           ],
+          "mutex": 133,
           "graphs": {
             "Only": [
               71
@@ -701,6 +702,7 @@
             67,
             64
           ],
+          "mutex": 133,
           "graphs": "All"
         },
         "75": {
@@ -837,6 +839,11 @@
             ]
           }
         }
+      },
+      "mutex_groups": {
+        "133": {
+          "name": "mutex_1"
+        }
       }
     }
   },

assets/demo_maps/test.site.json

@@ -38,6 +38,7 @@ pub(crate) fn add_site_assets(app: &mut App) {
     embedded_asset!(app, "src/", "textures/charging.png");
     embedded_asset!(app, "src/", "textures/parking.png");
     embedded_asset!(app, "src/", "textures/holding.png");
+    embedded_asset!(app, "src/", "textures/lockpad.png");
     embedded_asset!(app, "src/", "textures/empty.png");
     embedded_asset!(app, "src/", "textures/door_cue.png");
     embedded_asset!(app, "src/", "textures/door_cue_highlighted.png");
@@ -149,6 +150,7 @@ pub struct SiteAssets {
     pub charger_material: Handle<StandardMaterial>,
     pub holding_point_material: Handle<StandardMaterial>,
     pub parking_material: Handle<StandardMaterial>,
+    pub lockpad_material: Handle<StandardMaterial>,
     pub empty_billboard_material: Handle<StandardMaterial>,
     pub robot_path_rectangle_mesh: Handle<Mesh>,
     pub robot_path_circle_mesh: Handle<Mesh>,
@@ -185,6 +187,8 @@ impl FromWorld for SiteAssets {
             asset_server.load("embedded://librmf_site_editor/site/textures/holding.png");
         let parking_texture =
             asset_server.load("embedded://librmf_site_editor/site/textures/parking.png");
+        let lockpad_texture =
+            asset_server.load("embedded://librmf_site_editor/site/textures/lockpad.png");
         let empty_billboard_texture =
             asset_server.load("embedded://librmf_site_editor/site/textures/empty.png");
         let door_cue_texture =
@@ -283,6 +287,7 @@ impl FromWorld for SiteAssets {
             materials.add(billboard_material(charger_texture));
         let holding_point_material = materials.add(billboard_material(holding_point_texture));
         let parking_material = materials.add(billboard_material(parking_texture));
+        let lockpad_material = materials.add(billboard_material(lockpad_texture));
         let empty_billboard_material = materials.add(billboard_material(empty_billboard_texture));
 
         let mut meshes = world.get_resource_mut::<Assets<Mesh>>().unwrap();
@@ -393,6 +398,7 @@ impl FromWorld for SiteAssets {
             charger_material,
             holding_point_material,
             parking_material,
+            lockpad_material,
             empty_billboard_material,
             robot_path_rectangle_mesh,
             robot_path_circle_mesh,

crates/rmf_site_editor/src/site/assets.rs

@@ -549,6 +549,16 @@ fn generate_site_entities(
         consider_id(*fiducial_id);
     }
 
+    for (group_id, group) in &site_data.navigation.guided.mutex_groups {
+        let group_entity = commands
+            .spawn(group.clone())
+            .insert(SiteID(*group_id))
+            .insert(ChildOf(site_id))
+            .id();
+        id_to_entity.insert(*group_id, group_entity);
+        consider_id(*group_id);
+    }
+
     for (nav_graph_id, nav_graph_data) in &site_data.navigation.guided.graphs {
         let nav_graph = commands
             .spawn((Transform::default(), Visibility::default()))