diff --git a/docs/source/usage/path_planners.rst b/docs/source/usage/path_planners.rst
index 8ce04033..58d99a45 100644
--- a/docs/source/usage/path_planners.rst
+++ b/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!
diff --git a/pyrobosim/examples/demo.py b/pyrobosim/examples/demo.py
index 9afc584d..0e959451 100755
--- a/pyrobosim/examples/demo.py
+++ b/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")
diff --git a/pyrobosim/examples/demo_astar.py b/pyrobosim/examples/demo_astar.py
index 0e811d4e..d4edef9b 100755
--- a/pyrobosim/examples/demo_astar.py
+++ b/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__":
diff --git a/pyrobosim/examples/demo_prm.py b/pyrobosim/examples/demo_prm.py
index f694bf99..8d25f024 100755
--- a/pyrobosim/examples/demo_prm.py
+++ b/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__":
diff --git a/pyrobosim/examples/demo_rrt.py b/pyrobosim/examples/demo_rrt.py
index bcae83d3..80323110 100755
--- a/pyrobosim/examples/demo_rrt.py
+++ b/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__":
diff --git a/pyrobosim/pyrobosim/core/robot.py b/pyrobosim/pyrobosim/core/robot.py
index 0fa13e53..cdb0ea08 100644
--- a/pyrobosim/pyrobosim/core/robot.py
+++ b/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.
diff --git a/pyrobosim/pyrobosim/core/yaml_utils.py b/pyrobosim/pyrobosim/core/yaml_utils.py
index fbfdb470..bef5c859 100644
--- a/pyrobosim/pyrobosim/core/yaml_utils.py
+++ b/pyrobosim/pyrobosim/core/yaml_utils.py
@@ -10,7 +10,7 @@
from ..navigation import (
ConstantVelocityExecutor,
OccupancyGrid,
- PathPlanner,
+ get_planner_class,
)
from ..planning.actions import ExecutionOptions
from ..utils.general import replace_special_yaml_tokens
@@ -126,7 +126,7 @@ def add_robots(self):
max_angular_acceleration=robot_data.get(
"max_angular_acceleration", np.inf
),
- path_planner=self.get_local_path_planner(robot_data),
+ path_planner=self.get_path_planner(robot_data),
path_executor=self.get_path_executor(robot_data),
grasp_generator=self.get_grasp_generator(robot_data),
partial_observability=robot_data.get("partial_observability", False),
@@ -143,30 +143,18 @@ def add_robots(self):
pose = None
self.world.add_robot(robot, loc=loc, pose=pose)
- def get_local_path_planner(self, robot_data):
- """Gets local planner path planner to a robot."""
+ def get_path_planner(self, robot_data):
+ """Gets path planner to add to a robot."""
if "path_planner" not in robot_data:
return None
planner_data = robot_data["path_planner"]
+ planner_data["world"] = self.world
planner_type = planner_data["type"]
planner_data.pop("type")
- occupancy_grid = planner_data.get("occupancy_grid", None)
- if occupancy_grid:
- resolution = occupancy_grid.get("resolution", 0.05)
- inflation_radius = occupancy_grid.get("inflation_radius", 0.15)
- occupancy_grid = OccupancyGrid.from_world(
- self.world, resolution, inflation_radius
- )
- # Remove the metadata about occupancy grid.
- planner_data.pop("occupancy_grid")
- planner_data["grid"] = occupancy_grid
-
- # We only need to include a world object if occupancy grid was not specified.
- if not occupancy_grid:
- planner_data["world"] = self.world
- path_planner = PathPlanner(planner_type, **planner_data)
+ planner_class = get_planner_class(planner_type)
+ path_planner = planner_class(**planner_data)
return path_planner
def get_path_executor(self, robot_data):
diff --git a/pyrobosim/pyrobosim/data/test_world_multirobot.yaml b/pyrobosim/pyrobosim/data/test_world_multirobot.yaml
index cbcb3a2e..6409f21a 100644
--- a/pyrobosim/pyrobosim/data/test_world_multirobot.yaml
+++ b/pyrobosim/pyrobosim/data/test_world_multirobot.yaml
@@ -58,9 +58,8 @@ robots:
location: bedroom
path_planner: # Rapidly-expanding Random Tree (RRT) planner
type: astar
- occupancy_grid:
- resolution: 0.05
- inflation_radius: 0.15
+ grid_resolution: 0.05
+ grid_inflation_radius: 0.15
heuristic: euclidean
diagonal_motion: true
compress_path: false
diff --git a/pyrobosim/pyrobosim/gui/world_canvas.py b/pyrobosim/pyrobosim/gui/world_canvas.py
index 5980587b..5879e6a4 100644
--- a/pyrobosim/pyrobosim/gui/world_canvas.py
+++ b/pyrobosim/pyrobosim/gui/world_canvas.py
@@ -12,6 +12,7 @@
from PySide6.QtCore import QRunnable, QThreadPool, QTimer, Signal
from pyrobosim.core.robot import Robot
+from pyrobosim.navigation.visualization import plot_path_planner
from pyrobosim.utils.motion import Path
@@ -362,7 +363,7 @@ def draw_and_sleep(self):
self.fig.canvas.flush_events()
time.sleep(0.005)
- def show_planner_and_path(self, robot=None, show_graph=True, path=None):
+ def show_planner_and_path(self, robot=None, show_graphs=True, path=None):
"""
Plot the path planner and latest path, if specified.
This planner could be global (property of the world)
@@ -370,8 +371,8 @@ def show_planner_and_path(self, robot=None, show_graph=True, path=None):
:param robot: If set to a Robot instance, uses that robot for display.
:type robot: :class:`pyrobosim.core.robot.Robot`, optional
- :param show_graph: If True, shows the path planner's latest graph(s).
- :type show_graph: bool
+ :param show_graphs: If True, shows the path planner's latest graph(s).
+ :type show_graphs: bool
:param path: Path to goal location, defaults to None.
:type path: :class:`pyrobosim.utils.motion.Path`, optional
"""
@@ -384,8 +385,10 @@ def show_planner_and_path(self, robot=None, show_graph=True, path=None):
with self.draw_lock:
color = robot.color if robot is not None else "m"
if robot.path_planner:
- path_planner_artists = robot.path_planner.plot(
- self.axes, show_graph=show_graph, path=path, path_color=color
+ graphs = robot.path_planner.get_graphs() if show_graphs else []
+ path = path or robot.path_planner.get_latest_path()
+ path_planner_artists = plot_path_planner(
+ self.axes, graphs=graphs, path=path, path_color=color
)
for artist in self.path_planner_artists["graph"]:
diff --git a/pyrobosim/pyrobosim/navigation/__init__.py b/pyrobosim/pyrobosim/navigation/__init__.py
index af426b8c..2e2b99e9 100644
--- a/pyrobosim/pyrobosim/navigation/__init__.py
+++ b/pyrobosim/pyrobosim/navigation/__init__.py
@@ -9,4 +9,31 @@
from .occupancy_grid import *
# Planners
-from .path_planner import *
+from .a_star import AStarPlanner
+from .rrt import RRTPlanner
+from .prm import PRMPlanner
+from .world_graph import WorldGraphPlanner
+
+
+PATH_PLANNERS_MAP = {
+ "astar": AStarPlanner,
+ "rrt": RRTPlanner,
+ "prm": PRMPlanner,
+ "world_graph": WorldGraphPlanner,
+}
+
+
+def get_planner_class(planner_type):
+ """
+ Helper function that returns a path planner.
+
+ :param planner_type: The type of path planner.
+ :type planner_type: str
+ :return: The class corresponding to the planner type specified.
+ :rtype: PathPlanner
+ :raises ValueError: if the planner type is invalid.
+ """
+ if planner_type not in PATH_PLANNERS_MAP:
+ raise ValueError(f"{planner_type} is not a supported planner type.")
+
+ return PATH_PLANNERS_MAP[planner_type]
diff --git a/pyrobosim/pyrobosim/navigation/a_star.py b/pyrobosim/pyrobosim/navigation/a_star.py
index a0249264..a45b7417 100644
--- a/pyrobosim/pyrobosim/navigation/a_star.py
+++ b/pyrobosim/pyrobosim/navigation/a_star.py
@@ -4,20 +4,34 @@
import time
import warnings
from astar import AStar
-from pyrobosim.utils.pose import Pose
-from pyrobosim.utils.motion import Path, reduce_waypoints_grid
-from pyrobosim.navigation.planner_base import PathPlannerBase
+from .occupancy_grid import OccupancyGrid
+from ..utils.pose import Pose
+from ..utils.motion import Path, reduce_waypoints_grid
-class AStarGrid(AStar):
- """Occupancy grid based implementation of A*."""
+
+class AStarPlanner(AStar):
+ """Occupancy grid based implementation of the A* path planning algorithm."""
def __init__(
- self, grid, heuristic="euclidean", diagonal_motion=True, compress_path=False
+ self,
+ *,
+ world,
+ grid_resolution,
+ grid_inflation_radius,
+ heuristic="euclidean",
+ diagonal_motion=True,
+ compress_path=False,
):
"""
Creates an instance of grid based A* planner.
+ :param world: World object to use in the planner.
+ :type world: :class:`pyrobosim.core.world.World`
+ :param grid_resolution: The resolution of the occupancy grid, in meters.
+ :type grid_resolution: float
+ :param grid_inflation_radius: The inflation radius of the occupancy grid, in meters.
+ :type grid_inflation_radius: float
:param heuristic: The metric to be used as heuristic ('manhattan', 'euclidean', 'none').
:type heuristic: string
:param diagonal_motion: If true, expand nodes using diagonal motion.
@@ -26,13 +40,15 @@ def __init__(
:type compress_path: bool
"""
super().__init__()
-
- self.grid = grid
+ self.world = world
+ self.grid_resolution = grid_resolution
+ self.grid_inflation_radius = grid_inflation_radius
self.heuristic = heuristic
self.diagonal_motion = diagonal_motion
self.compress_path = compress_path
self._set_actions()
self._set_heuristic()
+ self.reset()
def _set_actions(self):
"""
@@ -80,9 +96,9 @@ def heuristic_cost_estimate(self, cell1, cell2):
Compute heuristic cost estimate using selected heuristic.
:param cell1: The source cell
- :type cell1: Tuple (int, int)
+ :type cell1: tuple(int, int)
:param cell2: The destination cell
- :type cell2: Tuple (int, int)
+ :type cell2: tuple(int, int)
"""
return self._heuristic(cell1, cell2)
@@ -91,9 +107,9 @@ def distance_between(self, cell1, cell2):
Computes the distance between 2 cells in the grid.
:param cell1: The source cell
- :type cell1: Tuple (int, int)
+ :type cell1: tuple(int, int)
:param cell2: The destination cell
- :type cell2: Tuple (int, int)
+ :type cell2: tuple(int, int)
"""
x1, y1 = cell1
x2, y2 = cell2
@@ -104,15 +120,23 @@ def neighbors(self, cell):
Get the neighbors of a cell in the grid.
:param cell: The cell for which the neighbors need to be computed.
- :type cell: Tuple (int, int)
+ :type cell: tuple(int, int)
"""
- neighbours_list = []
+ neighbors_list = []
for action in self.selected_actions:
dx, dy = self.actions[action]["action"]
x, y = cell[0] + dx, cell[1] + dy
if not self.grid.is_occupied((x, y)):
- neighbours_list.append((x, y))
- return neighbours_list
+ neighbors_list.append((x, y))
+ return neighbors_list
+
+ def reset(self):
+ """Resets the occupancy grid."""
+ self.grid = OccupancyGrid.from_world(
+ self.world,
+ resolution=self.grid_resolution,
+ inflation_radius=self.grid_inflation_radius,
+ )
def plan(self, start, goal):
"""
@@ -123,6 +147,7 @@ def plan(self, start, goal):
:param goal: Goal pose.
:type goal: :class:`pyrobosim.utils.pose.Pose`
"""
+ t_start = time.time()
start_grid = self.grid.world_to_grid((start.x, start.y))
goal_grid = self.grid.world_to_grid((goal.x, goal.y))
path = self.astar(start_grid, goal_grid)
@@ -142,42 +167,23 @@ def plan(self, start, goal):
self.latest_path = Path(poses=world_path)
self.latest_path.fill_yaws()
+ self.latest_path.planning_time = time.time() - t_start
return self.latest_path
-
-class AstarPlanner(PathPlannerBase):
- """Factory class for A* path planner."""
-
- def __init__(self, **planner_config):
+ def get_graphs(self):
"""
- Creates an instance of A* planner.
+ Returns the graphs generated by the planner, if any.
- :param planner_config: The configuration to be used.
- :type planner_config: dict
+ :return: List of graphs.
+ :rtype: list[:class:`pyrobosim.utils.search_graph.SearchGraph`]
"""
- super().__init__()
- self.impl = None # Holds the implementation.
-
- # Depending on if grid is provided, select the implementation.
- if planner_config.get("grid", None):
- self.impl = AStarGrid(**planner_config)
- else:
- raise NotImplementedError(
- "A-star does not have a standalone graph based implementation."
- )
+ return []
- def plan(self, start, goal):
+ def get_latest_path(self):
"""
- Plans a path from start to goal.
+ Returns the latest path generated by the planner, if any.
- :param start: Start pose.
- :type start: :class:`pyrobosim.utils.pose.Pose`
- :param goal: Goal pose.
- :type goal: :class:`pyrobosim.utils.pose.Pose`
- :return: Path from start to goal.
- :rtype: :class:`pyrobosim.utils.motion.Path`
+ :return: List of graphs.
+ :rtype: list[:class:`pyrobosim.utils.motion.Path`]
"""
- start_time = time.time()
- self.latest_path = self.impl.plan(start, goal)
- self.planning_time = time.time() - start_time
return self.latest_path
diff --git a/pyrobosim/pyrobosim/navigation/path_planner.py b/pyrobosim/pyrobosim/navigation/path_planner.py
deleted file mode 100644
index dd440d94..00000000
--- a/pyrobosim/pyrobosim/navigation/path_planner.py
+++ /dev/null
@@ -1,94 +0,0 @@
-""" Implementation of the generic path planner. """
-
-import warnings
-from pyrobosim.navigation.a_star import AstarPlanner
-from pyrobosim.navigation.rrt import RRTPlanner
-from pyrobosim.navigation.prm import PRMPlanner
-from pyrobosim.navigation.world_graph import WorldGraphPlanner
-
-
-class PathPlanner:
- """
- Creates a path planner.
- """
-
- def __init__(self, planner_type, **planner_config):
- """
- Creates a PathPlanner instance of given type and configuration
-
- :param planner_type: The type of planner to be used (astar, prm, rrt, world_graph)
- :type planner_type: str
- :param planner_config: The configuration to be used with the specified planner type.
- :type planner_config: dict
- """
-
- self.planners = {
- "astar": AstarPlanner,
- "rrt": RRTPlanner,
- "prm": PRMPlanner,
- "world_graph": WorldGraphPlanner,
- }
-
- if planner_type not in self.planners:
- warnings.warn(
- f"{planner_type} is not a supported planner type.", UserWarning
- )
- return None
- if not planner_config:
- warnings.warn(
- f"No planner configuration provided. Must provide either a World or OccupancyGrid object.",
- UserWarning,
- )
- return None
-
- self.planner_type = planner_type
- self.planner_config = planner_config
- self.planner = self.planners[self.planner_type](**self.planner_config)
-
- def plan(self, start, goal):
- """
- Plans a path from start to goal.
-
- :param start: Start pose or graph node.
- :type start: :class:`pyrobosim.utils.pose.Pose` /
- :class:`pyrobosim.utils.search_graph.Node`
- :param goal: Goal pose or graph node.
- :type goal: :class:`pyrobosim.utils.pose.Pose` /
- :class:`pyrobosim.utils.search_graph.Node`
- :return: Path from start to goal.
- :rtype: :class:`pyrobosim.utils.motion.Path`
- """
-
- self.latest_path = self.planner.plan(start, goal)
- return self.latest_path
-
- def plot(self, axes, show_graph=True, path=None, path_color="m"):
- """
- Plots the planned path on a specified set of axes.
-
- :param axes: The axes on which to draw.
- :type axes: :class:`matplotlib.axes.Axes`
- :param show_graph: If True, shows the path planner's latest graph(s).
- :type show_graph: bool
- :param path: Path to display, defaults to None.
- :type path: :class:`pyrobosim.utils.motion.Path`, optional
- :param path_color: Color of the path, as an RGB tuple or string.
- :type path_color: tuple[float] / str, optional
- :return: List of Matplotlib artists containing what was drawn,
- used for bookkeeping.
- :rtype: list[:class:`matplotlib.artist.Artist`]
- """
-
- return self.planner.plot(
- axes, show_graph=show_graph, path=path, path_color=path_color
- )
-
- def show(self):
- """Displays the planned path on the GUI."""
-
- self.planner.show()
-
- def info(self):
- """Display information about planning process."""
-
- self.planner.info()
diff --git a/pyrobosim/pyrobosim/navigation/planner_base.py b/pyrobosim/pyrobosim/navigation/planner_base.py
deleted file mode 100644
index c6c4481a..00000000
--- a/pyrobosim/pyrobosim/navigation/planner_base.py
+++ /dev/null
@@ -1,147 +0,0 @@
-""" Specification of the interface that all planners must implement. """
-
-import matplotlib.pyplot as plt
-from matplotlib.collections import LineCollection
-
-from pyrobosim.utils.motion import Path
-
-
-class PathPlannerBase:
- """The base class for path planners."""
-
- def __init__(self):
- """
- Creates an instance of PathPlannerBase.
- """
- self.goal = None
- self.start = None
- self.impl = None
- self.planning_time = 0.0
- self.graphs = []
- self.graphs_updated = False
- self.latest_path = Path()
-
- def reset(self):
- """
- Resets the state of the planner.
- Sub-classes should add their own relevant mechanisms
- """
- self.goal = None
- self.start = None
- self.planner = None
- self.planning_time = 0.0
- self.graphs = []
- self.latest_path = Path()
- self.graphs_updated = False
-
- def plan(self, start, goal):
- """
- Plans a path from start to goal.
-
- :param start: Start pose or graph node.
- :type start: :class:`pyrobosim.utils.pose.Pose` /
- :class:`pyrobosim.utils.search_graph.Node`
- :param goal: Goal pose or graph node.
- :type goal: :class:`pyrobosim.utils.pose.Pose` /
- :class:`pyrobosim.utils.search_graph.Node`
- :return: Path from start to goal.
- :rtype: :class:`pyrobosim.utils.motion.Path`
- """
-
- raise NotImplementedError(
- f"Subclasses must implement {self.__class__.__name__}.plan()"
- )
-
- def info(self):
- """Prints information about the last generated plan."""
- print(f"Planner : {self.impl.__class__.__name__}")
- print(f"Planning time : {self.planning_time}")
- print(f"Number of waypoints : {self.latest_path.num_poses}")
-
- def plot(self, axes, show_graph=True, path=None, path_color="m"):
- """
- Plots the planned path on a specified set of axes.
-
- :param axes: The axes on which to draw.
- :type axes: :class:`matplotlib.axes.Axes`
- :param show_graph: If True, shows the path planner's latest graph(s).
- :type show_graph: bool
- :param path: Path to display, defaults to None. If none, uses the `latest_path` attribute.
- :type path: :class:`pyrobosim.utils.motion.Path`, optional
- :param path_color: Color of the path, as an RGB tuple or string.
- :type path_color: tuple[float] / str, optional
- :return: List of Matplotlib artists containing what was drawn,
- used for bookkeeping.
- :rtype: list[:class:`matplotlib.artist.Artist`]
- """
-
- graph_artists = []
- path_artists = []
- artists = {}
-
- if not path:
- path = self.latest_path
-
- if show_graph:
- for graph in self.graphs:
- # Plot the markers
- (markers,) = axes.plot(
- [n.pose.x for n in graph.nodes],
- [n.pose.y for n in graph.nodes],
- color=graph.color,
- alpha=graph.color_alpha,
- linestyle="",
- marker="o",
- markerfacecolor=graph.color,
- markeredgecolor=graph.color,
- markersize=3,
- zorder=1,
- )
- graph_artists.append(markers)
-
- # Plot the edges as a LineCollection
- edge_coords = [
- [[e.nodeA.pose.x, e.nodeA.pose.y], [e.nodeB.pose.x, e.nodeB.pose.y]]
- for e in graph.edges
- ]
- line_segments = LineCollection(
- edge_coords,
- color=graph.color,
- alpha=graph.color_alpha,
- linewidth=0.5,
- linestyle="--",
- zorder=1,
- )
- axes.add_collection(line_segments)
- graph_artists.append(line_segments)
-
- if path and path.num_poses > 0:
- x = [p.x for p in path.poses]
- y = [p.y for p in path.poses]
- (path,) = axes.plot(
- x, y, linestyle="-", color=path_color, linewidth=3, alpha=0.5, zorder=1
- )
- (start,) = axes.plot(x[0], y[0], "go", zorder=2)
- (goal,) = axes.plot(x[-1], y[-1], "rx", zorder=2)
- path_artists.extend((path, start, goal))
-
- if graph_artists:
- artists["graph"] = graph_artists
- if path_artists:
- artists["path"] = path_artists
- return artists
-
- def show(self):
- """
- Shows the planned path in a new figure.
-
- :param show_path: If True, shows the last planned path.
- :type show_path: bool
- """
-
- f = plt.figure()
- ax = f.add_subplot(111)
- self.plot(ax)
- plt.title(self.impl.__class__.__name__)
- plt.axis("equal")
- plt.show()
diff --git a/pyrobosim/pyrobosim/navigation/prm.py b/pyrobosim/pyrobosim/navigation/prm.py
index a0304238..05f4f570 100644
--- a/pyrobosim/pyrobosim/navigation/prm.py
+++ b/pyrobosim/pyrobosim/navigation/prm.py
@@ -3,28 +3,30 @@
import time
import warnings
-from .planner_base import PathPlannerBase
from ..utils.motion import Path, reduce_waypoints_polygon
from ..utils.pose import Pose
from ..utils.search_graph import SearchGraph, Node
-class PRMPlannerPolygon:
+class PRMPlanner:
"""
- Polygon representation based implementation of PRM.
+ Implements a Probabilistic Roadmap (PRM) path planner.
"""
def __init__(
self,
+ *,
+ world,
compress_path=False,
collision_check_step_dist=0.025,
max_connection_dist=2.0,
max_nodes=50,
- world=None,
):
"""
Creates an instance of a PRM planner.
+ :param world: World object to use in the planner.
+ :type world: :class:`pyrobosim.core.world.World`
:param compress_path: If true, tries to shorten the path with polygon-based collision checks.
:type compress_path: bool
:param collision_check_step_dist: Step size for discretizing collision checking.
@@ -33,8 +35,7 @@ def __init__(
:type max_connection_dist: float
:param max_nodes: Maximum nodes sampled to build the PRM.
:type max_nodes: int
- :param world: World object to use in the planner.
- :type world: :class:`pyrobosim.core.world.World`
+
"""
# Parameters
self.collision_check_step_dist = collision_check_step_dist
@@ -47,14 +48,12 @@ def __init__(
def reset(self):
"""Resamples the PRM and resets planning metrics."""
- self.planning_time = self.sampling_time = 0.0
self.latest_path = Path()
# Create a search graph and sample nodes.
self.graph = SearchGraph(
color=[0, 0.4, 0.8], color_alpha=0.25, use_planner=True
)
- t_start = time.time()
for i in range(self.max_nodes):
n_sample = self.sample_configuration()
if not n_sample:
@@ -63,7 +62,6 @@ def reset(self):
node = Node(pose=n_sample)
self.graph.add_node(node)
self.connect_neighbors(node)
- self.sampling_time = time.time() - t_start
def connect_neighbors(self, node):
"""
@@ -98,7 +96,6 @@ def plan(self, start, goal):
"""
# Reset the path and time
self.latest_path = Path()
- self.planning_time = 0.0
# Create the start and goal nodes
if isinstance(start, Pose):
start = Node(start, parent=None)
@@ -119,7 +116,7 @@ def plan(self, start, goal):
)
self.latest_path.set_poses(compressed_poses)
self.latest_path.fill_yaws()
- self.planning_time = time.time() - t_start
+ self.latest_path.planning_time = time.time() - t_start
self.graph.remove_node(start)
self.graph.remove_node(goal)
return self.latest_path
@@ -142,36 +139,11 @@ def get_graphs(self):
"""
return [self.graph]
-
-class PRMPlanner(PathPlannerBase):
- """Factory class for Probabilistic RoadMap path planner."""
-
- def __init__(self, **planner_config):
- """
- Creates an instance of PRM planner.
- """
- super().__init__()
-
- self.impl = None
-
- if planner_config.get("grid", None):
- raise NotImplementedError("PRM planner does not support grid based search.")
- else:
- self.impl = PRMPlannerPolygon(**planner_config)
-
- def plan(self, start, goal):
+ def get_latest_path(self):
"""
- Plans a path from start to goal.
+ Returns the latest path generated by the planner, if any.
- :param start: Start pose.
- :type start: :class:`pyrobosim.utils.pose.Pose`
- :param goal: Goal pose.
- :type goal: :class:`pyrobosim.utils.pose.Pose`
- :return: Path from start to goal.
- :rtype: :class:`pyrobosim.utils.motion.Path`
+ :return: List of graphs.
+ :rtype: list[:class:`pyrobosim.utils.motion.Path`]
"""
- start_time = time.time()
- self.latest_path = self.impl.plan(start, goal)
- self.planning_time = time.time() - start_time
- self.graphs = self.impl.get_graphs()
return self.latest_path
diff --git a/pyrobosim/pyrobosim/navigation/rrt.py b/pyrobosim/pyrobosim/navigation/rrt.py
index dcf961cb..4b90d94a 100644
--- a/pyrobosim/pyrobosim/navigation/rrt.py
+++ b/pyrobosim/pyrobosim/navigation/rrt.py
@@ -5,19 +5,19 @@
import numpy as np
import warnings
-from .planner_base import PathPlannerBase
from ..utils.motion import Path, reduce_waypoints_polygon
from ..utils.pose import Pose
from ..utils.search_graph import SearchGraph, Node
-class RRTPlannerPolygon:
+class RRTPlanner:
"""
- Polygon representation based implementation of RRT.
+ Implements a Rapidly-exploring Random Tree (RRT) path planner.
"""
def __init__(
self,
+ *,
world,
bidirectional=False,
rrt_connect=False,
@@ -37,7 +37,7 @@ def __init__(
:param bidirectional: If True, uses bidirectional RRT to grow trees
from both start and goal.
:type bidirectional: bool
- :param rrt_connect: If True, uses RRT-connect to bias tree growth
+ :param rrt_connect: If True, uses RRTConnect to bias tree growth
towards goals.
:type rrt_connect: bool
:param rrt_star: If True, uses RRT* to rewire trees to smooth and
@@ -86,7 +86,6 @@ def reset(self):
if self.bidirectional:
self.graph_goal = SearchGraph(color=[0, 0.4, 0.8])
self.latest_path = Path()
- self.planning_time = 0.0
self.nodes_sampled = 0
self.n_rewires = 0
@@ -102,6 +101,8 @@ def plan(self, start, goal):
:rtype: :class:`pyrobosim.utils.motion.Path`
"""
self.reset()
+ t_start = time.time()
+ goal_found = False
# Create the start and goal nodes
n_start = Node(start, parent=None)
@@ -110,9 +111,6 @@ def plan(self, start, goal):
if self.bidirectional:
self.graph_goal.nodes = {n_goal}
- t_start = time.time()
- goal_found = False
-
# If the goal is within max connection distance of the start, connect them directly
if self.world.is_connectable(
n_start.pose,
@@ -123,7 +121,7 @@ def plan(self, start, goal):
path_poses = [n_start.pose, n_goal.pose]
self.latest_path = Path(poses=path_poses)
self.latest_path.fill_yaws()
- self.planning_time = time.time() - t_start
+ self.latest_path.planning_time = time.time() - t_start
return self.latest_path
while not goal_found:
@@ -171,7 +169,7 @@ def plan(self, start, goal):
# See if the new nodes can directly connect to the goal.
# This is done either as a single connection within max distance,
- # or using RRT-Connect.
+ # or using RRTConnect.
if self.bidirectional:
if connected_node:
# If we added a node to the start tree,
@@ -193,13 +191,13 @@ def plan(self, start, goal):
goal_found, _ = self.try_connect_until(self.graph_start, n_new, n_goal)
# Check max nodes sampled or max time elapsed
- self.planning_time = time.time() - t_start
+ planning_time = time.time() - t_start
if (
- self.planning_time > self.max_time
+ planning_time > self.max_time
or self.nodes_sampled > self.max_nodes_sampled
):
warnings.warn("Could not find a path from start to goal.")
- self.latest_path = Path()
+ self.latest_path = Path(planning_time=planning_time)
return self.latest_path
# Now back out the path
@@ -231,7 +229,8 @@ def plan(self, start, goal):
path_poses = reduce_waypoints_polygon(
self.world, path_poses, self.collision_check_step_dist
)
- self.latest_path = Path(poses=path_poses)
+ planning_time = time.time() - t_start
+ self.latest_path = Path(poses=path_poses, planning_time=planning_time)
self.latest_path.fill_yaws()
return self.latest_path
@@ -320,7 +319,7 @@ def try_connect_until(self, graph, n_curr, n_tgt):
"""
Try to connect a node ``n_curr`` to a target node ``n_tgt``.
This will keep extending the current node towards the target if
- RRT-Connect is enabled, or else will just try once.
+ RRTConnect is enabled, or else will just try once.
:param graph: The tree object.
:type graph: :class:`pyrobosim.utils.search_graph.SearchGraph`
@@ -352,7 +351,7 @@ def try_connect_until(self, graph, n_curr, n_tgt):
return True, n_tgt
if self.rrt_connect:
- # If using RRT-Connect, keep trying to connect.
+ # If using RRTConnect, keep trying to connect.
n_new = self.extend(n_curr, n_tgt.pose)
if self.world.is_connectable(
n_curr.pose,
@@ -365,7 +364,7 @@ def try_connect_until(self, graph, n_curr, n_tgt):
else:
return False, n_curr
else:
- # If not using RRT-Connect, we only get one chance to connect.
+ # If not using RRTConnect, we only get one chance to connect.
return False, n_curr
def get_graphs(self):
@@ -380,35 +379,11 @@ def get_graphs(self):
graphs.append(self.graph_goal)
return graphs
-
-class RRTPlanner(PathPlannerBase):
- """Factory class for Rapidly-Exploring Random Trees path planner."""
-
- def __init__(self, **planner_config):
- """
- Creates and instance of RRT Planner.
+ def get_latest_path(self):
"""
- super().__init__()
+ Returns the latest path generated by the planner, if any.
- self.impl = None
- if planner_config.get("grid", None):
- raise NotImplementedError("RRT planner does not support grid based search.")
- else:
- self.impl = RRTPlannerPolygon(**planner_config)
-
- def plan(self, start, goal):
- """
- Plans a path from start to goal.
-
- :param start: Start pose.
- :type start: :class:`pyrobosim.utils.pose.Pose`
- :param goal: Goal pose.
- :type goal: :class:`pyrobosim.utils.pose.Pose`
- :return: Path from start to goal.
- :rtype: :class:`pyrobosim.utils.motion.Path`
+ :return: List of graphs.
+ :rtype: list[:class:`pyrobosim.utils.motion.Path`]
"""
- start_time = time.time()
- self.latest_path = self.impl.plan(start, goal)
- self.planning_time = time.time() - start_time
- self.graphs = self.impl.get_graphs()
return self.latest_path
diff --git a/pyrobosim/pyrobosim/navigation/visualization.py b/pyrobosim/pyrobosim/navigation/visualization.py
new file mode 100644
index 00000000..95a0bd64
--- /dev/null
+++ b/pyrobosim/pyrobosim/navigation/visualization.py
@@ -0,0 +1,93 @@
+""" Visualization utilities for path planners. """
+
+import matplotlib.pyplot as plt
+from matplotlib.collections import LineCollection
+
+
+def plot_path_planner(axes, graphs=[], path=None, path_color="m"):
+ """
+ Plots the planned path on a specified set of axes.
+
+ :param axes: The axes on which to draw.
+ :type axes: :class:`matplotlib.axes.Axes`
+ :param graphs: A list of path planner graphs to display.
+ :type graphs: list[:class:`pyrobosim.utils.search_graph.SearchGraph`], optional
+ :param path: Path to display.
+ :type path: :class:`pyrobosim.utils.motion.Path`, optional
+ :param path_color: Color of the path, as an RGB tuple or string.
+ :type path_color: tuple[float] / str, optional
+ :return: List of Matplotlib artists containing what was drawn,
+ used for bookkeeping.
+ :rtype: list[:class:`matplotlib.artist.Artist`]
+ """
+
+ graph_artists = []
+ path_artists = []
+ artists = {}
+
+ for graph in graphs:
+ # Plot the markers
+ (markers,) = axes.plot(
+ [n.pose.x for n in graph.nodes],
+ [n.pose.y for n in graph.nodes],
+ color=graph.color,
+ alpha=graph.color_alpha,
+ linestyle="",
+ marker="o",
+ markerfacecolor=graph.color,
+ markeredgecolor=graph.color,
+ markersize=3,
+ zorder=1,
+ )
+ graph_artists.append(markers)
+
+ # Plot the edges as a LineCollection
+ edge_coords = [
+ [[e.nodeA.pose.x, e.nodeA.pose.y], [e.nodeB.pose.x, e.nodeB.pose.y]]
+ for e in graph.edges
+ ]
+ line_segments = LineCollection(
+ edge_coords,
+ color=graph.color,
+ alpha=graph.color_alpha,
+ linewidth=0.5,
+ linestyle="--",
+ zorder=1,
+ )
+ axes.add_collection(line_segments)
+ graph_artists.append(line_segments)
+
+ if path and path.num_poses > 0:
+ x = [p.x for p in path.poses]
+ y = [p.y for p in path.poses]
+ (path,) = axes.plot(
+ x, y, linestyle="-", color=path_color, linewidth=3, alpha=0.5, zorder=1
+ )
+ (start,) = axes.plot(x[0], y[0], "go", zorder=2)
+ (goal,) = axes.plot(x[-1], y[-1], "rx", zorder=2)
+ path_artists.extend((path, start, goal))
+
+ if graph_artists:
+ artists["graph"] = graph_artists
+ if path_artists:
+ artists["path"] = path_artists
+ return artists
+
+
+def show_path_planner(graphs=[], path=None, title="Path Planner Output"):
+ """
+ Shows the path planner output in a new figure.
+
+ :param graphs: A list of path planner graphs to display.
+ :type graphs: list[:class:`pyrobosim.utils.search_graph.SearchGraph`], optional
+ :param path: Path to display.
+ :type path: :class:`pyrobosim.utils.motion.Path`, optional
+ :param:
+ """
+
+ f = plt.figure()
+ ax = f.add_subplot(111)
+ plot_path_planner(ax, graphs=graphs, path=path)
+ plt.title(title)
+ plt.axis("equal")
+ plt.show()
diff --git a/pyrobosim/pyrobosim/navigation/world_graph.py b/pyrobosim/pyrobosim/navigation/world_graph.py
index df89bfce..6a5be26a 100644
--- a/pyrobosim/pyrobosim/navigation/world_graph.py
+++ b/pyrobosim/pyrobosim/navigation/world_graph.py
@@ -3,20 +3,21 @@
import time
import itertools
-from .planner_base import PathPlannerBase
+from ..core.locations import Location
from ..utils.motion import Path, reduce_waypoints_polygon
from ..utils.pose import Pose
from ..utils.search_graph import SearchGraph, Node
-class WorldGraphPlannerPolygon:
+class WorldGraphPlanner:
"""
- Polygon representation based implementation of world graph planner.
+ Implementation of a path planner which creates a visibility-based roadmap using the polygons in the world.
"""
def __init__(
self,
- world=None,
+ *,
+ world,
collision_check_step_dist=0.025,
max_connection_dist=None,
compress_path=False,
@@ -48,8 +49,6 @@ def reset(self):
"""
Initializes the graph from the entity nodes in the world linked to this planner.
"""
- from ..core.locations import Location
-
# Create a search graph from the nodes in the world.
self.graph = SearchGraph(color=[0, 0.4, 0.8], color_alpha=0.5, use_planner=True)
for entity in itertools.chain(
@@ -119,48 +118,22 @@ def plan(self, start, goal):
self.world, self.latest_path.poses, self.collision_check_step_dist
)
self.latest_path.set_poses(compressed_poses)
- self.latest_path.fill_yaws()
- self.planning_time = time.time() - t_start
self.graph.remove_node(start)
self.graph.remove_node(goal)
+
+ self.latest_path.fill_yaws()
+ self.latest_path.planning_time = time.time() - t_start
return self.latest_path
def get_graphs(self):
"""Returns the graphs generated by the planner if any."""
return [self.graph]
-
-class WorldGraphPlanner(PathPlannerBase):
- """Factory class for world graph path planner."""
-
- def __init__(self, **planner_config):
- """
- Creates an instance of world graph planner.
- """
- super().__init__()
-
- self.impl = None
-
- if planner_config.get("grid", None):
- raise NotImplementedError(
- "Grid based world graph planner is not supported. "
- )
- else:
- self.impl = WorldGraphPlannerPolygon(**planner_config)
-
- def plan(self, start, goal):
+ def get_latest_path(self):
"""
- Plans a path from start to goal.
+ Returns the latest path generated by the planner, if any.
- :param start: Start pose.
- :type start: :class:`pyrobosim.utils.pose.Pose`
- :param goal: Goal pose.
- :type goal: :class:`pyrobosim.utils.pose.Pose`
- :return: Path from start to goal.
- :rtype: :class:`pyrobosim.utils.motion.Path`
+ :return: List of graphs.
+ :rtype: list[:class:`pyrobosim.utils.motion.Path`]
"""
- start_time = time.time()
- self.latest_path = self.impl.plan(start, goal)
- self.planning_time = time.time() - start_time
- self.graphs = self.impl.get_graphs()
return self.latest_path
diff --git a/pyrobosim/pyrobosim/utils/motion.py b/pyrobosim/pyrobosim/utils/motion.py
index e36a421d..8b70828a 100644
--- a/pyrobosim/pyrobosim/utils/motion.py
+++ b/pyrobosim/pyrobosim/utils/motion.py
@@ -6,14 +6,17 @@
class Path:
"""Representation of a path for motion planning."""
- def __init__(self, poses=[]):
+ def __init__(self, poses=[], planning_time=None):
"""
Creates a Path object instance.
:param poses: List of poses representing a path.
:type poses: list[:class:`pyrobosim.utils.pose.Pose`], optional
+ :param planning_time: The time taken to generate this path.
+ :type planning_time: float, optional
"""
self.set_poses(poses)
+ self.planning_time = planning_time
def set_poses(self, poses):
"""
@@ -86,6 +89,13 @@ def print_details(self):
for i, p in enumerate(self.poses):
print_str += f"\n {i + 1}. {p}"
print_str += f"\nTotal Length: {self.length:.3f}"
+ if self.planning_time:
+ if self.planning_time > 0.01:
+ print_str += f"\nPlanning time: {self.planning_time:3f} seconds"
+ else:
+ print_str += (
+ f"\nPlanning time: {self.planning_time * 1000.0:3f} milliseconds"
+ )
print(print_str)
diff --git a/pyrobosim/test/core/test_robot.py b/pyrobosim/test/core/test_robot.py
index 0dd3e830..c0c43b9a 100644
--- a/pyrobosim/test/core/test_robot.py
+++ b/pyrobosim/test/core/test_robot.py
@@ -10,7 +10,7 @@
import threading
from pyrobosim.core import Pose, Robot, WorldYamlLoader
-from pyrobosim.navigation import ConstantVelocityExecutor, PathPlanner
+from pyrobosim.navigation import ConstantVelocityExecutor, WorldGraphPlanner
from pyrobosim.planning.actions import (
ExecutionStatus,
ExecutionOptions,
@@ -124,7 +124,7 @@ def test_robot_path_planner(self):
robot = Robot(
pose=init_pose,
- path_planner=PathPlanner("world_graph", world=self.test_world),
+ path_planner=WorldGraphPlanner(world=self.test_world),
)
robot.world = self.test_world
robot.location = self.test_world.get_entity_by_name("kitchen")
@@ -163,7 +163,7 @@ def test_robot_path_executor(self):
robot = Robot(
pose=init_pose,
- path_planner=PathPlanner("world_graph", world=self.test_world),
+ path_planner=WorldGraphPlanner(world=self.test_world),
path_executor=ConstantVelocityExecutor(linear_velocity=5.0, dt=0.1),
)
robot.world = self.test_world
@@ -201,7 +201,7 @@ def test_robot_nav_validation(self):
robot = Robot(
pose=init_pose,
- path_planner=PathPlanner("world_graph", world=self.test_world),
+ path_planner=WorldGraphPlanner(world=self.test_world),
path_executor=ConstantVelocityExecutor(
linear_velocity=3.0,
dt=0.1,
@@ -492,7 +492,7 @@ def test_execute_action(self):
init_pose = Pose(x=1.0, y=0.5, yaw=0.0)
robot = Robot(
pose=init_pose,
- path_planner=PathPlanner("world_graph", world=self.test_world),
+ path_planner=WorldGraphPlanner(world=self.test_world),
path_executor=ConstantVelocityExecutor(linear_velocity=5.0, dt=0.1),
action_execution_options={"navigate": ExecutionOptions(battery_usage=1.0)},
)
@@ -533,7 +533,7 @@ def test_execute_action_simulated_options(self):
}
robot = Robot(
pose=init_pose,
- path_planner=PathPlanner("world_graph", world=self.test_world),
+ path_planner=WorldGraphPlanner(world=self.test_world),
path_executor=ConstantVelocityExecutor(linear_velocity=5.0, dt=0.1),
action_execution_options=action_execution_options,
initial_battery_level=80.0,
@@ -569,7 +569,7 @@ def test_execute_action_cancel(self):
init_pose = Pose(x=1.0, y=0.5, yaw=0.0)
robot = Robot(
pose=init_pose,
- path_planner=PathPlanner("world_graph", world=self.test_world),
+ path_planner=WorldGraphPlanner(world=self.test_world),
path_executor=ConstantVelocityExecutor(linear_velocity=3.0, dt=0.1),
)
robot.location = "kitchen"
@@ -604,7 +604,7 @@ def test_execute_plan(self):
init_pose = Pose(x=1.0, y=0.5, yaw=0.0)
robot = Robot(
pose=init_pose,
- path_planner=PathPlanner("world_graph", world=self.test_world),
+ path_planner=WorldGraphPlanner(world=self.test_world),
path_executor=ConstantVelocityExecutor(linear_velocity=5.0, dt=0.1),
)
robot.location = "kitchen"
@@ -632,7 +632,7 @@ def test_execute_plan_cancel(self):
init_pose = Pose(x=1.0, y=0.5, yaw=0.0)
robot = Robot(
pose=init_pose,
- path_planner=PathPlanner("world_graph", world=self.test_world),
+ path_planner=WorldGraphPlanner(world=self.test_world),
path_executor=ConstantVelocityExecutor(linear_velocity=5.0, dt=0.1),
)
robot.location = "kitchen"
diff --git a/pyrobosim/test/core/test_world.py b/pyrobosim/test/core/test_world.py
index e08765b5..051eb2d5 100644
--- a/pyrobosim/test/core/test_world.py
+++ b/pyrobosim/test/core/test_world.py
@@ -175,11 +175,11 @@ def test_create_object():
def test_add_robot():
"""Tests adding a robot to the world"""
from pyrobosim.core import Robot
- from pyrobosim.navigation import PathPlanner
+ from pyrobosim.navigation import RRTPlanner
world_robots = TestWorldModeling.world.robots
path_planner_config = {"world": TestWorldModeling.world}
- path_planner = PathPlanner("rrt", **path_planner_config)
+ path_planner = RRTPlanner(**path_planner_config)
# Add a robot at a specific location, but let the pose be sampled.
robot = Robot(name="test_robot", radius=0.05, path_planner=path_planner)
diff --git a/pyrobosim/test/core/test_yaml_utils.py b/pyrobosim/test/core/test_yaml_utils.py
index e56d70cb..1293c929 100644
--- a/pyrobosim/test/core/test_yaml_utils.py
+++ b/pyrobosim/test/core/test_yaml_utils.py
@@ -319,7 +319,7 @@ def test_create_robots_from_yaml():
GraspGenerator,
ParallelGraspProperties,
)
- from pyrobosim.navigation import PathPlanner
+ from pyrobosim.navigation import RRTPlanner
from pyrobosim.navigation.execution import ConstantVelocityExecutor
loader = TestWorldYamlLoading.yaml_loader
@@ -413,13 +413,12 @@ def test_create_robots_from_yaml():
assert robot1.battery_level == 50.0
path_planner = robot1.path_planner
- assert isinstance(path_planner, PathPlanner)
- assert path_planner.planner_type == "rrt"
- assert path_planner.planner_config["collision_check_step_dist"] == 0.025
- assert path_planner.planner_config["max_connection_dist"] == 0.5
- assert path_planner.planner_config["bidirectional"] == True
- assert path_planner.planner_config["rrt_star"] == True
- assert path_planner.planner_config["rewire_radius"] == 1.5
+ assert isinstance(path_planner, RRTPlanner)
+ assert path_planner.collision_check_step_dist == 0.025
+ assert path_planner.max_connection_dist == 0.5
+ assert path_planner.bidirectional
+ assert path_planner.rrt_star
+ assert path_planner.rewire_radius == 1.5
path_executor = robot1.path_executor
assert isinstance(path_executor, ConstantVelocityExecutor)
diff --git a/pyrobosim/test/navigation/test_astar.py b/pyrobosim/test/navigation/test_astar.py
index a4265e9d..d6b4dbbd 100644
--- a/pyrobosim/test/navigation/test_astar.py
+++ b/pyrobosim/test/navigation/test_astar.py
@@ -1,11 +1,11 @@
#!/usr/bin/env python3
-"""Unit tests for A-star planner"""
+"""Unit tests for A* planner"""
import os
from pyrobosim.core import WorldYamlLoader
-from pyrobosim.navigation import OccupancyGrid, PathPlanner
+from pyrobosim.navigation import AStarPlanner, OccupancyGrid
from pyrobosim.utils.general import get_data_folder
from pyrobosim.utils.pose import Pose
@@ -22,14 +22,14 @@ def test_astar():
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,
}
- astar_planner = PathPlanner("astar", **planner_config)
+ astar_planner = AStarPlanner(**planner_config)
full_path = astar_planner.plan(start, goal).poses
@@ -39,14 +39,14 @@ def test_astar():
# Plan for same start and goal with path compression enabled
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": True,
}
- astar_planner = PathPlanner("astar", **planner_config)
+ astar_planner = AStarPlanner(**planner_config)
compressed_path = astar_planner.plan(start, goal).poses
diff --git a/pyrobosim/test/navigation/test_prm.py b/pyrobosim/test/navigation/test_prm.py
index 7bd757ea..8f9eae95 100644
--- a/pyrobosim/test/navigation/test_prm.py
+++ b/pyrobosim/test/navigation/test_prm.py
@@ -7,7 +7,7 @@
import pytest
from pyrobosim.core import WorldYamlLoader
-from pyrobosim.navigation import OccupancyGrid, PathPlanner
+from pyrobosim.navigation import PRMPlanner
from pyrobosim.utils.general import get_data_folder
from pyrobosim.utils.pose import Pose
@@ -20,7 +20,7 @@ def test_prm_default():
planner_config = {"world": world}
np.random.seed(1234) # Fix seed for reproducibility
- prm = PathPlanner("prm", **planner_config)
+ prm = PRMPlanner(**planner_config)
start = Pose(x=-1.6, y=2.8)
goal = Pose(x=2.5, y=3.0)
@@ -38,7 +38,7 @@ def test_prm_default():
planner_config = {"world": world}
np.random.seed(1234) # Fix seed for reproducibility
- prm = PathPlanner("prm", **planner_config)
+ prm = PRMPlanner(**planner_config)
start = Pose(x=-1.6, y=2.8)
goal = Pose(x=2.5, y=3.0)
@@ -55,7 +55,7 @@ def test_prm_no_path():
)
planner_config = {"world": world}
- prm = PathPlanner("prm", **planner_config)
+ prm = PRMPlanner(**planner_config)
start = Pose(x=-1.6, y=2.8)
goal = Pose(x=12.5, y=3.0)
@@ -65,23 +65,6 @@ def test_prm_no_path():
assert warn_info[0].message.args[0] == "Could not find a path from start to goal."
-def test_prm_grid_not_supported():
- """Test that PRM is not (yet) supported with occupancy grid maps."""
- world = WorldYamlLoader().from_yaml(
- os.path.join(get_data_folder(), "test_world.yaml")
- )
- robot = world.robots[0]
- planner_config = {
- "grid": OccupancyGrid.from_world(
- world, resolution=0.05, inflation_radius=1.5 * robot.radius
- ),
- }
-
- with pytest.raises(NotImplementedError) as exc_info:
- PathPlanner("prm", **planner_config)
- assert exc_info.value.args[0] == "PRM planner does not support grid based search."
-
-
def test_prm_compress_path():
"""Tests planning with path compression option."""
world = WorldYamlLoader().from_yaml(
@@ -91,7 +74,7 @@ def test_prm_compress_path():
"world": world,
"compress_path": False,
}
- prm = PathPlanner("prm", **planner_config)
+ prm = PRMPlanner(**planner_config)
start = Pose(x=-0.3, y=0.6)
goal = Pose(x=2.5, y=3.0)
@@ -100,7 +83,7 @@ def test_prm_compress_path():
assert len(orig_path.poses) >= 2
np.random.seed(1234) # Use same seed for reproducibility
- prm.planner.impl.compress_path = True
+ prm.compress_path = True
new_path = prm.plan(start, goal)
assert len(new_path.poses) >= 2
assert new_path.length < orig_path.length
diff --git a/pyrobosim/test/navigation/test_rrt.py b/pyrobosim/test/navigation/test_rrt.py
index cc1cff51..9bc9acb1 100644
--- a/pyrobosim/test/navigation/test_rrt.py
+++ b/pyrobosim/test/navigation/test_rrt.py
@@ -7,7 +7,7 @@
import pytest
from pyrobosim.core import WorldYamlLoader
-from pyrobosim.navigation import OccupancyGrid, PathPlanner
+from pyrobosim.navigation import RRTPlanner
from pyrobosim.utils.general import get_data_folder
from pyrobosim.utils.pose import Pose
@@ -23,7 +23,7 @@ def test_rrt_long_distance():
"rrt_connect": False,
"rrt_star": False,
}
- rrt = PathPlanner("rrt", **planner_config)
+ rrt = RRTPlanner(**planner_config)
start = Pose(x=-1.6, y=2.8)
goal = Pose(x=2.5, y=3.0)
@@ -44,7 +44,7 @@ def test_rrt_short_distance_connect():
"rrt_connect": False,
"rrt_star": False,
}
- rrt = PathPlanner("rrt", **planner_config)
+ rrt = RRTPlanner(**planner_config)
start = Pose(x=-1.6, y=2.8)
goal = Pose(x=-1.6, y=3.0)
@@ -64,7 +64,7 @@ def test_rrt_no_path():
"max_time": 0.5, # To make the test fail more quickly.
}
- rrt = PathPlanner("rrt", **planner_config)
+ rrt = RRTPlanner(**planner_config)
start = Pose(x=-1.6, y=2.8)
goal = Pose(x=12.5, y=3.0)
@@ -85,7 +85,7 @@ def test_rrt_bidirectional():
"rrt_connect": False,
"rrt_star": False,
}
- rrt = PathPlanner("rrt", **planner_config)
+ rrt = RRTPlanner(**planner_config)
start = Pose(x=-1.6, y=2.8)
goal = Pose(x=2.5, y=3.0)
@@ -106,7 +106,7 @@ def test_rrt_connect():
"rrt_connect": True,
"rrt_star": False,
}
- rrt = PathPlanner("rrt", **planner_config)
+ rrt = RRTPlanner(**planner_config)
start = Pose(x=-1.6, y=2.8)
goal = Pose(x=2.5, y=3.0)
@@ -127,7 +127,7 @@ def test_rrt_star():
"rrt_connect": False,
"rrt_star": True,
}
- rrt = PathPlanner("rrt", **planner_config)
+ rrt = RRTPlanner(**planner_config)
start = Pose(x=-1.6, y=2.8)
goal = Pose(x=2.5, y=3.0)
@@ -137,23 +137,6 @@ def test_rrt_star():
assert path.poses[-1] == goal
-def test_rrt_grid_not_supported():
- """Test that RRT is not (yet) supported with occupancy grid maps."""
- world = WorldYamlLoader().from_yaml(
- os.path.join(get_data_folder(), "test_world.yaml")
- )
- robot = world.robots[0]
- planner_config = {
- "grid": OccupancyGrid.from_world(
- world, resolution=0.05, inflation_radius=1.5 * robot.radius
- ),
- }
-
- with pytest.raises(NotImplementedError) as exc_info:
- PathPlanner("rrt", **planner_config)
- assert exc_info.value.args[0] == "RRT planner does not support grid based search."
-
-
def test_rrt_compress_path():
"""Tests planning with path compression option."""
world = WorldYamlLoader().from_yaml(
@@ -166,7 +149,7 @@ def test_rrt_compress_path():
"rrt_star": False,
"compress_path": False,
}
- rrt = PathPlanner("rrt", **planner_config)
+ rrt = RRTPlanner(**planner_config)
start = Pose(x=-0.3, y=0.6)
goal = Pose(x=2.5, y=3.0)
@@ -175,7 +158,7 @@ def test_rrt_compress_path():
assert len(orig_path.poses) >= 2
np.random.seed(1234) # Use same seed for reproducibility
- rrt.planner.impl.compress_path = True
+ rrt.compress_path = True
new_path = rrt.plan(start, goal)
assert len(new_path.poses) >= 2
assert new_path.length < orig_path.length
diff --git a/pyrobosim/test/navigation/test_world_graph.py b/pyrobosim/test/navigation/test_world_graph_planner.py
similarity index 88%
rename from pyrobosim/test/navigation/test_world_graph.py
rename to pyrobosim/test/navigation/test_world_graph_planner.py
index 53095361..4c50455b 100644
--- a/pyrobosim/test/navigation/test_world_graph.py
+++ b/pyrobosim/test/navigation/test_world_graph_planner.py
@@ -6,7 +6,7 @@
import pytest
from pyrobosim.core import WorldYamlLoader
-from pyrobosim.navigation import PathPlanner
+from pyrobosim.navigation import WorldGraphPlanner
from pyrobosim.utils.general import get_data_folder
from pyrobosim.utils.pose import Pose
@@ -19,7 +19,7 @@ def test_world_graph_default():
planner_config = {
"world": world,
}
- planner = PathPlanner("world_graph", **planner_config)
+ planner = WorldGraphPlanner(**planner_config)
start = Pose(x=-1.6, y=2.8)
goal = Pose(x=2.5, y=3.0)
@@ -39,7 +39,7 @@ def test_world_graph_short_connection_distance():
"collision_check_step_dist": 0.025,
"max_connection_dist": 1.0,
}
- planner = PathPlanner("world_graph", **planner_config)
+ planner = WorldGraphPlanner(**planner_config)
start = Pose(x=-1.6, y=2.8)
goal = Pose(x=2.5, y=3.0)
diff --git a/pyrobosim/test/planning/test_pddlstream_manip.py b/pyrobosim/test/planning/test_pddlstream_manip.py
index 2b59a303..12112acf 100644
--- a/pyrobosim/test/planning/test_pddlstream_manip.py
+++ b/pyrobosim/test/planning/test_pddlstream_manip.py
@@ -15,7 +15,7 @@
from pyrobosim.core import Robot, World
from pyrobosim.gui import start_gui
from pyrobosim.manipulation import GraspGenerator, ParallelGraspProperties
-from pyrobosim.navigation import ConstantVelocityExecutor, PathPlanner
+from pyrobosim.navigation import ConstantVelocityExecutor, RRTPlanner
from pyrobosim.planning.pddlstream import PDDLStreamPlanner, get_default_domains_folder
from pyrobosim.utils.general import get_data_folder
from pyrobosim.utils.pose import Pose
@@ -88,7 +88,7 @@ def create_test_world(add_alt_desk=True):
"collision_check_step_dist": 0.025,
"max_connection_dist": 1.0,
}
- rrt = PathPlanner("rrt", **planner_config)
+ rrt = RRTPlanner(**planner_config)
robot.set_path_planner(rrt)
return world
diff --git a/pyrobosim/test/planning/test_pddlstream_nav.py b/pyrobosim/test/planning/test_pddlstream_nav.py
index 1c19a619..4fe50813 100644
--- a/pyrobosim/test/planning/test_pddlstream_nav.py
+++ b/pyrobosim/test/planning/test_pddlstream_nav.py
@@ -13,7 +13,7 @@
from pyrobosim.core import Robot, World
from pyrobosim.gui import start_gui
-from pyrobosim.navigation import ConstantVelocityExecutor, PathPlanner
+from pyrobosim.navigation import ConstantVelocityExecutor, RRTPlanner
from pyrobosim.planning.pddlstream.planner import PDDLStreamPlanner
from pyrobosim.planning.pddlstream.utils import get_default_domains_folder
from pyrobosim.utils.general import get_data_folder
@@ -72,7 +72,7 @@ def create_test_world(add_hallway=True):
"collision_check_step_dist": 0.025,
"max_connection_dist": 1.0,
}
- rrt = PathPlanner("rrt", **planner_config)
+ rrt = RRTPlanner(**planner_config)
robot.set_path_planner(rrt)
return world
diff --git a/pyrobosim_ros/CMakeLists.txt b/pyrobosim_ros/CMakeLists.txt
index 02541d1c..ab8548a3 100644
--- a/pyrobosim_ros/CMakeLists.txt
+++ b/pyrobosim_ros/CMakeLists.txt
@@ -10,6 +10,9 @@ find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(pyrobosim_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
# Install examples
install(PROGRAMS
diff --git a/pyrobosim_ros/examples/demo.py b/pyrobosim_ros/examples/demo.py
index e188d369..7e48b9dd 100755
--- a/pyrobosim_ros/examples/demo.py
+++ b/pyrobosim_ros/examples/demo.py
@@ -11,7 +11,7 @@
from pyrobosim.core import Robot, World, WorldYamlLoader
from pyrobosim.gui import start_gui
-from pyrobosim.navigation import ConstantVelocityExecutor, PathPlanner
+from pyrobosim.navigation import ConstantVelocityExecutor, RRTPlanner
from pyrobosim.utils.general import get_data_folder
from pyrobosim.utils.pose import Pose
from pyrobosim_ros.ros_interface import WorldROSWrapper
@@ -92,7 +92,7 @@ def create_world():
"rewire_radius": 1.5,
"compress_path": False,
}
- path_planner = PathPlanner("rrt", **planner_config)
+ path_planner = RRTPlanner(**planner_config)
robot = Robot(
name="robot",
radius=0.1,
diff --git a/pyrobosim_ros/package.xml b/pyrobosim_ros/package.xml
index c6cabfc2..e7b3c883 100644
--- a/pyrobosim_ros/package.xml
+++ b/pyrobosim_ros/package.xml
@@ -17,6 +17,7 @@
rclpy
ros2launch
pyrobosim_msgs
+ std_srvs
ament_cmake_pytest
diff --git a/pyrobosim_ros/pyrobosim_ros/ros_interface.py b/pyrobosim_ros/pyrobosim_ros/ros_interface.py
index 4fa2fce2..2e13f401 100644
--- a/pyrobosim_ros/pyrobosim_ros/ros_interface.py
+++ b/pyrobosim_ros/pyrobosim_ros/ros_interface.py
@@ -25,6 +25,8 @@
)
from pyrobosim_msgs.msg import ExecutionResult, RobotState, LocationState, ObjectState
from pyrobosim_msgs.srv import RequestWorldState, SetLocationState
+from std_srvs.srv import Trigger
+
from .ros_conversions import (
execution_result_to_ros,
path_from_ros,
@@ -58,6 +60,7 @@ def __init__(
* Publish states for each robot on ``/robot_state`` topics.
* Subscribe to velocity commands for each robot on ``/cmd_vel`` topics.
* Allow path planning and following for each robot on ``/plan_path`` and ``/follow_path`` action servers, respectively.
+ * Allow path planner reset on a ``/reset_path_planner`` service server.
* Allow object detection for each robot on a ``/detect_objects`` action server.
* Serve a ``request_world_state`` service to retrieve the world state for planning.
* Serve a ``execute_action`` action server to run single actions on a robot.
@@ -134,6 +137,7 @@ def __init__(
self.robot_state_pub_threads = []
self.robot_plan_path_servers = []
self.robot_follow_path_servers = []
+ self.robot_reset_path_planner_servers = []
self.robot_object_detection_servers = []
self.latest_robot_cmds = {}
@@ -252,6 +256,15 @@ def add_robot_ros_interfaces(self, robot):
)
self.robot_follow_path_servers.append(follow_path_server)
+ # Service server for resetting path planner.
+ reset_path_planner_srv = self.create_service(
+ Trigger,
+ f"{robot.name}/reset_path_planner",
+ partial(self.robot_path_planner_reset_callback, robot=robot),
+ callback_group=ReentrantCallbackGroup(),
+ )
+ self.robot_reset_path_planner_servers.append(reset_path_planner_srv)
+
# Specialized action server for object detection.
object_detection_server = ActionServer(
self,
@@ -286,6 +299,11 @@ def remove_robot_ros_interfaces(self, robot):
plan_follow_server = self.robot_follow_path_servers.pop(idx)
plan_follow_server.destroy()
del plan_follow_server
+ reset_path_planner_server = self.robot_reset_path_planner_servers.pop(
+ idx
+ )
+ self.destroy_service(reset_path_planner_server)
+ del reset_path_planner_server
detect_objects_server = self.robot_object_detection_servers.pop(idx)
detect_objects_server.destroy()
del detect_objects_server
@@ -535,6 +553,33 @@ def robot_path_cancel_callback(self, goal_handle, robot=None):
self.get_logger().info(f"Canceling path following for {robot}.")
return CancelResponse.ACCEPT
+ def robot_path_planner_reset_callback(self, request, response, robot=None):
+ """
+ Resets a robot's path planner as a response to a service request.
+
+ :param request: The service request.
+ :type request: :class:`std_srvs.srv.Trigger.Request`
+ :param response: The unmodified service response.
+ :type response: :class:`std_srvs.srv.Trigger.Response`
+ :return: The modified service response containing the reset result.
+ :rtype: :class:`std_srvs.srv.Trigger.Response`
+ """
+ self.get_logger().info(f"Resetting path planner for {robot}.")
+
+ if not robot.path_planner:
+ message = f"{robot} does not have a path planner. Cannot reset."
+ self.get_logger().warn(message)
+ return Trigger.Response(success=False, message=message)
+
+ robot.path_planner.reset()
+ if self.world.has_gui:
+ show_graphs = True
+ path = None
+ self.world.gui.canvas.show_planner_and_path_signal.emit(
+ robot, show_graphs, path
+ )
+ return Trigger.Response(success=True)
+
def robot_detect_objects_callback(self, goal_handle, robot=None):
"""
Handle object detection action callback for a specific robot.