$search
00001 # Software License Agreement (BSD License) 00002 # 00003 # Copyright (c) 2009, Willow Garage, Inc. 00004 # All rights reserved. 00005 # 00006 # Redistribution and use in source and binary forms, with or without 00007 # modification, are permitted provided that the following conditions 00008 # are met: 00009 # 00010 # * Redistributions of source code must retain the above copyright 00011 # notice, this list of conditions and the following disclaimer. 00012 # * Redistributions in binary form must reproduce the above 00013 # copyright notice, this list of conditions and the following 00014 # disclaimer in the documentation and/or other materials provided 00015 # with the distribution. 00016 # * Neither the name of Willow Garage, Inc. nor the names of its 00017 # contributors may be used to endorse or promote products derived 00018 # from this software without specific prior written permission. 00019 # 00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 # POSSIBILITY OF SUCH DAMAGE. 00032 00033 PKG = 'rxbag_plugins' 00034 import roslib; roslib.load_manifest(PKG) 00035 import rospy 00036 00037 import bisect 00038 import csv 00039 import sys 00040 import time 00041 00042 import Image 00043 import numpy 00044 import pylab 00045 import wx 00046 import wx.lib.wxcairo 00047 00048 from rxbag import TopicMessageView 00049 00050 from chart import Chart 00051 from plot_configure_frame import PlotConfigureFrame 00052 from plot_data_loader import PlotDataLoader 00053 import image_helper 00054 00055 class PlotView(TopicMessageView): 00056 name = 'Plot' 00057 00058 rows = [( 1, 'All'), 00059 ( 2, 'Every 2nd message'), 00060 ( 3, 'Every 3rd message'), 00061 ( 4, 'Every 4th message'), 00062 ( 5, 'Every 5th message'), 00063 ( 10, 'Every 10th message'), 00064 ( 20, 'Every 20th message'), 00065 ( 50, 'Every 50th message'), 00066 ( 100, 'Every 100th message'), 00067 (1000, 'Every 1000th message')] 00068 00069 def __init__(self, timeline, parent): 00070 TopicMessageView.__init__(self, timeline, parent) 00071 00072 self._topic = None 00073 self._message = None 00074 self._plot_paths = [] 00075 self._playhead = None 00076 self._chart = Chart() 00077 self._data_loader = None 00078 self._x_view = None 00079 self._dirty_count = 0 00080 self._csv_data_loader = None 00081 self._csv_path = None 00082 self._csv_row_stride = None 00083 00084 self._clicked_pos = None 00085 self._dragged_pos = None 00086 00087 self._configure_frame = None 00088 00089 self._max_interval_pixels = 1.0 00090 00091 tb = self.parent.ToolBar 00092 icons_dir = roslib.packages.get_pkg_dir(PKG) + '/icons/' 00093 tb.AddSeparator() 00094 tb.Bind(wx.EVT_TOOL, lambda e: self.configure(), tb.AddLabelTool(wx.ID_ANY, '', wx.Bitmap(icons_dir + 'cog.png'))) 00095 00096 self.parent.Bind(wx.EVT_SIZE, self._on_size) 00097 self.parent.Bind(wx.EVT_PAINT, self._on_paint) 00098 self.parent.Bind(wx.EVT_LEFT_DOWN, self._on_left_down) 00099 self.parent.Bind(wx.EVT_MIDDLE_DOWN, self._on_middle_down) 00100 self.parent.Bind(wx.EVT_RIGHT_DOWN, self._on_right_down) 00101 self.parent.Bind(wx.EVT_LEFT_UP, self._on_left_up) 00102 self.parent.Bind(wx.EVT_MIDDLE_UP, self._on_middle_up) 00103 self.parent.Bind(wx.EVT_RIGHT_UP, self._on_right_up) 00104 self.parent.Bind(wx.EVT_MOTION, self._on_mouse_move) 00105 self.parent.Bind(wx.EVT_MOUSEWHEEL, self._on_mousewheel) 00106 self.parent.Bind(wx.EVT_CLOSE, self._on_close) 00107 00108 wx.CallAfter(self.configure) 00109 00110 ## TopicMessageView implementation 00111 00112 def message_viewed(self, bag, msg_details): 00113 TopicMessageView.message_viewed(self, bag, msg_details) 00114 00115 topic, msg, t = msg_details 00116 00117 if not self._data_loader: 00118 self._topic = topic 00119 self.start_loading() 00120 00121 self._message = msg 00122 00123 self.playhead = (t - self.timeline.start_stamp).to_sec() 00124 00125 def message_cleared(self): 00126 self._message = None 00127 00128 TopicMessageView.message_cleared(self) 00129 00130 wx.CallAfter(self.parent.Refresh) 00131 00132 def timeline_changed(self): 00133 # If timeline end_stamp is within the plot view, then invalidate the data loader 00134 if self._x_view is not None: 00135 end_elapsed = (self.timeline.end_stamp - self.timeline.start_stamp).to_sec() 00136 if end_elapsed > self._x_view[0] and end_elapsed < self._x_view[1] and self._data_loader: 00137 self._data_loader.invalidate() 00138 00139 wx.CallAfter(self.parent.Refresh) 00140 00141 # property: plot_paths 00142 00143 def _get_plot_paths(self): return self._plot_paths 00144 00145 def _set_plot_paths(self, plot_paths): 00146 self._plot_paths = plot_paths 00147 00148 # Update the data loader with the paths to plot 00149 if self._data_loader: 00150 paths = [] 00151 for plot in self._plot_paths: 00152 for path in plot: 00153 if path not in paths: 00154 paths.append(path) 00155 00156 self._data_loader.paths = paths 00157 00158 # Update the chart with the new areas 00159 self._chart.create_areas(self._plot_paths) 00160 00161 self._update_max_interval() 00162 00163 wx.CallAfter(self.parent.Refresh) 00164 00165 plot_paths = property(_get_plot_paths, _set_plot_paths) 00166 00167 # property: playhead 00168 00169 def _get_playhead(self): return self._playhead 00170 00171 def _set_playhead(self, playhead): 00172 self._playhead = playhead 00173 00174 # Check if playhead is visible. If not, then move the view region. 00175 if self._x_view is not None: 00176 if self._playhead < self._x_view[0]: 00177 x_view = self._x_view[1] - self._x_view[0] 00178 self._x_view = (self._playhead, self._playhead + x_view) 00179 self._update_data_loader_interval() 00180 00181 elif self._playhead > self._x_view[1]: 00182 x_view = self._x_view[1] - self._x_view[0] 00183 self._x_view = (self._playhead - x_view, self._playhead) 00184 self._update_data_loader_interval() 00185 00186 wx.CallAfter(self.parent.Refresh) 00187 00188 playhead = property(_get_playhead, _set_playhead) 00189 00190 def _update_max_interval(self): 00191 if not self._data_loader: 00192 return 00193 00194 if len(self._chart.areas) > 0: 00195 secs_per_px = (self._data_loader.end_stamp - self._data_loader.start_stamp).to_sec() / self._chart._width 00196 self._data_loader.max_interval = secs_per_px * self._max_interval_pixels 00197 00198 ## Events 00199 00200 def _on_paint(self, event): 00201 if not self._data_loader or len(self._chart._areas) == 0: 00202 return 00203 00204 self._update_chart_info() 00205 00206 dc = wx.lib.wxcairo.ContextFromDC(wx.PaintDC(self.parent)) 00207 00208 self._chart.paint(dc) 00209 00210 def _update_chart_info(self): 00211 for area_index, plot in enumerate(self._plot_paths): 00212 area = self._chart.areas[area_index] 00213 00214 area.x_view = self._x_view 00215 00216 if self._message is not None: 00217 area.x_indicator = self._playhead 00218 else: 00219 area.x_indicator = None 00220 00221 area.x_range = (0.0, (self.timeline.end_stamp - self.timeline.start_stamp).to_sec()) 00222 00223 if self._data_loader.is_load_complete: 00224 area.data_alpha = 1.0 00225 else: 00226 area.data_alpha = 0.5 00227 00228 area._series_list = plot 00229 00230 data = {} 00231 for plot_path in plot: 00232 if plot_path in self._data_loader._data: 00233 data[plot_path] = self._data_loader._data[plot_path] 00234 area._series_data = data 00235 00236 def _on_size(self, event): 00237 self._chart.set_size(self.parent.ClientSize) 00238 00239 self._update_max_interval() 00240 00241 def _on_left_down(self, event): 00242 self._clicked_pos = self._dragged_pos = event.Position 00243 if len(self._chart.areas) > 0 and self._chart.areas[0].view_min_x is not None and self._chart.areas[0].view_max_x is not None: 00244 self.timeline.playhead = self.timeline.start_stamp + rospy.Duration.from_sec(max(0.01, self._chart.areas[0].x_chart_to_data(event.Position[0]))) 00245 00246 def _on_middle_down(self, event): 00247 self._clicked_pos = self._dragged_pos = event.Position 00248 00249 def _on_right_down(self, event): 00250 self._clicked_pos = self._dragged_pos = event.Position 00251 self.parent.PopupMenu(PlotPopupMenu(self.parent, self), self._clicked_pos) 00252 00253 def _on_left_up (self, event): self._on_mouse_up(event) 00254 def _on_middle_up(self, event): self._on_mouse_up(event) 00255 def _on_right_up (self, event): self._on_mouse_up(event) 00256 00257 def _on_mouse_up(self, event): self.parent.Cursor = wx.StockCursor(wx.CURSOR_ARROW) 00258 00259 def _on_mouse_move(self, event): 00260 x, y = event.Position 00261 00262 if event.Dragging(): 00263 if event.MiddleIsDown() or event.ShiftDown(): 00264 # Middle or shift: zoom 00265 00266 dx_click, dy_click = x - self._clicked_pos[0], y - self._clicked_pos[1] 00267 dx_drag, dy_drag = x - self._dragged_pos[0], y - self._dragged_pos[1] 00268 00269 if dx_drag != 0 and len(self._chart.areas) > 0: 00270 dsecs = self._chart.areas[0].dx_chart_to_data(dx_drag) # assuming areas share x axis 00271 self._translate_plot(dsecs) 00272 00273 if dy_drag != 0: 00274 self._zoom_plot(1.0 + 0.005 * dy_drag) 00275 00276 self._update_data_loader_interval() 00277 00278 wx.CallAfter(self.parent.Refresh) 00279 00280 self.parent.Cursor = wx.StockCursor(wx.CURSOR_HAND) 00281 00282 elif event.LeftIsDown(): 00283 if len(self._chart.areas) > 0 and self._chart.areas[0].view_min_x is not None and self._chart.areas[0].view_max_x is not None: 00284 self.timeline.playhead = self.timeline.start_stamp + rospy.Duration.from_sec(max(0.01, self._chart.areas[0].x_chart_to_data(x))) 00285 00286 wx.CallAfter(self.parent.Refresh) 00287 00288 self._dragged_pos = event.Position 00289 00290 def _on_mousewheel(self, event): 00291 dz = event.WheelRotation / event.WheelDelta 00292 self._zoom_plot(1.0 - dz * 0.2) 00293 00294 self._update_data_loader_interval() 00295 00296 wx.CallAfter(self.parent.Refresh) 00297 00298 def _on_close(self, event): 00299 if self._configure_frame: 00300 self._configure_frame.Close() 00301 00302 self.stop_loading() 00303 00304 event.Skip() 00305 00306 ## 00307 00308 def _update_data_loader_interval(self): 00309 self._data_loader.set_interval(self.timeline.start_stamp + rospy.Duration.from_sec(max(0.01, self._x_view[0])), 00310 self.timeline.start_stamp + rospy.Duration.from_sec(max(0.01, self._x_view[1]))) 00311 00312 def _zoom_plot(self, zoom): 00313 if self._x_view is None: 00314 self._x_view = (0.0, (self.timeline.end_stamp - self.timeline.start_stamp).to_sec()) 00315 00316 x_view_interval = self._x_view[1] - self._x_view[0] 00317 if x_view_interval == 0.0: 00318 return 00319 00320 playhead_fraction = (self._playhead - self._x_view[0]) / x_view_interval 00321 00322 new_x_view_interval = zoom * x_view_interval 00323 00324 # Enforce zoom limits (0.1s, 1.5 * range) 00325 max_zoom_interval = (self.timeline.end_stamp - self.timeline.start_stamp).to_sec() * 1.5 00326 if new_x_view_interval > max_zoom_interval: 00327 new_x_view_interval = max_zoom_interval 00328 elif new_x_view_interval < 0.1: 00329 new_x_view_interval = 0.1 00330 00331 interval_0 = self._playhead - playhead_fraction * new_x_view_interval 00332 interval_1 = interval_0 + new_x_view_interval 00333 00334 timeline_range = (self.timeline.end_stamp - self.timeline.start_stamp).to_sec() 00335 interval_0 = min(interval_0, timeline_range - 0.1) 00336 interval_1 = max(interval_1, 0.1) 00337 00338 self._x_view = (interval_0, interval_1) 00339 00340 self._update_max_interval() 00341 00342 def _translate_plot(self, dsecs): 00343 if self._x_view is None: 00344 self._x_view = (0.0, (self.timeline.end_stamp - self.timeline.start_stamp).to_sec()) 00345 00346 new_start = self._x_view[0] - dsecs 00347 new_end = self._x_view[1] - dsecs 00348 00349 timeline_range = (self.timeline.end_stamp - self.timeline.start_stamp).to_sec() 00350 new_start = min(new_start, timeline_range - 0.1) 00351 new_end = max(new_end, 0.1) 00352 00353 self._x_view = (new_start, new_end) 00354 00355 ## 00356 00357 def stop_loading(self): 00358 if self._data_loader: 00359 self._data_loader.stop() 00360 self._data_loader = None 00361 00362 def start_loading(self): 00363 if self._topic and not self._data_loader: 00364 self._data_loader = PlotDataLoader(self.timeline, self._topic) 00365 self._data_loader.add_progress_listener(self._data_loader_updated) 00366 self._data_loader.add_complete_listener(self._data_loader_complete) 00367 self._data_loader.start() 00368 00369 def _data_loader_updated(self): 00370 self._dirty_count += 1 00371 if self._dirty_count > 5: 00372 wx.CallAfter(self.parent.Refresh) 00373 self._dirty_count = 0 00374 00375 def _data_loader_complete(self): 00376 wx.CallAfter(self.parent.Refresh) 00377 00378 def configure(self): 00379 if self._configure_frame is not None or self._message is None: 00380 return 00381 00382 self._configure_frame = PlotConfigureFrame(self) 00383 00384 frame = self.parent.TopLevelParent 00385 self._configure_frame.Position = (frame.Position[0] + frame.Size[0] + 10, frame.Position[1]) 00386 self._configure_frame.Show() 00387 00388 ## Export to CSV... 00389 00390 def export_csv(self, rows): 00391 dialog = wx.FileDialog(self.parent.Parent, 'Export to CSV...', wildcard='CSV files (*.csv)|*.csv', style=wx.FD_SAVE) 00392 if dialog.ShowModal() == wx.ID_OK: 00393 if self.timeline.start_background_task('Exporting to "%s"' % dialog.Path): 00394 wx.CallAfter(wx.GetApp().GetTopWindow().StatusBar.gauge.Show) 00395 00396 export_series = set() 00397 for plot in self._plot_paths: 00398 for path in plot: 00399 export_series.add(path) 00400 00401 if self._x_view is None: 00402 self._x_view = (0.0, (self.timeline.end_stamp - self.timeline.start_stamp).to_sec()) 00403 00404 self._csv_path = dialog.Path 00405 self._csv_row_stride = rows 00406 00407 self._csv_data_loader = PlotDataLoader(self.timeline, self._topic) 00408 self._csv_data_loader.add_complete_listener(self._csv_data_loaded) 00409 self._csv_data_loader.paths = export_series 00410 self._csv_data_loader.set_interval(self.timeline.start_stamp + rospy.Duration.from_sec(max(0.01, self._x_view[0])), 00411 self.timeline.start_stamp + rospy.Duration.from_sec(max(0.01, self._x_view[1]))) 00412 self._csv_data_loader.start() 00413 00414 dialog.Destroy() 00415 00416 def _csv_data_loaded(self): 00417 # Collate data 00418 i = 0 00419 series_dict = {} 00420 unique_stamps = set() 00421 for series in self._csv_data_loader._data: 00422 d = {} 00423 series_dict[series] = d 00424 point_num = 0 00425 for x, y in self._csv_data_loader._data[series].points: 00426 if point_num % self._csv_row_stride == 0: 00427 d[x] = y 00428 unique_stamps.add(x) 00429 point_num += 1 00430 i += 1 00431 series_columns = sorted(series_dict.keys()) 00432 00433 try: 00434 csv_writer = csv.DictWriter(open(self._csv_path, 'w'), ['elapsed'] + series_columns) 00435 00436 # Write header row 00437 header_dict = { 'elapsed' : 'elapsed' } 00438 for column in series_columns: 00439 header_dict[column] = column 00440 csv_writer.writerow(header_dict) 00441 00442 # Initialize progress monitoring 00443 progress = 0 00444 def update_progress(v): 00445 wx.GetApp().TopWindow.StatusBar.progress = v 00446 total_stamps = len(unique_stamps) 00447 stamp_num = 0 00448 00449 # Write data 00450 for stamp in sorted(unique_stamps): 00451 if self.timeline.background_task_cancel: 00452 break 00453 00454 row = { 'elapsed' : stamp } 00455 for column in series_dict: 00456 if stamp in series_dict[column]: 00457 row[column] = series_dict[column][stamp] 00458 00459 csv_writer.writerow(row) 00460 00461 new_progress = int(100.0 * (float(stamp_num) / total_stamps)) 00462 if new_progress != progress: 00463 progress = new_progress 00464 wx.CallAfter(update_progress, progress) 00465 00466 stamp_num += 1 00467 00468 except Exception, ex: 00469 print >> sys.stderr, 'Error writing to CSV file: %s' % str(ex) 00470 00471 # Hide progress monitoring 00472 if not self.timeline.background_task_cancel: 00473 wx.CallAfter(wx.GetApp().TopWindow.StatusBar.gauge.Hide) 00474 00475 self.timeline.stop_background_task() 00476 00477 ## Save plot to... 00478 00479 def export_image(self): 00480 dialog = wx.FileDialog(self.parent.Parent, 'Save plot to...', wildcard='PNG files (*.png)|*.png', style=wx.FD_SAVE) 00481 if dialog.ShowModal() == wx.ID_OK: 00482 path = dialog.Path 00483 00484 bitmap = wx.EmptyBitmap(self._chart._width, self._chart._height) 00485 mem_dc = wx.MemoryDC() 00486 mem_dc.SelectObject(bitmap) 00487 mem_dc.SetBackground(wx.WHITE_BRUSH) 00488 mem_dc.Clear() 00489 cairo_dc = wx.lib.wxcairo.ContextFromDC(mem_dc) 00490 self._chart.paint(cairo_dc) 00491 mem_dc.SelectObject(wx.NullBitmap) 00492 00493 bitmap.SaveFile(path, wx.BITMAP_TYPE_PNG) 00494 00495 dialog.Destroy() 00496 00497 class PlotPopupMenu(wx.Menu): 00498 def __init__(self, parent, plot): 00499 wx.Menu.__init__(self) 00500 00501 self.parent = parent 00502 self.plot = plot 00503 00504 # Configure... 00505 configure_item = wx.MenuItem(self, wx.NewId(), 'Configure...') 00506 self.AppendItem(configure_item) 00507 self.Bind(wx.EVT_MENU, lambda e: self.plot.configure(), id=configure_item.Id) 00508 00509 # Export to PNG... 00510 export_image_item = wx.MenuItem(self, wx.NewId(), 'Export to PNG...') 00511 self.AppendItem(export_image_item) 00512 self.Bind(wx.EVT_MENU, lambda e: self.plot.export_image(), id=export_image_item.Id) 00513 00514 # Export to CSV... 00515 self.export_csv_menu = wx.Menu() 00516 self.AppendSubMenu(self.export_csv_menu, 'Export to CSV...', 'Export data to CSV file') 00517 00518 for rows, label in plot.rows: 00519 rows_item = self.ExportCSVMenuItem(self.export_csv_menu, wx.NewId(), label, rows, plot) 00520 self.export_csv_menu.AppendItem(rows_item) 00521 00522 if label == 'All': 00523 self.export_csv_menu.AppendSeparator() 00524 00525 class ExportCSVMenuItem(wx.MenuItem): 00526 def __init__(self, parent, id, label, rows, plot): 00527 wx.MenuItem.__init__(self, parent, id, label) 00528 00529 self.rows = rows 00530 self.plot = plot 00531 00532 parent.Bind(wx.EVT_MENU, self._on_menu, id=self.Id) 00533 00534 def _on_menu(self, event): 00535 self.plot.export_csv(self.rows)