Finally have the a_star algorithm / controller working again. A much cleaner implementation too! Can no longer draw detected obstacles easily, since they are now stored in the a_star planner, which does no drawing.
Fixed tracking (red dots on followed path). Improved the A* controller to only replan if a new obstacle was added. Still have many problems.
Split the a_star planner into two types; one with vfh as the local navigator and the other with the potential controller (doesn't work!). vfh_a_star and wavefront are both working fairly well.