Not adding new variable
for use in asl still fixing RRT
fixed plotting problem in windows. It is not using helper now but will be changed again later
plotting fixed. If there are concern about RRT, just set max_angleto 6.28 and it should work
when next region is the same as current regions, arrived is true. redo the Tree when the next regions changes
modify to work with Pioneer