[jtc] Improve trajectory sampling efficiency #1297
Open
+137
−4
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
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, theTrajectory
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 betweenwrite()
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.