动态更新Matplotlib 3D绘图

时间:2015-07-02 01:58:33

标签: python matplotlib ros

我正在尝试使用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()

1 个答案:

答案 0 :(得分:0)

由于这是一篇老文章,并且在社区中似乎仍然很活跃,因此我将提供一个示例,总的来说,我们如何进行实时绘图。在这里,我使用了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)