Reformatted to run multiple tests
Three state model with on/off CA. untested.
typo in collision avoidance set goal waypoint
Three state model, two states tested. CA_ON undef.