Serialize worlds to YAML and allow resetting all worlds (#296)

.pre-commit-config.yaml

@@ -34,6 +34,12 @@ repos:
     hooks:
       - id: black
 
+  # Removes unused imports.
+  - repo: https://github.com/PyCQA/autoflake
+    rev: v2.3.1
+    hooks:
+      - id: autoflake
+
   # Finds spelling issues in code.
   - repo: https://github.com/codespell-project/codespell
     rev: v2.3.0

docs/source/usage/path_planners.rst

@@ -128,5 +128,19 @@ For visualization, you can provide ``get_graphs()`` and ``get_latest_paths()`` m
         def get_latest_path(self):
             return self.latest_path
 
+To serialize to file, which is needed to reset the world, you should also implement the ``to_dict()`` method.
+Note the ``get_planner_string()`` helper function, which extracts the name of the planner you defined in ``PATH_PLANNERS_MAP`` earlier on.
+
+.. code-block:: python
+
+        def to_dict(self, start, goal):
+            from pyrobosim.navigation import get_planner_string
+
+            return {
+                "type": get_planner_string(self),
+                "grid_resolution": self.grid_resolution,
+                "grid_inflation_radius": self.grid_inflation_radius,
+            }
+
 If you would like to implement your own path planner, it is highly recommended to look at the existing planner implementations as a reference.
 You can also always ask the maintainers through a Git issue!

docs/source/yaml/index.rst

@@ -40,7 +40,7 @@ Then, the world can be loaded from file as follows.
 
    from pyrobosim.core import WorldYamlLoader
 
-   world = WorldYamlLoader().from_yaml("/path/to/world_file.yaml")
+   world = WorldYamlLoader().from_file("/path/to/world_file.yaml")
 
 Refer to the following sections for more details on the schemas.
 

pyrobosim/examples/demo.py

@@ -172,7 +172,7 @@ def create_world(multirobot=False):
 
 
 def create_world_from_yaml(world_file):
-    return WorldYamlLoader().from_yaml(os.path.join(data_folder, world_file))
+    return WorldYamlLoader().from_file(os.path.join(data_folder, world_file))
 
 
 def parse_args():

pyrobosim/examples/demo_astar.py

@@ -10,7 +10,7 @@
 
 # Load a test world.
 world_file = os.path.join(get_data_folder(), "test_world.yaml")
-world = WorldYamlLoader().from_yaml(world_file)
+world = WorldYamlLoader().from_file(world_file)
 
 
 def demo_astar():

pyrobosim/examples/demo_pddl.py

@@ -36,7 +36,7 @@ def parse_args():
 def load_world():
     """Load a test world."""
     world_file = os.path.join(get_data_folder(), "pddlstream_simple_world.yaml")
-    return WorldYamlLoader().from_yaml(world_file)
+    return WorldYamlLoader().from_file(world_file)
 
 
 def start_planner(world, args):

pyrobosim/examples/demo_prm.py

@@ -9,7 +9,7 @@
 
 # Load a test world.
 world_file = os.path.join(get_data_folder(), "test_world.yaml")
-world = WorldYamlLoader().from_yaml(world_file)
+world = WorldYamlLoader().from_file(world_file)
 
 
 def test_prm():

pyrobosim/examples/demo_rrt.py

@@ -9,7 +9,7 @@
 
 # Load a test world.
 world_file = os.path.join(get_data_folder(), "test_world.yaml")
-world = WorldYamlLoader().from_yaml(world_file)
+world = WorldYamlLoader().from_file(world_file)
 
 
 def test_rrt():

pyrobosim/examples/demo_world_save.py

@@ -53,7 +53,7 @@ def main():
     # Load a test world from YAML file.
     data_folder = get_data_folder()
     loader = WorldYamlLoader()
-    world = loader.from_yaml(os.path.join(data_folder, args.world_file))
+    world = loader.from_file(os.path.join(data_folder, args.world_file))
 
     # Export a Gazebo world.
     exp = WorldGazeboExporter(world)

pyrobosim/pyrobosim/core/hallway.py

@@ -1,6 +1,6 @@
 """ Hallway representation for world modeling. """
 
-import numpy as np
+import math
 from shapely import intersects_xy
 from shapely.geometry import LineString, MultiLineString
 from shapely.plotting import patch_from_polygon
@@ -90,12 +90,12 @@ def __init__(
         if conn_method == "auto" or conn_method == "angle":
             theta, length = get_bearing_range(room_start.centroid, room_end.centroid)
             if conn_method == "angle":
-                length = length * np.cos(theta - conn_angle)
+                length = length * math.cos(theta - conn_angle)
                 theta = conn_angle
 
             # Calculate start and end points for the hallway
-            c = np.cos(theta)
-            s = np.sin(theta)
+            c = math.cos(theta)
+            s = math.sin(theta)
             x, y = room_start.centroid
             pt_start = [x - offset * s, y + offset * c]
             pt_end = [pt_start[0] + length * c, pt_start[1] + length * s]
@@ -260,6 +260,26 @@ def add_graph_nodes(self):
 
         self.nav_poses = [self.graph_nodes[0].pose, self.graph_nodes[-1].pose]
 
+    def to_dict(self):
+        """
+        Serializes the hallway to a dictionary.
+
+        :return: A dictionary containing the hallway information.
+        :rtype: dict[str, Any]
+        """
+        return {
+            "name": self.name,
+            "room_start": self.room_start.name,
+            "room_end": self.room_end.name,
+            "width": self.width,
+            "wall_width": self.wall_width,
+            "conn_method": "points",
+            "conn_points": self.points,
+            "color": self.viz_color,
+            "is_open": self.is_open,
+            "is_locked": self.is_locked,
+        }
+
     def __repr__(self):
         """Returns printable string."""
         return f"Hallway: {self.name}"

pyrobosim/pyrobosim/core/locations.py

@@ -17,6 +17,8 @@ class Location:
     """Representation of a location in the world."""
 
     # Default class attributes
+    metadata = EntityMetadata()
+    """ Metadata for location categories. """
     height = 1.0
     """ Vertical height of location. """
     viz_color = (0, 0, 0)
@@ -202,6 +204,24 @@ def add_graph_nodes(self):
         for spawn in self.children:
             spawn.add_graph_nodes()
 
+    def to_dict(self):
+        """
+        Serializes the location to a dictionary.
+
+        :return: A dictionary containing the location information.
+        :rtype: dict[str, Any]
+        """
+        return {
+            "name": self.name,
+            "category": self.category,
+            "parent": self.parent.name,
+            "pose": self.pose.to_dict(),
+            "color": self.viz_color,
+            "is_open": self.is_open,
+            "is_locked": self.is_locked,
+            "is_charger": self.is_charger,
+        }
+
     def __repr__(self):
         """Returns printable string."""
         return f"Location: {self.name}"

pyrobosim/pyrobosim/core/objects.py

@@ -18,6 +18,8 @@ class Object:
     """Represents an object in the world."""
 
     # Default class attributes
+    metadata = EntityMetadata()
+    """ Metadata for object categories. """
     height = 0.1
     """ Vertical height of object. """
     viz_color = (0, 0, 1)
@@ -188,6 +190,21 @@ def get_grasp_cuboid_pose(self):
             )
         )
 
