Revision history [back]

You can definitely visualize all of the above using Rviz, using nav_msgs/Path and nav_msgs/OccupancyGrid outputs from your algorithm.

If you algorithm fits into the move_base state machine approach, it may be best to develop your planner as a plugin (probably for the local planner). If not, you'll probably still find reusing layered costmaps as a very helpful for sensor processing and world state.