Path Planners

This section explains how to use (and optionally extend) path planners in PyRoboSim.

Path Planner Definitions

The pyrobosim/navigation module contains all path planner implementations.

What to Implement in a Planner

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.

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. Note that the parent class’ method can be called using super().reset(), which automatically resets common attributes such as latest_path, robot, and world.

from pyrobosim.navigation.types import PathPlanner
from pyrobosim.utils.path import Path

class MyNewPlanner(PathPlanner):

    plugin_name = "my_planner"  # Needed to register the plugin

    def __init__(
        self,
        *,
        grid_resolution: float,
        grid_inflation_radius: float,
    ) -> None:
        super().__init__()
        self.reset()

    def reset(self) -> None:
        super().reset()
        if self.world is not None:
            self.grid = OccupancyGrid.from_world(
                world,
                resolution=grid_resolution,
                inflation_radius=grid_inflation_radius,
            )

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.

import time
from pyrobosim.utils.pose import Pose

    def plan(self, start: Pose, goal: Pose) -> Path:
        t_start = time.time()

        # Your planning logic goes here

        return Path(
            poses=[start, goal],
            planning_time=time.time() - t_start
        )

For visualization, you can provide get_graphs() and get_latest_paths() methods.

from pyrobosim.utils.search_graph.SearchGraph

    def plan(self, start: Pose, goal: Pose) -> Path:
        t_start = time.time()
        self.search_graph = SearchGraph()

        # Your planning logic goes here

        self.latest_path = Path(
            poses=[start, goal],
            planning_time=time.time() - t_start
        )
        return self.latest_path

    def get_graphs(self) -> list[SearchGraph]:
        return [SearchGraph()]

    def get_latest_path(self) -> Path:
        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 plugin_name attribute, which contains the name of the planner you defined earlier on.

def to_dict(self) -> dict[str, Any]:
    return {
        "type": self.plugin_name,
        "grid_resolution": self.grid_resolution,
        "grid_inflation_radius": self.grid_inflation_radius,
    }

At this point, you can import your own path planner in code and load it dynamically using the PathPlanner parent class.

from pyrobosim.navigation import PathPlanner
from my_module import MyNewPlanner  # Still need to import this!

planner_class = PathPlanner.registered_plugins["my_planner"]
planner = planner_class(grid_resolution=0.01, grid_inflation_radius=0.1)

… or from YAML world files.

robots:
  name: robot
  path_planner:
    type: my_planner
    grid_resolution: 0.01
    grid_inflation_radius: 0.1

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!