Check extended

Fixed map
Updated node XY and Pose getting methods to make sure that they are
consistent with eachother
Updated RRTTest to match new changes
Replaced RRTRobot.backLen with radius