plot_odom.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # import roslib; roslib.load_manifest('stereo_slam')
00003 import rospy
00004 import pylab
00005 import math
00006 import numpy as np
00007 import string
00008 import weakref
00009 import time
00010 import ntpath
00011 import os
00012 from matplotlib import pyplot
00013 from mpl_toolkits.mplot3d import Axes3D
00014 from nav_msgs.msg import Odometry
00015 
00016 # Global variables
00017 ax = None
00018 ax_list = []
00019 
00020 class Error(Exception):
00021   """ Base class for exceptions in this module. """
00022   pass
00023 
00024 
00025 def check_file_len(file):
00026   """ Check if the file length is > 0 """
00027   f = open(file)
00028   lines = f.readlines()
00029   f.close()
00030   return len(lines) > 0
00031 
00032 
00033 def rm_ax(ax_id):
00034   """ Remove the axes lines """
00035   if (ax_id is not None and ax_id):
00036     l = ax_id.pop(0)
00037     wl = weakref.ref(l)
00038     l.remove()
00039     del l
00040 
00041 
00042 def odometry_callback(odom_msg, filepath):
00043   """ Callback to save the odometry data into a file """
00044 
00045   # Extract the data
00046   line = ( str(odom_msg.header.stamp.secs) +
00047            str(odom_msg.header.stamp.nsecs) + ',' +
00048            str(odom_msg.header.seq) + ',' +
00049            str(odom_msg.header.stamp.secs) +
00050            str(odom_msg.header.stamp.nsecs) + ',' +
00051            str(odom_msg.child_frame_id) + ',' +
00052            str(odom_msg.header.frame_id) + ',' +
00053            str(odom_msg.pose.pose.position.x) + ',' +
00054            str(odom_msg.pose.pose.position.y) + ',' +
00055            str(odom_msg.pose.pose.position.z) + ',' +
00056            str(odom_msg.pose.pose.orientation.x) + ',' +
00057            str(odom_msg.pose.pose.orientation.y) + ',' +
00058            str(odom_msg.pose.pose.orientation.z) + ',' +
00059            str(odom_msg.pose.pose.orientation.w) + '\n')
00060 
00061   # Append the data to the file
00062   with open(filepath, "a") as odomfile:
00063       odomfile.write(line)
00064 
00065 
00066 def real_time_plot(files):
00067   """ Function to plot the data saved into the files in real time """
00068   global ax, ax_list
00069 
00070   # Defines
00071   graph_vertices_file = 'graph_vertices.txt'
00072   graph_edges_file = 'graph_edges.txt'
00073 
00074   # Define colors
00075   colors = ['g','r','b','k','c','m','y']
00076   linewidth = 2.0
00077 
00078   # Remove all previous axes
00079   for axes in ax_list:
00080     rm_ax(axes)
00081 
00082   ax_list = []
00083   i_color = 0
00084   for filename in files:
00085 
00086     # Check if file exists
00087     if (filename != "" and os.path.exists(filename) and check_file_len(filename)):
00088 
00089       # Get the file contents
00090       try:
00091         data = pylab.loadtxt(filename, delimiter=',', comments='%', usecols=(5,6,7))
00092       except Exception, e:
00093         rospy.loginfo("Impossible to read the file %s", filename)
00094         return
00095 
00096       # Check dimension
00097       if (len(data.shape) == 1):
00098         data = np.array([data])
00099 
00100       # Get legend
00101       base_name = os.path.basename(filename)
00102       base_name = os.path.splitext(base_name)
00103       base_name = base_name[0]
00104 
00105       # Plot
00106       ax_tmp = ax.plot(data[:,0], data[:,1], data[:,2], colors[i_color], linewidth=linewidth, label=base_name)
00107       ax_list.append(ax_tmp)
00108       i_color = i_color + 1
00109 
00110       # Update color
00111       if i_color >= len(colors):
00112         i_color = 0;
00113 
00114   # Plot the graph vertices and edges
00115   if (os.path.exists(graph_vertices_file) and check_file_len(graph_vertices_file)):
00116 
00117     try:
00118       data = pylab.loadtxt(graph_vertices_file, delimiter=',', usecols=(5,6,7))
00119     except Exception, e:
00120       rospy.loginfo("Impossible to read the file %s", graph_vertices_file)
00121       return
00122 
00123     if (len(data.shape) == 1):
00124       data = np.array([data])
00125     ax_tmp = ax.plot(data[:,0], data[:,1], data[:,2], colors[i_color], linewidth=linewidth, label='Stereo Slam', marker='o')
00126     ax_list.append(ax_tmp)
00127 
00128   if (os.path.exists(graph_edges_file) and check_file_len(graph_edges_file)):
00129 
00130     try:
00131       data = pylab.loadtxt(graph_edges_file, delimiter=',', usecols=(2,3,4,9,10,11))
00132     except Exception, e:
00133       rospy.loginfo("Impossible to read the file %s", graph_edges_file)
00134       return
00135 
00136     if (len(data.shape) == 1):
00137       data = np.array([data])
00138     for i in range(len(data)):
00139       vect = []
00140       vect.append([data[i,0], data[i,1], data[i,2]])
00141       vect.append([data[i,3], data[i,4], data[i,5]])
00142       vect =  np.array(vect)
00143       ax_tmp = ax.plot(vect[:,0], vect[:,1], vect[:,2], colors[i_color], linewidth=linewidth-1, linestyle='--')
00144       ax_list.append(ax_tmp)
00145 
00146   # Update the plot
00147   pyplot.draw()
00148 
00149   # Show legend
00150   if (ax_list):
00151     ax.legend_ = None
00152     ax.legend()
00153 
00154 
00155 def plot_odom(list_to_plot):
00156   """ Main node """
00157   global ax
00158 
00159   # Setup the font for the graphics
00160   font = {'family' : 'Sans',
00161           'weight' : 'normal',
00162           'size'   : 16}
00163   pylab.rc('font', **font)
00164 
00165   # Init figure
00166   fig = pylab.figure(1)
00167   ax = Axes3D(fig)
00168   ax.grid(True)
00169   ax.set_xlabel("X")
00170   ax.set_ylabel("Y")
00171   ax.set_zlabel("Z")
00172 
00173   # Determine if user enters files or topics
00174   list_of_files = []
00175   for to_plot in list_to_plot:
00176     if os.path.exists(to_plot) is False:
00177 
00178       # It is a topic, create the file
00179       filename = to_plot
00180       if (to_plot[0] == '/'):
00181         filename = filename[1:]
00182       filepath = os.path.join(os.path.expanduser('~'), '.ros', filename.replace('/', '_') + '.txt')
00183       f = open(filepath, 'w')
00184       f.close();
00185 
00186       # Add this file to the list to be plotted
00187       list_of_files.append(filepath)
00188 
00189       # Create the subscriber
00190       rospy.Subscriber(to_plot,
00191                        Odometry,
00192                        odometry_callback,
00193                        callback_args = filepath,
00194                        queue_size = 1)
00195 
00196     else:
00197       list_of_files.append(to_plot)
00198 
00199 
00200   # Start timer for real time plot
00201   timer = fig.canvas.new_timer(2000)
00202   real_time_plot(list_of_files)
00203   timer.add_callback(real_time_plot, list_of_files)
00204   timer.start()
00205   pylab.show()
00206 
00207 
00208 if __name__ == "__main__":
00209   import argparse
00210   parser = argparse.ArgumentParser(
00211           description='Plot 3D graphics of odometry data files (.txt) or ros topics in real time.',
00212           formatter_class=argparse.ArgumentDefaultsHelpFormatter)
00213   parser.add_argument('list_to_plot', metavar='L', nargs='+',
00214                    help='list of files or topics to be plotted')
00215   args = parser.parse_args()
00216   list_to_plot = args.list_to_plot
00217 
00218   try:
00219       # Init node
00220       rospy.init_node('plot_odom')
00221       plot_odom(list_to_plot)
00222       rospy.spin()
00223   except rospy.ROSInterruptException:
00224       pass


stereo_slam
Author(s): Pep Lluis Negre
autogenerated on Thu Jun 6 2019 21:40:57