Simplify path planner API and allow planner resets (#241)

docs/source/usage/path_planners.rst

@@ -3,93 +3,130 @@
 Path Planners
 =============
 
-This section explains how to use and optionally extend the path planners in ``pyrobosim``.
+This section explains how to use (and optionally extend) path planners in ``pyrobosim``.
 
-``PathPlanner`` Interface
--------------------------
+Path Planner Definitions
+------------------------
 
-``pyrobosim`` uses an interface based approach to path planners.
-A specific planner can be selected through the planner interface, by providing the planner type and a set of planner-specific keyword arguments.
+The ``pyrobosim/navigation`` module contains all path planner implementations.
+
+In the ``__init__.py`` file available, you will see a list of implemented planner classes that looks like this:
 
 .. code-block:: python
 
-    from pyrobosim.navigation import PathPlanner
+    # Planners
+    from .a_star import AStarPlanner
+    from .rrt import RRTPlanner
+    from .prm import PRMPlanner
+    from .world_graph import WorldGraphPlanner
 
-    path_planner = PathPlanner("planner_type", **planner_config)
 
-Available planner types and their available implementations can be found below :
+    PATH_PLANNERS_MAP = {
+        "astar": AStarPlanner,
+        "rrt": RRTPlanner,
+        "prm": PRMPlanner,
+        "world_graph": WorldGraphPlanner,
+    }
 
-- ``"astar"`` : :py:mod:`pyrobosim.navigation.a_star`
-- ``"prm"`` : :py:mod:`pyrobosim.navigation.prm`
-- ``"rrt"`` : :py:mod:`pyrobosim.navigation.rrt`
+When loading path planners from YAML, the ``planner.type`` entry will correspond to one of these entries in the ``PATH_PLANNERS_MAP`` dictionary.
+As such, if you would like to add your own planner, you can do so in this file.
 
-Each planner can potentially have multiple implementations.
-A specific implementation can be selected by providing the relevant parameters in the keyword arguments contained in ``**planner_config``.
+.. code-block:: python
 
+    # Planners
+    from .a_star import AStarPlanner
+    from .rrt import RRTPlanner
+    from .prm import PRMPlanner
+    from .world_graph import WorldGraphPlanner
+    from my_module.my_file import MyNewPlanner
 
-Adding New Planners
--------------------
 
-The path planners in ``pyrobosim`` are designed to be extensible, so that you can add your own implementation for an existing planner type, or even a new planner type.
+    PATH_PLANNERS_MAP = {
+        "astar": AStarPlanner,
+        "rrt": RRTPlanner,
+        "prm": PRMPlanner,
+        "world_graph": WorldGraphPlanner,
+        "my_planner": MyNewPlanner,
+    }
 
-For example, to add a new planner type called ``NewPlanner``:
 
-- Create the planner factory class ``NewPlanner`` which inherits :py:mod:`pyrobosim.navigation.planner_base`.
+What to Implement in a Planner
+------------------------------
 
-    .. code-block:: python
+The path planners implemented in ``pyrobosim`` provide general functionality to plan paths in a known world, as well as data for visualization in the UI.
 
-        from pyrobosim.navigation import PathPlannerBase
+First, you want to make a constructor that accepts a world in a ``world`` keyword argument, along with any other data you might expect.
+For example, this planner accepts a world and a few parameters and builds an occupancy grid.
 
-        class NewPlanner(PathPlannerBase):
-            pass
+.. code-block:: python
 
-- Create concrete implementations of your planner similar to :py:class:`pyrobosim.navigation.rrt.RRTPlannerPolygon`.
+    class MyNewPlanner:
+        def __init__(self, *, world, grid_resolution, grid_inflation_radius):
+            self.grid = OccupancyGrid.from_world(
+                world,
+                resolution=grid_resolution,
+                inflation_radius=grid_inflation_radius,
+            )
 
-    .. code-block:: python
+Next, you should implement a ``reset()`` method.
+This is needed if, for example, the world changes and you want to generate new persistent data structures such as a new occupancy grid or roadmap.
+If your planner does not have any such data, you still must implement this.
 
-        class NewPlannerPolygon:
-            pass
+.. code-block:: python
 
-- Next, you should specify the mechanism to select the concrete implementation of your planner type in the ``__init__()`` method, and implement the ``plan()`` method.
-  Refer to :py:class:`pyrobosim.navigation.rrt.RRTPlanner` for an example on how to do this.
+    class MyNewPlanner:
+        def __init__(self, *, world, grid_resolution, grid_inflation_radius):
+            self.world = world
+            self.grid_resolution = grid_resolution
+            self.grid_inflation_radius = grid_inflation_radius
+            self.reset()
 
-    .. code-block:: python
+        def reset(self):
+            self.grid = OccupancyGrid.from_world(
+                self.world,
+                resolution=self.grid_resolution,
+                inflation_radius=self.grid_inflation_radius,
+            )
 
-        from pyrobosim.navigation import PathPlannerBase
+Then, you need to implement the actual path planning.
+This is done using a ``plan()`` method that accepts a start and goal pose and returns a ``Path`` object.
 
-        class NewPlanner(PathPlannerBase):
+.. code-block:: python
+
+    import time
+    from pyrobosim.utils.motion import Path
 
-            def __init__(self, **planner_config):
-                # Select the implementation here
-                if planner_config.get("some_param", None):
-                    self.impl = NewPlannerPolygon(**planner_config)
-                else:
-                    raise NotImplementedError("This configuration is not valid!")
+        def plan(self, start, goal):
+            t_start = time.time()
+            # Your planning logic goes here
+            return Path(
+                poses=[start, goal],
+                planning_time=time.time() - t_start
+            )
 
-            def plan(self, start, goal):
-                # Call implementations to compute path , do any pre- or post- processing.
-                path = self.impl.plan(start, goal)
-                return path
+For visualization, you can provide ``get_graphs()`` and ``get_latest_paths()`` methods.
 
-- Add the planner type to the list of supported planners in :py:mod:`pyrobosim.navigation.path_planner`.
+.. code-block:: python
 
-    .. code-block:: python
+    from pyrobosim.utils.search_graph.SearchGraph
 
-        self.planners = {
-            "astar": AstarPlanner,
-            "rrt": RRTPlanner,
-            "prm": PRMPlanner,
-            "new_planner": NewPlanner,  # Here is our new planner!
-        }
+        def plan(self, start, goal):
+            t_start = time.time()
+            self.search_graph = SearchGraph()
 
-- Use the ``PathPlanner`` interface to use your new planner.
+            # Your planning logic goes here
 
-    .. code-block:: python
+            self.latest_path = Path(
+                poses=[start, goal],
+                planning_time=time.time() - t_start
+            )
+            return self.latest_path
 
-        planner_config = {"some_param": some_value, "some_other_param": some_other_value}
-        new_path_planner  = PathPlanner("new_planner", **planner_config)
+        def get_graphs(self):
+            return [SearchGraph()]
 
-.. note::
+        def get_latest_path(self):
+            return self.latest_path
 
-    Planner implementations that need to display graphs and planned paths should set the ``graphs`` and ``latest_path`` attributes of ``PathPlannerBase``.
-    Refer to :py:func:`pyrobosim.navigation.rrt.RRTPlanner.plan` and :py:func:`pyrobosim.navigation.prm.PRMPlanner.plan` for some example implementations.
+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!

pyrobosim/examples/demo.py

@@ -10,7 +10,12 @@
 from pyrobosim.core import Robot, World, WorldYamlLoader
 from pyrobosim.gui import start_gui
 from pyrobosim.manipulation import GraspGenerator, ParallelGraspProperties
-from pyrobosim.navigation import ConstantVelocityExecutor, OccupancyGrid, PathPlanner
+from pyrobosim.navigation import (
+    ConstantVelocityExecutor,
+    AStarPlanner,
+    PRMPlanner,
+    RRTPlanner,
+)
 from pyrobosim.utils.general import get_data_folder
 from pyrobosim.utils.pose import Pose
 
@@ -120,7 +125,7 @@ def create_world(multirobot=False):
         "rewire_radius": 1.5,
         "compress_path": False,
     }
