Add debug output on configuration validity
In PathPlanner::solve, try direct connection before starting extending roadmap.
Call for a roadmap reset if a collision obstacle is loaded