refactor: Return angle between 0 and 2pi
refactor: Return angle between 0 and 2pi

Consider cases when angle is greater than 2pi and less than -2pi, and when it is exactly 2pi.
bit0001/trajectory_tracking,bit0001/trajectory_tracking