multisense joint state runaway

[Stephen Hart]

In monitoring the /multisense/joint_states topic, it seems like the position data is drifting dramatically over time even though the head is not moving in the sim.

Here's an rxplot screenshot over about a 5 minutes time period:

Unless i'm confused about something I would expect 1) this data to be the neck tilt data, and 2) the value to stay within reasonable bounds in radians (whatever the RoM of the neck is).

[btshrewsbury NA]

/multisense/joint_states should only contain the spindle q. I believe it is always spinning as we have an internal controller that we haven't exposed set to a constant velocity. We can wrap the q which should fix the runaway. I'll look into both the constant velocity and the wrap bug

[Stephen Hart]

@btshrewsbury ah, sorry i thought it was neck q, but obviously this is spindle q. Regardless, the accumulation should probably be turned into a wrapped number. Thanks!