Return zero velocity command
Add small delta yaw condition to replan point selection

Replan point should be far enough ahead and zero curvature to ensure stitching
Simplify search for replan pose in replanned path

Keep interator into last_path_msg around so we don't have to search
again when a new plan arrives.
Add return statement to scurve
Try again to get submodules file correct