-    rrt_planner = PathPlanner("rrt", **planner_config_rrt)
+    rrt_planner = RRTPlanner(**planner_config_rrt)
     robot0.set_path_planner(rrt_planner)
     world.add_robot(robot0, loc="kitchen")
 
@@ -140,7 +145,7 @@ def create_world(multirobot=False):
             "max_nodes": 100,
             "compress_path": False,
         }
-        prm_planner = PathPlanner("prm", **planner_config_prm)
+        prm_planner = PRMPlanner(**planner_config_prm)
         robot1.set_path_planner(prm_planner)
         world.add_robot(robot1, loc="bathroom")
 
@@ -153,13 +158,13 @@ def create_world(multirobot=False):
             partial_observability=args.partial_observability,
         )
         planner_config_astar = {
-            "grid": OccupancyGrid.from_world(
-                world, resolution=0.05, inflation_radius=0.15
-            ),
+            "world": world,
+            "grid_resolution": 0.05,
+            "grid_inflation_radius": 0.15,
             "diagonal_motion": True,
             "heuristic": "euclidean",
         }
-        astar_planner = PathPlanner("astar", **planner_config_astar)
+        astar_planner = AStarPlanner(**planner_config_astar)
         robot2.set_path_planner(astar_planner)
         world.add_robot(robot2, loc="bedroom")
 

