hope you're all doing great.
I am quite new in Python and am working on a tiny client- server project, where I am receiving data from the client and the goal is that the server plot this data in graphic form in real time.
This is the code from the server part, which I am having struggles right now.
import socket
import sys
import math
import numpy as np
import struct
import time
import os
import ctypes as c
import multiprocessing
import matplotlib.pyplot as plt
from matplotlib import animation
from matplotlib import style
HOST = '127.0.0.1'
PORT = 6543
receive_size = 4096
def run_server(shared_data_time, shared_data_signal):
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as server:
server.bind((HOST, PORT))
server.listen()
conn, addr = server.accept()
with conn:
print(f"Connected by {addr}")
while True:
data = conn.recv(receive_size)
if len(data) == 4096:
payload = np.frombuffer(data, dtype = 'float64')
print(payload)
print('received data')
deinterleaved = [payload[idx::2] for idx in range(2)]
shared_data_time = deinterleaved[0]
shared_data_signal = deinterleaved[1]
print(f'received {len(data)} bytes')
if __name__ == '__main__':
HOST = '127.0.0.1'
PORT = 6543
receive_size = 4096
shared_data_time = multiprocessing.Array('f', 2048)
shared_data_signal = multiprocessing.Array('f', 2048)
process1 = multiprocessing.Process(target = run_server, args =(shared_data_time, shared_data_signal))
process1.start()
def animate(i, shared_data_time, shared_data_signal):
ax1.clear()
ax1.plot(shared_data_time, shared_data_signal)
style.use('fivethirtyeight')
fig = plt.figure()
ax1 = fig.add_subplot(1,1,1)
ani = animation.FuncAnimation(fig, animate, fargs = (shared_data_time, shared_data_signal), interval = 100)
plt.show()
The communication between server and client works but I am only getting am empty graph, with no actualization. Could everyone helpe me? I would really appreciate it.
Thanks
without having access to the server you connect to, it's difficult to determine the exact problem, but please see this example I made to animate data coming from a child process via shared multiprocessing.Array's:
import multiprocessing as mp
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import numpy as np
from time import sleep, time
def server(arr_x, arr_y):
#tie shared buffers to numpy arrays
x = np.frombuffer(arr_x.get_obj(), dtype='f4')
y = np.frombuffer(arr_y.get_obj(), dtype='f4')
T = time()
while True:
t = time() - T #elapsed time
#we should technically lock access while we're writing to the array,
# but mistakes in the data are not real important if it's just a gui.
x[:] = np.linspace(t, t + np.pi*2, len(x)) #update data in shared array
y[:] = np.sin(x)
sleep(1/30) #data updating faster or slower than animation framerate is not a big issue...
if __name__ == "__main__":
fig = plt.figure()
ax = plt.subplot()
#init data
arr_x = mp.Array('f', 1000) #type "d" == np.dtype("f8")
arr_y = mp.Array('f', 1000)
#tie shared buffers to numpy arrays
x = np.frombuffer(arr_x.get_obj(), dtype='f4')
y = np.frombuffer(arr_y.get_obj(), dtype='f4')
#calculate initial value
x[:] = np.linspace(0, np.pi*2, len(x))
y[:] = np.sin(x)
#daemon process to update values (server)
mp.Process(target=server, args=(arr_x, arr_y), daemon=True).start()
#plot initial data because we need a line instance to update continually
line = ax.plot(x, y)[0]
#fps counting vars
last_second = time()
last_frame = 0
def animate(frame, x, y):
global last_second, last_frame
#might be cleaner to wrap animate and use nonlocal rather than global,
# but it would be more complicated for just this simple example.
#set data with most recent values
line.set_data(x, y)
line.axes.set_xlim(x[0], x[999])
#periodically report fps
interval = time() - last_second
if interval >= 1:
print("fps: ", (frame-last_frame) / interval)
last_second = time()
last_frame = frame
ani = FuncAnimation(fig, animate, fargs=(x, y), interval = 20)
plt.show()
FPS counting can obviously be removed easily, and converting shared arrays to numpy arrays isn't strictly necessary, but I find it easier to work with, and it is not difficult.
Related
When I run this code I get this error: ax = plt.subplot("311")
MatplotlibDeprecationWarning: Passing non-integers as three-element position specification is deprecated since 3.3 and will be removed two minor releases later. I made those changes that was suggested now I get errors in other parts of the code.
I have provided a small snippet of the code
Any suggestions on how I can fix this
import socket, traceback
import matplotlib
matplotlib.use('TkAgg')
import matplotlib.pyplot as plt
from scipy.signal import butter, lfilter,iirfilter,savgol_filter
import math
import pylab
from pylab import *
import time
import numpy as np
host = ''
port = 5555
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
s.bind((host, port))
# lists for plotting
Ax = [0.0] * 50
Ay = [0.0] * 50
Az = [0.0] * 50
G = [0.0] * 50
x = [i for i in range(len(Ax))]
#used for debugging
fig = plt.figure(figsize=(16,10))
# raw data
ax = plt.subplot(311)
ax.set_xlim(0, 50)
ax.set_ylim(-2, 2)
ax.set_title("Raw acceleration data")
ax.set_ylabel("g$/m^2$",fontsize=18)
line = ax.plot(Ax,label='Acc x')[0]
line2 = ax.plot(Ay,label='Acc y')[0]
line3 = ax.plot(Az,label='Acc z')[0]
# filtered data
ax2 = plt.subplot(312)
ax2.set_xlim(0, 50)
ax2.set_ylim(-2, 2)
ax2.set_title(" acceleration data")
ax2.set_ylabel("g$/m^2$",fontsize=18)
f_line = ax2.plot(Ax,label='Acc x')[0]
f_line2 = ax2.plot(Ay,label='Acc y')[0]
f_line3 = ax2.plot(Az,label='Acc z')[0]
# tilt angle plot
ax3 = plt.subplot(313)
ax3.set_ylim([-180,180])
ax3.set_title("Tilt Angles")
ax3.set_ylabel("degrees",fontsize=18)
t_line = ax3.plot(G)[0]
fig.suptitle('Three-axis accelerometer streamed from Sensorstream',fontsize=18)
plt.show(False)
plt.draw()
# cache the background
background = fig.canvas.copy_from_bbox(fig.bbox)
count = 0
print("Success binding")
while 1:
# time it
tstart = time.time()
message, address = s.recvfrom(8192)
messageString = message.decode("utf-8")
Acc = messageString.split(',')[2:5]
Acc = [float(Acc[i])/10.0 for i in range(3)]
# appending and deleting is order 10e-5 sec
Ax.append(Acc[0])
del Ax[0]
Ay.append(Acc[1])
del Ay[0]
Az.append(Acc[2])
del Az[0]
G.append(np.sqrt(Ax[-1]**2 + Ay[-1]**2 + Az[-1]**2))
del G[0]
# filter
acc_x_savgol = savgol_filter(Ax, window_length=5, polyorder=3)
acc_y_savgol = savgol_filter(Ay, window_length=5, polyorder=3)
acc_z_savgol = savgol_filter(Az, window_length=5, polyorder=3)
tilt_angles = []
for i,val in enumerate(G):
angle = math.atan2(Ax[i], -1*Ay[i]) * (180 / math.pi)
if (math.isnan(angle)):
tilt_angles.append(0)
else:
tilt_angles.append(angle)
print(Ax[0],Ay[1],Az[2])
line.set_xdata(x)
line.set_ydata(Ax)
line2.set_xdata(x)
line2.set_ydata(Ay)
line3.set_xdata(x)
line3.set_ydata(Az)
ax.set_xlim(count, count+50)
f_line.set_xdata(x)
f_line.set_ydata(acc_x_savgol)
f_line2.set_xdata(x)
f_line2.set_ydata(acc_y_savgol)
f_line3.set_xdata(x)
f_line3.set_ydata(acc_z_savgol)
ax2.set_xlim(count, count+50)
t_line.set_xdata(x)
t_line.set_ydata(tilt_angles)
ax3.set_xlim(count, count+50)
# restore background
fig.canvas.restore_region(background)
# redraw just the points
ax.draw_artist(line)
ax.draw_artist(line2)
ax.draw_artist(line3)
ax2.draw_artist(f_line)
ax2.draw_artist(f_line2)
ax2.draw_artist(f_line3)
ax3.draw_artist(t_line)
# fill in the axes rectangle
fig.canvas.blit(fig.bbox)
count+=1
x = np.arange(count,count+50,1)
# tops out at about 25 fps :|
print ("Total time for 1 plot is: ",(time.time() - tstart))
The following code executes without warnings on my laptop.
ax = plt.subplot(311)
ax.set_xlim(0, 50)
ax.set_ylim(-2, 2)
ax.set_title("Raw acceleration data")
ax.set_ylabel("g$/m^2$",fontsize=18)
I removed ax.hold(True) and changed "311" to 311.
I'm trying to plot can-data using the python-can and matplotlib library. I have a basic example on how to plot live data with matplotlib:
import numpy as np
import matplotlib.pyplot as plt
plt.axis([0, 10, 0, 1])
for i in range(10):
y = np.random.random()
plt.scatter(i, y)
# plt.pause(0.05)
plt.draw()
plt.show()
And I used a basic python-can example and added the matplotlib:
import sys
import argparse
import socket
from datetime import datetime
import can
from can import Bus, BusState, Logger
import numpy as np
import matplotlib.pyplot as plt
def main():
plt.axis([0, 10, 0, 1])
bus = Bus(bustype ='pcan')
i = 0
print(f"Connected to {bus.__class__.__name__}: {bus.channel_info}")
print(f"Can Logger (Started on {datetime.now()})")
plt.show()
try:
while True:
msg = bus.recv(1)
if msg is not None:
y = np.random.random()
plt.scatter(i, y)
plt.draw()
i += 1
print('Message id {id}'.format(id=msg.arbitration_id))
except KeyboardInterrupt:
pass
finally:
plt.show()
bus.shutdown()
if __name__ == "__main__":
main()
However as soon as I add the plt commands the script stops working, it just doesn't output the print statement anymore. When I Debug into it and step manually it works. Anything I'm doing wrong? Is there a better way to solve this? I'm not set on matplotlib, just the fastest way to plot something :)
I found a working solution using the blit function, its not perfect yet but its a good starting point:
from datetime import datetime
import can
from can import Bus, BusState, Logger
import numpy as np
import matplotlib.pyplot as plt
import struct
def main():
bus = Bus(bustype ='pcan')
i = 0
print(f"Connected to {bus.__class__.__name__}: {bus.channel_info}")
print(f"Can Logger (Started on {datetime.now()})")
x = np.linspace(0, 1000, num=1000)
y = np.zeros(1000)
fig = plt.figure()
ax1 = fig.add_subplot(1, 1, 1)
line, = ax1.plot([], lw=3)
text = ax1.text(0.8, 0.5, "")
ax1.set_xlim(x.min(), x.max())
ax1.set_ylim([-10, 10])
fig.canvas.draw() # note that the first draw comes before setting data
ax2background = fig.canvas.copy_from_bbox(ax1.bbox)
plt.show(block=False)
k = 0
try:
while True:
msg = bus.recv(1)
if msg is not None:
y = np.roll(y, 1)
value = struct.unpack('f', msg.data)
y[0] = np.int8(value)
line.set_data(x, y)
fig.canvas.restore_region(ax2background)
ax1.draw_artist(line)
ax1.draw_artist(text)
fig.canvas.blit(ax1.bbox)
fig.canvas.flush_events()
# print('Message data {data}, value {value}, counter {counter}'.format(data=msg.data, value=value, counter=k))
k += 1
if k > 100:
k = 0
except KeyboardInterrupt:
pass
finally:
plt.show()
bus.shutdown()
if __name__ == "__main__":
main()
The script assumes that the can message carries one single float over the first four uint8 and just plots it.
I modified the code of this tutorial to create my own real time plot:
https://learn.sparkfun.com/tutorials/graph-sensor-data-with-python-and-matplotlib/speeding-up-the-plot-animation
I needed to plot the data from a proximity sensor in real time, the data is sent through USB cable to the computer and I read it with the serial port, so the code is already working how I wanted to, but I also want to modify the y-axis and x-axis, not let it static, because sometimes the peaks are 3000 and sometimes 2000 and when the sensor is not being touched, the peaks are at around 200 because it also detects the ambient light. any clue how can I make it?
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import serial
# Data from serial port
port = 'COM8'
baudrate = 9600
tout = 0.01 # Miliseconds
# Time to update the data of the sensor signal real time Rs=9600baud T=1/Rs
tiempo = (1 / baudrate) * 1000
# Parameters
x_len = 200 # Number of points to display
y_range = [20000, 40000] # Range of Y values to display
# Create figure for plotting
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
xs = list(range(0, x_len))
ys = [0] * x_len
ax.set_ylim(y_range)
# Create a blank line. We will update the line in animate
line, = ax.plot(xs, ys)
# Markers
startMarker = 60 # Start marker "<"
endMarker = 62 # End marker ">"
# Begin Arduino communication, Port COM8, speed 9600
serialPort = serial.Serial(port, baudrate, timeout=tout)
# Begin to save the arduino data
def arduinodata():
global startMarker, endMarker
ck = ""
x = "z" # any value that is not an end- or startMarker
bytecount = -1 # to allow for the fact that the last increment will be one too many
# wait for the start character
while ord(x) != startMarker:
x = serialPort.read()
# save data until the end marker is found
while ord(x) != endMarker:
if ord(x) != startMarker:
ck = ck + x.decode()
bytecount += 1
x = serialPort.read()
return ck
def readarduino():
# Wait until the Arduino sends '<Arduino Ready>' - allows time for Arduino reset
# It also ensures that any bytes left over from a previous message are discarded
msg = ""
while msg.find("<Arduino is ready>") == -1:
while serialPort.inWaiting() == 0:
pass
# delete for example the "\r\n" that may contain the message
msg = arduinodata()
msg = msg.split("\r\n")
msg = ''.join(msg)
# If the sensor send very big numbers over 90000 they will be deleted
if msg and len(msg) <= 5:
msg = int(msg)
return msg
elif msg and len(msg) >= 4:
msg = int(msg)
return msg
# This function is called periodically from FuncAnimation
def animate(i, ys):
# Read pulse from PALS2
pulse = readarduino()
# Add y to list
ys.append(pulse)
# Limit x and y lists to set number of items
ys = ys[-x_len:]
# Update line with new Y values
line.set_ydata(ys)
return line,
# Plot labels
plt.title('Heart frequency vs Time')
plt.ylabel('frequency ')
plt.xlabel('Samples')
# Set up plot to call animate() function periodically
ani = animation.FuncAnimation(fig, animate, fargs=(ys,), interval=tiempo, blit=True)
plt.show()
plt.close()
serialPort.close()
This is how the graph looks like, the x and y axis are always the same:
If you want to autoscale the y-axis, then you can simply adjust the y-axis limits in your animate() function:
def animate(i, ys):
# Read pulse from PALS2
pulse = readarduino()
# Add y to list
ys.append(pulse)
# Limit x and y lists to set number of items
ys = ys[-x_len:]
ymin = np.min(ys)
ymax = np.max(ys)
ax.set_ylim(ymin, ymax)
# Update line with new Y values
line.set_ydata(ys)
return line,
HOWEVER, the result will not be what you expect if you use blit=True. This is because blitting is attempting to only draw the parts of the graphs that have changed, and the ticks on the axes are excluded from that. If you need to change the limits and therefore the ticks, you should use blit=False in your call to FuncAnimation. Note that you will encounter a performance hit since matplotlib will have to redraw the whole plot at every frame, but if you want to change the limits, there is no way around that.
So I made some changes to the last code of this link https://www.learnpyqt.com/courses/graphics-plotting/plotting-pyqtgraph/
and I could solve the problem. x and Y axis are now autoscaling
import PyQt5
from PyQt5 import QtWidgets, QtCore
from pyqtgraph import PlotWidget, plot
import pyqtgraph as pg
import sys # We need sys so that we can pass argv to QApplication
import os
from random import randint
import serial
class MainWindow(QtWidgets.QMainWindow):
def __init__(self, *args, **kwargs):
super(MainWindow, self).__init__(*args, **kwargs)
self.graphWidget = pg.PlotWidget()
self.setCentralWidget(self.graphWidget)
# Data from serial port
self.port = 'COM8'
self.baudrate = 9600
self.tout = 0.01 # Miliseconds
# Time to update the data of the sensor signal Rs=9600baud T=1/Rs
self.tiempo = (1 / self.baudrate) * 1000
self.x_len = 200
self.x = list(range(0, self.x_len)) # 100 time points
self.y = [0] * self.x_len # 100 data points
self.graphWidget.setBackground('w')
# Markers
self.startMarker = 60 # Start marker "<"
self.endMarker = 62 # End marker ">"
# Begin Arduino communication, Port COM8, speed 9600
self.serialPort = serial.Serial(self.port, self.baudrate, timeout=self.tout)
pen = pg.mkPen(color=(255, 0, 0))
self.data_line = self.graphWidget.plot(self.x, self.y, pen=pen)
self.timer = QtCore.QTimer()
self.timer.setInterval(self.tiempo)
self.timer.timeout.connect(self.update_plot_data)
self.timer.start()
# Begin to save the arduino data
def arduinodata(self):
ck = ""
x = "z" # any value that is not an end- or startMarker
bytecount = -1 # to allow for the fact that the last increment will be one too many
# wait for the start character
while ord(x) != self.startMarker:
x = self.serialPort.read()
# save data until the end marker is found
while ord(x) != self.endMarker:
if ord(x) != self.startMarker:
ck = ck + x.decode()
bytecount += 1
x = self.serialPort.read()
return ck
def readarduino(self):
# Wait until the Arduino sends '<Arduino Ready>' - allows time for Arduino reset
# It also ensures that any bytes left over from a previous message are discarded
msg = ""
while msg.find("<Arduino is ready>") == -1:
while self.serialPort.inWaiting() == 0:
pass
# delete for example the "\r\n" that may contain the message
msg = self.arduinodata()
msg = msg.split("\r\n")
msg = ''.join(msg)
# If the sensor send very big numbers over 90000 they will be deleted
if msg and len(msg) <= 5:
msg = int(msg)
return msg
elif msg and len(msg) >= 4:
msg = int(msg)
return msg
def update_plot_data(self):
pulse = self.readarduino()
self.x = self.x[1:] # Remove the first y element.
self.x.append(self.x[-1] + 1) # Add a new value 1 higher than the last.
self.y = self.y[1:] # Remove the first
self.y.append(pulse) # Add a new random value.
self.data_line.setData(self.x, self.y) # Update the data.
app = QtWidgets.QApplication(sys.argv)
w = MainWindow()
w.show()
sys.exit(app.exec_())
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()
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)
I'm trying to plot data from mpu6050 imu with arduino. MPU6050 sends data faster than plot. Arduino code gives 6 data which are yaw, pitch, roll, ax,ay and az from serial port. I need suggestions for fast plot .
Python Code:
import serial
import matplotlib.pyplot as plt #import matplotlib library
from drawnow import *
ser = serial.Serial('COM9', 115200)
yaw = 0.0
pitch =0.0
roll =0.0
ax =0.0
ay =0.0
az =0.0
o_yaw= [0]
o_pitch= [0]
o_roll= [0]
o_ax= [0]
o_ay= [0]
o_az= [0]
plt.ion()
cnt=0
def makeFig():
plt.ylim(-1000,1000)
plt.grid(True)
plt.ylabel('Magnitude')
plt.plot(olculen_ax, 'ro-', label='ax')
plt.plot(olculen_ay, 'bo-', label='ay')
plt.plot(olculen_az, 'go-', label='az')
plt.legend()
while True:
incoming=ser.readline()
if ("hand" in incoming):
incoming=incoming.split(":")
if len(incoming)==8:
yaw = float(incoming[1])
pitch = float(incoming[2])
roll = float(incoming[3])
ax = float(incoming[4])
ay = float(incoming[5])
az = float(incoming[6])
print "Split works"
else:
print incoming
o_ax.append(ax)
o_ay.append(ay)
o_az.append(az)
o_yaw.append(yaw)
o_pitch.append(pitch)
o_roll.append(roll)
drawnow(makeFig)
plt.pause(.00001)
cnt=cnt+1
if(cnt>50):
o_ax.pop(0)
o_ay.pop(0)
o_az.pop(0)
Arduino Code (I just add loop. code derived from this):
void loop() {
if (!dmpReady) return;
while (!mpuInterrupt && fifoCount < packetSize) {
}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
mpu.resetFIFO();
//Serial.println(F("FIFO overflow!"));
} else if (mpuIntStatus & 0x02) {
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("hand:");
Serial.print(ypr[0] * 180/M_PI);
Serial.print(":");
Serial.print(ypr[1] * 180/M_PI);
Serial.print(":");
Serial.print(ypr[2] * 180/M_PI);
Serial.print(":");
Serial.print(aaWorld.x);
Serial.print(":");
Serial.print(aaWorld.y);
Serial.print(":");
Serial.print(aaWorld.z);
Serial.println(":");
}
}
The pyqtgraph module is a great solution. It is very fast and easy.
Here is new code:
from pyqtgraph.Qt import QtGui, QtCore
import numpy as np
import pyqtgraph as pg
from pyqtgraph.ptime import time
import serial
app = QtGui.QApplication([])
p = pg.plot()
p.setWindowTitle('live plot from serial')
curve = p.plot()
data = [0]
raw=serial.Serial('COM9', 115200)
def update():
global curve, data
line = raw.readline()
if ("hand" in line):
line=line.split(":")
if len(line)==8:
data.append(float(line[4]))
xdata = np.array(data, dtype='float64')
curve.setData(xdata)
app.processEvents()
timer = QtCore.QTimer()
timer.timeout.connect(update)
timer.start(0)
if __name__ == '__main__':
import sys
if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'):
QtGui.QApplication.instance().exec_()
Plotting real-time in Matplotlib? There's price to pay for all the power you are buying. Autoscaling, autoaxis, etc... It all costs time!
A much faster solution is using a toolkit such as Gtk, and doing the plotting yourself, with a canvas such as GooCanvas.
Still, there are a few things that you can do to speed up your plotting.
Why the Pause statement?
Split the communication to another thread and make the plot take the following sample when it's ready (skipping values from the input queue)
You are appending, which makes the plot rescale frequently. Remove the first sample after N samples have been acquired.
I believe it is possible to plot in the background with MatPlotLib (not sure) and then copy to the screen less frequently.