I know it's far from optimal, but we only use it very seldom. Once for every joint movement test.

Oddly enough ROS doesn't have a stable API to get this information, and for C++ not even an unstable API.