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.
Related
I am trying to make a virtual flight tracker with Python and API data, and I have managed so far to draw the points, update their location every 10 seconds and annotate them. Unfortunately, they are being annotated for each position they are drawn in when in fact I want them to only be shown in the spot where the point is at that time, or else there is a constant trail of labels.
The code is below:
import requests
import json
import time
import cartopy.crs as ccrs
import cartopy
import matplotlib.pyplot as plt
from cartopy.io.img_tiles import GoogleTiles
from matplotlib import animation
#SET AXES
fig, ax = plt.subplots()
ax=plt.axes(projection=ccrs.PlateCarree())
'''
ax.set_ylim(48,61.5)
ax.set_xlim(-12.5, 3.3)
'''
#ADD OSM BASEMAP
osm_tiles=GoogleTiles(style='satellite')
ax.add_image(osm_tiles,8)
ax.stock_img()
ax.set_extent([-12.5, 3.3, 48, 61.55])
ax.add_feature(cartopy.feature.BORDERS)
#mplcursors.cursor(hover=True)
#PLOT TRACK
track, = ax.plot([], [], 'wo')
# RELOAD API EVERY 15"
def update (self):
vapi = requests.get("https://data.vatsim.net/v3/vatsim-data.json")
# LOAD DATA AS JSON
data = vapi.text
parse_json = json.loads(data)
# LOAD PILOTS
pilots = parse_json['pilots']
no_pilots = len(pilots)
# GET INFO FOR EACH AIRCRAFT
x = 0
callsigns, lat, lon, alt, head, gspeed = [], [], [], [], [], []
while x < no_pilots:
xcall = pilots[x]['callsign']
xlat = pilots[x]['latitude']
xlon = pilots[x]['longitude']
xgspeed = pilots[x]['groundspeed']
xalt = pilots[x]['altitude']
xhead = pilots[x]['heading']
callsigns.append(xcall)
lat.append(xlat)
lon.append(xlon)
alt.append(xalt)
head.append(xhead)
gspeed.append(xgspeed)
x += 1
for i, txt in enumerate(callsigns):
ax.annotate(txt, (lon[i], lat[i]))
track.set_data(lon,lat)
return track, callsigns,
anim = animation.FuncAnimation(fig, update,interval=10000, blit=False)
plt.show()
It leaves a trail of annotations like this
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.
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.
Matplotlib axes have Major and Minor ticks. How do I add a third level of tick below Minor?
For example
import matplotlib.pyplot as plt
import numpy as np
import matplotlib.ticker
t = np.arange(0.0, 100.0, 0.1)
s = np.sin(0.1*np.pi*t)*np.exp(-t*0.01)
fig, ax = plt.subplots()
plt.plot(t, s)
ax1 = ax.twiny()
ax1.plot(t, s)
ax1.xaxis.set_ticks_position('bottom')
majors = np.linspace(0, 100, 6)
minors = np.linspace(0, 100, 11)
thirds = np.linspace(0, 100, 101)
ax.xaxis.set_major_locator(matplotlib.ticker.FixedLocator(majors))
ax.xaxis.set_minor_locator(matplotlib.ticker.FixedLocator(minors))
ax1.xaxis.set_major_locator(matplotlib.ticker.FixedLocator([]))
ax1.xaxis.set_minor_locator(matplotlib.ticker.FixedLocator(thirds))
ax1.tick_params(which='minor', length=2)
ax.tick_params(which='minor', length=4)
ax.tick_params(which='major', length=6)
ax.grid(which='both',axis='x',linestyle='--')
plt.axhline(color='gray')
plt.show()
produces the effect I want using twinned x-axes.
Is there a better way?
As I stated that you can achieve what you want by deriving from some key classes, I decided to do so (but as I said, it's probably not worth the effort). Anyway, here is what I've got:
from matplotlib import pyplot as plt
from matplotlib import axes as maxes
from matplotlib import axis as maxis
import matplotlib.ticker as mticker
import matplotlib.cbook as cbook
from matplotlib.projections import register_projection
from matplotlib import ticker
import numpy as np
class SubMinorXAxis(maxis.XAxis):
def __init__(self,*args,**kwargs):
self.subminor = maxis.Ticker()
self.subminorTicks = []
self._subminor_tick_kw = dict()
super(SubMinorXAxis,self).__init__(*args,**kwargs)
def reset_ticks(self):
cbook.popall(self.subminorTicks)
##self.subminorTicks.extend([self._get_tick(major=False)])
self.subminorTicks.extend([maxis.XTick(self.axes, 0, '', major=False, **self._subminor_tick_kw)])
self._lastNumSubminorTicks = 1
super(SubMinorXAxis,self).reset_ticks()
def set_subminor_locator(self, locator):
"""
Set the locator of the subminor ticker
ACCEPTS: a :class:`~matplotlib.ticker.Locator` instance
"""
self.isDefault_minloc = False
self.subminor.locator = locator
locator.set_axis(self)
self.stale = True
def set_subminor_formatter(self, formatter):
"""
Set the formatter of the subminor ticker
ACCEPTS: A :class:`~matplotlib.ticker.Formatter` instance
"""
self.isDefault_minfmt = False
self.subminor.formatter = formatter
formatter.set_axis(self)
self.stale = True
def get_subminor_ticks(self, numticks=None):
'get the subminor tick instances; grow as necessary'
if numticks is None:
numticks = len(self.get_subminor_locator()())
if len(self.subminorTicks) < numticks:
# update the new tick label properties from the old
for i in range(numticks - len(self.subminorTicks)):
##tick = self._get_tick(major=False)
tick = maxis.XTick(self.axes, 0, '', major=False, **self._subminor_tick_kw)
self.subminorTicks.append(tick)
if self._lastNumSubminorTicks < numticks:
protoTick = self.subminorTicks[0]
for i in range(self._lastNumSubminorTicks, len(self.subminorTicks)):
tick = self.subminorTicks[i]
tick.gridOn = False
self._copy_tick_props(protoTick, tick)
self._lastNumSubminorTicks = numticks
ticks = self.subminorTicks[:numticks]
return ticks
def set_tick_params(self, which='major', reset=False, **kwargs):
if which == 'subminor':
kwtrans = self._translate_tick_kw(kwargs, to_init_kw=True)
if reset:
self.reset_ticks()
self._subminor_tick_kw.clear()
self._subminor_tick_kw.update(kwtrans)
for tick in self.subminorTicks:
tick._apply_params(**self._subminor_tick_kw)
else:
super(SubMinorXAxis, self).set_tick_params(which=which, reset=reset, **kwargs)
def cla(self):
'clear the current axis'
self.set_subminor_locator(mticker.NullLocator())
self.set_subminor_formatter(mticker.NullFormatter())
super(SubMinorXAxis,self).cla()
def iter_ticks(self):
"""
Iterate through all of the major and minor ticks.
...and through the subminors
"""
majorLocs = self.major.locator()
majorTicks = self.get_major_ticks(len(majorLocs))
self.major.formatter.set_locs(majorLocs)
majorLabels = [self.major.formatter(val, i)
for i, val in enumerate(majorLocs)]
minorLocs = self.minor.locator()
minorTicks = self.get_minor_ticks(len(minorLocs))
self.minor.formatter.set_locs(minorLocs)
minorLabels = [self.minor.formatter(val, i)
for i, val in enumerate(minorLocs)]
subminorLocs = self.subminor.locator()
subminorTicks = self.get_subminor_ticks(len(subminorLocs))
self.subminor.formatter.set_locs(subminorLocs)
subminorLabels = [self.subminor.formatter(val, i)
for i, val in enumerate(subminorLocs)]
major_minor = [
(majorTicks, majorLocs, majorLabels),
(minorTicks, minorLocs, minorLabels),
(subminorTicks, subminorLocs, subminorLabels),
]
for group in major_minor:
for tick in zip(*group):
yield tick
class SubMinorAxes(maxes.Axes):
name = 'subminor'
def _init_axis(self):
self.xaxis = SubMinorXAxis(self)
self.spines['top'].register_axis(self.xaxis)
self.spines['bottom'].register_axis(self.xaxis)
self.yaxis = maxis.YAxis(self)
self.spines['left'].register_axis(self.yaxis)
self.spines['right'].register_axis(self.yaxis)
register_projection(SubMinorAxes)
if __name__ == '__main__':
fig = plt.figure()
ax = fig.add_subplot(111,projection = 'subminor')
t = np.arange(0.0, 100.0, 0.1)
s = np.sin(0.1*np.pi*t)*np.exp(-t*0.01)
majors = np.linspace(0, 100, 6)
minors = np.linspace(0, 100, 11)
thirds = np.linspace(0, 100, 101)
ax.plot(t, s)
ax.xaxis.set_ticks_position('bottom')
ax.xaxis.set_major_locator(ticker.FixedLocator(majors))
ax.xaxis.set_minor_locator(ticker.FixedLocator(minors))
ax.xaxis.set_subminor_locator(ticker.FixedLocator(thirds))
##some things in set_tick_params are not being set correctly
##by default. For instance 'top=False' must be stated
##explicitly
ax.tick_params(which='subminor', length=2, top=False)
ax.tick_params(which='minor', length=4)
ax.tick_params(which='major', length=6)
ax.grid(which='both',axis='x',linestyle='--')
plt.show()
It's not perfect, but for the use case you provided it's working fine. I drew some ideas from this matplotlib example and by going through the source codes directly. The result looks like this:
I tested the code on both Python 2.7 and Python 3.5.
EDIT:
I noticed that the subminor gridlines would always be drawn if the grid is turned on (while I had intended for it not to be drawn at all). I rectified this in the code above, i.e. the subminor ticks should never produce grid lines. If gridlines should be implemented properly, some more work will be needed.
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)