Animated points leaving trail of annotations behind - python

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

Related

ECG Graph not showing on Pi

I am using an AD 8232 ECG sensor with ADC MCP3008. I have connected the ADC to Raspberry according to the instructions here (see hardware SPI) https://learn.adafruit.com/raspberry-pi-analog-to-digital-converters/mcp3008.
Then I have connected Ground of my sensor to Grnd, Vin to 3.3v and Output to A0 of ADC. other three ports of AD 8232 are not connected. After that I am using this code to read data from ADC and show it in a grap on Pi. But nothing is showing. Is there a problem in code or may be in wiring?
# program to read ECG through SPI and display an updating graph
import board
import time
import Adafruit_MCP3008
from matplotlib import pyplot as plt
from matplotlib import animation
Nsamp = 1250
def get_data_from_MCP(T, filename):
#fs = 250
#i = 0
# Nsamp = T*fs
#milestones = []
SPI_PORT = 0
SPI_DEVICE = 0
#for x in range(1, 11): # List to store Nsamp/multiples of 10 for %age calc
#milestones.append(int(x * (Nsamp / 10)))
mcp = Adafruit_MCP3008.MCP3008(spi=SPI.SpiDev(SPI_PORT, SPI_DEVICE))
values = []
percentage = 10
fig = plt.figure(figsize=(10, 5))
ax = plt.axes(xlim=(0, Nsamp), ylim=(280, 420))
# ax.get_xaxis().set_visible(False)
plt.title("ECG Real-Time Data")
plt.xlabel("Time [" + str(Nsamp / 250) + " second window]")
plt.ylabel("Amplitude")
ax.grid(True)
(graph,) = ax.plot([], [], "b")
t = list(range(0, Nsamp))
dat = []
for i in range(0, Nsamp):
dat.append(0)
def init():
graph.set_data([], [])
return (graph,)
# continuously update pyplot
def animate(i):
global t, dat
dat.append(float(spi.readline().decode("utf-8")))
dat.pop(0)
graph.set_data(t, dat)
return (graph,)
anim = animation.FuncAnimation(fig, animate, init_func=init, interval=1, blit=True)
plt.show()
I tried to run the code and the graph window appears but there is no ECG plot showing.

How do i animate subplots in python using matplotlib

import sqlite3
import matplotlib.animation as animation
import matplotlib.pyplot as plt
def animate(i):
con = sqlite3.connect('newbase1.db')
c = con.cursor()
c.execute('SELECT Cell_1_V,Cell_2_V,Cell_1_T, time_stamp FROM Measurements')
data = c.fetchall()
Cell_1_V = []
Cell_2_V = []
Cell_1_T = []
tim = []
for row in data:
Cell_1_V.append(row[0])
Cell_2_V.append(row[1])
Cell_1_T.append(row[2])
tim.append(row[3])
fig , (sb1,sb2) = plt.subplots(nrows=2,ncols= 1)
sb1.set_xlabel("TIME---->")
sb1.set_ylabel("VOLTAGE--->")
sb2.set_xlabel("TIME---->")
sb2.set_ylabel("TEMP--->")
sb1.plot(tim,Cell_1_V,label='Cell_1_V')
sb2.plot(tim, Cell_1_T, label='Cell_1_T')
sb1.legend(loc='upper right')
sb2.legend(loc='upper right')
ani = animation.FuncAnimation(plt.gcf(), animate, interval=500)
plt.tight_layout()
plt.show()
The above is the code where I am trying to animate both subplots in the same figure but all i get is an empty plot. Any help would be appreciated.
Thanks in advance.
The basics of animation are initialization and updating data within the animation function. This is the same for a single graph and a subplot. The data has been created appropriately; the x-axis is graphed as a date.
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import matplotlib.dates as mdates
from matplotlib.dates import DateFormatter
time_rng = pd.date_range('2021-01-01', freq='1d', periods=100)
vol1 = np.random.randn(100)*10
vol2 = np.random.randn(100)*10
temp1 = np.random.randn(100)+30
data = pd.DataFrame({'VOLTAGE1':vol1, 'VOLTAGE2':vol2, 'TEMP':temp1,'TIME':pd.to_datetime(time_rng)})
Cell_1_V, Cell_2_V, Cell_1_T, tim = [], [], [], []
for idx,row in data.iterrows():
Cell_1_V.append(row[0])
Cell_2_V.append(row[1])
Cell_1_T.append(row[2])
tim.append(row[3])
fig , (sb1,sb2) = plt.subplots(nrows=2,ncols= 1)
sb1.set(xlim=(mdates.date2num(tim[0]),mdates.date2num(tim[-1])), ylim=(data.VOLTAGE1.min(), data.VOLTAGE1.max()))
sb2.set(xlim=(mdates.date2num(tim[0]),mdates.date2num(tim[-1])), ylim=(data.TEMP.min(), data.TEMP.max()))
fig.subplots_adjust(hspace=0.4)
sb1.set_xlabel("TIME---->")
sb1.set_ylabel("VOLTAGE--->")
sb2.set_xlabel("TIME---->")
sb2.set_ylabel("TEMP--->")
line1, = sb1.plot([], [], label='Cell_1_V', color='C0')
line2, = sb2.plot([], [], label='Cell_1_T', color='C1')
sb1.legend(loc='upper right')
sb2.legend(loc='upper right')
date_fmt = DateFormatter("%Y-%m-%d")
sb1.xaxis.set_major_formatter(date_fmt)
sb2.xaxis.set_major_formatter(date_fmt)
line = [line1, line2]
def animate(i):
line[0].set_data(mdates.date2num(tim[:i]), Cell_1_V[:i])
line[1].set_data(mdates.date2num(tim[:i]), Cell_1_T[:i])
return line
ani = FuncAnimation(fig, animate, frames=len(data), interval=200, repeat=False)
# plt.tight_layout()
plt.show()

making animation with matplotlib.animation

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.

How to add third level of ticks in python matplotlib

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.

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

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)

Categories