Added visited set to global_planner A* 

This change was made to prevent loops, to improve speed, and for correctness.

(Thanks to Sachi Hemachandra.)