Support Multiple Sources of Location and Object Metadata

docs/source/yaml/index.rst

@@ -9,15 +9,25 @@ Specifically, each world draws from a set of **location** and **object** metadat
 
 Worlds themselves can be created programmatically, or defined using world YAML files.
 
-For the programmatic approach, you can create a world as follows.
+For the programmatic approach, you can create a world as follows either using ``set_metadata`` or ``add_metadata``.
 
 .. code-block:: python
 
    from pyrobosim.core import Robot, World
 
    world = World()
-   world.set_metadata(locations="/path/to/location_data.yaml",
-                      objects="/path/to/object_data.yaml")
+
+   # locations and objects can accept either a single file path or a list of file paths.
+   world.add_metadata(
+      locations=[
+         "/path/to/location_data1.yaml",
+         "/path/to/location_data2.yaml"
+      ],
+      objects=[
+         "/path/to/object_data1.yaml",
+         "/path/to/object_data2.yaml"
+      ]
+   )
 
    # Then, you can add the other entities
    world.add_robot(...)
@@ -26,6 +36,7 @@ For the programmatic approach, you can create a world as follows.
    world.add_location(...)
    world.add_object(...)
 
+
 For the YAML approach, you can directly point to the location and object metadata in the world YAML file itself.
 
 .. code-block:: yaml

docs/source/yaml/world_schema.rst

@@ -14,8 +14,16 @@ The world schema looks as follows, where ``<angle brackets>`` are placeholders:
 
    # Metadata
    metadata:
-      locations: </path/to/location_data_file.yaml>
-      objects: </path/to/object_data_file.yaml>
+      locations:
+       - <path/to/location_data1_file.yaml>
+       - <path/to/location_data2_file.yaml>
+       - ...
+       - <path/to/location_dataN_file.yaml>
+      objects:
+       - </path/to/object_data1_file.yaml>
+       - </path/to/object_data2_file.yaml>
+       - ...
+       - </path/to/object_dataN_file.yaml>
 
    # Robots: Each robot contains basic properties, plus other add-ons such as path planners and grasp generators
    robots:

pyrobosim/examples/demo.py

@@ -28,9 +28,15 @@ def create_world(multirobot=False):
     world = World()
 
     # Set the location and object metadata
