Increased the tolerance to 0.03 meters.
Changed the reference frame to base_link
Modify pathnodes.