plot_imu_data_rate.py

#!/usr/bin/env python

# Plot in real time the data rate of the IMU sensor messages.
# This script was written in order to debug the IMU not outputting messages at
# the configurade rate of 200Hz.

import rospy
from sensor_msgs.msg import Imu

import numpy as np
import pandas as pd
import matplotlib.animation as animation
import matplotlib.pyplot as plt
import seaborn as sns
sns.set(style='darkgrid')

from collections import deque
from threading import Lock


def cb_imu(msg):
    if not hasattr(cb_imu, 'prev_time'):
        cb_imu.buffer = deque(maxlen=800)
        cb_imu.prev_time = msg.header.stamp
        cb_imu.lock = Lock()
        return

    cb_imu.lock.acquire(True)
    curr_time = msg.header.stamp
    delta = (curr_time - cb_imu.prev_time).to_sec()
    cb_imu.buffer.append(delta)
    cb_imu.prev_time = curr_time
    cb_imu.lock.release()


def plot(frame):
    if not hasattr(cb_imu, 'buffer'):
        return

    plt.cla()
    cb_imu.lock.acquire(True)
    plt.plot(cb_imu.buffer, '.')
    cb_imu.lock.release()


def main():
    fig = plt.figure()

    rospy.init_node('sub_imu')

    rospy.Subscriber('spatial/imu', Imu, cb_imu, queue_size=10)

    ani = animation.FuncAnimation(fig, plot, interval=1)
    plt.show()


if __name__ == '__main__':
    main()