Python实时绘制ROS数据

Question

我正在尝试绘制使用 python 进入计算机的实时数据。数据来自 ROS 主题，我使用 'rospy' 订阅主题以获取数据。这是我写的代码

import rospy
from sensor_msgs.msg import ChannelFloat32
import matplotlib.pyplot as plt

N = 200
i = 0

topic = "chatter"

x = range(N)
lmotor = [0]*N
rmotor = [0]*N

plt.ion()

fig = plt.figure()
ax = fig.add_subplot(111)
ax.set_xlim([0,N])
ax.set_ylim([-1,1])

line1, = ax.plot(lmotor, 'r-')
line2, = ax.plot(rmotor, 'g')

def plotThrottle(data):
    global x, lmotor, rmotor, i

    [x[i],lmotor[i],rmotor[i], tmp] = data

    line1.set_ydata(lmotor)
    line1.set_xdata(x)
    line2.set_ydata(rmotor)
    line2.set_xdata(x)

    fig.canvas.draw()

def callBack(packet):
    data = list(packet.values)
    plotThrottle(data)


def listner():
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber(topic, ChannelFloat32, callBack)
    rospy.spin()

if __name__ == '__main__':
    listner()

我的问题是当我使用从 rostopic 获得的数据调用 plotThrottle() 时，出现以下错误。

[ERROR]
[WallTime: 1454388985.317080] bad callback: <function callBack at 0x7f13d98ba6e0>
Traceback (most recent call last):
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/topics.py", line 720, in _invoke_callback
    cb(msg)
  File "dummy2.py", line 41, in callBack
    plotThrottle(data)
  File "dummy2.py", line 37, in plotThrottle
    fig.canvas.draw()
  File "/usr/lib/pymodules/python2.7/matplotlib/backends/backend_tkagg.py", line 349, in draw
    tkagg.blit(self._tkphoto, self.renderer._renderer, colormode=2)
  File "/usr/lib/pymodules/python2.7/matplotlib/backends/tkagg.py", line 13, in blit
    tk.call("PyAggImagePhoto", photoimage, id(aggimage), colormode, id(bbox_array))
RuntimeError: main thread is not in main loop

但是，如果我使用相同的函数并传递代码中生成的一些数据（一些随机数据），绘图就可以正常工作。我绝对是 python 的初学者。我搜索了这个错误，它说这是因为一些线程问题。但我不明白如何修复这段代码。如果有人可以解释问题并帮助修复此代码，我将不胜感激。

Answer 1

这里有两个线程运行、rospy.spin() 和 top.mainloop()（来自 Tkinter，在你的例子中是 matplotlib 的后端）。

来自this answer：

The problems stem from the fact that the _tkinter module attempts to gain control of the main thread via a polling technique when processing calls from other threads.

Your Tkinter code in Thread-1 is trying to peek into the main thread to find the main loop, and it's not there.

来自this answer：

If there is another blocking call that keeps your program running, there is no need to call rospy.spin(). Unlike in C++ where spin() is needed to process all the threads, in python all it does is block.

所以你可以使用 plt.show(block=True) 来防止你的程序关闭，在这种情况下你将使用 Tkinter 主循环，重绘你的 canvas 没有问题。

侦听器函数应如下所示：

    def listener():
        rospy.init_node('listener', anonymous=True)
        rospy.Subscriber(topic, ChannelFloat32, callBack)
        # rospy.spin()
        plt.show(block=True)

无论如何，对于其他替代方案，这似乎有点解决方法，请再次查看 this answer 或简单地使用单独的节点进行绘图，即 ros 建议的工具，如 rqt_graph。

Answer 2

既然是老post，好像还活跃在社区里，我举个例子，一般来说，我们如何real-time作图。这里我使用了matplotlib的FuncAnimation函数。

import matplotlib.pyplot as plt
import rospy
import tf
from nav_msgs.msg import Odometry
from tf.transformations import quaternion_matrix
import numpy as np
from matplotlib.animation import FuncAnimation


class Visualiser:
    def __init__(self):
        self.fig, self.ax = plt.subplots()
        self.ln, = plt.plot([], [], 'ro')
        self.x_data, self.y_data = [] , []

    def plot_init(self):
        self.ax.set_xlim(0, 10000)
        self.ax.set_ylim(-7, 7)
        return self.ln
    
    def getYaw(self, pose):
        quaternion = (pose.orientation.x, pose.orientation.y, pose.orientation.z,
                pose.orientation.w)
        euler = tf.transformations.euler_from_quaternion(quaternion)
        yaw = euler[2] 
        return yaw   

    def odom_callback(self, msg):
        yaw_angle = self.getYaw(msg.pose.pose)
        self.y_data.append(yaw_angle)
        x_index = len(self.x_data)
        self.x_data.append(x_index+1)
    
    def update_plot(self, frame):
        self.ln.set_data(self.x_data, self.y_data)
        return self.ln


rospy.init_node('lidar_visual_node')
vis = Visualiser()
sub = rospy.Subscriber('/dji_sdk/odometry', Odometry, vis.odom_callback)

ani = FuncAnimation(vis.fig, vis.update_plot, init_func=vis.plot_init)
plt.show(block=True)

注意：根据需要更改 rospy.Subscriber('/dji_sdk/odometry', Odometry, vis.odom_callback) 并相应地进行必要的更改。

Python实时绘制ROS数据

Python real time plotting ROS data

python

multithreading

matplotlib

python-multithreading

rospy