Better trajectory republishing

[niko-holmes]

Proposed changes

Improved the republish_trajectory tool to be more robust to variance in message publishing timings.

The tool is intended to republish the trajectory command msgs being played from a bag file with the timestamp of all commands shifted to be in the present.

The timestamps are shifted to be 10ms in the future from when they are republished to ensure they never arrive late at the trajectory controller.

The old version of the republish node used the time of msg receipt as the new timestamp for all trajectory commands; a method which works but is susceptible to timing variance.

The new version of the node records the msg timestamp and receipt time of the very first message and uses those values to consistently time-shift all msgs. The command msgs are shifted an additional 10ms into the future to ensure they always arrive at the trajectory controller before they are meant to be executed.

Checklist

Before posting a PR ensure that from each of the below categories AT LEAST ONE BOX HAS BEEN CHECKED. If more than one category is applicable then more can be checked. Also ensure that the proposed changes have been filled out with relevant information for reviewers.

Tests

• No tests required to be added. (For small changes that will be tested by CI/CD infrastructure).
• Added/Modified automated and PhantomHand CI tests (if a new class is added (Python or C++), the interface of that class must be unit tested).
• Manually tested in simulation (if simulation specific or no hardware required to test the functionality).
• Manually tested on hardware (if hardware specific or related).

Documentation

• No documentation required to be added.
• Added documentation (For any new feature, explain what it does and how to use it. Write the documentation in a relevant space, e.g. Github, Confluence, etc).
• Updated documentation (For changes to pre-existing features mentioned in the documentation).

[chrisw-shadow]

jolly good pr

[dg-shadow]

if this is true, the sleep will never get hit, so the loop will still just run at full speed.

[niko-holmes]

True, 20ae3b9