Add modular planner interface.

[ibrahiminfinite]

This PR adds a new planner interface that makes it easy to integrate and use new planners in pyrobosim.
The new interface does not break any existing pipelines as the APIs and behaviours remain the same.

Will close #81, #15

TODO :

• Add planner interface
• Planner base class
• A* planner implementation (grid)
• A* planner implementation (graph)
• RRT implementation (graph)
• Fix RRT connection issue
• PRM implementation (graph)
• Add method to print metrics
• Support displaying the planner graph
• Add Waypoint reduction for planners
• Add demo for new planner interface
• Update YAML loader to use new planner interface. (To be implemented after API unification PR is merged)
• Refactor search_graph and cascading changes.
• Add documentation for how to use the interface to implement new planners.

Edit : Add graph_from_world method (No planners currently need this, postponing till this is needed)

NOTE: Might break the pipeline when loaded as the "world" global planner. However I am not sure if the world should have a planner as the "planning" is a behaviour of the robot and so, it makes sense for only robots to have planners. Might be a good idea for a robot to have both a global and local planner , instead of the global planner being a part of the world.

[ibrahiminfinite]

However I am not sure if the world should have a planner as the "planning" is a behaviour of the robot and so, it makes sense for only robots to have planners. Might be a good idea for a robot to have both a global and local planner , instead of the global planner being a part of the world.

@sea-bass , I would like to know your opinion on this.

[sea-bass]

I think separating planners from the world is the way to go.

At some point I thought, why not just have 1 planner that multiple robots can call? But then if they're different sizes or have different dynamics, that doesn't reallt work.

Also, this tool's original definition of "global" vs "local" is different from what it typically means. All the planners available now are technically global planners, in that they use knowledge about the world to plan the whole path.

So go with your idea!

[ibrahiminfinite]

If we are proceeding with the "no world planner" idea. There are some changes that will need to be made.

1. We no longer need to build the world graph during world construction. It can be stripped out into the a separate method in (should be renamed to something else) search_graph.py as graph_from_world, similar to occupancy_grid_from_world.
2. The SearchGraph no longer needs to have any search functionality within it, and can just be a simple graph data structure without any dependencies , and can be reused easily elsewhere.
3. The planners would then take in the Graph instead of the World similar to OccupancyGrid , hence decoupling the planning process from the World. Resulting in much cleaner implementations for the planner, as each planner can then just have a Grid or Graph version without dependency on the World object.

It might also be good for occupancy_grid_from_world and graph_from_world to be classmethods of OccupancyGrid and WorldGraph instead of free functions.

What do you think @sea-bass

[sea-bass]

The above all seems reasonable -- thanks!

[sea-bass]

I just tested again and found some more bugs.

First, I realized that the robot orientation doesn't actually change with the A* paths. Maybe the fill_yaws() call got lost?

Also, if the robot is at counter0_right and you hit "Pick", it seems we're still looking for objects in counter0_left, which seems to be a bug.

When I start the multirobot demo, navigating with the PRM is animating so slow it never re-renders. Are the graph artists getting re-displayed constantly or something?

Also in the multirobot demo, when I switch from the PRM robot robot1 to the RRT robot robot2 and then switch back out, the graph doesn't get cleared and the old RRT keeps showing up forever. I think the graph needs to get cleared out when robots/planners are switched. Added a comment in the UI code which I think will fix that.

[ibrahiminfinite]

Also, if the robot is at counter0_right and you hit "Pick", it seems we're still looking for objects in counter0_left, which seems to be a bug.

Is that not the expected behaviour that we talked about yesterday ?
Or do you mean if it is at counter0_right and we hit Pick , it should only look for objects in counter0_right?

When I start the multirobot demo, navigating with the PRM is animating so slow it never re-renders. Are the graph artists getting re-displayed constantly or something?

I did not face any issues with the multirobot PRM.
Can you check with the latest changes and let me know ?

[sea-bass]

Or do you mean if it is at counter0_right and we hit Pick , it should only look for objects in counter0_right?

Exactly this!

[ibrahiminfinite]

I think the reason for high rendering (and possibly search ) times is because every edge is getting added twice now.
Edge(A, B) and Edge(B, A) while these are conceptually same, the current logic does not check if one or the other exist and restrict a redundant adding of edges.

pyrobosim/pyrobosim/pyrobosim/navigation/world_graph.py

Line 75 in a7afed2

def add_edge(self, nodeA, nodeB):

EDIT : Looks like the docstring need to be updated here since it is still referring to 'search_graph`

[ibrahiminfinite]

This was already there , but I don't think these Node object comparisons actually work since there is no __eq__ or __hash__ operator defined.
Even if 2 nodes had the same attributes the nodes would be reported as not equal.
Is there something I am missing here ? @sea-bass

[sea-bass]

I dug into that issue about the counter and object resolution and am going to say this is working as intended, so no need to look at that further. It's fine, and not a change introduced by your updates at all.

In testing, I found an issue that the A* planner always started and ended with zero yaw because the actual start/goal orientations were fully discarded. So I fixed that plus some other minor stuff.

I just have 1 last major comment about removing the remove_edge() function in the graph class. Once that is resolved, should be good.

[sea-bass]

I went ahead and fixed all the remaining things I found, so LGTM.

@ibrahiminfinite let me know if you maybe wanna do your own round of final testing and then we can get this in.

Thanks for the work on this one! Big improvements to planners, for sure.

[ibrahiminfinite]

It's fine, and not a change introduced by your updates at all.

I was wondering what unit tests I should put in to make sure this was not breaking anything.
Since the issue was not introduced from this PR.
We can get this in and add unit tests for small utility functions like entity_from_query etc as the next step.

Thanks for guiding me !!