Added goal checking and stopping once reached goal.
Added red lines to show path taken after goal found changed the part in buildRRT that was buggy
Authored by: twiske 2012-03-13
Parent: [r20]
Child: [r22]
