rewrite lg_nav_to_device in c++ (or optimize the current python code)

[minshallj]

There's a bit of a delay after letting go of the spacenav where earth is still rotating. Looking at the ros topics, it looks like there's only 0's in the twist data, so it's likely coming from the translation of twist to device events in lg_nav_to_device.