real_time_odometry_plot.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('plot_tools')
00004 import sys
00005 import pylab
00006 import math
00007 import numpy as np
00008 import string
00009 import random
00010 import time
00011 import ntpath
00012 from matplotlib import pyplot
00013 from mpl_toolkits.mplot3d import Axes3D
00014 
00015 # Global variables
00016 len_data = 0
00017 first_iter = True
00018 colors = ['g','r','b']
00019 
00020 class Error(Exception):
00021   """ Base class for exceptions in this module. """
00022   pass
00023 
00024 def real_time_plot(files):
00025   """
00026   Function to plot the data saved into the files in real time
00027   """
00028   global len_data, first_iter, colors
00029 
00030   for i,F in enumerate(files):
00031 
00032     # Load data
00033     data = pylab.loadtxt(F, delimiter=',', skiprows=1, usecols=(5,6,7))
00034 
00035     # Check if new data
00036     if (len_data!= len(data[:,0])):
00037 
00038       # Plot
00039       label = ntpath.basename(F)
00040       label = label[0:-4]
00041       ax.plot(data[:,0], data[:,1], data[:,2], colors[i], label=label)
00042 
00043       pyplot.draw()
00044 
00045       # Update globals
00046       len_data = len(data[:,0])
00047 
00048   if (first_iter == True):
00049     ax.legend()
00050     first_iter = False
00051 
00052 if __name__ == "__main__":
00053   import argparse
00054   parser = argparse.ArgumentParser(
00055           description='Plot 3D graphics of odometry data files in real time.',
00056           formatter_class=argparse.ArgumentDefaultsHelpFormatter)
00057   parser.add_argument('ground_truth_files', 
00058           help='file with ground truth odometry', 
00059           nargs='+')
00060   parser.add_argument('-ts','--time-step',
00061           help='update frequency (in milliseconds)',
00062           default='200')
00063   args = parser.parse_args()
00064 
00065   # Init figure
00066   fig = pylab.figure(1)
00067   ax = Axes3D(fig)
00068   ax.grid(True)
00069   ax.set_title("Realtime Odometry Plot")
00070   ax.set_xlabel("X")
00071   ax.set_ylabel("Y")
00072   ax.set_zlabel("Z")
00073 
00074   timer = fig.canvas.new_timer(interval=args.time_step)
00075   timer.add_callback(real_time_plot, args.ground_truth_files)
00076   timer.start()
00077   
00078   pylab.show() 


plot_tools
Author(s): Pep Lluis Negre
autogenerated on Sat Jun 8 2019 20:10:20