00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 from python_qt_binding.QtCore import QPointF, QRectF, Qt
00035 from python_qt_binding.QtGui import QBrush, QCursor, QColor, QFont, QFontMetrics, QGraphicsItem, QPainterPath, QPen, QPolygonF
00036 import rospy
00037
00038 import bisect
00039 import threading
00040
00041 import plugins
00042
00043 from .index_cache_thread import IndexCacheThread
00044 from .plugin.raw_view import RawView
00045
00046
00047 class _SelectionMode(object):
00048 """
00049 SelectionMode states consolidated for readability
00050 NONE = no region marked or started
00051 LEFT_MARKED = one end of the region has been marked
00052 MARKED = both ends of the region have been marked
00053 SHIFTING = region is marked; currently dragging the region
00054 MOVE_LEFT = region is marked; currently changing the left boundry of the selected region
00055 MOVE_RIGHT = region is marked; currently changing the right boundry of the selected region
00056 """
00057 NONE = 'none'
00058 LEFT_MARKED = 'left marked'
00059 MARKED = 'marked'
00060 SHIFTING = 'shifting'
00061 MOVE_LEFT = 'move left'
00062 MOVE_RIGHT = 'move right'
00063
00064
00065 class TimelineFrame(QGraphicsItem):
00066 """
00067 TimelineFrame Draws the framing elements for the bag messages (time delimiters, labels, topic names and backgrounds)
00068 Also handles mouse callbacks since they interact closely with the drawn elements
00069 """
00070 def __init__(self):
00071 super(TimelineFrame, self).__init__()
00072
00073 self._clicked_pos = None
00074 self._dragged_pos = None
00075
00076
00077 self._start_stamp = None
00078 self._end_stamp = None
00079 self._stamp_left = None
00080 self._stamp_right = None
00081 self._history_top = 30
00082 self._history_left = 0
00083 self._history_width = 0
00084 self._history_bottom = 0
00085 self._history_bounds = {}
00086 self._margin_left = 4
00087 self._margin_right = 8
00088 self._margin_bottom = 6
00089 self._history_top = 30
00090
00091
00092 self._bag_end_color = QColor(0, 0, 0, 25)
00093 self._history_background_color_alternate = QColor(179, 179, 179, 25)
00094 self._history_background_color = QColor(204, 204, 204, 102)
00095
00096
00097
00098
00099
00100
00101
00102
00103 self._sec_divisions = [0.001, 0.005, 0.01, 0.05, 0.1, 0.5,
00104 1, 5, 15, 30,
00105 1 * 60, 2 * 60, 5 * 60, 10 * 60, 15 * 60, 30 * 60,
00106 1 * 60 * 60, 2 * 60 * 60, 3 * 60 * 60, 6 * 60 * 60, 12 * 60 * 60,
00107 1 * 60 * 60 * 24, 7 * 60 * 60 * 24]
00108 self._minor_spacing = 15
00109 self._major_spacing = 50
00110 self._major_divisions_label_indent = 3
00111 self._major_division_pen = QPen(QBrush(Qt.black), 0, Qt.DashLine)
00112 self._minor_division_pen = QPen(QBrush(QColor(153, 153, 153, 128)), 0, Qt.DashLine)
00113 self._minor_division_tick_pen = QPen(QBrush(QColor(128, 128, 128, 128)), 0)
00114
00115
00116 self.topics = []
00117 self._topics_by_datatype = {}
00118 self._topic_font_height = None
00119 self._topic_name_sizes = None
00120 self._topic_name_spacing = 3
00121 self._topic_font_size = 10.0
00122 self._topic_font = QFont("cairo")
00123 self._topic_font.setPointSize(self._topic_font_size)
00124 self._topic_font.setBold(False)
00125 self._topic_vertical_padding = 4
00126
00127
00128 self._time_tick_height = 5
00129 self._time_font_height = None
00130 self._time_font_size = 10.0
00131 self._time_font = QFont("cairo")
00132 self._time_font.setPointSize(self._time_font_size)
00133 self._time_font.setBold(False)
00134
00135
00136 self._default_brush = QBrush(Qt.black, Qt.SolidPattern)
00137 self._default_pen = QPen(Qt.black)
00138 self._default_datatype_color = QColor(0, 0, 102, 204)
00139 self._datatype_colors = {
00140 'sensor_msgs/CameraInfo': QColor(0, 0, 77, 204),
00141 'sensor_msgs/Image': QColor(0, 77, 77, 204),
00142 'sensor_msgs/LaserScan': QColor(153, 0, 0, 204),
00143 'pr2_msgs/LaserScannerSignal': QColor(153, 0, 0, 204),
00144 'pr2_mechanism_msgs/MechanismState': QColor(0, 153, 0, 204),
00145 'tf/tfMessage': QColor(0, 153, 0, 204),
00146 }
00147 self._default_msg_combine_px = 1.0
00148 self._active_message_line_width = 3
00149
00150
00151 self._selected_region_color = QColor(0, 179, 0, 21)
00152 self._selected_region_outline_top_color = QColor(0.0, 77, 0.0, 51)
00153 self._selected_region_outline_ends_color = QColor(0.0, 77, 0.0, 102)
00154 self._selecting_mode = _SelectionMode.NONE
00155 self._selected_left = None
00156 self._selected_right = None
00157 self._selection_handle_width = 3.0
00158
00159
00160 self._playhead = None
00161 self._paused = False
00162 self._playhead_pointer_size = (6, 6)
00163 self._playhead_line_width = 1
00164 self._playhead_color = QColor(255, 0, 0, 191)
00165
00166
00167 self._zoom_sensitivity = 0.005
00168 self._min_zoom_speed = 0.5
00169 self._max_zoom_speed = 2.0
00170 self._min_zoom = 0.0001
00171 self._max_zoom = 50000.0
00172
00173
00174 self._viewer_types = {}
00175 self._timeline_renderers = {}
00176 self._rendered_topics = set()
00177 self.load_plugins()
00178
00179
00180 self.index_cache_cv = threading.Condition()
00181 self.index_cache = {}
00182 self.invalidated_caches = set()
00183 self._index_cache_thread = IndexCacheThread(self)
00184
00185
00186
00187
00188
00189 def _get_playhead(self):
00190 return self._playhead
00191
00192 def _set_playhead(self, playhead):
00193 """
00194 Sets the playhead to the new position, notifies the threads and updates the scene so it will redraw
00195 :signal: emits status_bar_changed_signal if the playhead is successfully set
00196 :param playhead: Time to set the playhead to, ''rospy.Time()''
00197 """
00198 with self.scene()._playhead_lock:
00199 if playhead == self._playhead:
00200 return
00201
00202 self._playhead = playhead
00203 if self._playhead != self._end_stamp:
00204 self.scene().stick_to_end = False
00205
00206 playhead_secs = playhead.to_sec()
00207 if playhead_secs > self._stamp_right:
00208 dstamp = playhead_secs - self._stamp_right + (self._stamp_right - self._stamp_left) * 0.75
00209 if dstamp > self._end_stamp.to_sec() - self._stamp_right:
00210 dstamp = self._end_stamp.to_sec() - self._stamp_right
00211 self.translate_timeline(dstamp)
00212
00213 elif playhead_secs < self._stamp_left:
00214 dstamp = self._stamp_left - playhead_secs + (self._stamp_right - self._stamp_left) * 0.75
00215 if dstamp > self._stamp_left - self._start_stamp.to_sec():
00216 dstamp = self._stamp_left - self._start_stamp.to_sec()
00217 self.translate_timeline(-dstamp)
00218
00219
00220 for topic in self.topics:
00221 bag, entry = self.scene().get_entry(self._playhead, topic)
00222 if entry:
00223 if topic in self.scene()._playhead_positions and self.scene()._playhead_positions[topic] == (bag, entry.position):
00224 continue
00225 new_playhead_position = (bag, entry.position)
00226 else:
00227 new_playhead_position = (None, None)
00228 with self.scene()._playhead_positions_cvs[topic]:
00229 self.scene()._playhead_positions[topic] = new_playhead_position
00230 self.scene()._playhead_positions_cvs[topic].notify_all()
00231 self.scene().update()
00232 self.scene().status_bar_changed_signal.emit()
00233
00234 playhead = property(_get_playhead, _set_playhead)
00235
00236
00237 @property
00238 def _history_right(self):
00239 return self._history_left + self._history_width
00240
00241 @property
00242 def has_selected_region(self):
00243 return self._selected_left is not None and self._selected_right is not None
00244
00245 @property
00246 def play_region(self):
00247 if self.has_selected_region:
00248 return (rospy.Time.from_sec(self._selected_left), rospy.Time.from_sec(self._selected_right))
00249 else:
00250 return (self._start_stamp, self._end_stamp)
00251
00252 @property
00253 def start_stamp(self):
00254 return self._start_stamp
00255
00256 @property
00257 def end_stamp(self):
00258 return self._end_stamp
00259
00260
00261 def boundingRect(self):
00262 return QRectF(0, 0, 100, 100)
00263
00264 def paint(self, painter, option, widget):
00265 if self._start_stamp is None:
00266 return
00267
00268 self._layout()
00269 self._draw_topic_dividers(painter)
00270 self._draw_selected_region(painter)
00271 self._draw_time_divisions(painter)
00272 self._draw_topic_histories(painter)
00273 self._draw_bag_ends(painter)
00274 self._draw_topic_names(painter)
00275 self._draw_history_border(painter)
00276 self._draw_playhead(painter)
00277
00278
00279
00280
00281 def _layout(self):
00282 """
00283 Recalculates the layout of the of the timeline to take into account any changes that have occured
00284 """
00285
00286 max_topic_name_width = -1
00287 for topic in self.topics:
00288 topic_width = QFontMetrics(self._topic_font).width(topic)
00289 if max_topic_name_width <= topic_width:
00290 max_topic_name_width = topic_width
00291
00292
00293 self._topic_font_height = -1
00294 for topic in self.topics:
00295 topic_height = QFontMetrics(self._topic_font).height()
00296 if self._topic_font_height <= topic_height:
00297 self._topic_font_height = topic_height
00298
00299
00300 new_history_left = self._margin_left + max_topic_name_width + self._topic_name_spacing
00301 new_history_width = self.scene().views()[0].size().width() - new_history_left - self._margin_right
00302 updated_history = (new_history_left != self._history_left or new_history_width != self._history_width)
00303 if updated_history:
00304 self._history_left = new_history_left
00305 self._history_width = new_history_width
00306
00307
00308 self._history_bounds = {}
00309 y = self._history_top
00310 for topic in self.topics:
00311 datatype = self.scene().get_datatype(topic)
00312
00313 topic_height = None
00314 if topic in self._rendered_topics:
00315 renderer = self._timeline_renderers.get(datatype)
00316 if renderer:
00317 topic_height = renderer.get_segment_height(topic)
00318 if not topic_height:
00319 topic_height = self._topic_font_height + self._topic_vertical_padding
00320
00321 self._history_bounds[topic] = (self._history_left, y, self._history_width, topic_height)
00322
00323 y += topic_height
00324
00325
00326 new_history_bottom = max([y + h for (_, y, _, h) in self._history_bounds.values()]) - 1
00327 if new_history_bottom != self._history_bottom:
00328 self._history_bottom = new_history_bottom
00329
00330 def _draw_topic_histories(self, painter):
00331 """
00332 Draw all topic messages
00333 :param painter: allows access to paint functions,''QPainter''
00334 """
00335 for topic in sorted(self._history_bounds.keys()):
00336 self._draw_topic_history(painter, topic)
00337
00338 def _draw_topic_history(self, painter, topic):
00339 """
00340 Draw boxes corrisponding to message regions on the timeline.
00341 :param painter: allows access to paint functions,''QPainter''
00342 :param topic: the topic for which message boxes should be drawn, ''str''
00343 """
00344
00345
00346 _, y, _, h = self._history_bounds[topic]
00347
00348 msg_y = y + 2
00349 msg_height = h - 2
00350
00351 datatype = self.scene().get_datatype(topic)
00352
00353
00354 renderer = None
00355 msg_combine_interval = None
00356 if topic in self._rendered_topics:
00357 renderer = self._timeline_renderers.get(datatype)
00358 if not renderer is None:
00359 msg_combine_interval = self.map_dx_to_dstamp(renderer.msg_combine_px)
00360 if msg_combine_interval is None:
00361 msg_combine_interval = self.map_dx_to_dstamp(self._default_msg_combine_px)
00362
00363
00364 if topic not in self.index_cache:
00365 return
00366 all_stamps = self.index_cache[topic]
00367
00368
00369 end_index = bisect.bisect_left(all_stamps, self._stamp_right)
00370
00371 datatype_color = self._datatype_colors.get(datatype, self._default_datatype_color)
00372
00373 width_interval = self._history_width / (self._stamp_right - self._stamp_left)
00374
00375
00376 for (stamp_start, stamp_end) in self._find_regions(all_stamps[:end_index], self.map_dx_to_dstamp(self._default_msg_combine_px)):
00377 if stamp_end < self._stamp_left:
00378 continue
00379
00380 region_x_start = self._history_left + (stamp_start - self._stamp_left) * width_interval
00381 if region_x_start < self._history_left:
00382 region_x_start = self._history_left
00383 region_x_end = self._history_left + (stamp_end - self._stamp_left) * width_interval
00384 region_width = max(1, region_x_end - region_x_start)
00385
00386 painter.setBrush(QBrush(datatype_color))
00387 painter.setPen(QPen(datatype_color, 1))
00388 painter.drawRect(region_x_start, msg_y, region_width, msg_height)
00389
00390
00391 if topic in self.scene()._listeners:
00392 curpen = painter.pen()
00393 oldwidth = curpen.width()
00394 curpen.setWidth(self._active_message_line_width)
00395 painter.setPen(curpen)
00396 playhead_stamp = None
00397 playhead_index = bisect.bisect_right(all_stamps, self.playhead.to_sec()) - 1
00398 if playhead_index >= 0:
00399 playhead_stamp = all_stamps[playhead_index]
00400 if playhead_stamp > self._stamp_left and playhead_stamp < self._stamp_right:
00401 playhead_x = self._history_left + (all_stamps[playhead_index] - self._stamp_left) * width_interval
00402 painter.drawLine(playhead_x, msg_y, playhead_x, msg_y + msg_height)
00403 curpen.setWidth(oldwidth)
00404 painter.setPen(curpen)
00405
00406
00407 if renderer:
00408
00409 for (stamp_start, stamp_end) in self._find_regions(all_stamps[:end_index], msg_combine_interval):
00410 if stamp_end < self._stamp_left:
00411 continue
00412
00413 region_x_start = self._history_left + (stamp_start - self._stamp_left) * width_interval
00414 region_x_end = self._history_left + (stamp_end - self._stamp_left) * width_interval
00415 region_width = max(1, region_x_end - region_x_start)
00416 renderer.draw_timeline_segment(painter, topic, stamp_start, stamp_end, region_x_start, msg_y, region_width, msg_height)
00417
00418 painter.setBrush(self._default_brush)
00419 painter.setPen(self._default_pen)
00420
00421 def _draw_bag_ends(self, painter):
00422 """
00423 Draw markers to indicate the area the bag file represents within the current visible area.
00424 :param painter: allows access to paint functions,''QPainter''
00425 """
00426 x_start, x_end = self.map_stamp_to_x(self._start_stamp.to_sec()), self.map_stamp_to_x(self._end_stamp.to_sec())
00427 painter.setBrush(QBrush(self._bag_end_color))
00428 painter.drawRect(self._history_left, self._history_top, x_start - self._history_left, self._history_bottom - self._history_top)
00429 painter.drawRect(x_end, self._history_top, self._history_left + self._history_width - x_end, self._history_bottom - self._history_top)
00430 painter.setBrush(self._default_brush)
00431 painter.setPen(self._default_pen)
00432
00433 def _draw_topic_dividers(self, painter):
00434 """
00435 Draws horizontal lines between each topic to visually separate the messages
00436 :param painter: allows access to paint functions,''QPainter''
00437 """
00438 clip_left = self._history_left
00439 clip_right = self._history_left + self._history_width
00440
00441 row = 0
00442 for topic in self.topics:
00443 (x, y, w, h) = self._history_bounds[topic]
00444
00445 if row % 2 == 0:
00446 painter.setPen(Qt.lightGray)
00447 painter.setBrush(QBrush(self._history_background_color_alternate))
00448 else:
00449 painter.setPen(Qt.lightGray)
00450 painter.setBrush(QBrush(self._history_background_color))
00451 left = max(clip_left, x)
00452 painter.drawRect(left, y, min(clip_right - left, w), h)
00453 row += 1
00454 painter.setBrush(self._default_brush)
00455 painter.setPen(self._default_pen)
00456
00457 def _draw_selected_region(self, painter):
00458 """
00459 Draws a box around the selected region
00460 :param painter: allows access to paint functions,''QPainter''
00461 """
00462 if self._selected_left is None:
00463 return
00464
00465 x_left = self.map_stamp_to_x(self._selected_left)
00466 if self._selected_right is not None:
00467 x_right = self.map_stamp_to_x(self._selected_right)
00468 else:
00469 x_right = self.map_stamp_to_x(self.playhead.to_sec())
00470
00471 left = x_left
00472 top = self._history_top - self._playhead_pointer_size[1] - 5 - self._time_font_size - 4
00473 width = x_right - x_left
00474 height = self._history_top - top
00475
00476 painter.setPen(self._selected_region_color)
00477 painter.setBrush(QBrush(self._selected_region_color))
00478 painter.drawRect(left, top, width, height)
00479
00480 painter.setPen(self._selected_region_outline_ends_color)
00481 painter.setBrush(Qt.NoBrush)
00482 painter.drawLine(left, top, left, top + height)
00483 painter.drawLine(left + width, top, left + width, top + height)
00484
00485 painter.setPen(self._selected_region_outline_top_color)
00486 painter.setBrush(Qt.NoBrush)
00487 painter.drawLine(left, top, left + width, top)
00488
00489 painter.setPen(self._selected_region_outline_top_color)
00490 painter.drawLine(left, self._history_top, left, self._history_bottom)
00491 painter.drawLine(left + width, self._history_top, left + width, self._history_bottom)
00492
00493 painter.setBrush(self._default_brush)
00494 painter.setPen(self._default_pen)
00495
00496 def _draw_playhead(self, painter):
00497 """
00498 Draw a line and 2 triangles to denote the current position being viewed
00499 :param painter: ,''QPainter''
00500 """
00501 px = self.map_stamp_to_x(self.playhead.to_sec())
00502 pw, ph = self._playhead_pointer_size
00503
00504
00505 painter.setPen(QPen(self._playhead_color))
00506 painter.setBrush(QBrush(self._playhead_color))
00507 painter.drawLine(px, self._history_top - 1, px, self._history_bottom + 2)
00508
00509
00510 py = self._history_top - ph
00511 painter.drawPolygon(QPolygonF([QPointF(px, py + ph), QPointF(px + pw, py), QPointF(px - pw, py)]))
00512
00513
00514 py = self._history_bottom + 1
00515 painter.drawPolygon(QPolygonF([QPointF(px, py), QPointF(px + pw, py + ph), QPointF(px - pw, py + ph)]))
00516
00517 painter.setBrush(self._default_brush)
00518 painter.setPen(self._default_pen)
00519
00520 def _draw_history_border(self, painter):
00521 """
00522 Draw a simple black rectangle frame around the timeline view area
00523 :param painter: ,''QPainter''
00524 """
00525 bounds_width = min(self._history_width, self.scene().width())
00526 x, y, w, h = self._history_left, self._history_top, bounds_width, self._history_bottom - self._history_top
00527
00528 painter.setBrush(Qt.NoBrush)
00529 painter.setPen(Qt.black)
00530 painter.drawRect(x, y, w, h)
00531 painter.setBrush(self._default_brush)
00532 painter.setPen(self._default_pen)
00533
00534 def _draw_topic_names(self, painter):
00535 """
00536 Calculate positions of existing topic names and draw them on the left, one for each row
00537 :param painter: ,''QPainter''
00538 """
00539 topics = self._history_bounds.keys()
00540 coords = [(self._margin_left, y + (h / 2) + (self._topic_font_height / 2)) for (_, y, _, h) in self._history_bounds.values()]
00541
00542 for text, coords in zip([t.lstrip('/') for t in topics], coords):
00543 path = QPainterPath()
00544 path.addText(coords[0], coords[1], self._topic_font, text)
00545 painter.setBrush(self._default_brush)
00546 painter.setPen(self._default_pen)
00547 painter.drawPath(path)
00548
00549 def _draw_time_divisions(self, painter):
00550 """
00551 Draw vertical grid-lines showing major and minor time divisions.
00552 :param painter: allows access to paint functions,''QPainter''
00553 """
00554 x_per_sec = self.map_dstamp_to_dx(1.0)
00555 major_divisions = [s for s in self._sec_divisions if x_per_sec * s >= self._major_spacing]
00556 if len(major_divisions) == 0:
00557 major_division = max(self._sec_divisions)
00558 else:
00559 major_division = min(major_divisions)
00560
00561 minor_divisions = [s for s in self._sec_divisions if x_per_sec * s >= self._minor_spacing and major_division % s == 0]
00562 if len(minor_divisions) > 0:
00563 minor_division = min(minor_divisions)
00564 else:
00565 minor_division = None
00566
00567 start_stamp = self._start_stamp.to_sec()
00568
00569 major_stamps = list(self._get_stamps(start_stamp, major_division))
00570 self._draw_major_divisions(painter, major_stamps, start_stamp, major_division)
00571
00572 if minor_division:
00573 minor_stamps = [s for s in self._get_stamps(start_stamp, minor_division) if s not in major_stamps]
00574 self._draw_minor_divisions(painter, minor_stamps, start_stamp, minor_division)
00575
00576 def _draw_major_divisions(self, painter, stamps, start_stamp, division):
00577 """
00578 Draw black hashed vertical grid-lines showing major time divisions.
00579 :param painter: allows access to paint functions,''QPainter''
00580 """
00581 label_y = self._history_top - self._playhead_pointer_size[1] - 5
00582 for stamp in stamps:
00583 x = self.map_stamp_to_x(stamp, False)
00584
00585 label = self._get_label(division, stamp - start_stamp)
00586 label_x = x + self._major_divisions_label_indent
00587 if label_x + QFontMetrics(self._topic_font).width(label) < self.scene().width():
00588 painter.setPen(self._default_pen)
00589 painter.setBrush(QBrush(Qt.black))
00590 path = QPainterPath()
00591 path.addText(label_x, label_y, self._time_font, label)
00592 painter.drawPath(path)
00593
00594 painter.setPen(self._major_division_pen)
00595 painter.drawLine(x, label_y - self._time_tick_height - self._time_font_size, x, self._history_bottom)
00596
00597 painter.setBrush(self._default_brush)
00598 painter.setPen(self._default_pen)
00599
00600 def _draw_minor_divisions(self, painter, stamps, start_stamp, division):
00601 """
00602 Draw grey hashed vertical grid-lines showing minor time divisions.
00603 :param painter: allows access to paint functions,''QPainter''
00604 """
00605 xs = [self.map_stamp_to_x(stamp) for stamp in stamps]
00606 painter.setPen(self._minor_division_pen)
00607 for x in xs:
00608 painter.drawLine(x, self._history_top, x, self._history_bottom)
00609
00610 painter.setPen(self._minor_division_tick_pen)
00611 for x in xs:
00612 painter.drawLine(x, self._history_top - self._time_tick_height, x, self._history_top)
00613
00614 painter.setBrush(self._default_brush)
00615 painter.setPen(self._default_pen)
00616
00617
00618
00619 def handle_close(self):
00620 for renderer in self._timeline_renderers.values():
00621 renderer.close()
00622 self._index_cache_thread.stop()
00623
00624
00625
00626 def get_viewer_types(self, datatype):
00627 return [RawView] + self._viewer_types.get('*', []) + self._viewer_types.get(datatype, [])
00628
00629 def load_plugins(self):
00630 for view, timeline_renderer, msg_types in plugins.load_plugins():
00631 for msg_type in msg_types:
00632 self._viewer_types.setdefault(msg_type, []).append(view)
00633 if timeline_renderer:
00634 self._timeline_renderers[msg_type] = timeline_renderer(self)
00635
00636
00637
00638 def get_renderers(self):
00639 """
00640 :returns: a list of the currently loaded renderers for the plugins
00641 """
00642 renderers = []
00643
00644 for topic in self.topics:
00645 datatype = self.scene().get_datatype(topic)
00646 renderer = self._timeline_renderers.get(datatype)
00647 if renderer is not None:
00648 renderers.append((topic, renderer))
00649 return renderers
00650
00651 def is_renderer_active(self, topic):
00652 return topic in self._rendered_topics
00653
00654 def toggle_renderers(self):
00655 idle_renderers = len(self._rendered_topics) < len(self.topics)
00656
00657 self.set_renderers_active(idle_renderers)
00658
00659 def set_renderers_active(self, active):
00660 if active:
00661 for topic in self.topics:
00662 self._rendered_topics.add(topic)
00663 else:
00664 self._rendered_topics.clear()
00665 self.scene().update()
00666
00667 def set_renderer_active(self, topic, active):
00668 if active:
00669 if topic in self._rendered_topics:
00670 return
00671 self._rendered_topics.add(topic)
00672 else:
00673 if not topic in self._rendered_topics:
00674 return
00675 self._rendered_topics.remove(topic)
00676 self.scene().update()
00677
00678
00679
00680 def _update_index_cache(self, topic):
00681 """
00682 Updates the cache of message timestamps for the given topic.
00683 :return: number of messages added to the index cache
00684 """
00685 if self._start_stamp is None or self._end_stamp is None:
00686 return 0
00687
00688 if topic not in self.index_cache:
00689
00690 start_time = self._start_stamp
00691 topic_cache = []
00692 self.index_cache[topic] = topic_cache
00693 else:
00694 topic_cache = self.index_cache[topic]
00695
00696
00697 if topic not in self.invalidated_caches:
00698 return 0
00699
00700 if len(topic_cache) == 0:
00701 start_time = self._start_stamp
00702 else:
00703 start_time = rospy.Time.from_sec(max(0.0, topic_cache[-1]))
00704
00705 end_time = self._end_stamp
00706
00707 topic_cache_len = len(topic_cache)
00708
00709 for entry in self.scene().get_entries(topic, start_time, end_time):
00710 topic_cache.append(entry.time.to_sec())
00711
00712 if topic in self.invalidated_caches:
00713 self.invalidated_caches.remove(topic)
00714
00715 return len(topic_cache) - topic_cache_len
00716
00717 def _find_regions(self, stamps, max_interval):
00718 """
00719 Group timestamps into regions connected by timestamps less than max_interval secs apart
00720 :param start_stamp: a list of stamps, ''list''
00721 :param stamp_step: seconds between each division, ''int''
00722 """
00723 region_start, prev_stamp = None, None
00724 for stamp in stamps:
00725 if prev_stamp:
00726 if stamp - prev_stamp > max_interval:
00727 region_end = prev_stamp
00728 yield (region_start, region_end)
00729 region_start = stamp
00730 else:
00731 region_start = stamp
00732
00733 prev_stamp = stamp
00734
00735 if region_start and prev_stamp:
00736 yield (region_start, prev_stamp)
00737
00738 def _get_stamps(self, start_stamp, stamp_step):
00739 """
00740 Generate visible stamps every stamp_step
00741 :param start_stamp: beginning of timeline stamp, ''int''
00742 :param stamp_step: seconds between each division, ''int''
00743 """
00744 if start_stamp >= self._stamp_left:
00745 stamp = start_stamp
00746 else:
00747 stamp = start_stamp + int((self._stamp_left - start_stamp) / stamp_step) * stamp_step + stamp_step
00748
00749 while stamp < self._stamp_right:
00750 yield stamp
00751 stamp += stamp_step
00752
00753 def _get_label(self, division, elapsed):
00754 """
00755 :param division: number of seconds in a division, ''int''
00756 :param elapsed: seconds from the beginning, ''int''
00757 :returns: relevent time elapsed string, ''str''
00758 """
00759 secs = int(elapsed) % 60
00760
00761 mins = int(elapsed) / 60
00762 hrs = mins / 60
00763 days = hrs / 24
00764 weeks = days / 7
00765
00766 if division >= 7 * 24 * 60 * 60:
00767 return '%dw' % weeks
00768 elif division >= 24 * 60 * 60:
00769 return '%dd' % days
00770 elif division >= 60 * 60:
00771 return '%dh' % hrs
00772 elif division >= 5 * 60:
00773 return '%dm' % mins
00774 elif division >= 1:
00775 return '%d:%02d' % (mins, secs)
00776 elif division >= 0.1:
00777 return '%d.%s' % (secs, str(int(10.0 * (elapsed - int(elapsed)))))
00778 elif division >= 0.01:
00779 return '%d.%02d' % (secs, int(100.0 * (elapsed - int(elapsed))))
00780 else:
00781 return '%d.%03d' % (secs, int(1000.0 * (elapsed - int(elapsed))))
00782
00783
00784 def map_x_to_stamp(self, x, clamp_to_visible=True):
00785 """
00786 converts a pixel x value to a stamp
00787 :param x: pixel value to be converted, ''int''
00788 :param clamp_to_visible: disallow values that are greater than the current timeline bounds,''bool''
00789 :returns: timestamp, ''int''
00790 """
00791 fraction = float(x - self._history_left) / self._history_width
00792
00793 if clamp_to_visible:
00794 if fraction <= 0.0:
00795 return self._stamp_left
00796 elif fraction >= 1.0:
00797 return self._stamp_right
00798
00799 return self._stamp_left + fraction * (self._stamp_right - self._stamp_left)
00800
00801 def map_dx_to_dstamp(self, dx):
00802 """
00803 converts a distance in pixel space to a distance in stamp space
00804 :param dx: distance in pixel space to be converted, ''int''
00805 :returns: distance in stamp space, ''float''
00806 """
00807 return float(dx) * (self._stamp_right - self._stamp_left) / self._history_width
00808
00809 def map_stamp_to_x(self, stamp, clamp_to_visible=True):
00810 """
00811 converts a timestamp to the x value where that stamp exists in the timeline
00812 :param stamp: timestamp to be converted, ''int''
00813 :param clamp_to_visible: disallow values that are greater than the current timeline bounds,''bool''
00814 :returns: # of pixels from the left boarder, ''int''
00815 """
00816 if self._stamp_left is None:
00817 return None
00818 fraction = (stamp - self._stamp_left) / (self._stamp_right - self._stamp_left)
00819
00820 if clamp_to_visible:
00821 fraction = min(1.0, max(0.0, fraction))
00822
00823 return self._history_left + fraction * self._history_width
00824
00825 def map_dstamp_to_dx(self, dstamp):
00826 return (float(dstamp) * self._history_width) / (self._stamp_right - self._stamp_left)
00827
00828
00829 def reset_timeline(self):
00830 self.reset_zoom()
00831
00832 self._selected_left = None
00833 self._selected_right = None
00834 self._selecting_mode = _SelectionMode.NONE
00835
00836 if self._stamp_left is not None:
00837 self.playhead = rospy.Time.from_sec(self._stamp_left)
00838
00839 def set_timeline_view(self, stamp_left, stamp_right):
00840 self._stamp_left = stamp_left
00841 self._stamp_right = stamp_right
00842
00843 def translate_timeline(self, dstamp):
00844 self.set_timeline_view(self._stamp_left + dstamp, self._stamp_right + dstamp)
00845 self.scene().update()
00846
00847 def translate_timeline_left(self):
00848 self.translate_timeline((self._stamp_right - self._stamp_left) * -0.05)
00849
00850 def translate_timeline_right(self):
00851 self.translate_timeline((self._stamp_right - self._stamp_left) * 0.05)
00852
00853
00854 def reset_zoom(self):
00855 start_stamp, end_stamp = self._start_stamp, self._end_stamp
00856 if start_stamp is None:
00857 return
00858
00859 if (end_stamp - start_stamp) < rospy.Duration.from_sec(5.0):
00860 end_stamp = start_stamp + rospy.Duration.from_sec(5.0)
00861
00862 self.set_timeline_view(start_stamp.to_sec(), end_stamp.to_sec())
00863 self.scene().update()
00864
00865 def zoom_in(self):
00866 self.zoom_timeline(0.5)
00867
00868 def zoom_out(self):
00869 self.zoom_timeline(2.0)
00870
00871 def can_zoom_in(self):
00872 return self.can_zoom(0.5)
00873
00874 def can_zoom_out(self):
00875 return self.can_zoom(2.0)
00876
00877 def can_zoom(self, desired_zoom):
00878 if not self._stamp_left or not self.playhead:
00879 return False
00880
00881 new_interval = self.get_zoom_interval(desired_zoom)
00882 if not new_interval:
00883 return False
00884
00885 new_range = new_interval[1] - new_interval[0]
00886 curr_range = self._stamp_right - self._stamp_left
00887 actual_zoom = new_range / curr_range
00888
00889 if desired_zoom < 1.0:
00890 return actual_zoom < 0.95
00891 else:
00892 return actual_zoom > 1.05
00893
00894 def zoom_timeline(self, zoom):
00895 interval = self.get_zoom_interval(zoom)
00896 if not interval:
00897 return
00898
00899 self._stamp_left, self._stamp_right = interval
00900
00901 self.scene().update()
00902
00903 def get_zoom_interval(self, zoom):
00904 if self._stamp_left is None:
00905 return None
00906
00907 stamp_interval = self._stamp_right - self._stamp_left
00908 playhead_fraction = (self.playhead.to_sec() - self._stamp_left) / stamp_interval
00909
00910 new_stamp_interval = zoom * stamp_interval
00911 if new_stamp_interval == 0:
00912 return None
00913
00914 px_per_sec = self._history_width / new_stamp_interval
00915 if px_per_sec < self._min_zoom:
00916 new_stamp_interval = self._history_width / self._min_zoom
00917 elif px_per_sec > self._max_zoom:
00918 new_stamp_interval = self._history_width / self._max_zoom
00919
00920 left = self.playhead.to_sec() - playhead_fraction * new_stamp_interval
00921 right = left + new_stamp_interval
00922
00923 return (left, right)
00924
00925
00926 def on_middle_down(self, event):
00927 self._clicked_pos = self._dragged_pos = event.pos()
00928 self._paused = True
00929
00930 def on_left_down(self, event):
00931 if self.playhead == None:
00932 return
00933
00934 self._clicked_pos = self._dragged_pos = event.pos()
00935
00936 self._paused = True
00937
00938 if event.modifiers() == Qt.ShiftModifier:
00939 return
00940
00941 x = self._clicked_pos.x()
00942 y = self._clicked_pos.y()
00943 if x >= self._history_left and x <= self._history_right:
00944 if y >= self._history_top and y <= self._history_bottom:
00945
00946 playhead_secs = self.map_x_to_stamp(x)
00947 if playhead_secs <= 0.0:
00948 self.playhead = rospy.Time(0, 1)
00949 else:
00950 self.playhead = rospy.Time.from_sec(playhead_secs)
00951 self.scene().update()
00952
00953 elif y <= self._history_top:
00954
00955 if self._selecting_mode == _SelectionMode.NONE:
00956 self._selected_left = None
00957 self._selected_right = None
00958 self._selecting_mode = _SelectionMode.LEFT_MARKED
00959 self.scene().update()
00960
00961 elif self._selecting_mode == _SelectionMode.MARKED:
00962 left_x = self.map_stamp_to_x(self._selected_left)
00963 right_x = self.map_stamp_to_x(self._selected_right)
00964 if x < left_x - self._selection_handle_width or x > right_x + self._selection_handle_width:
00965 self._selected_left = None
00966 self._selected_right = None
00967 self._selecting_mode = _SelectionMode.LEFT_MARKED
00968 self.scene().update()
00969 elif self._selecting_mode == _SelectionMode.SHIFTING:
00970 self.scene().views()[0].setCursor(QCursor(Qt.ClosedHandCursor))
00971
00972 def on_mouse_up(self, event):
00973 self._paused = False
00974
00975 if self._selecting_mode in [_SelectionMode.LEFT_MARKED, _SelectionMode.MOVE_LEFT, _SelectionMode.MOVE_RIGHT, _SelectionMode.SHIFTING]:
00976 if self._selected_left is None:
00977 self._selecting_mode = _SelectionMode.NONE
00978 else:
00979 self._selecting_mode = _SelectionMode.MARKED
00980 self.scene().views()[0].setCursor(QCursor(Qt.ArrowCursor))
00981 self.scene().update()
00982
00983 def on_mousewheel(self, event):
00984 dz = event.delta() / 120.0
00985 self.zoom_timeline(1.0 - dz * 0.2)
00986
00987 def on_mouse_move(self, event):
00988 if not self._history_left:
00989 return
00990
00991 x = event.pos().x()
00992 y = event.pos().y()
00993
00994 if event.buttons() == Qt.NoButton:
00995
00996 if self._selecting_mode in [_SelectionMode.MARKED, _SelectionMode.MOVE_LEFT, _SelectionMode.MOVE_RIGHT, _SelectionMode.SHIFTING]:
00997 if y <= self._history_top and self._selected_left is not None:
00998 left_x = self.map_stamp_to_x(self._selected_left)
00999 right_x = self.map_stamp_to_x(self._selected_right)
01000
01001 if abs(x - left_x) <= self._selection_handle_width:
01002 self._selecting_mode = _SelectionMode.MOVE_LEFT
01003 self.scene().views()[0].setCursor(QCursor(Qt.SizeHorCursor))
01004 return
01005 elif abs(x - right_x) <= self._selection_handle_width:
01006 self._selecting_mode = _SelectionMode.MOVE_RIGHT
01007 self.scene().views()[0].setCursor(QCursor(Qt.SizeHorCursor))
01008 return
01009 elif x > left_x and x < right_x:
01010 self._selecting_mode = _SelectionMode.SHIFTING
01011 self.scene().views()[0].setCursor(QCursor(Qt.OpenHandCursor))
01012 return
01013 else:
01014 self._selecting_mode = _SelectionMode.MARKED
01015 self.scene().views()[0].setCursor(QCursor(Qt.ArrowCursor))
01016 else:
01017
01018 if event.buttons() == Qt.MidButton or event.modifiers() == Qt.ShiftModifier:
01019
01020 dx_drag, dy_drag = x - self._dragged_pos.x(), y - self._dragged_pos.y()
01021
01022 if dx_drag != 0:
01023 self.translate_timeline(-self.map_dx_to_dstamp(dx_drag))
01024 if (dx_drag == 0 and abs(dy_drag) > 0) or (dx_drag != 0 and abs(float(dy_drag) / dx_drag) > 0.2 and abs(dy_drag) > 1):
01025 zoom = min(self._max_zoom_speed, max(self._min_zoom_speed, 1.0 + self._zoom_sensitivity * dy_drag))
01026 self.zoom_timeline(zoom)
01027
01028 self.scene().views()[0].setCursor(QCursor(Qt.ClosedHandCursor))
01029 elif event.buttons() == Qt.LeftButton:
01030
01031 clicked_x = self._clicked_pos.x()
01032 clicked_y = self._clicked_pos.y()
01033
01034 x_stamp = self.map_x_to_stamp(x)
01035
01036 if y <= self._history_top:
01037 if self._selecting_mode == _SelectionMode.LEFT_MARKED:
01038
01039 clicked_x_stamp = self.map_x_to_stamp(clicked_x)
01040
01041 self._selected_left = min(clicked_x_stamp, x_stamp)
01042 self._selected_right = max(clicked_x_stamp, x_stamp)
01043 self.scene().update()
01044
01045 elif self._selecting_mode == _SelectionMode.MOVE_LEFT:
01046 self._selected_left = x_stamp
01047 self.scene().update()
01048
01049 elif self._selecting_mode == _SelectionMode.MOVE_RIGHT:
01050 self._selected_right = x_stamp
01051 self.scene().update()
01052
01053 elif self._selecting_mode == _SelectionMode.SHIFTING:
01054 dx_drag = x - self._dragged_pos.x()
01055 dstamp = self.map_dx_to_dstamp(dx_drag)
01056
01057 self._selected_left = max(self._start_stamp.to_sec(), min(self._end_stamp.to_sec(), self._selected_left + dstamp))
01058 self._selected_right = max(self._start_stamp.to_sec(), min(self._end_stamp.to_sec(), self._selected_right + dstamp))
01059 self.scene().update()
01060
01061 elif clicked_x >= self._history_left and clicked_x <= self._history_right and clicked_y >= self._history_top and clicked_y <= self._history_bottom:
01062
01063 if x_stamp <= 0.0:
01064 self.playhead = rospy.Time(0, 1)
01065 else:
01066 self.playhead = rospy.Time.from_sec(x_stamp)
01067 self.scene().update()
01068 self._dragged_pos = event.pos()