typo in collision avoidance set goal waypoint
Three state model, two states tested. CA_ON undef.