Go to the documentation of this file.00001
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
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
00033 data = pylab.loadtxt(F, delimiter=',', skiprows=1, usecols=(5,6,7))
00034
00035
00036 if (len_data!= len(data[:,0])):
00037
00038
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
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
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()