Tag Archives

3 Articles

Using matplotlib to plot on ROS subscriber callback

by NigoroJr 0 Comments

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()


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()

No PyCharm completion for rospy

Premises

I have a version of ros_comm cloned from GitHub.

  • Ubuntu 16.04
  • ROS Lunar

Problem

In PyCharm Professional 2018.1.1, no completion for rospy was shown. The message said:

Cannot find reference 'init_node' in '__init__.py'

Screenshot of the error message

TL;DR

If $( rospack find rospy )/src exists, add it to PYTHONPATH.

Cause of the Problem

After sourcing setup.bash (and friends), PYTHONPATH points to /path/to/workspace/devel/lib/python2.7/dist-packages. Doing import rospy in the Python script uses the following __init__.py:

# -*- coding: utf-8 -*-
# generated from catkin/cmake/template/__init__.py.in
# keep symbol table as clean as possible by deleting all unnecessary symbols

from os import path as os_path
from sys import path as sys_path

from pkgutil import extend_path

__extended_path = "/home/naoki/ros/workspaces/lunar/hsas/src/ros_comm/clients/rospy/src".split(";")
for p in reversed(__extended_path):
    sys_path.insert(0, p)
    del p
del sys_path

__path__ = extend_path(__path__, __name__)
del extend_path

__execfiles = []
for p in __extended_path:
    src_init_file = os_path.join(p, __name__ + '.py')
    if os_path.isfile(src_init_file):
        __execfiles.append(src_init_file)
    else:
        src_init_file = os_path.join(p, __name__, '__init__.py')
        if os_path.isfile(src_init_file):
            __execfiles.append(src_init_file)
    del src_init_file
    del p
del os_path
del __extended_path

for __execfile in __execfiles:
    with open(__execfile, 'r') as __fh:
        exec(__fh.read())
    del __fh
    del __execfile
del __execfiles

The problem is that PyCharm expects __all__ to be populated in order for the completion to function correctly. That is done in /path/to/workspace/src/ros_comm/clients/rospy/src/rospy/__init__.py (i.e. $( rospack find rospy )/src/rospy).

Solution

Add $( rospack find rospy )/src to the front of PYTHONPATH if it exists. For rospy that’s installed with APT, there’s no src subdirectory (i.e. rospack find rospy gives you /opt/ros/DISTRO/share/rospy in which there is no src subdirectory).

if [[ -d $( rospack find rospy )/src ]]; then
    export PYTHONPATH="$( rospack find rospy )/src:$PYTHONPATH"
fi

After that if you start up PyCharm it should give you completion for rospy.

StaticTransformBroadcasterで複数のTransformをbroadcastする

by NigoroJr 0 Comments

以前にもハマったことがある記憶があるので,メモ.

TF2にはStaticTransformBroadcasterというクラスがあり,TF2で追加された /tf_static というstatic transformをブロードキャストすることができます.使い方はこんな感じ:

import tf2_ros
from geometry_msgs.msg import TransformStamped

tf_sb = tf2_ros.StaticTransformBroadcaster()
tform = TransformStamped()
tform.header.frame_id = 'hoge'
# いろいろやる
tf_sb.sendTransform(tform)

ただ,一つのノードで複数のTransformをブロードキャストすると, rostopic echo /tf_static で最後の変換しか表示されません.

tf_sb.sendTransform(transform1)
tf_sb.sendTransform(transform2)
tf_sb.sendTransform(transform3)
# transform3しか使えない

どうやらStaticTransformBroadcasterは一つのTransformしか保持することができないらしいです.複数のTransformをブロードキャストしたいときは,まとめて送りましょう.

tforms = [transform1, transform2, transform3]
tf_sb.sendTransform(tforms)

ちなみにタプルを使うと Inbound TCP/IP connection failed: 'tuple' object has no attribute 'header' というエラーが出てダメな模様.リストを使いましょう.

複数クラスでStatic Transformを作るときは,結構めんどくさいです.TransformStampedを返すようにする等しないといけないみたいですね.