-    world.set_metadata(
-        locations=os.path.join(data_folder, "example_location_data.yaml"),
-        objects=os.path.join(data_folder, "example_object_data.yaml"),
+    world.add_metadata(
+        locations=[
+            os.path.join(data_folder, "example_location_data_furniture.yaml"),
+            os.path.join(data_folder, "example_location_data_accessories.yaml"),
+        ],
+        objects=[
+            os.path.join(data_folder, "example_object_data_food.yaml"),
+            os.path.join(data_folder, "example_object_data_drink.yaml"),
+        ],
     )
 
     # Add rooms

pyrobosim/pyrobosim/core/locations.py

@@ -34,6 +34,29 @@ def set_metadata(cls, filename):
         """
         cls.metadata = EntityMetadata(filename)
 
+    @classmethod
+    def add_metadata(cls, filename):
+        """
+        Add location metadata from a new file to existing metadata.
+
+        :param filename: Path to location metadata YAML file.
+        :type filename: str
+        """
+        if not hasattr(cls, "metadata"):
+            cls.metadata = EntityMetadata()
+
+        cls.metadata.add(filename)
+
+    @classmethod
+    def clearout_metadata(cls):
+        """
+        Clear out old location metadata.
+        """
+        if not hasattr(cls, "metadata"):
+            cls.metadata = EntityMetadata()
+        cls.metadata.data = {}
+        cls.metadata.sources = []
+
     def __init__(
         self,
         name=None,

pyrobosim/pyrobosim/core/objects.py

@@ -35,6 +35,29 @@ def set_metadata(cls, filename):
         """
         cls.metadata = EntityMetadata(filename)
 
+    @classmethod
+    def add_metadata(cls, filename):
+        """
+        Add object metadata from a new file to existing metadata.
+
+        :param filename: Path to object metadata YAML file.
+        :type filename: str
+        """
+        if not hasattr(cls, "metadata"):
+            cls.metadata = EntityMetadata()
+
+        cls.metadata.add(filename)
+
+    @classmethod
+    def clearout_metadata(cls):
+        """
+        Clear out old object metadata.
+        """
+        if not hasattr(cls, "metadata"):
+            cls.metadata = EntityMetadata()
+        cls.metadata.data = {}
+        cls.metadata.sources = []
+
     def __init__(
         self,
         name=None,

pyrobosim/pyrobosim/core/world.py

@@ -91,15 +91,39 @@ def set_metadata(self, locations=None, objects=None):
         """
         Sets location and object metadata from the specified files.
 
-        :param locations: Path to location metadata YAML file.
-        :type locations: str, optional
-        :param objects: Path to object metadata YAML file.
-        :type objects: str, optional
-        """
-        if locations is not None:
-            Location.set_metadata(locations)
-        if objects is not None:
-            Object.set_metadata(objects)
+        :param locations: Path(s) to location metadata YAML file(s).
+        :type locations: str | list[str] | None
+        :param objects: Path(s) to object metadata YAML file(s).
+        :type objects: str | list[str] | None
+        """
+        # Clear out old metadata
+        Location.clearout_metadata()
+        Object.clearout_metadata()
+
+        self.add_metadata(locations, objects)
+
+    def add_metadata(self, locations=None, objects=None):
+        """
+        Add location and object metadata from the specified files, allowing
+        additional metadata to be incrementally merged with the existing data.
+
+        :param locations: Path(s) to location metadata YAML file(s).
+        :type locations: str | list[str] | None
+        :param objects: Path(s) to object metadata YAML file(s).
+        :type objects: str | list[str] | None
+        """
+
+        if isinstance(locations, list):
+            for location in locations:
+                Location.add_metadata(location)
+        elif isinstance(locations, str):
+            Location.add_metadata(locations)
+
+        if isinstance(objects, list):
+            for object in objects:
+                Object.add_metadata(object)
+        elif isinstance(objects, str):
+            Object.add_metadata(objects)
 
     def get_location_metadata(self):
         """

pyrobosim/pyrobosim/core/yaml_utils.py

@@ -3,6 +3,7 @@
 import copy
 import os
 import yaml
+from itertools import zip_longest
 
 from .robot import Robot
 from .world import World
@@ -75,7 +76,8 @@ def create_world(self):
                 )
             else:
                 obj_data = None
-            self.world.set_metadata(locations=loc_data, objects=obj_data)
+
+            self.world.add_metadata(locations=loc_data, objects=obj_data)
 
     def add_rooms(self):
         """Add rooms to the world."""
@@ -225,11 +227,12 @@ def to_dict(self, world):
         }
 
         # Extract the location and object metadata.
-        loc_metadata_file = world.get_location_metadata().filename or ""
-        obj_metadata_file = world.get_object_metadata().filename or ""
+        loc_metadata_files = world.get_location_metadata().sources
+        obj_metadata_files = world.get_object_metadata().sources
+
         world_dict["metadata"] = {
-            "locations": loc_metadata_file,
-            "objects": obj_metadata_file,
+            "locations": loc_metadata_files,
+            "objects": obj_metadata_files,
         }
 
         # Go through all the entities in the world and similarly add them to the dictionary.

pyrobosim/pyrobosim/data/example_location_data_accessories.yaml

