Small cleanup of nao_path_follower, fix potential problems with std::distance
and angular distance (+-)