Add mechanism to reset multi-query and occupancy grid based path planners

[sea-bass]

When we modify the world, single-query path planners like RRT can just grab that new information no problem.

However, in the case of our PRM planner, or occupancy-grid based planners, this data must be regenerated to keep track of changes in the world.

It's okay if this ends up being an explicit action that the robot must take, which means:

• All planners must have such a reset interface, whether it be roadmap or grid based planners.
• There should be actions (with the Python API, ROS Interface, and GUI) to do these resets.
• We'll likely need to backpedal on some of the "factory" and "PIMPL" idioms that were stood up for path planning, which IMO are overkill for this tool.

[ibrahiminfinite]

Looks like something I can look into 👍

[ibrahiminfinite]

Were there any specific issues faced due to the factory pattern or is it just that it adds more complexity without much returns for it ?

[sea-bass]

Looks like something I can look into 👍

Cool! The motivation is that now you can magically close a hallway door mid-navigation and have the path execution stop due to impending collision. That all works.

But, to correctly replan with the new obstacle, depending on the planner you need to regenerate the PRM and/or pass in an updated occupancy grid. RRT still works fine because it just uses the latest world info in subsequent plans.

---

Were there any specific issues faced due to the factory pattern or is it just that it adds more complexity without much returns for it ?

I think it's mostly the complexity for what's supposed to be an educational tool... but I do like at least having all the common visualization code in a base class.

Haven't thought this through very much, but my idea is maybe use the abc module to create an abstract class that holds this common visualization stuff, and then every other planner just directly inherits from that and implements the methods dictated by the abstract base class.

Any code that needs to be reused beyond that can just be a free function from utilities, maybe?

---

As another upside, there are now a LOT more tests since last time the path planners were touched :)

[ibrahiminfinite]

Yeah, you are right about the "educational" tool part.
I did like the factory patterns flexiblity, but for an educational tool, that probably just increases the barrier for people since they have to understand the pattern first.
Will change it to use a simple inheritance pattern 👍

[ibrahiminfinite]

@sea-bass
The PRM already have the reset implemented

PRM:

pyrobosim/pyrobosim/pyrobosim/navigation/prm.py

Line 48 in e8591ec

def reset(self):

Looks like only the A-start planner does not have the reset

[sea-bass]

Right. And I think the reset interface for an occupancy grid based planner like our A* implementation might make things trickier, because right now that grid is a constructor argument in the planner.

[ibrahiminfinite]

What if we just pass in the world like for PRM and compute the occupancy grid inside the planner ?
That would allow us to recompute the grid from world from within reset using the world file.

[sea-bass]

What if we just pass in the world like for PRM and compute the occupancy grid inside the planner ? That would allow us to recompute the grid from world from within reset using the world file.

I like this, it gets away from having to contextually figure out whether your kwargs have a "world" or "grid" entry (but not both!), and replace it with something simpler.

EDIT: I think there was probably a design choice we considered earlier, that we shouldn't store the occupancy grid in the planner or in the robot, because what if you have multiple robots sharing the same occupancy grid? But I think that's maybe premature optimization especially since this is not a nav framework.

[ibrahiminfinite]

Ok lets try doing it that way , it would also simplfy things (get rid of some code) during world creartion from yaml in the get_local_path_planner

[ibrahiminfinite]

I just remembered once of the main considerations for why we did it the current way
It was because each robot could have different grid params, like inflation, resolutions etc.

To address this in our new design, we could have an optional grid_params (dict) argument for grid based planners.
For the default values we could have resolution set as 0.05 , any recommened default for inflation radius ?

[sea-bass]

Right, exactly that. I think the default inflation radius should be None, and then the code can do something like

if inflation_radius is None:
    self.inflation_radius = robot.radius

or as I learned you can do in newer Python versions:

self.inflation_radius = inflation_radius or robot.radius

this should be fine because the planner is linked to a robot itself?

[ibrahiminfinite]

this should be fine because the planner is linked to a robot itself?

Planner has no information about the robot right now (by design)
Maybe we should standarzie passing in the robot instance to the planner?
Since planners like Reeds-Shepp would require information such as the turning radius of the robot

[sea-bass]

Maybe we should standarzie passing in the robot instance to the planner?

Makes sense... and since robots themselves have a world attribute, you can just swap those arguments out :)

That also gives you the benefit of raising errors if you try to run a planner that needs the robot to have a world connected, but there is no world attribute?

[ibrahiminfinite]

Yes, just tried out the approach on the Astar and tested using demo file.
Works nicely !

[ibrahiminfinite]

For grid vs graph planner we can provide a more meaningful use_gird argument with default value set accroding to context.
Eg: use_grid=True for A-star but use_grid=Fal)se for RRT by default ( can be changed if we end up having a grid based RRT later)

[ibrahiminfinite]

Another simplification we could do is to split the grid and graph implementations completely into separate files and have the user specify the type explicitly using naming like rrt_grid or rrt_graph. this would allow us to keep the clean factory interface while getting ridding of :

1. The pimpl pattern
2. Ambiguity in specifying the planner type (grid vs graph)