Different first choice for strafing based on dir
Moved costmap checker out of state machine