I am trying to make animated gif using matplotlib. I found simple example here : https://tomroelandts.com/articles/how-to-create-animated-gifs-with-python
And now i am trying to adapt it to my problem. Below is the code i worked out :
from __future__ import division
from mpl_toolkits.mplot3d import Axes3D
import matplotlib as mpl
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
from mpl_toolkits.mplot3d import proj3d
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as anim
class AnimatedGif:
def __init__(self,radius = 10):
############################################################
self.fig = plt.figure(frameon=False,figsize=(12,10))
self.fig.subplots_adjust(left=0, right=1, bottom=0, top=1)
ax = Axes3D(self.fig)
ax.set_xlim((-.7*radius,.7*radius))
ax.set_ylim((-.7*radius,.7*radius))
ax.set_zlim((0*radius,1.4*radius))
ax.axis('off')
ax.xaxis.pane.set_edgecolor('black')
ax.yaxis.pane.set_edgecolor('black')
ax.xaxis.pane.fill = False
ax.yaxis.pane.fill = False
ax.zaxis.pane.fill = False
self.images = []
####################################################################
def add(self,step, area='',azim = 0, elev = 7,color = 'k',numOfLayer = 10,Length = 1.,alpha = 0.8,ids = False):
from mpl_toolkits.mplot3d import Axes3D
import matplotlib as mpl
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
from mpl_toolkits.mplot3d import proj3d
################################################
ax = self.fig.gca()
verts = [zip([step*0.5+0.,5,1.,2,1,2],[0.,3,2.,2,1,2],[0.,9,3.,2,1,2])]
pc = Poly3DCollection(verts,alpha = alpha,linewidths=1, facecolor = color)
pc.set_edgecolor('k')
ax.add_collection3d(pc)
ax.view_init(azim = azim,elev=elev)
plt_im = ax
self.images.append([plt_im])
####################################################################
def save(self, filename):
animation = anim.ArtistAnimation(self.fig, self.images)
print animation
animation.save(filename, writer='imagemagick', fps=1)
##################################################################
endStep = 100
stepsize = 5
##################################################################
animated_gif = AnimatedGif()
step = 0
area = 0
animated_gif.add(step,area=str(area))
images = []
for step in range(stepsize+1, endStep,stepsize):
area = 10
animated_gif.add(step, area=str(area))
animated_gif.save('cell-animated1.gif')
I need to use Poly3DCollection to make bunch of polygons on 3d and they move with timestep. Here I just made a simple case with polygon moving in x-axis with step. But there is no output. The output is just blank white.
My confusion mainly came from lack of good examples on use of ArtistAnimation. On the documentation for it, artists argument is mentioned as "Each list entry a collection of artists that represent what needs to be enabled on each frame. These will be disabled for other frames.". However, I did not get what is exactly expected. I made the Poly3DCollection but again just passing it by adding it to the self.images was not enough as it needed to be added to the axes too (ax.add_collection(pc)) in the end. I figured directly images needed to be added so tried adding the pc to the collection and append the ax to the self.images, which was completely wrong in the end.
Also most of the error I got was :
---------------------------------------------------------------------------
AttributeError Traceback (most recent call last)
<ipython-input-1-bd3b8eac5524> in <module>()
56 animated_gif.add(step, area=str(area))
57
---> 58 animated_gif.save('cell-animated1.gif')
<ipython-input-1-bd3b8eac5524> in save(self, filename)
42 animation = anim.ArtistAnimation(self.fig, self.images)
43 print animation
---> 44 animation.save(filename, writer='imagemagick', fps=4)
45 ##################################################################
46 endStep = 100
/Users/name/anaconda2/lib/python2.7/site-packages/matplotlib/animation.pyc in save(self, filename, writer, fps, dpi, codec, bitrate, extra_args, metadata, extra_anim, savefig_kwargs)
1055 for anim in all_anim:
1056 # Clear the initial frame
-> 1057 anim._init_draw()
1058 for data in zip(*[a.new_saved_frame_seq()
1059 for a in all_anim]):
/Users/name/anaconda2/lib/python2.7/site-packages/matplotlib/animation.pyc in _init_draw(self)
1374 # Flush the needed figures
1375 for fig in figs:
-> 1376 fig.canvas.draw_idle()
1377
1378 def _pre_draw(self, framedata, blit):
AttributeError: 'NoneType' object has no attribute 'canvas'
I do not get the error at all. I could not find good description for canvas. I was passing fig to the save, so it should not be a None.
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
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.
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.
Referring to this link: https://aqibsaeed.github.io/2016-09-03-urban-sound-classification-part-1/, I am trying to make the same waveplot figure, however,i run the code through .py, there is the error:
(tensorflow) yyydeMacBook-Pro:~ yyy$ python /Users/yyy/Desktop/1.py
Traceback (most recent call last):
File "/Users/yyy/Desktop/1.py", line 82, in <module>
plot_waves(sound_names,raw_sounds)
File "/Users/yyy/Desktop/1.py", line 42, in plot_waves
librosa.display.waveplot(np.array(f),sr=22050)
AttributeError: 'module' object has no attribute 'display'
From this github issue I read that it is now necessary to import librosa.display.
[Update 2022] Apparently now also waveplot needs to be changed to waveshow, like below:
(thanks to Hammad Hassan in comments!)
import librosa.display
plt.figure(figsize=(12, 4))
librosa.display.waveshow(data, sr=sampling_rate)
If you are running librosa 0.9.0 you will run into following Error using waveplot:
AttributeError: module 'librosa.display' has no attribute 'waveplot'
instead: import librosa.display and use waveshow
path = "sample.wav"
data, sampling_rate = librosa.load(path)
librosa.display.waveshow(data, sr=sampling_rate)
just import the display
import librosa.display
plt.figure(figsize=(12, 4))
librosa.display.waveplot(data, sr=sampling_rate)
Because of the changes in version 0.6.0 of librosa you are getting those errors. I have fixed all the issues in http://aqibsaeed.github.io/2016-09-03-urban-sound-classification-part-1/ and made it work on Python 3, librosa=0.6.0
import glob
import os
import librosa
import librosa.display
import numpy as np
import matplotlib.pyplot as plt
import tensorflow as tf
from matplotlib.pyplot import specgram
%matplotlib inline
def load_sound_files(file_paths):
raw_sounds = []
for fp in file_paths:
X,sr = librosa.load(fp)
raw_sounds.append(X)
return raw_sounds
def plot_waves(sound_names,raw_sounds):
i = 1
#fig = plt.figure(figsize=(25,60), dpi = 900)
fig = plt.figure(figsize=(25,60))
for n,f in zip(sound_names,raw_sounds):
plt.subplot(10,1,i)
librosa.display.waveplot(np.array(f),sr=22050)
plt.title(n.title())
i += 1
plt.suptitle("Figure 1: Waveplot",x=0.5, y=0.915,fontsize=18)
plt.show()
def plot_specgram(sound_names,raw_sounds):
i = 1
#fig = plt.figure(figsize=(25,60), dpi = 900)
fig = plt.figure(figsize=(25,60))
for n,f in zip(sound_names,raw_sounds):
plt.subplot(10,1,i)
specgram(np.array(f), Fs=22050)
plt.title(n.title())
i += 1
plt.suptitle("Figure 2: Spectrogram",x=0.5, y=0.915,fontsize=18)
plt.show()
def plot_log_power_specgram(sound_names,raw_sounds):
i = 1
#fig = plt.figure(figsize=(25,60), dpi = 900)
fig = plt.figure(figsize=(25,60))
for n,f in zip(sound_names,raw_sounds):
plt.subplot(10,1,i)
#D = librosa.logamplitude(np.abs(librosa.stft(f))**2, ref_power=np.max)
D = librosa.core.amplitude_to_db(np.abs(librosa.stft(f))**2, ref=np.max)
librosa.display.specshow(D,x_axis='time' ,y_axis='log')
plt.title(n.title())
i += 1
plt.suptitle("Figure 3: Log power spectrogram",x=0.5, y=0.915,fontsize=18)
plt.show()
sound_file_paths = ["57320-0-0-7.wav","24074-1-0-3.wav","15564-2-0-1.wav","31323-3-0-1.wav",
"46669-4-0-35.wav","89948-5-0-0.wav","40722-8-0-4.wav",
"103074-7-3-2.wav","106905-8-0-0.wav","108041-9-0-4.wav"]
sound_names = ["air conditioner","car horn","children playing",
"dog bark","drilling","engine idling", "gun shot",
"jackhammer","siren","street music"]
raw_sounds = load_sound_files(sound_file_paths)
plot_waves(sound_names,raw_sounds)
plot_specgram(sound_names,raw_sounds)
plot_log_power_specgram(sound_names,raw_sounds)
import os
import numpy as np
%pylab inline
import glob
import librosa.display
import matplotlib.pyplot as plt
import tensorflow as tf
plt.figure(figsize=(12,4))
librosa.display.waveplot(data,sr=sampling_rate)
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)