Tag Archives

Objective

Update a plot every time a callback function of a ROS subscriber is called.

TL;DR

Use plt.show() (or fig.show()) instead of rospy.spin(). Call plt.draw() (or fig.canvas.draw()) in the callback function.

Approaches I tried (but didn’t work)

• matplotlib.animation
  • cannot be used because calls to callback functions are not periodical
• Calling plt.show() in the callback function
  • plt.show() blocks, so the plot is not updated after the first message
• Using fig = plt.figure() and running fig.show
  • main thread is not in main loop, since callback function are called in a separate thread

Solution

The solution is to remove rospy.spin() and use plt.show() instead. They do the same thing. That is, all they do is they both simply spin in the main loop.

Following is the code that I used to create a plot every time a callback is fired. What it does is it calculates the curvature of the nav_msgs/Path, but that is irrelevant to what we’re trying to do now.

The important point is to have plt.show() inside the main thread; otherwise, you’ll get a RuntimeError: main thread is not in main loop.

#!/usr/bin/env python

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

import rospy
from nav_msgs.msg import Path

from path_smoother import PathConverter, Utils


def cb_path(msg):
    df = PathConverter.path_to_df(msg)
    curvatures = Utils.curvatures(df)

    plt.cla()
    plt.plot(curvatures)
    plt.axhline(cb_path.max, c='darkorange')
    plt.draw_all()


def main():
    rospy.init_node('realtime_curvature_plot')

    cb_path.max = rospy.get_param('~kappa_max', 0.2)

    rospy.Subscriber('path', Path, cb_path)

    plt.axhline(cb_path.max, c='darkorange')
    plt.show()
    # Note: No need to spin! plt.show() spins instead
    # rospy.spin()


if __name__ == '__main__':
    main()

Also, following code snippet uses plt.figure(). Other than that it’s identical to the snippet above.

#!/usr/bin/env python

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

import rospy
from nav_msgs.msg import Path

from path_smoother import PathConverter, Utils


def cb_path(msg):
    df = PathConverter.path_to_df(msg)
    curvatures = Utils.curvatures(df)

    cb_path.ax.cla()
    cb_path.ax.plot(curvatures)
    cb_path.ax.axhline(cb_path.max, c='darkorange')
    cb_path.fig.canvas.draw()


def main():
    rospy.init_node('realtime_curvature_plot')

    cb_path.fig = plt.figure()
    cb_path.ax = cb_path.fig.add_subplot(111)

    cb_path.max = rospy.get_param('~kappa_max', 0.2)

    rospy.Subscriber('path', Path, cb_path)

    plt.show()
    # Note: No need to spin! plt.show() spins instead
    # rospy.spin()


if __name__ == '__main__':
    main()

ProTip

If you don’t want to use fig = plt.figure() and create a variable that you have to look after, you can also use plt.plot() with no argument to have an empty plot:

def main():
    rospy.init_node('foo')
    rospy.Subscriber('bar', Baz, cb_func)
    ...
    plt.plot()
    plt.show()