Python отображает данные ROS в реальном времени

Я пытаюсь построить данные в реальном времени, поступающие на компьютер, используя 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()

Моя проблема в том, что когда я вызываю plotThrottle() с данными, полученными от rostopic, я получаю следующую ошибку.

[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. Я искал об этой ошибке, и он говорит, что это из-за какой-то проблемы с потоками. Но я не понимаю, как исправить этот код. Я очень благодарен, если кто-то может объяснить проблему и помочь исправить этот код.


python multithreading matplotlib python-multithreading rospy
person Teshan Shanuka J    schedule 02.02.2016    source источник

Ответы (2)


arrow_upward
2
arrow_downward

Здесь у вас есть два запущенных потока: rospy.spin() и top.mainloop() (из Tkinter, в вашем случае бэкэнд matplotlib).

Из этот ответ:

Проблемы связаны с тем, что модуль _tkinter пытается получить контроль над основным потоком с помощью метода опроса при обработке вызовов из других потоков.

Ваш код Tkinter в Thread-1 пытается заглянуть в основной поток, чтобы найти основной цикл, но его там нет.

Из этого ответа:

Если есть еще один блокирующий вызов, поддерживающий работу вашей программы, нет необходимости вызывать rospy.spin(). В отличие от C++, где функция spin() необходима для обработки всех потоков, в Python все, что она делает, — это блокирует.

Таким образом, вы можете использовать plt.show(block=True), чтобы ваша программа не закрывалась, в этом случае вы будете использовать основной цикл Tkinter, без проблем перерисовывая свой холст.

Функция слушателя должна выглядеть так:

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

В любом случае, это кажется немного обходным путем для других альтернатив, см. снова этот ответ или просто используйте отдельный узел для построения графика, т. е. предлагаемые ros инструменты, такие как rqt_graph.

person tmms    schedule 16.02.2016

arrow_upward
1
arrow_downward

Поскольку это старый пост, и, кажется, он все еще активен в сообществе, я собираюсь привести пример, как мы можем делать графики в реальном времени. Здесь я использовал функцию 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) по своему усмотрению и внесите соответствующие изменения.

person GPrathap    schedule 25.08.2020