00001
00002
00003 from __future__ import with_statement
00004
00005 PKG = 'mtrace_tools'
00006 import roslib; roslib.load_manifest(PKG)
00007 import rospy
00008 from ethercat_hardware.msg import MotorTrace
00009 import ethercat_hardware.msg
00010 import getopt
00011 import rosbag
00012 from mtrace_recorder import MtraceRecorder
00013
00014 WXVER = '2.8'
00015 import wxversion
00016 if wxversion.checkInstalled(WXVER):
00017 wxversion.select(WXVER)
00018 else:
00019 print >> sys.stderr, "This application requires wxPython version %s"%(WXVER)
00020 sys.exit(1)
00021
00022 import re
00023
00024 import sys, os
00025
00026 import wx
00027 from wx import richtext
00028 import threading
00029
00030 import math, time
00031 import copy
00032
00033 import matplotlib
00034 matplotlib.use('WX')
00035 from matplotlib.figure import Figure
00036 from matplotlib.backends.backend_wxagg import \
00037 FigureCanvasWxAgg as FigCanvas, \
00038 NavigationToolbar2WxAgg as NavigationToolbar
00039
00040 import pylab
00041 import numpy
00042
00043 def displayErrorDialog(parent, message):
00044 dlg = wx.MessageDialog(parent, message, caption="Error", style=(wx.OK | wx.CENTRE | wx.ICON_ERROR))
00045 dlg.ShowModal()
00046 dlg.Destroy()
00047
00048 _ros_started = False
00049
00050 class MotorTracePlotException(Exception): pass
00051
00052
00053 class MtracePlotData:
00054 __slots__ = [ 'xdata', 'ydata', 'color', 'legend' ]
00055 def __init__(self, xdata = [], ydata = [], color = 'b-', legend = '_nolegend_'):
00056 self.xdata = xdata
00057 self.ydata = ydata
00058 self.color = color
00059 self.legend = legend
00060
00061
00062 class struct_of_arrays(object):
00063 def __init__(self, list_of_structs):
00064 if type(list_of_structs).__name__ != 'list':
00065 print "object is not a list"
00066 return
00067 if len(list_of_structs) == 0:
00068 print "list is empty"
00069 return
00070
00071 names = dir(list_of_structs[0])
00072 struct_names=[]
00073 for name in names:
00074 if name[0] != '_':
00075 struct_names.append(name)
00076 self.__setattr__(name,[])
00077
00078 for struct in list_of_structs:
00079 for name in struct_names:
00080 tmp = self.__getattribute__(name)
00081 tmp.append(struct.__getattribute__(name))
00082
00083 for name in struct_names:
00084 tmp = self.__getattribute__(name)
00085 if type(tmp[0]).__name__ == 'bool' : tmp = [ int(v) for v in tmp ]
00086 array_tmp = numpy.array(tmp)
00087 self.__setattr__(name,array_tmp)
00088
00089
00090 class MotorTraceAnalyzed:
00091 def __init__(self, data):
00092 size = len(data.samples)
00093 if size <= 1 :
00094 raise MotorTracePlotException("No data in message")
00095
00096 a = struct_of_arrays(data.samples)
00097
00098 self.stamp = data.header.stamp.to_sec()
00099 self.reason = data.reason
00100
00101 self.motor_voltage_error_limit = a.motor_voltage_error_limit
00102
00103
00104 self.time = a.timestamp - a.timestamp[0]
00105 self.measured_motor_voltage = a.measured_motor_voltage
00106 self.supply_voltage = a.supply_voltage
00107 self.measured_current = a.measured_current
00108 self.executed_current = a.executed_current
00109 self.programmed_pwm = a.programmed_pwm
00110 self.velocity = a.velocity
00111 self.encoder_position = a.encoder_position
00112 self.encoder_errors = a.encoder_error_count
00113 self.enabled = a.enabled
00114
00115 self.filtered_voltage_error = a.filtered_motor_voltage_error
00116 self.filtered_abs_voltage_error = a.filtered_abs_motor_voltage_error
00117 self.filtered_current_error = a.filtered_current_error
00118 self.filtered_abs_current_error = a.filtered_abs_current_error
00119
00120 self.actuator_info = data.actuator_info
00121 self.board_info = data.board_info
00122
00123 self.backemf_constant = 1.0 / (self.actuator_info.speed_constant * 2.0 * math.pi * 1.0/60.0);
00124 self.resistance = self.actuator_info.motor_resistance
00125 self.board_resistance = self.board_info.board_resistance
00126
00127
00128 A = numpy.array([self.measured_current, self.velocity, numpy.ones(len(self.velocity))])
00129 x = numpy.linalg.lstsq(A.transpose(), self.measured_motor_voltage)
00130
00131 self.resistance_est = x[0][0]
00132 self.backemf_const_est = -x[0][1]
00133 self.voltage_offset_est = x[0][2]
00134
00135
00136 A = numpy.array([self.measured_current])
00137 x = numpy.linalg.lstsq(A.transpose(), self.measured_motor_voltage - self.velocity * self.backemf_constant)
00138
00139 self.resistance_est_no_offset = x[0][0]
00140
00141
00142 A = numpy.array([self.measured_current, numpy.ones(len(self.measured_current))])
00143 B = self.supply_voltage * self.programmed_pwm - self.measured_motor_voltage;
00144 x = numpy.linalg.lstsq(A.transpose(), B)
00145
00146 self.board_resistance_est = x[0][0]
00147
00148
00149 self.board_output_voltage = self.supply_voltage * self.programmed_pwm - self.board_resistance * self.measured_current
00150 self.backemf_voltage = self.velocity * self.actuator_info.encoder_reduction * self.backemf_constant
00151 self.resistance_voltage = self.resistance * self.measured_current
00152 self.motor_model_voltage = self.backemf_voltage + self.resistance_voltage
00153
00154 self.angular_position = numpy.mod(self.encoder_position, 2.0 * math.pi)
00155
00156
00157 self.est_resistance = numpy.zeros(len(self.measured_current))
00158 self.est_resistance_accuracy = numpy.zeros(len(self.measured_current))
00159
00160 min_current_for_est = 0.02 * self.board_info.hw_max_current + 0.005
00161 for i in range(0,len(self.measured_current)-1):
00162 if (abs(self.measured_current[i]) > min_current_for_est):
00163 self.est_resistance[i] = (self.board_output_voltage[i] - self.backemf_voltage[i]) / self.measured_current[i]
00164 self.est_resistance_accuracy[i] = 1.0 / (1.0 + abs(self.backemf_voltage[i] / self.resistance_voltage[i]))
00165
00166
00167
00168
00169
00170
00171
00172 def plotMotorVoltage(data):
00173 voltage_data = MtracePlotData()
00174 voltage_data.xdata = data.time
00175 voltage_data.ydata = data.measured_motor_voltage
00176 voltage_data.color = 'r-'
00177 voltage_data.legend = 'Measured Motor (by ADC on board)'
00178
00179 board_voltage = MtracePlotData()
00180 board_voltage.xdata = data.time
00181 board_voltage.ydata = data.board_output_voltage
00182 board_voltage.color = 'g-'
00183 board_voltage.legend = 'Board Output (PWM * Vsupply - R_brd*I)'
00184
00185 model_data = MtracePlotData()
00186 model_data.xdata = data.time
00187 model_data.ydata = data.motor_model_voltage
00188 model_data.color = 'b-'
00189 model_data.legend = 'Motor Model (backEMF + R*I)'
00190
00191 motor_board_output = MtracePlotData()
00192 motor_board_output.xdata = data.time
00193 motor_board_output.ydata = data.measured_motor_voltage - data.board_output_voltage
00194 motor_board_output.color = 'r-'
00195 motor_board_output.legend = 'Measured Motor - Board Output'
00196
00197 model_board_output = MtracePlotData()
00198 model_board_output.xdata = data.time
00199 model_board_output.ydata = data.motor_model_voltage - data.board_output_voltage
00200 model_board_output.color = 'b-'
00201 model_board_output.legend = 'Motor Model - Board Output'
00202
00203 motor_error_limit = MtracePlotData()
00204 motor_error_limit.xdata = data.time
00205 motor_error_limit.ydata = data.motor_voltage_error_limit
00206 motor_error_limit.color = 'k-'
00207
00208 motor_error_limit_low = copy.deepcopy(motor_error_limit)
00209 motor_error_limit_low.ydata = -1 * motor_error_limit.ydata
00210 motor_error_limit_low.legend = 'Motor Voltage Error Limit'
00211
00212 name = "Motor Voltage: %s" % data.actuator_info.name
00213
00214 return name, [ [voltage_data, board_voltage, model_data],
00215 [motor_board_output, model_board_output, motor_error_limit, motor_error_limit_low] ]
00216
00217
00218
00219 def plotMotorCurrent(data):
00220 executed_current_data = MtracePlotData()
00221 executed_current_data.xdata = data.time
00222 executed_current_data.ydata = data.executed_current
00223 executed_current_data.color = 'r-'
00224 executed_current_data.legend = 'Executed Motor Current'
00225
00226 measured_current_data = MtracePlotData()
00227 measured_current_data.xdata = data.time
00228 measured_current_data.ydata = data.measured_current
00229 measured_current_data.color = 'b-'
00230 measured_current_data.legend = 'Measured Motor Current'
00231
00232
00233
00234
00235
00236
00237
00238
00239 measured_current_error_data = MtracePlotData()
00240 measured_current_error_data.xdata = data.time
00241 measured_current_error_data.ydata = data.measured_current - data.executed_current
00242 measured_current_error_data.color = 'r-'
00243 measured_current_error_data.legend = 'Measured-Executed Motor Current'
00244
00245
00246 name = "Motor Current: %s" % data.actuator_info.name
00247
00248 return name, [ [executed_current_data, measured_current_data ],
00249 [measured_current_error_data] ]
00250
00251
00252
00253 def plotMotorEnabled(data):
00254 enabled_data = MtracePlotData()
00255 enabled_data.xdata = data.time
00256 enabled_data.ydata = data.enabled
00257 enabled_data.color = 'r.'
00258 enabled_data.legend = 'Motor Enabled'
00259
00260 true_data = MtracePlotData()
00261 true_data.xdata = [data.time[0], data.time[-1]]
00262 true_data.ydata = [1.01, 1.01]
00263 true_data.color = 'b-'
00264 true_data.legend = 'True'
00265
00266 false_data = MtracePlotData()
00267 false_data.xdata = [data.time[0], data.time[-1]]
00268 false_data.ydata = [-0.01, -0.01]
00269 false_data.color = 'g-'
00270 false_data.legend = 'False'
00271
00272 name = "Motor Enabled: %s" % data.actuator_info.name
00273
00274 return name, [ [true_data, false_data, enabled_data] ]
00275
00276
00277
00278 def plotSupplyVoltage(data):
00279 supply_voltage_data = MtracePlotData()
00280 supply_voltage_data.xdata = data.time
00281 supply_voltage_data.ydata = data.supply_voltage
00282 supply_voltage_data.color = 'r-'
00283 supply_voltage_data.legend = 'Supply Voltage'
00284
00285 name = "Supply Voltage: %s" % data.actuator_info.name
00286
00287 return name, [ [supply_voltage_data ] ]
00288
00289
00290
00291
00292 def plotEncoderData(data):
00293 encoder_position_data = MtracePlotData()
00294 encoder_position_data.xdata = data.time
00295 encoder_position_data.ydata = data.encoder_position
00296 encoder_position_data.color = 'r-'
00297 encoder_position_data.legend = 'Encoder position'
00298
00299 encoder_position_data = MtracePlotData()
00300 encoder_position_data.xdata = data.time
00301 encoder_position_data.ydata = data.encoder_position
00302 encoder_position_data.color = 'r-'
00303 encoder_position_data.legend = 'Encoder position'
00304
00305 encoder_velocity_data = MtracePlotData()
00306 encoder_velocity_data.xdata = data.time
00307 encoder_velocity_data.ydata = data.velocity
00308 encoder_velocity_data.color = 'b-'
00309 encoder_velocity_data.legend = 'Velocity'
00310
00311 encoder_error_data = MtracePlotData()
00312 encoder_error_data.xdata = data.time
00313 encoder_error_data.ydata = data.encoder_errors
00314 encoder_error_data.color = 'g-'
00315 encoder_error_data.legend = 'Encoder error count'
00316
00317 name = "Encoder Position and Velocity : %s" % data.actuator_info.name
00318
00319 return name, [ [encoder_position_data ],
00320 [encoder_error_data ],
00321 [encoder_velocity_data ] ]
00322
00323
00324 def plotEncoderTickData(data):
00325 reduction = data.actuator_info.pulses_per_revolution / (2. * math.pi)
00326
00327 encoder_position_data = MtracePlotData()
00328 encoder_position_data.xdata = data.time
00329 encoder_position_data.ydata = data.encoder_position * reduction
00330 encoder_position_data.color = 'r-'
00331 encoder_position_data.legend = 'Encoder position (ticks)'
00332
00333 encoder_rel_position_data = MtracePlotData()
00334 encoder_rel_position_data.xdata = data.time
00335 encoder_rel_position_data.ydata = (data.encoder_position - data.encoder_position[0]) * reduction
00336 encoder_rel_position_data.color = 'b-'
00337 encoder_rel_position_data.legend = 'Encoder relative position (ticks)'
00338
00339
00340 encoder_error_data = MtracePlotData()
00341 encoder_error_data.xdata = data.time
00342 encoder_error_data.ydata = data.encoder_errors
00343 encoder_error_data.color = 'g-'
00344 encoder_error_data.legend = 'Encoder error count'
00345
00346 name = "Encoder Position and Velocity (ticks) : %s" % data.actuator_info.name
00347
00348 return name, [ [encoder_position_data ],
00349 [encoder_rel_position_data ],
00350 [encoder_error_data ] ]
00351
00352
00353
00354
00355 def plot_error_v_position(data):
00356 voltage_error = MtracePlotData()
00357 voltage_error.xdata = data.angular_position
00358 voltage_error.ydata = data.motor_model_voltage - data.board_output_voltage
00359 voltage_error.color = 'r.'
00360 voltage_error.legend = 'Voltage Error'
00361
00362 current_error = MtracePlotData()
00363 current_error.xdata = data.angular_position
00364 current_error.ydata = data.measured_current - data.executed_current
00365 current_error.color = 'b.'
00366 current_error.legend = 'Current Error'
00367
00368 name = "Error v. Position: %s" % data.actuator_info.name
00369
00370 return name, [ [voltage_error], [current_error] ]
00371
00372
00373 def plot_motor_resistance(data):
00374 name = "Motor Resistance: %s" % data.actuator_info.name
00375
00376 resistance = MtracePlotData()
00377 resistance.xdata = data.time
00378 resistance.ydata = data.est_resistance
00379 resistance.color = 'r.'
00380 resistance.legend = 'Estimated Resistance'
00381
00382 resistance_accuracy = MtracePlotData()
00383 resistance_accuracy.xdata = data.time
00384 resistance_accuracy.ydata = data.est_resistance_accuracy
00385 resistance_accuracy.color = 'g-'
00386 resistance_accuracy.legend = 'Resistance Est. Accuracy'
00387
00388 return name, [ [resistance], [resistance_accuracy] ]
00389
00390
00391 def plot_motor_voltage_rel_error(data):
00392 name = "Motor Voltage Relative Error: %s" % data.actuator_info.name
00393
00394 measured = MtracePlotData()
00395 measured.xdata = data.time
00396 measured.ydata = data.measured_current
00397 measured.color = 'b-'
00398 measured.legend = 'Measured'
00399
00400 executed = MtracePlotData()
00401 executed.xdata = data.time
00402 executed.ydata = data.executed_current
00403 executed.color = 'r-'
00404 executed.legend = 'Executed'
00405
00406 return name, [ [ measured, executed ] ]
00407
00408 class MtraceStatsPanel(wx.Frame):
00409 def __init__(self, manager, mtrace_data):
00410 wx.Frame.__init__(self, manager, -1, 'Trace Stats : ' + mtrace_data.actuator_info.name)
00411
00412 self._manager = manager
00413
00414 self._data = mtrace_data
00415
00416 self._sizer = wx.BoxSizer(wx.VERTICAL)
00417
00418 self._text_ctrl = richtext.RichTextCtrl(self, wx.ID_ANY, wx.EmptyString, wx.DefaultPosition, wx.DefaultSize, wx.TE_READONLY)
00419 self._sizer.Add(self._text_ctrl, 1, wx.EXPAND)
00420
00421 self.SetSizer(self._sizer)
00422
00423 self._write_stats()
00424
00425 def _write_line(self, key, value):
00426 self._text_ctrl.BeginBold()
00427 self._text_ctrl.WriteText("%s: "%(key))
00428 self._text_ctrl.EndBold()
00429
00430 self._text_ctrl.WriteText(value)
00431
00432 self._text_ctrl.Newline()
00433
00434 def _write_stats(self):
00435 self._write_line('Trace Info', ' ')
00436
00437 stamp = self._data.stamp
00438 time_str = time.strftime("%m/%d/%Y %H:%M.%S %Z", time.localtime(stamp))
00439
00440 self._write_line('Timestamp', str(stamp))
00441 self._write_line('Time', time_str)
00442 self._write_line('Reason', self._data.reason)
00443 self._text_ctrl.Newline()
00444
00445 self._write_line('MCB Info', ' ')
00446 self._write_line('Description', self._data.board_info.description)
00447 self._write_line('Product Code', str(self._data.board_info.product_code))
00448 self._write_line('PCA', str(self._data.board_info.pca))
00449 self._write_line('PCB', str(self._data.board_info.pcb))
00450 self._write_line('Serial', str(self._data.board_info.serial))
00451 self._write_line('Firmware Major', str(self._data.board_info.firmware_major))
00452 self._write_line('Firmware Minor', str(self._data.board_info.firmware_minor))
00453 self._write_line('Board Resistance', str(self._data.board_info.board_resistance))
00454 self._write_line('Max PWM Ratio', str(self._data.board_info.max_pwm_ratio))
00455 self._write_line('HW Max Current', str(self._data.board_info.hw_max_current))
00456 self._write_line('Poor Measured Motor Voltage', str(self._data.board_info.poor_measured_motor_voltage))
00457 self._text_ctrl.Newline()
00458
00459 self._write_line('Motor Data', ' ')
00460 self._write_line('Acutuator Name', self._data.actuator_info.name)
00461 self._write_line('Robot Name', self._data.actuator_info.robot_name)
00462 self._write_line('Motor Make', self._data.actuator_info.motor_make)
00463 self._write_line('Motor Model', self._data.actuator_info.motor_model)
00464 self._write_line('Max Current', str(self._data.actuator_info.max_current))
00465 self._write_line('Speed Constant', str(self._data.actuator_info.speed_constant))
00466 self._write_line('Motor Resistance', str(self._data.actuator_info.motor_resistance))
00467 self._write_line('Motor Torque Constant', str(self._data.actuator_info.motor_torque_constant))
00468 self._write_line('Encoder Reduction', str(self._data.actuator_info.encoder_reduction))
00469 self._write_line('Pulses per Revolution', str(self._data.actuator_info.pulses_per_revolution))
00470
00471
00472
00473
00474 class MtracePlotPanel(wx.Frame):
00475 def __init__(self, manager, name, mtrace_data):
00476 wx.Frame.__init__(self, manager, -1, name)
00477
00478 self._manager = manager
00479
00480 self._data = mtrace_data
00481
00482 self.Bind(wx.EVT_CLOSE, self.on_close)
00483
00484 self.create_main_canvas()
00485
00486 def init_plot(self):
00487 self.dpi = 100
00488 rc = matplotlib.figure.SubplotParams(left=0.05, bottom=0.05, right=0.99, top=0.95, wspace=0.001, hspace=0.1)
00489 self.fig = Figure((3.0, 3.0), dpi=self.dpi, subplotpars=rc)
00490
00491 num_sub_plots = len(self._data)
00492
00493 fp = matplotlib.font_manager.FontProperties(size=10)
00494
00495 for i, subplot in enumerate(self._data):
00496 subplot_val = 100 * num_sub_plots + 10 + i + 1
00497
00498 axes = self.fig.add_subplot(subplot_val)
00499 axes.set_axis_bgcolor('white')
00500 pylab.setp(axes.get_xticklabels(), fontsize=6)
00501 pylab.setp(axes.get_yticklabels(), fontsize=8)
00502
00503 for data in subplot:
00504 axes.plot(data.xdata,
00505 data.ydata,
00506 data.color,
00507 linewidth = 1,
00508 picker = 5,
00509 label = data.legend
00510 )[0]
00511 axes.legend(loc='best', prop=fp)
00512
00513 axes.grid(True, color='gray')
00514 pylab.setp(axes.get_xticklabels(), visible=True)
00515
00516 def onpick(self, event = None):
00517 pass
00518
00519 def create_main_canvas(self):
00520 self.init_plot()
00521 self.canvas = FigCanvas(self, -1, self.fig)
00522 self.canvas.mpl_connect('pick_event', self.onpick)
00523
00524 self.sizer = wx.BoxSizer(wx.VERTICAL)
00525 self.sizer.Add(self.canvas, 1, flag=wx.LEFT | wx.TOP | wx.GROW)
00526 self.SetSizer(self.sizer)
00527
00528
00529
00530 def on_close(self, event):
00531 self.Destroy()
00532 pass
00533
00534
00535
00536
00537 class MtracePlotFrame(wx.Frame):
00538 def __init__(self, app, msg_recorder):
00539 wx.Frame.__init__(self, app, wx.ID_ANY, "Motor Trace Plotter")
00540
00541 self._msg_recorder = msg_recorder
00542
00543 self._plot_functions = { 'Motor Voltage and Error': plotMotorVoltage,
00544 'Motor Current and Error' : plotMotorCurrent,
00545 'Motor Resistance Estimate' : plot_motor_resistance,
00546 'Motor Voltage Relative Error': plot_motor_voltage_rel_error,
00547 'Motor Error v. Position': plot_error_v_position,
00548 'Encoder Position and Velocity' : plotEncoderData,
00549 'Encoder Position and Velocity (Ticks)' : plotEncoderTickData,
00550 'Supply Voltage' : plotSupplyVoltage,
00551 'Motor Enabled' : plotMotorEnabled
00552 }
00553
00554 plot_names = ['Statistics']
00555 plot_names.extend(self._plot_functions.keys())
00556
00557 self._filter_string = ""
00558 self._filter_msg_list = []
00559
00560
00561 file_menu= wx.Menu()
00562 self._file_menu = file_menu
00563
00564 open_menu_item = wx.MenuItem(file_menu, -1, text="&Open Bag File",help="Load MotorTrace messages bag file")
00565 self.Bind(wx.EVT_MENU, self.onOpen, open_menu_item)
00566 file_menu.AppendItem(open_menu_item)
00567
00568 save_menu_item = wx.MenuItem(file_menu, -1, text="&Save Bag File",help="Save currently selected message to bag file")
00569 self.Bind(wx.EVT_MENU, self.onSave, save_menu_item)
00570 file_menu.AppendItem(save_menu_item)
00571
00572 clear_menu_item = wx.MenuItem(file_menu, -1, text="&Clear Messages",help="Clear message list")
00573 self.Bind(wx.EVT_MENU, self.onClear, clear_menu_item)
00574 file_menu.AppendItem(clear_menu_item)
00575
00576 global _ros_started
00577 self._start_ros_menu_item = None
00578 if not _ros_started:
00579 self._start_ros_menu_item = wx.MenuItem(file_menu, -1, text="&Start ROS Listener",help="Start ROS node and listen for MotorTrace messages")
00580 self.Bind(wx.EVT_MENU, self.onStartROS, self._start_ros_menu_item)
00581 file_menu.AppendItem(self._start_ros_menu_item)
00582
00583
00584
00585
00586
00587 menu_bar = wx.MenuBar()
00588 menu_bar.Append(file_menu,"&File")
00589 self.SetMenuBar(menu_bar)
00590
00591
00592
00593
00594 self._filter_textctrl_label = wx.StaticText(self, label="Message Filter:")
00595 self._filter_textctrl = wx.TextCtrl(self, style = wx.TE_PROCESS_ENTER)
00596 self._filter_textctrl.SetToolTip(wx.ToolTip("Regex filter for messages"))
00597 self._filter_textctrl.Bind(wx.EVT_TEXT_ENTER, self.onFilterEnter)
00598
00599 self._msg_listbox_label = wx.StaticText(self, label="Message List:")
00600 self._msg_listbox = wx.ListBox(self, style=wx.LB_MULTIPLE)
00601 self._msg_listbox.SetToolTip(wx.ToolTip("Motor Msg to Analyze"))
00602
00603 self._plot_listbox_label = wx.StaticText(self, label="Plot List:")
00604 self._plot_listbox = wx.ListBox(self, style=wx.LB_MULTIPLE)
00605 self._plot_listbox.SetToolTip(wx.ToolTip("Graphs to plot"))
00606 self._plot_listbox.SetItems(plot_names)
00607
00608 self._plot_button = wx.Button(self, label="Plot")
00609 self._plot_button.Bind(wx.EVT_BUTTON, self.onMakePlot)
00610 self._plot_button.SetToolTip(wx.ToolTip("Make motor trace plot"))
00611
00612
00613 self.updateMsgListBox()
00614
00615 hsizer = wx.BoxSizer(wx.HORIZONTAL)
00616 hsizer.Add(self._filter_textctrl_label, 0, wx.EXPAND | wx.ALIGN_CENTER_VERTICAL | wx.TOP, 5)
00617 hsizer.Add(self._filter_textctrl, 1, wx.EXPAND)
00618
00619 vsizer = wx.BoxSizer(wx.VERTICAL)
00620 vsizer.Add(hsizer, 0, wx.EXPAND)
00621 vsizer.Add(self._msg_listbox_label, 0, wx.EXPAND)
00622 vsizer.Add(self._msg_listbox, 1, wx.EXPAND)
00623 vsizer.Add(self._plot_listbox_label, 0, wx.EXPAND)
00624 vsizer.Add(self._plot_listbox, 1, wx.EXPAND)
00625 vsizer.Add(self._plot_button, 0, wx.EXPAND)
00626
00627 self.SetSizer(vsizer)
00628 self.SetAutoLayout(1)
00629 vsizer.SetMinSize((600,500))
00630 vsizer.Fit(self)
00631
00632
00633 self.timer = wx.Timer(self,-1)
00634 self.Bind(wx.EVT_TIMER, self.onTimer, self.timer)
00635 self.timer.Start(1000)
00636
00637 self.Show(True)
00638
00639 def onStartROS(self,event):
00640 global _ros_started
00641 if _ros_started:
00642 displayErrorDialog(self, "ROS node already started")
00643 else:
00644 _ros_started = True
00645 self._file_menu.DestroyItem(self._start_ros_menu_item)
00646 rospy.init_node('mtrace_plotter', anonymous=True, disable_signals=True)
00647 self._msg_recorder.startRosRecording()
00648
00649
00650 def onOpen(self, event):
00651 dlg = wx.FileDialog(self, "Select bag file to open", style=(wx.FD_OPEN | wx.FD_MULTIPLE) )
00652 if dlg.ShowModal() == wx.ID_OK:
00653 bag_filenames = os.path.join(dlg.GetPaths())
00654 try:
00655 for bag_filename in bag_filenames:
00656 self._msg_recorder.loadBagFileAsync(bag_filename)
00657 except Exception, e:
00658 self.displayError("Error opening bag", str(e))
00659 return
00660 self.updateMsgListBox()
00661 dlg.Destroy()
00662
00663
00664 def onSave(self, event):
00665 msgs = self.getMsgSelections()
00666 if len(msgs) == 0:
00667 self.displayError("Error saving message", "No messages selected to save")
00668 return
00669 dlg = wx.FileDialog(self, "Select bag file to open", style=wx.FD_OPEN)
00670 if dlg.ShowModal() == wx.ID_OK:
00671 bag_filename = os.path.join(dlg.GetPaths()[0])
00672 try:
00673 bag = rosbag.Bag(bag_filename, 'w')
00674 for msg in msgs:
00675 bag.write(msg.topic, msg.msg)
00676 except Exception, e:
00677 self.displayError("Error saving messages to bag", str(e))
00678 finally:
00679 bag.close();
00680 print "Saved %d msgs to '%s'" % (len(msgs), bag_filename)
00681 dlg.Destroy()
00682
00683 def onClear(self, event):
00684 dlg = wx.MessageDialog(self, message="Are you sure you want to clear all messages?", caption="Are you sure?", style=wx.OK | wx.CANCEL | wx.CENTRE | wx.NO_DEFAULT | wx.ICON_QUESTION)
00685 if dlg.ShowModal() == wx.ID_OK:
00686 self._msg_recorder.clearMsgList()
00687 self.updateMsgListBox()
00688 dlg.Destroy()
00689
00690 def onTimer(self, event):
00691 self.updateMsgListBox()
00692
00693 def onFilterEnter(self, event):
00694 self._filter_string = self._filter_textctrl.GetValue()
00695 self.updateMsgListBox()
00696
00697 def updateMsgListBox(self):
00698 """ Update _msg_listbox with _msg_list values and filter """
00699 try:
00700 filter_re = re.compile(self._filter_string, re.IGNORECASE)
00701 except Exception, e:
00702 self.displayError("Regular Expression Error", "Regular Expression Error : " + str(e))
00703 return
00704 msgs = self._msg_recorder.getMsgList()
00705 filter_msgs = []
00706 for msg in msgs:
00707 if filter_re.search(msg.description):
00708 filter_msgs.append(msg)
00709 if filter_msgs != self._filter_msg_list:
00710
00711
00712 selected_msgs = self.getMsgSelections()
00713 self._filter_msg_list = filter_msgs
00714 descriptions = [msg.description for msg in filter_msgs]
00715 self._msg_listbox.SetItems(descriptions)
00716 for index,msg in enumerate(filter_msgs):
00717 if msg in selected_msgs:
00718 self._msg_listbox.Select(index)
00719 self._msg_listbox_label.SetLabel("Filtered Message List : Showing %d of %d messages" % (len(filter_msgs), len(msgs) ))
00720
00721
00722 def displayError(self, title, msg):
00723 wx.MessageBox(msg, title, wx.OK|wx.ICON_ERROR, self)
00724
00725 def getMsgSelections(self):
00726 """ returns list of msgs that were selected in msg_listbox """
00727 msg_index_list = self._msg_listbox.GetSelections()
00728 msgs = [ self._filter_msg_list[index] for index in msg_index_list ]
00729 return msgs
00730
00731
00732 def onMakePlot(self, event):
00733 msgs = self.getMsgSelections()
00734 if len(msgs) == 0:
00735 self.displayError("Error", "No message selected")
00736 return
00737
00738 for msg in msgs:
00739 data = MotorTraceAnalyzed(msg.msg)
00740
00741 plot_names = [self._plot_listbox.GetString(index) for index in self._plot_listbox.GetSelections()]
00742
00743 if len(plot_names) == 0:
00744 self.displayError("Error", "No plot types selected")
00745 return
00746
00747 for plot_name in plot_names:
00748 if plot_name == 'Statistics':
00749 self.make_stats_viewer(data)
00750 else:
00751 plot_fn = self._plot_functions[plot_name]
00752 self.make_plot(data, [plot_fn])
00753
00754
00755 def make_stats_viewer(self, data):
00756 viewer = MtraceStatsPanel(self, data)
00757 viewer.SetSize(wx.Size(500, 700))
00758 viewer.Layout()
00759 viewer.Show(True)
00760 viewer.Raise()
00761
00762 def make_plot(self, data, fns):
00763 for fn in fns:
00764
00765 name, plot_data = fn(data)
00766
00767 plotter = MtracePlotPanel(self, name, plot_data)
00768 plotter.SetSize(wx.Size(600, 600))
00769 plotter.Layout()
00770 plotter.Show(True)
00771 plotter.Raise()
00772
00773
00774
00775
00776 class MtracePlotApp(wx.App):
00777 def __init__(self, msg_recorder):
00778 self._msg_recorder = msg_recorder
00779 wx.App.__init__(self, clearSigInt = False)
00780
00781 def OnInit(self):
00782 self._frame = MtracePlotFrame(None, self._msg_recorder)
00783 self._frame.SetMinSize(self._frame.GetEffectiveMinSize())
00784 self._frame.Layout()
00785 self._frame.Show()
00786
00787 return True
00788
00789
00790 def usage():
00791 print """
00792 Usage : mtrace_plot [--no-ros] [-N] [-f <bagfile>]
00793 -f : Load <bagfile> on startup
00794 -N, --no-ros : don't start ROS node at startup
00795 """
00796
00797
00798 def main():
00799 global _ros_started
00800 try:
00801 msg_recorder = MtraceRecorder()
00802
00803
00804 useROS = True
00805 short_opts = 'hf:N'
00806 long_opts = ['no-ros']
00807 argv = sys.argv[1:]
00808 optlist,ignore = getopt.gnu_getopt(argv, short_opts, long_opts)
00809 for opt,arg in optlist:
00810 if (opt == "-N") or (opt == '--no-ros'):
00811 useROS = False
00812
00813 if useROS:
00814 _ros_started = True
00815 rospy.init_node('mtrace_plotter', anonymous=True, disable_signals=True)
00816 argv = rospy.myargv(argv=argv)
00817 msg_recorder.startRosRecording()
00818
00819 optlist,argv = getopt.gnu_getopt(argv, short_opts, long_opts);
00820 for opt,arg in optlist:
00821 if (opt == "-f"):
00822 msg_recorder.loadBagFileAsync(arg)
00823 elif (opt == "-h"):
00824 usage()
00825 return 0
00826 elif (opt == '-N') or (opt == '--no-ros'):
00827 pass
00828 else :
00829 print "Internal error : opt = ", opt
00830 return 1
00831
00832 app = MtracePlotApp(msg_recorder)
00833 app.MainLoop()
00834
00835 except wx.PyDeadObjectError:
00836 pass
00837 except KeyboardInterrupt:
00838 pass
00839 except:
00840 print 'Printing exc'
00841 import traceback
00842 traceback.print_exc()
00843
00844 if _ros_started:
00845 rospy.signal_shutdown("Shutdown");
00846
00847
00848 if __name__ == '__main__':
00849 main()
00850