Test whether we have reached the waypoint with distance
Test whether we have reached the waypoint with distance

Previously we just checked whether the angle had increased beyond our
tolerance; however, this lead to significant overshoot in the physical
rovers (as noted in #58).

BCLab-UNM/SwarmBaseCode-ROS,BCLab-UNM/SwarmBaseCode-ROS,BCLab-UNM/SwarmBaseCode-ROS,BCLab-UNM/SwarmBaseCode-ROS,BCLab-UNM/SwarmBaseCode-ROS,BCLab-UNM/SwarmBaseCode-ROS