+    def to_dict(self):
+        """
+        Serializes the object to a dictionary.
+
+        :return: A dictionary containing the object information.
+        :rtype: dict[str, Any]
+        """
+        return {
+            "name": self.name,
+            "category": self.category,
+            "parent": self.parent.name,
+            "pose": self.pose.to_dict(),
+            "color": self.viz_color,
+        }
+
     def __repr__(self):
         """Returns printable string."""
         return f"Object: {self.name}"

pyrobosim/pyrobosim/core/robot.py

@@ -1,6 +1,5 @@
 """ Defines a robot which operates in a world. """
 
-import logging
 import time
 import numpy as np
 
@@ -1008,6 +1007,40 @@ def execute_plan(self, plan, delay=0.5):
         self.current_plan = None
         return result, num_completed
 
+    def to_dict(self):
+        """
+        Serializes the robot to a dictionary.
+
+        :return: A dictionary containing the robot information.
+        :rtype: dict[str, Any]
+        """
+        pose = self.get_pose()
+
+        robot_dict = {
+            "name": self.name,
+            "radius": self.radius,
+            "height": self.height,
+            "color": self.color,
+            "pose": pose.to_dict(),
+            "max_linear_velocity": float(self.dynamics.vel_limits[0]),
+            "max_angular_velocity": float(self.dynamics.vel_limits[-1]),
+            "max_linear_acceleration": float(self.dynamics.accel_limits[0]),
+            "max_angular_acceleration": float(self.dynamics.accel_limits[-1]),
+        }
+
+        if self.world:
+            location = self.world.get_location_from_pose(pose)
+            if location is not None:
+                robot_dict["location"] = location.name
+        if self.path_planner:
+            robot_dict["path_planner"] = self.path_planner.to_dict()
+        if self.path_executor:
+            robot_dict["path_executor"] = self.path_executor.to_dict()
+        if self.grasp_generator:
+            robot_dict["grasping"] = self.grasp_generator.to_dict()
+
+        return robot_dict
+
     def __repr__(self):
         """Returns printable string."""
         return f"Robot: {self.name}"