pyrobosim/examples/demo_astar.py

@@ -4,10 +4,9 @@
 
 from pyrobosim.core import WorldYamlLoader
 from pyrobosim.gui import start_gui
-from pyrobosim.navigation import PathPlanner
+from pyrobosim.navigation import AStarPlanner
 from pyrobosim.utils.general import get_data_folder
 from pyrobosim.utils.pose import Pose
-from pyrobosim.navigation import OccupancyGrid
 
 # Load a test world.
 world_file = os.path.join(get_data_folder(), "test_world.yaml")
@@ -18,21 +17,22 @@ def demo_astar():
     """Creates an occupancy grid based A* planner and plans a path."""
     robot = world.robots[0]
     planner_config = {
-        "grid": OccupancyGrid.from_world(
-            world, resolution=0.05, inflation_radius=1.5 * robot.radius
-        ),
+        "world": world,
+        "grid_resolution": 0.05,
+        "grid_inflation_radius": 1.5 * robot.radius,
         "diagonal_motion": True,
         "heuristic": "euclidean",
         "compress_path": False,
     }
 
-    planner = PathPlanner("astar", **planner_config)
+    planner = AStarPlanner(**planner_config)
     start = Pose(x=-0.5, y=-0.5)
     goal = Pose(x=3.0, y=3.0)
     robot.set_pose(start)
     robot.set_path_planner(planner)
-    result = robot.plan_path(start, goal)
-    planner.info()
+    path = robot.plan_path(start, goal)
+    if path:
+        path.print_details()
 
 
 if __name__ == "__main__":

pyrobosim/examples/demo_prm.py

@@ -3,7 +3,7 @@
 
 from pyrobosim.core import WorldYamlLoader
 from pyrobosim.gui import start_gui
-from pyrobosim.navigation import PathPlanner
+from pyrobosim.navigation import PRMPlanner
 from pyrobosim.utils.general import get_data_folder
 from pyrobosim.utils.pose import Pose
 
@@ -21,15 +21,16 @@ def test_prm():
         "max_connection_dist": 1.5,
         "compress_path": False,
     }
-    prm = PathPlanner("prm", **planner_config)
+    prm = PRMPlanner(**planner_config)
     start = Pose(x=-0.5, y=-0.5)
     goal = Pose(x=3.0, y=3.0)
 
     robot = world.robots[0]
     robot.set_pose(start)
     robot.set_path_planner(prm)
-    result = robot.plan_path(start, goal)
-    prm.info()
+    path = robot.plan_path(start, goal)
+    if path:
+        path.print_details()
 
 
 if __name__ == "__main__":

pyrobosim/examples/demo_rrt.py

@@ -3,7 +3,7 @@
 
 from pyrobosim.core import WorldYamlLoader
 from pyrobosim.gui import start_gui
-from pyrobosim.navigation import PathPlanner
+from pyrobosim.navigation import RRTPlanner
 from pyrobosim.utils.general import get_data_folder
 from pyrobosim.utils.pose import Pose
 
@@ -21,15 +21,16 @@ def test_rrt():
         "rrt_star": True,
         "compress_path": False,
     }
-    rrt = PathPlanner("rrt", **planner_config)
+    rrt = RRTPlanner(**planner_config)
     start = Pose(x=-0.5, y=-0.5)
     goal = Pose(x=3.0, y=3.0)
 
     robot = world.robots[0]
     robot.set_pose(start)
     robot.set_path_planner(rrt)
-    result = robot.plan_path(start, goal)
-    rrt.info()
+    path = robot.plan_path(start, goal)
+    if path:
+        path.print_details()
 
 
 if __name__ == "__main__":

pyrobosim/pyrobosim/core/robot.py

@@ -282,9 +282,9 @@ def plan_path(self, start=None, goal=None):
 
         path = self.path_planner.plan(start, goal)
         if self.world and self.world.has_gui:
-            show_graph = True
+            show_graphs = True
             self.world.gui.canvas.show_planner_and_path_signal.emit(
-                self, show_graph, path
+                self, show_graphs, path
             )
         return path
 
@@ -403,9 +403,9 @@ def navigate(
                 )
                 return self.last_nav_result
         elif self.world and self.world.has_gui:
-            show_graph = False
+            show_graphs = False
             self.world.gui.canvas.show_planner_and_path_signal.emit(
-                self, show_graph, path
+                self, show_graphs, path
             )
 
         # Simulate execution options.