Add local minima detection routine

[gerth2]

It's quite possible we'll change the base map params in the future (different obstacle size, strenght... differnet goal strenght)

Add logic to detect and warn about local minima the robot coudl get stuck in.

[gerth2]

One way to do this: write a unit test case that does something like this:

for all goals:
   for a grid of X/Y start positions:
       set pose = start poisition
       while(pose not at goal):
           pose = pathplanner.update(curPose)
           if(too many steps):
                 Fail
Pass

This will be a long testcase, but it can exhaustively show that we can plan from any start spot to any goal without getting stuck.