While I test the UGV rover, I notice that current master version fails to conduct a planned mission.
I am not sure whether this issue is known or not. Just in case, I would like to mention this issue in here.
The mission consists of three waypoints and return to launch. The rover just keeps changing yaw angle and then it leads to the fail-safe.