[jtc] Improve trajectory sampling efficiency (backport #1297)

[mergify]

This PR addresses the increasing effort required in Trajectory::sample() raised in #1293 by exploiting the monotonically increasing access time for any trajectory. Instead of scanning through the complete trajectory to find the relevant trajectory points for the current sampling time, the Trajectory class keeps track of the last accessed index and only searches from there on. This supersedes the proposed binary search in #1294 and is based on #474.

Based on the requests in #474, i added checks for the new trajectory state to existing tests and added a new test to make sure that the index is correctly reset when calling Trajectory::update(). I verified that this indeed solves the problem by re-running the trajectory from #1293 and again plotting the periods between write() calls in my hardware interface:

The upper plot shows the progression of the period before this change, the lower one the behavior wtih this PR applied. The large spikes at the beginning and end are the start and end of the trajectory, the spikes in the middle are probably a result of me testing this on my normal laptop without RT kernel and doing some things on the side.

---

This is an automatic backport of pull request #1297 done by Mergify.

[christophfroehlich]

CI fails because of #1354

[codecov]

Codecov Report

Attention: Patch coverage is 44.08602% with 52 lines in your changes missing coverage. Please review.

Project coverage is 64.94%. Comparing base (fd8b434) to head (b2f4da7).
Report is 1 commits behind head on iron.

Files with missing lines	Patch %	Lines
...int_trajectory_controller/test/test_trajectory.cpp	40.90%	0 Missing and 52 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             iron    #1358      +/-   ##
==========================================
- Coverage   65.08%   64.94%   -0.15%     
==========================================
  Files         107      107              
  Lines       12191    12283      +92     
  Branches     7498     7576      +78     
==========================================
+ Hits         7935     7977      +42     
+ Misses       1540     1534       -6     
- Partials     2716     2772      +56     

Flag	Coverage Δ
unittests	64.94% <44.08%> (-0.15%)	⬇️

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

Files with missing lines	Coverage Δ
...include/joint_trajectory_controller/trajectory.hpp	76.47% <100.00%> (+1.47%)	⬆️
joint_trajectory_controller/src/trajectory.cpp	90.40% <100.00%> (+0.14%)	⬆️
...int_trajectory_controller/test/test_trajectory.cpp	63.27% <40.90%> (-3.78%)	⬇️

... and 2 files with indirect coverage changes