// If it is already in the waypoint set, then
// don't bother adding it again
if(waypoint_set.find(waypoint)!=waypoint_set.end())
// If it is already in the waypoint set, then
// don't bother adding it again
if(waypoint_set.find(waypoint)!=waypoint_set.end())