Unify SP obstacle margins and set configuration fallback to 0.2 m
Reset SP planning start after FSM-confirmed standstill