@@ -0,0 +1,54 @@
+trash_can:
+  footprint:
+    type: mesh
+    model_path: $DATA/sample_models/first_2015_trash_can
+    mesh_path: meshes/trash_can.dae
+  locations:
+    - name: "top"
+      footprint:
+        type: parent
+        padding: 0.05
+  nav_poses:
+    - position:  # left
+        x: -0.5
+        y: 0.0
+    - position:  # right
+        x: 0.5
+        y: 0.0
+      rotation_eul:
+        yaw: 3.14
+  color: [0, 0.35, 0.2]
+
+charger:
+  footprint:
+    type: polygon
+    coords:
+      - [-0.3, -0.15]
+      - [0.3, -0.15]
+      - [0.3, 0.15]
+      - [-0.3, 0.15]
+    height: 0.1
+  locations:
+    - name: "dock"
+      footprint:
+        type: parent
+      nav_poses:
+        - position:  # below
+            x: 0.0
+            y: -0.5
+          rotation_eul:
+            yaw: 1.57
+        - position:  # left
+            x: -0.35
+            y: 0.0
+        - position:  # above
+            x: 0.0
+            y: 0.5
+          rotation_eul:
+            yaw: -1.57
+        - position:  # right
+            x: 0.35
+            y: 0.0
+          rotation_eul:
+            yaw: 3.14
+  color: [0.4, 0.4, 0]

pyrobosim/pyrobosim/data/example_location_data_furniture.yaml

@@ -0,0 +1,101 @@
+table:
+  footprint:
+    type: box
+    dims: [0.9, 1.2]
+    height: 0.5
+  nav_poses:
+    - position:  # left
+        x: -0.75
+        y: 0.0
+    - position:  # right
+        x: 0.75
+        y: 0.0
+      rotation_eul:
+        yaw: 3.14
+  locations:
+    - name: "tabletop"
+      footprint:
+        type: parent
+        padding: 0.1
+  color: [0.2, 0, 0]
+
+desk:
+  footprint:
+    type: polygon
+    coords:
+      - [-0.3, -0.3]
+      - [0.3, -0.3]
+      - [0.3, 0.3]
+      - [-0.3, 0.3]
+    height: 0.3
+  locations:
+    - name: "desktop"
+      footprint:
+        type: parent
+      nav_poses:
+        - position:  # below
+            x: 0.0
+            y: -0.5
+          rotation_eul:
+            yaw: 1.57
+        - position:  # left
+            x: -0.5
+            y: 0.0
+        - position:  # above
+            x: 0.0
+            y: 0.5
+          rotation_eul:
+            yaw: -1.57
+        - position:  # right
+            x: 0.5
+            y: 0.0
+          rotation_eul:
+            yaw: 3.14
+
+counter:
+  footprint:
+    type: box
+    dims: [1.2, 0.6]
+    height: 0.75
+  locations:
+    - name: "left"
+      footprint:
+        type: polygon
+        coords:
+          - [-0.25, -0.25]
+          - [0.25, -0.25]
+          - [0.25, 0.25]
+          - [-0.25, 0.25]
+        offset: [0.3, 0]
+      nav_poses:
+        - position:  # below
+            x: 0.0
+            y: -0.5
+          rotation_eul:
+            yaw: 1.57
+        - position:  # above
+            x: 0.0
+            y: 0.5
+          rotation_eul:
+            yaw: -1.57
+    - name: "right"
+      footprint:
+        type: polygon
+        coords:
+          - [-0.25, -0.25]
+          - [0.25, -0.25]
+          - [0.25, 0.25]
+          - [-0.25, 0.25]
+        offset: [-0.3, 0]
+      nav_poses:
+        - position:  # below
+            x: 0.0
+            y: -0.5
+          rotation_eul:
+            yaw: 1.57
+        - position:  # above
+            x: 0.0
+            y: 0.5
+          rotation_eul:
+            yaw: -1.57
+  color: [0, 0.2, 0]

pyrobosim/pyrobosim/data/example_object_data_drink.yaml

@@ -0,0 +1,17 @@
+water:
+  footprint:
+    type: polygon
+    coords:
+      - [0.035, -0.075]
+      - [0.035, 0.075]
+      - [0.0, 0.1]
+      - [-0.035, 0.075]
+      - [-0.035, -0.075]
+  color: [0.0, 0.1, 0.7]
+
+coke:
+  footprint:
+    type: mesh
+    model_path: $DATA/sample_models/coke_can
+    mesh_path: meshes/coke_can.dae
+  color: [0.8, 0, 0]