[JTC] Angle wraparound for first segment of trajectory

[christophfroehlich]

I experienced a problem with the angle_wraparound of continuous joints if the current position and the first point of the trajectory have an offset of 2*pi.

I forgot that case when I implemented angle_wraparound, and saw now that this was already part of ROS 1. So I ported the existing implementation and updated the tests (I think now that they were wrong before anyways).

This wraparound of state_before_traj_msg_.positions could also be done inside the update method of the controller instead of the trajectory class (avoiding ABI break). Comment if you think it should be changed.

I added new tests for the trajectory class to check if wraparound_joint works. position_error_not_angle_wraparound and position_error_angle_wraparound could also be improved, I opened #802

[codecov]

Codecov Report

Attention: 18 lines in your changes are missing coverage. Please review.

Comparison is base (9f7e9e9) 47.71% compared to head (d716819) 71.75%.
Report is 19 commits behind head on master.

Additional details and impacted files

@@             Coverage Diff             @@
##           master     #796       +/-   ##
===========================================
+ Coverage   47.71%   71.75%   +24.03%     
===========================================
  Files          41       41               
  Lines        3871     3639      -232     
  Branches     1833     1780       -53     
===========================================
+ Hits         1847     2611      +764     
+ Misses        751      709       -42     
+ Partials     1273      319      -954     

Flag	Coverage Δ
unittests	71.75% <73.13%> (+24.03%)	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files	Coverage Δ
...de/diff_drive_controller/diff_drive_controller.hpp	100.00% <ø> (ø)
...iff_drive_controller/src/diff_drive_controller.cpp	68.63% <100.00%> (+20.57%)	⬆️
...jectory_controller/joint_trajectory_controller.hpp	100.00% <ø> (+20.00%)	⬆️
...include/joint_trajectory_controller/trajectory.hpp	73.33% <ø> (+23.33%)	⬆️
...nclude/tricycle_controller/tricycle_controller.hpp	100.00% <ø> (ø)
...roadcaster/src/force_torque_sensor_broadcaster.cpp	82.53% <75.00%> (+21.93%)	⬆️
...ory_controller/src/joint_trajectory_controller.cpp	80.45% <87.50%> (+33.33%)	⬆️
joint_trajectory_controller/src/trajectory.cpp	89.67% <77.77%> (+22.10%)	⬆️
tricycle_controller/src/tricycle_controller.cpp	60.94% <60.60%> (+28.29%)	⬆️

... and 28 files with indirect coverage changes

[bmagyar]

I think it's more important to keep Trajectory class consistent than avoiding ABI breakages.
Also... better to not load more things into update()

I'm in favour of this approach

[christophfroehlich]

@bmagyar do you agree with the changes now and might want to approve it?

[mergify]

This pull request is in conflict. Could you fix it @christophfroehlich?

[destogl]

Looking good! Thanks

[bmagyar]

Very good! I only split the tests into the separate cases