pyrobosim/pyrobosim/core/room.py

@@ -52,8 +52,12 @@ def __init__(
         self.height = height
         if isinstance(footprint, list):
             self.polygon = Polygon(footprint)
+            self.footprint = {"type": "polygon", "coords": footprint}
         else:
-            self.polygon, _ = polygon_and_height_from_footprint(footprint)
+            self.polygon, height = polygon_and_height_from_footprint(footprint)
+            self.footprint = footprint
+            if height is not None:
+                self.height = height
         if self.polygon.is_empty:
             raise Exception("Room footprint cannot be empty.")
 
@@ -138,6 +142,22 @@ def add_graph_nodes(self):
         """Creates graph nodes for searching."""
         self.graph_nodes = [Node(p, parent=self) for p in self.nav_poses]
 
+    def to_dict(self):
+        """
+        Serializes the room to a dictionary.
+
+        :return: A dictionary containing the room information.
+        :rtype: dict[str, Any]
+        """
+        return {
+            "name": self.name,
+            "color": self.viz_color,
+            "wall_width": self.wall_width,
+            "footprint": self.footprint,
+            "height": self.height,
+            "nav_poses": [pose.to_dict() for pose in self.nav_poses],
+        }
+
     def __repr__(self):
         """Returns printable string."""
         return f"Room: {self.name}"

pyrobosim/pyrobosim/core/world.py

@@ -1,7 +1,6 @@
 """ Main file containing the core world modeling tools. """
 
 import itertools
-import logging
 import numpy as np
 
 from .hallway import Hallway
@@ -42,7 +41,8 @@ def __init__(
         """
         self.name = name
         self.wall_height = wall_height
-        self.source_file = None
+        self.source_yaml = None
+        self.source_yaml_file = None
         self.logger = create_logger(self.name)
 
         # Connected apps
@@ -51,10 +51,8 @@ def __init__(
         self.has_ros_node = False
         self.ros_node = False
 
-        # Robots
+        # World entities (robots, locations, objects, etc.)
         self.robots = []
-
-        # World entities (rooms, locations, objects, etc.)
         self.name_to_entity = {}
         self.rooms = []
         self.hallways = []
@@ -103,18 +101,23 @@ def set_metadata(self, locations=None, objects=None):
         if objects is not None:
             Object.set_metadata(objects)
 
-    def set_inflation_radius(self, inflation_radius=0.0):
+    def get_location_metadata(self):
         """
-        Sets world inflation radius.
+        Returns the location metadata associated with this world.
 
-        :param inflation_radius: Inflation radius, in meters.
-        :type inflation_radius: float, optional
+        :return: The location metadata.
+        :rtype: :class:`pyrobosim.utils.general.EntityMetadata`
         """
-        self.inflation_radius = inflation_radius
-        for loc in self.locations:
-            loc.update_collision_polygon(self.inflation_radius)
-        for entity in itertools.chain(self.rooms, self.hallways):
-            entity.update_collision_polygons(self.inflation_radius)
+        return Location.metadata
+
+    def get_object_metadata(self):
+        """
+        Returns the object metadata associated with this world.
+
+        :return: The object metadata.
+        :rtype: :class:`pyrobosim.utils.general.EntityMetadata`
+        """
+        return Object.metadata
 
     ##########################
     # World Building Methods #
@@ -1055,6 +1058,19 @@ def remove_robot(self, robot_name):
             self.logger.warning(f"Could not find robot {robot_name} to remove.")
             return False
 
+    def set_inflation_radius(self, inflation_radius=0.0):
+        """
+        Sets world inflation radius.
+
+        :param inflation_radius: Inflation radius, in meters.
+        :type inflation_radius: float, optional
+        """
+        self.inflation_radius = inflation_radius
+        for loc in self.locations:
+            loc.update_collision_polygon(self.inflation_radius)
+        for entity in itertools.chain(self.rooms, self.hallways):
+            entity.update_collision_polygons(self.inflation_radius)
+
     def update_bounds(self, entity, remove=False):
         """
         Updates the X and Y bounds of the world.