Rotate step if current yaw orientation is bigger than tolerance

[rafaelbarretorb]

Rotate before following the trajectory if the current orientation of the robot is bigger than a yaw tolerance in relation to the trajectory direction. The current solution is appropriate only if the robot is already correctly oriented to the trajectory direction.