Dynamic updating a Matplotlib 3D plot when working with ROS callback

Question

I am trying to update a 3D plot using matplotlib. I am collecting data using ROS. I want to update the plot as I get data. I have looked around and found this, Dynamically updating plot in matplotlib

but I cannot get it to work. I am very new to python and do not full understand how it works yet. I apologize if my code is disgusting.

I keep get this error.

[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

This is the code I am trying to run

#!/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()

It's hard to see what's really going on, and since I don't know rospy, I'm going to have a rough guess from the error message: rospy spins off tasks into threads (`rospy.spin()`) suggests that, but since you're plotting in the TK backend, and TK also uses thread (as GUIs tend to do), you run into problems. Somehow, you have to separate the reading part entirely from the plotting part. One way perhaps, could be to have one program read the data using rospy, and send those data to another program that only plots data. — , Jul 02 '15 at 03:08
On second reading of your code, btw, I see you create a figure in the main body of the program. Then, rospy spins off threads, and inside those threads, you then try to update and plot the new figure. So perhaps, you could try to move all the plotting related stuff inside the same thread/function, including setting up the plot. — , Jul 02 '15 at 03:10

score 1 · Answer 1 · answered Aug 25 '20 at 13:18

Since this is an old post and still seems to be active in the community, I am going to provide an example, in general, how can we do real-time plotting. Here I used matplotlib FuncAnimation function.

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)

Very helpful answer. Also note that the above method can be adapted to draw frames (usually achieved with `imshow()`) by using `self.im = plt.imshow(np.random.random( N,M))` inside **init**, then setting the data of `im` inside **update_plot** and finally returning `self.im` in place of `self.ln`. — vyi, Dec 20 '20 at 18:07

Dynamic updating a Matplotlib 3D plot when working with ROS callback

1 Answers1