[Navigation][Trajectory] solve the discontinued problem of yaw rotation

[Liyunong20000]

What is this

The yaw angle has the discontinued problem around PI. This PR solve the wrong rotation path in trajectory generation, that is, the trajectory must choose the shortest path.

Details

This is achieved by checking diff angle with previous yaw angle, and add an offset to remove a big gap

if (diff > M_PI) yaw_angle -= 2 * M_PI;
if (diff < -M_PI) yaw_angle += 2 * M_PI;

[tongtybj]

@Liyunong20000

Have you confirmed that this patch solve your problem with the real robot?

[tongtybj]

@Liyunong20000

Have you reproduced the corner case?

[sugikazu75]

@Liyunong20000
FYI: You can use angles::normalize_angle https://docs.ros.org/en/jade/api/angles/html/namespaceangles.html#a90ae8baf394c3ce775a3a14e48c7e7be

[tongtybj]

@sugikazu75

Thx for your suggenstion!
That's the case for https://github.com/jsk-ros-pkg/jsk_aerial_robot/pull/634/files#diff-9c1ce395efa7ef3a077e6e1471de383649423c7453dd7c227c41adf5fd490eb8R1006-R1007.

But for https://github.com/jsk-ros-pkg/jsk_aerial_robot/pull/634/files#diff-c6a4f3e29ec6e7e00ea548e81f405d43d9fe45cd6776cf42c285133d81122b06R124-R125,
the purpose is now to normalize yaw_angle, but to find the closest value from the previous yaw angle. So the modified yaw_angle might be out of the range of (-pi, pi).

[Liyunong20000]

@tongtybj @sugikazu75
Thanks for your help!
This problem have been solved!