Improve estimation of VISION_TO_ROBOT_DELAY_S in step_primitive

[mkhlb]

Description of the task

Currently the VISION_TO_ROBOT_DELAY_S in step_primitive is a tuned constant. Given some of the fluctuation in RTT seen at robocup, there is a case to be made to make this dynamic.

Testing would need to be done to confirm the variance of the delay is in real time, and the effects of a mistuned delay in either direction.

Having a better estimate of delay from vision to the robot could allow 2 things:

1. Improvement in motion planning, as the robot will be better synced up to the given motion profile.
2. This could be a cause for inter-robot variance, as delay may vary.
3. If on robot localization is implemented, it could be heavily improved if the delay of vision packets is known, which would also improve motion planning, allowing for more consistent "feedback" control on the robot.

I talked a bit about this with Nima last year, but have done no more than preliminary research, so this is a very open ended problem.

Acceptance criteria

• [ ] Test the effects of different values of the current constants.
• [ ] Test the variability of delay for packets to arrive to robots (can be estimated by RTT?)
• [ ] In thunderloop, when a packet is recieved, it includes a vision age, i.e. the estimated time since the vision snapshot happened.

Blocked By

[nimazareian]

To test this as well, you could also consider using tools that allow you to simulate different network conditions (e.g. add extra latency, add extra packet loss, etc.) to make sure that the dynamic latency value gets updated quickly and is accurate.

Also, another approach to measuring latency could be using Network Time Protocol (NTP, maybe there are other protocols as well) to sync the AI computer's clock with the Pi's clock to some variance (hopefully fairly low). Then including the time a primitive was sent in the packet and using the synchronized clock on the Pi to measure how long it took to be transmitted. The benefit of this over using RTT is that you know the latency (up to some precision) for the current packet, instead of using RTT of the previous packets to estimate the latency of the current packets. Though worth noting that I've never tried this approach and don't know how good it is, but in theory it seems sound and is probably worth the investigation.