Tag Archives

4 Articles

Using matplotlib to plot on ROS subscriber callback

by NigoroJr 0 Comments


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


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


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

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.axhline(cb_path.max, c='darkorange')

def main():

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

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

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

if __name__ == '__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

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.axhline(cb_path.max, c='darkorange')

def main():

    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)

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

if __name__ == '__main__':


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.Subscriber('bar', Baz, cb_func)

Exporting colored COLLADA for use in Gazebo

by NigoroJr 0 Comments


Read in an STL file into Blender, add color to it and export it as a DAE file (COLLADA), then feed that into Gazebo.


The color is not displayed correctly. It becomes darker (or brighter, depending on the emission) than the expected color. Expected color (part of the robot):

However, what I get when loading this onto Gazebo is:

The red in the figure above is lighter than the expected red (ignore the stripes; that’s normal for Gazebo). In this article it is assumed that you already have your model in Blender, colored and ready for export. I will not go through how to set up the environment or write the URDF for your robot.


You can’t automatically export the desired color with COLLADA. You need to manually edit the DAE file exported from Blender and set the emission to proper values. Most of the times it’s enough to set it to the same value as the dispertion value.


  • Ubuntu 18.04
  • Gazebo 9.5.0
  • Blender 2.79


Firstly, whatever you render using Cycles Render is not refleceted to the exported COLLADA file. Whatever you see when you export it using Blender Render is apparently what you get in the COLLADA file. Except, you don’t. When you use a color other than grayscale, you end up getting a color either lighter or darker than what you expect.

The problem arises from the fact that Blender’s COLLADA exported doesn’t handle colored emissions (actually, it seems like a limitation in Blender Render since there’s only one value to be specified for the emission). Emission is the color that the object itself emits. In other words, it’s the color of the object when seen in pitch black. It takes a value between 0 and 1, where 0 is no light emitted (black) and white at 1.

When you specify a value (say 0.4) for emission in Blender, it exports the COLLADA file as follows:

    <effect id="Red-effect">
        <technique sid="common">
              <color sid="emission">0.4 0.4 0.4 1</color>
              <color sid="ambient">0 0 0 1</color>
              <color sid="diffuse">0.6194061 0.024045 0.02145169 1</color>
              <color sid="specular">0.5 0.5 0.5 1</color>
              <float sid="shininess">40</float>
              <float sid="index_of_refraction">1</float>

Notice that the emission is all 0.4, which is gray. We want this to be red-ish, so we just replace that part manually with:

              <color sid="emission">0.6194061 0.024045 0.02145169 1</color>

Sadly, there is no way to do this automatically at the moment. This means that if you want to update some of the meshes in Blender and export that to COLLADA, the manual change you just made will be overwritten.

No PyCharm completion for rospy


I have a version of ros_comm cloned from GitHub.

  • Ubuntu 16.04
  • ROS Lunar


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


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):
        src_init_file = os_path.join(p, __name__, '__init__.py')
        if os_path.isfile(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:
    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).


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"

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

Using TF2 on with Python 3

by NigoroJr 0 Comments

ROS1 officially supports Python 2, but there are many official libraries that are written with compatibility for Python 3. TF2 is one of them, and it may be a good idea to start using Python 3 for simple applications that make use of TF2.

What’s Required

My Environment

  • Ubuntu 16.04
  • ROS Kinetic
  • Python 2.7.12 (system)
  • Python 3.5.2 (system)
  • Python 3.6.4 (installed with pyenv)


Note: You may want to create a different catkin workspace for this. Otherwise, you’ll probably need to recompile all the packages that depend on TF2 libraries.

  1. Install pyenv following the instructions in the URL above
  2. CONFIGURE_OPTS=--enable-shared pyenv install <version> to install the latest Python 3 with the shared libraries
  3. Download geometry2 and put it in your catkin workspace
  4. Go into your catkin workspace and run pyenv local <python version you installed>
  5. Run pyenv rehash and make sure python --version displays the right version
  6. Install dependencies with: pip install catkin_pkg pyyaml empy rospkg numpy
  7. Run catkin_make -DPYTHON_EXECUTABLE=$( pyenv prefix )/bin/python3 -DPYTHON_LIBRARY=$( pyenv prefix )/lib/libpython3.so -DPYTHON_VERSION=3
  8. In a Python 3 console, run import tf2_ros.

If you get the following error when running catkin_make, make sure that the executables you’re pointing to exists, and that they’re the right version.

When I forgot to specify -DPYTHON_EXECUTABLE so cmake couldn’t find the right Python interpreter,

CMake Error at /usr/share/cmake-3.5/Modules/FindPackageHandleStandardArgs.cmake:148 (message):
  Could NOT find PythonInterp: Found unsuitable version "2.7.12", but
  required is at least "3" (found /usr/bin/python)
Call Stack (most recent call first):
  /usr/share/cmake-3.5/Modules/FindPackageHandleStandardArgs.cmake:386 (_FPHSA_FAILURE_MESSAGE)
  /usr/share/cmake-3.5/Modules/FindPythonInterp.cmake:163 (FIND_PACKAGE_HANDLE_STANDARD_ARGS)
  /opt/ros/kinetic/share/catkin/cmake/python.cmake:8 (find_package)
  /opt/ros/kinetic/share/catkin/cmake/all.cmake:147 (include)
  /opt/ros/kinetic/share/catkin/cmake/catkinConfig.cmake:20 (include)
  CMakeLists.txt:52 (find_package)

I got this while compiling rviz because I didn’t specify -DPYTHON_LIBRARY.

CMake Error at /usr/share/cmake-3.5/Modules/FindPackageHandleStandardArgs.cmake:148 (message):
  Could NOT find PythonLibs: Found unsuitable version "2.7.12", but required
  is at least "3.6" (found

If you get the following error in the Python console, suspect that you’re not using the right Python version:

>>> import tf2_ros
Cannot load geometry_msgs moduleTraceback (most recent call last):
  File "<frozen importlib._bootstrap>", line 969, in _find_and_load
  File "<frozen importlib._bootstrap>", line 958, in _find_and_load_unlocked
  File "<frozen importlib._bootstrap>", line 673, in _load_unlocked
  File "<frozen importlib._bootstrap_external>", line 665, in exec_module
  File "<frozen importlib._bootstrap>", line 222, in _call_with_frames_removed
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/__init__.py", line 49, in <module>
    from .client import spin, myargv, init_node, \
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/client.py", line 52, in <module>
    import roslib
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslib/__init__.py", line 50, in <module>
    from roslib.launcher import load_manifest
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslib/launcher.py", line 42, in <module>
    import rospkg
ImportError: No module named 'rospkg'


Python 3.6.4 (default, Jan 21 2018, 19:20:10)
[GCC 5.4.0 20160609] on linux
Type "help", "copyright", "credits" or "license" for more information.
>>> import rospy
>>> import tf2_ros
>>> from geometry_msgs.msg import TransformStamped
>>> rospy.init_node('sample')
>>> static_tf = TransformStamped()
>>> tf_sb = tf2_ros.StaticTransformBroadcaster()
>>> static_tf.header.stamp = rospy.Time.now()
>>> static_tf.header.frame_id = 'base_link'
>>> static_tf.child_frame_id = 'laser'
>>> static_tf.transform.rotation.w = 1.0
>>> tf_sb.sendTransform(static_tf)

And that’s how you use TF with Python 3!