使用ROS回调函数动态更新Matplotlib 3D图形的方法

Question

使用ROS回调函数动态更新Matplotlib 3D图形的方法

3

我正在尝试使用matplotlib更新3D图。我正在使用ROS收集数据。我希望在获取数据时更新绘图。我查找了一些资料，发现了这个链接： Dynamically updating plot in matplotlib

但是我无法使它工作。我对Python非常陌生，还没有完全理解它的工作原理。如果我的代码很丑陋，我深感抱歉。

我一直得到这个错误。

[ERROR] [WallTime: 1435801577.604410] bad callback: <function usbl_move at 0x7f1e45c4c5f0>
Traceback (most recent call last):
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/topics.py", line 709, in _invoke_callback
    cb(msg, cb_args)
  File "/home/nathaniel/simulation/src/move_videoray/src/move_filtered.py", line 63, in usbl_move
    if filter(pos.pose.position.x,pos.pose.position.y,current.position.z):
  File "/home/nathaniel/simulation/src/move_videoray/src/move_filtered.py", line 127, in filter
    plt.draw()
  File "/usr/lib/pymodules/python2.7/matplotlib/pyplot.py", line 555, in draw
    get_current_fig_manager().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

这是我正在尝试运行的代码。

#!/usr/bin/env python


'''
    Ths program moves the videoray model in rviz using 
    data from the /usble_pose node
    based on "Using urdf with robot_state_publisher" tutorial

'''

import rospy
import roslib
import math
import tf
#import outlier_filter
from geometry_msgs.msg import Twist, Vector3, Pose, PoseStamped, TransformStamped
from matplotlib import matplotlib_fname
from mpl_toolkits.mplot3d import Axes3D
import sys
from matplotlib.pyplot import plot
from numpy import mean, std
import matplotlib as mpl

import numpy as np
import pandas as pd
import random
import matplotlib.pyplot as plt
#plt.ion()
import matplotlib
mpl.rc("savefig", dpi=150)
import matplotlib.animation as animation
import time

#filter stuff
#window size
n = 10
#make some starting values
#random distance
md =[random.random() for _ in range(0, n)]
#random points
x_list = [random.random() for _ in range(0, n)]
y_list =[random.random() for _ in range(0, n)]
#set up graph
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
#ax.scatter(filt_x,filt_y,filt_depth,color='b')
#ax.scatter(outlier_x,outlier_y,outlier_depth,color='r') 
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('XY Outlier rejection \n with Mahalanobis distance and rolling mean3')



#set the robot at the center


#//move the videoray using the data from the /pose_only node
def usbl_move(pos,current):


    broadcaster = tf.TransformBroadcaster()
    if filter(pos.pose.position.x,pos.pose.position.y,current.position.z):
        current.position.x = pos.pose.position.x
        current.position.y = pos.pose.position.y


    broadcaster.sendTransform( (current.position.x,current.position.y,current.position.z), 
                                (current.orientation.x,current.orientation.y,current.orientation.z,current.orientation.w),
                                     rospy.Time.now(), "odom", "body" )


#move the videoray using the data from the /pose_only node  
def pose_move(pos,current):

    #pos.position.z is in kPa, has to be convereted to depth
    # P  = P0 + pgz ----> pos.position.z = P0 + pg*z_real
    z_real = -1*(pos.position.z -101.325)/9.81;

    #update the movement
    broadcaster = tf.TransformBroadcaster()
    current.orientation.x = pos.orientation.x
    current.orientation.y = pos.orientation.y
    current.orientation.z = pos.orientation.z
    current.orientation.w = pos.orientation.w
    current.position.z = z_real
    broadcaster.sendTransform( (current.position.x,current.position.y,current.position.z), 
                                (current.orientation.x,current.orientation.y,current.orientation.z,current.orientation.w),
                                     rospy.Time.now(), "odom", "body" )



#call the fitle the date 
def filter(x,y,z):
    # update the window
    is_good = False
    x_list.append(x)
    y_list.append(y)
    x_list.pop(0)
    y_list.pop(0)
    #get the covariance matrix
    v = np.linalg.inv(np.cov(x_list,y_list,rowvar=0))
    #get the mean vector
    r_mean = mean(x_list), mean(y_list)  
    #subtract the mean vector from the point
    x_diff = np.array([i - r_mean[0] for i in x_list])
    y_diff = np.array([i - r_mean[1] for i in y_list])
    #combinded and transpose the x,y diff matrix
    diff_xy = np.transpose([x_diff, y_diff])
    # calculate the Mahalanobis distance
    dis = np.sqrt(np.dot(np.dot(np.transpose(diff_xy[n-1]),v),diff_xy[n-1]))
    # update the window 
    md.append( dis)
    md.pop(0)
    #find mean and standard standard deviation of the standard deviation list
    mu  = np.mean(md)
    sigma = np.std(md)
    #threshold to find if a outlier
    if dis < mu + 1.5*sigma:
        #filt_x.append(x)
        #filt_y.append(y)
        #filt_depth.append(z)
        ax.scatter(x,y,z,color='b')
        is_good =  True
    else:
        ax.scatter(x,y,z,color='r')
    plt.draw()
    return is_good




if __name__ == '__main__':
    #set up the node
    rospy.init_node('move_unfiltered', anonymous=True)
    #make a broadcaster foir the tf frame
    broadcaster = tf.TransformBroadcaster()
    #make intilial values
    current = Pose()
    current.position.x = 0
    current.position.y = 0
    current.position.z = 0
    current.orientation.x = 0
    current.orientation.y = 0
    current.orientation.z = 0
    current.orientation.w = 0
    #send the tf frame
    broadcaster.sendTransform( (current.position.x,current.position.y,current.position.z), 
                                (current.orientation.x,current.orientation.y,current.orientation.z,current.orientation.w),
                                     rospy.Time.now(), "odom", "body" )

    #listen for information

    rospy.Subscriber("/usbl_pose", PoseStamped, usbl_move,current)
    rospy.Subscriber("/pose_only", Pose, pose_move, current);
    rospy.spin()

- user1376339

1

很难看清楚到底发生了什么，因为我不知道rospy，所以我只能从错误信息中猜测：rospy.spin()将任务分配到线程中，但由于你正在TK后端绘图，而TK也使用线程（因为GUI通常会这样做），所以你会遇到问题。不知何故，你必须完全将读取部分与绘图部分分开。一种可能的方法是让一个程序使用rospy读取数据，并将这些数据发送到另一个仅绘制数据的程序。 - user707650

1

顺便再读一遍你的代码，我发现你在程序主体中创建了一个图形。然后，rospy会分离线程，在这些线程内，你尝试更新和绘制新的图形。因此，也许你可以尝试将所有与绘图相关的内容都移动到同一个线程/函数中，包括设置绘图。 - user707650

1个回答

网页内容由stack overflow 提供, 点击上面的

可以查看英文原文，
原文链接

- GPrathap · Accepted Answer

由于这是一篇旧文章，但在社区中仍然活跃，因此我将提供一个示例，介绍如何进行实时绘图。在这里，我使用了 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)