ROS IMU timing quality issues on RB5
-
Hello,
We’ve been trying to use voxl-mpa-to-ros on our RB5 to feed IMU and imagery to a couple visual-inertial odometry algorithms we’re evaluating. We’ve noticed a few issues that are impacting the quality of IMU data in ROS:
- IMU dropouts
- IMU “chunking”
- IMU header timestamp drift
All of these issues are captured in the plot below, which compares IMU header stamp and ROS record stamp (effectively the time at which data is arriving to subscribers) versus the IMU sequence number.
- Dropouts make the average IMU rate appear to be 220Hz (this matches Jagatpreet’s observation), which equates to ~78% of IMU droput in ROS. We’ve been able to mostly get around this by increasing the queue size dramatically (from 1 to 100), but still seeing a lost sample here and there.
- The “chunking” issue appears to cause bursts of IMU data to hit ROS subscribers in quick succession, periodically. We would really prefer to see more temporally uniform data transmission, even if header stamps are accurate.
- We aren’t sure the cause of the backwards time shift in IMU header stamps that happens at ~2.81 seconds in the plot. We’ve reproduced this with internet / wifi off, only connected to the RB5 via ADB. We have confirmed that this is indeed caused by ROS wall time shifting, but again, unsure how or why.
Attached is the plot after increasing the queue size (from 1 to 100):
(Note that we've also included monotonic time here to further highlight the time shift at ~0.3 seconds.)
We’re not sure if increasing the queue size fixed the data dropouts issue because:
- The ROS publisher queue fills up in quick bursts when IMU messages are copied out of the the pipe, and the ROS publisher queue can’t keep up with size of 1 (we haven’t investigated this yet, but a comparison of monotonic time when the IMU is added to the pipe vs when it is popped from the pipe should be telling)
- ROS subscribers can't keep up with the 1Khz publish rate of the IMU
- Possibly some combination of these
In any case, increasing the queue size really helps with the dropouts.
We could really use some help answering the following questions:
- What can we do to improve uniformity of IMU message throughput from voxl-mpa-to-ros to ROS subscribers (to better match the time interval associated with monotonic time)? Possible solutions might include integrating voxl-mpa-to-ros as a nodelet, but if chunking is caused by something else before prior to getting IMU data out of the pipe, this probably wouldn’t help much.
- What can we do to resolve the apparent time reversal on the RB5? We're a bit confused here, since the data in the plot above was supposedly captured without internet. If the issue is due to some kind of time sync, we would ideally like to perform an initial time sync, then disable subsequent syncing and apply an EMA filter ROS time offsets to determine IMU header stamps. Or, publish a clock from voxl-to-mpa-ros and use this for ROS time (assuming that drift is low over relatively short time intervals). Anything to stop this, haha.
Any feedback would be greatly appreciated!
-
can you please provide the software version? Do you have voxl-inspect-cpu, and if so, can you please provide the output? It sounds like you are starving the voxl-mpa-to-ros of CPU resources to empty the queue
-
Hi, I'm on the same team as @eric.
Here's the output of
voxl-version:-------------------------------------------------------------------------------- system-image: 1.4.1-M0052-14.1a-perf kernel: #1 SMP PREEMPT Thu Oct 13 18:59:44 UTC 2022 4.19.125 -------------------------------------------------------------------------------- hw version: M0052 -------------------------------------------------------------------------------- voxl-suite: -------------------------------------------------------------------------------- [1mPackages:[0m Repo: http://voxl-packages.modalai.com/ ./dists/qrb5165/sdk-0.9/binary-arm64/ Last Updated: 2023-08-18 19:06:41 List: libmodal-exposure 0.0.7 libmodal-journal 0.2.1 libmodal-json 0.4.3 libmodal-pipe 2.8.2 libqrb5165-io 0.1.0 libvoxl-cci-direct 0.1.5 libvoxl-cutils 0.1.1 mv-voxl 0.1-r0 qrb5165-bind 0.1-r0 qrb5165-imu-server 0.5.0 qrb5165-slpi-test-sig 01-r0 qrb5165-system-tweaks 0.1.5 qrb5165-tflite 2.8.0-2 voxl-bind 0.0.1 voxl-camera-server 1.3.5 voxl-cpu-monitor 0.3.0 voxl-docker-support 1.2.4 voxl-gphoto2-server 0.0.10 voxl-jpeg-turbo 2.1.3-4 voxl-libgphoto2 0.0.4 voxl-libuvc 1.0.7 voxl-logger 0.3.4 voxl-mavlink 0.1.0 voxl-mavlink-server 0.3.0 voxl-modem 0.16.1 voxl-mongoose 7.7.0-1 voxl-mpa-to-ros 0.3.6-202308232023 voxl-mpa-tools 0.7.6 voxl-portal 0.5.0 voxl-px4 1.12.31 voxl-px4-imu-server 0.1.2 voxl-qvio-server 0.8.2 voxl-remote-id 0.0.5 voxl-streamer 0.4.1 voxl-utils 1.2.2 voxl-uvc-server 0.1.3 voxl-vision-px4 1.4.1 --------------------------------------------------------------------------------Here are screenshots from voxl-inspect-cpu and top. They are made while running voxl-mpa-to-ros and
rosbag record /imu_px4.
Here's the output of voxl-inspect-services:
-
@dscott Can you please try updating to SDK 0.9.5? https://developer.modalai.com/asset/download/124
That 0.9 software version is pretty old
-
I reflashed using the linked image. voxl-mpa-to-ros appears to behave the same.
Here's all the same information as before. Like before, voxl-mpa-to-ros and
rosbag record /imu_px4were the only other things running that were not ran automatically.-------------------------------------------------------------------------------- system-image: 1.4.1-M0052-14.1a-perf kernel: #1 SMP PREEMPT Thu Oct 13 18:59:44 UTC 2022 4.19.125 -------------------------------------------------------------------------------- hw version: M0052 -------------------------------------------------------------------------------- voxl-suite: 0.9.5 -------------------------------------------------------------------------------- [1mPackages:[0m Repo: http://voxl-packages.modalai.com/ ./dists/qrb5165/sdk-0.9/binary-arm64/ Last Updated: 2023-08-25 17:16:14 List: libmodal-cv 0.2.3 libmodal-exposure 0.0.7 libmodal-journal 0.2.1 libmodal-json 0.4.3 libmodal-pipe 2.8.2 libqrb5165-io 0.1.0 libvoxl-cci-direct 0.1.5 libvoxl-cutils 0.1.1 mv-voxl 0.1-r0 qrb5165-bind 0.1-r0 qrb5165-dfs-server 0.1.0 qrb5165-imu-server 0.5.0 qrb5165-slpi-test-sig 01-r0 qrb5165-system-tweaks 0.1.5 qrb5165-tflite 2.8.0-2 voxl-bind 0.0.1 voxl-camera-calibration 0.2.3 voxl-camera-server 1.3.5 voxl-cpu-monitor 0.3.0 voxl-docker-support 1.2.4 voxl-gphoto2-server 0.0.10 voxl-jpeg-turbo 2.1.3-4 voxl-libgphoto2 0.0.4 voxl-libuvc 1.0.7 voxl-logger 0.3.4 voxl-mavlink 0.1.0 voxl-mavlink-server 0.3.0 voxl-modem 0.16.1 voxl-mongoose 7.7.0-1 voxl-mpa-to-ros 0.3.6 voxl-mpa-tools 0.7.6 voxl-opencv 4.5.5-1 voxl-portal 0.5.0 voxl-px4 1.12.31 voxl-px4-imu-server 0.1.2 voxl-qvio-server 0.8.2 voxl-remote-id 0.0.5 voxl-streamer 0.4.1 voxl-suite 0.9.5 voxl-tag-detector 0.0.4 voxl-tflite-server 0.3.1 voxl-utils 1.2.2 voxl-uvc-server 0.1.3 voxl-vision-px4 1.4.0 --------------------------------------------------------------------------------
-
@dscott A couple of ideas
- use voxl-logger to log the IMU data and make sure there are no missing data points. We don't see dropped packets in our system, but we don't use ROS too frequently
- increase the server's FIFO pipe size. Maybe change the code here
from
info.size_bytes = IMU_RECOMMENDED_PIPE_SIZE;to
info.size_bytes = IMU_RECOMMENDED_PIPE_SIZE * 4; -
@Moderator Sorry if it wasn't clear, but we have been using voxl-logger and haven't seen any of these issues there. I believe the "monotonic stamp" data on the second plot is taken from collecting data through both voxl-logger and voxl-mpa-to-ros at the same time.
From what I understand, is it not the same server provides the pipes that both voxl-logger and voxl-mpa-to-ros use?
-
@dscott Indeed, the monotonic plot is displaying data captured from running voxl-logger in parallel with voxl-mpa-to-ros, after adjusting for initial offset between the realtime (ROS) clock and monotonic clock.
@Moderator Here is a plot showing dt from IMU data captured by voxl-logger. As you can see, there are a few outliers and a slight bias (making actual rate closer to 950Hz instead of 1KHz), but this is not causing any of the issues we reported in the original message when subscribing to the ROS IMU messages on the RB5.
-
@eric yes, it's the same datasource. So, something in ROS is causing the delay. did you try increasing the FIFO size since ROS is slow to drain the FIFO?
-
@Moderator Do you mean on the voxl-imu-server side? We've confirmed that data is not being dropped before getting to ROS.
In voxl-to-mpa-ros, IMU data is read from the pipe from imu-server, then published to ROS. We observed that IMU messages are read from the pipe in batches of about 10 or so. This explains why data was being dropped in ROS:
- A group of 10 or so IMU messages were read from the IMU pipe and placed inside a ROS publisher (with a queue size of 1).
- ROS subscribers can't copy data from the ROS queue faster than imu messages are copied into the queue.
However, we have already addressed this specific issue as described in the original post. We are not dropping any data after increasing the ROS publisher queue size in voxl-to-mpa-ros.
We are still having issues with latency on the ROS network, and "time travel" due to ROS usage of the realtime clock (instead of monotonic clock), and could use some help to resolve these issues, as described in the original post (refer to the second plot).
Apologies if I am misunderstanding your request
-
@Moderator We've determined that the issue where time appears to be jumping backwards in ROS is actually caused by the system clock - see https://forum.modalai.com/topic/2612/rb5-system-clock-issues
This is a big issue and I hope that we and the rest of the community can get some support.
Regarding rosbag record latency, we've determined that a lot of the latency seems to be attributable to the recording utility we are using vs the ROS network as a whole. We did some experiments using ROS subscribers to directly measure latency (writing results to CSV), and performance seemed much better than indicated by rosbag timestamps.
-
@eric Maybe the ROS timer can be changed to using the monotonic clock?
Regarding dropped IMU samples, these folks reduced the publishing rate and it seemed to help https://forum.modalai.com/topic/2570/imu_apps-seems-to-be-publishing-in-bursts/4
-
@Moderator Unfortunately, there is not a way to configure ROS1 to use steady_clock instead of system_clock. In ROS2, it seems that this is configurable, but ModalAI does not support ROS2.
