34 from python_qt_binding.QtCore
import qDebug, QPointF, QRectF, Qt, qWarning, Signal
35 from python_qt_binding.QtGui
import QBrush, QCursor, QColor, QFont, \
36 QFontMetrics, QPen, QPolygonF
37 from python_qt_binding.QtWidgets
import QGraphicsItem
43 from .index_cache_thread
import IndexCacheThread
44 from .plugins.raw_view
import RawView
50 SelectionMode states consolidated for readability 51 NONE = no region marked or started 52 LEFT_MARKED = one end of the region has been marked 53 MARKED = both ends of the region have been marked 54 SHIFTING = region is marked; currently dragging the region 55 MOVE_LEFT = region is marked; currently changing the left boundry of the selected region 56 MOVE_RIGHT = region is marked; currently changing the right boundry of the selected region 59 LEFT_MARKED =
'left marked' 62 MOVE_LEFT =
'move left' 63 MOVE_RIGHT =
'move right' 69 TimelineFrame Draws the framing elements for the bag messages 70 (time delimiters, labels, topic names and backgrounds). 71 Also handles mouse callbacks since they interact closely with the drawn elements 75 super(TimelineFrame, self).
__init__()
110 1 * 60, 2 * 60, 5 * 60, 10 * 60, 15 * 60, 30 * 60,
111 1 * 60 * 60, 2 * 60 * 60, 3 * 60 * 60, 6 * 60 * 60, 12 * 60 * 60,
112 1 * 60 * 60 * 24, 7 * 60 * 60 * 24]
130 self._topic_font.setBold(
False)
141 self._time_font.setBold(
False)
148 'sensor_msgs/CameraInfo': QColor(0, 0, 77, 204),
149 'sensor_msgs/Image': QColor(0, 77, 77, 204),
150 'sensor_msgs/LaserScan': QColor(153, 0, 0, 204),
151 'pr2_msgs/LaserScannerSignal': QColor(153, 0, 0, 204),
152 'pr2_mechanism_msgs/MechanismState': QColor(0, 153, 0, 204),
153 'tf/tfMessage': QColor(0, 153, 0, 204),
204 Sets the playhead to the new position, notifies the threads and updates the scene 206 :signal: emits status_bar_changed_signal if the playhead is successfully set 207 :param playhead: Time to set the playhead to, ''rospy.Time()'' 209 with self.scene()._playhead_lock:
215 self.scene().stick_to_end =
False 217 playhead_secs = playhead.to_sec()
221 if dstamp > self._end_stamp.to_sec() - self.
_stamp_right:
228 if dstamp > self.
_stamp_left - self._start_stamp.to_sec():
229 dstamp = self.
_stamp_left - self._start_stamp.to_sec()
234 bag, entry = self.scene().get_entry(self.
_playhead, topic)
236 if topic
in self.scene()._playhead_positions
and \
237 self.scene()._playhead_positions[topic] == (bag, entry.position):
239 new_playhead_position = (bag, entry.position)
241 new_playhead_position = (
None,
None)
242 with self.scene()._playhead_positions_cvs[topic]:
243 self.scene()._playhead_positions[topic] = new_playhead_position
245 self.scene()._playhead_positions_cvs[topic].notify_all()
246 self.scene().update()
247 self.scene().status_bar_changed_signal.emit()
249 playhead = property(_get_playhead, _set_playhead)
270 if(play_region[0]
is not None and play_region[1]
is not None):
271 self.scene().selected_region_changed.emit(*play_region)
288 def paint(self, painter, option, widget):
310 This function trims the topic name down to a reasonable percentage of the viewable scene 315 trimmed_return = topic_name
319 split_name = topic_name.split(
'/')
320 split_name = list(filter(
lambda a: a !=
'', split_name))
323 if self.
_qfont_width(split_name[-1]) < .5 * allowed_width:
325 last_item = split_name[-1]
326 split_name = split_name[:-1]
327 allowed_width = allowed_width - self.
_qfont_width(last_item)
329 for item
in split_name:
330 if self.
_qfont_width(item) > allowed_width / float(len(split_name)):
331 trimmed_item = item[:-3] +
'..' 332 while self.
_qfont_width(trimmed_item) > allowed_width / float(len(split_name)):
333 if len(trimmed_item) >= 3:
334 trimmed_item = trimmed_item[:-3] +
'..' 337 trimmed = trimmed +
'/' + trimmed_item
339 trimmed = trimmed +
'/' + item
341 trimmed = trimmed +
'/' + last_item
342 trimmed = trimmed[1:]
343 trimmed_return = trimmed
344 return trimmed_return
348 Recalculates the layout of the of the timeline to take into account any changes that have 354 max_topic_name_width = -1
357 if max_topic_name_width <= topic_width:
358 max_topic_name_width = topic_width
363 topic_height = QFontMetrics(self.
_topic_font).height()
381 renderer = self._timeline_renderers.get(datatype)
383 topic_height = renderer.get_segment_height(topic)
392 new_history_bottom = max([y + h
for (_, y, _, h)
in self._history_bounds.values()]) - 1
398 Draw all topic messages 399 :param painter: allows access to paint functions,''QPainter'' 401 for topic
in sorted(self._history_bounds.keys()):
406 Draw boxes corrisponding to message regions on the timeline. 407 :param painter: allows access to paint functions,''QPainter'' 408 :param topic: the topic for which message boxes should be drawn, ''str'' 421 msg_combine_interval =
None 423 renderer = self._timeline_renderers.get(datatype)
424 if not renderer
is None:
426 if msg_combine_interval
is None:
435 end_index = bisect.bisect_left(all_stamps, self.
_stamp_right)
442 for (stamp_start, stamp_end)
in \
444 all_stamps[:end_index],
453 region_width = max(1, region_x_end - region_x_start)
455 painter.setBrush(QBrush(datatype_color))
456 painter.setPen(QPen(datatype_color, 1))
457 painter.drawRect(region_x_start, msg_y, region_width, msg_height)
460 if topic
in self.scene()._listeners:
461 curpen = painter.pen()
462 oldwidth = curpen.width()
464 painter.setPen(curpen)
465 playhead_stamp =
None 466 playhead_index = bisect.bisect_right(all_stamps, self.playhead.to_sec()) - 1
467 if playhead_index >= 0:
468 playhead_stamp = all_stamps[playhead_index]
471 (all_stamps[playhead_index] - self.
_stamp_left) * width_interval
472 painter.drawLine(playhead_x, msg_y, playhead_x, msg_y + msg_height)
473 curpen.setWidth(oldwidth)
474 painter.setPen(curpen)
479 for (stamp_start, stamp_end)
in \
480 self.
_find_regions(all_stamps[:end_index], msg_combine_interval):
487 region_width = max(1, region_x_end - region_x_start)
488 renderer.draw_timeline_segment(
489 painter, topic, stamp_start, stamp_end,
490 region_x_start, msg_y, region_width, msg_height)
497 Draw markers to indicate the area the bag file represents within the current visible area. 498 :param painter: allows access to paint functions,''QPainter'' 501 self._start_stamp.to_sec()), self.
map_stamp_to_x(self._end_stamp.to_sec())
512 Draws horizontal lines between each topic to visually separate the messages 513 :param painter: allows access to paint functions,''QPainter'' 523 painter.setPen(Qt.lightGray)
526 painter.setPen(Qt.lightGray)
528 left = max(clip_left, x)
529 painter.drawRect(left, y, min(clip_right - left, w), h)
536 Draws a box around the selected region 537 :param painter: allows access to paint functions,''QPainter'' 550 width = x_right - x_left
555 painter.drawRect(left, top, width, height)
558 painter.setBrush(Qt.NoBrush)
559 painter.drawLine(left, top, left, top + height)
560 painter.drawLine(left + width, top, left + width, top + height)
563 painter.setBrush(Qt.NoBrush)
564 painter.drawLine(left, top, left + width, top)
575 Draw a line and 2 triangles to denote the current position being viewed 576 :param painter: ,''QPainter'' 589 QPolygonF([QPointF(px, py + ph), QPointF(px + pw, py), QPointF(px - pw, py)]))
594 QPolygonF([QPointF(px, py), QPointF(px + pw, py + ph), QPointF(px - pw, py + ph)]))
601 Draw a simple black rectangle frame around the timeline view area 602 :param painter: ,''QPainter'' 608 painter.setBrush(Qt.NoBrush)
609 painter.setPen(Qt.black)
610 painter.drawRect(x, y, w, h)
616 Calculate positions of existing topic names and draw them on the left, one for each row 617 :param painter: ,''QPainter'' 619 topics = self._history_bounds.keys()
621 for (_, y, _, h)
in self._history_bounds.values()]
623 for text, coords
in zip([t.lstrip(
'/')
for t
in topics], coords):
631 Draw vertical grid-lines showing major and minor time divisions. 632 :param painter: allows access to paint functions,''QPainter'' 636 if len(major_divisions) == 0:
639 major_division = min(major_divisions)
642 if x_per_sec * s >= self.
_minor_spacing and major_division % s == 0]
643 if len(minor_divisions) > 0:
644 minor_division = min(minor_divisions)
646 minor_division =
None 648 start_stamp = self._start_stamp.to_sec()
650 major_stamps = list(self.
_get_stamps(start_stamp, major_division))
655 s
for s
in self.
_get_stamps(start_stamp, minor_division)
if s
not in major_stamps]
660 Draw black hashed vertical grid-lines showing major time divisions. 661 :param painter: allows access to paint functions,''QPainter'' 667 label = self.
_get_label(division, stamp - start_stamp)
669 if label_x + self.
_qfont_width(label) < self.scene().width():
673 painter.drawText(label_x, label_y, label)
684 Draw grey hashed vertical grid-lines showing minor time divisions. 685 :param painter: allows access to paint functions,''QPainter'' 702 for renderer
in self._timeline_renderers.values():
704 self._index_cache_thread.stop()
709 return [RawView] + self._viewer_types.get(
'*', []) + self._viewer_types.get(datatype, [])
715 plugin_descriptors = self.plugin_provider.discover(
None)
716 for plugin_descriptor
in plugin_descriptors:
718 plugin = self.plugin_provider.load(
719 plugin_descriptor.plugin_id(), plugin_context=
None)
720 except Exception
as e:
721 qWarning(
'rqt_bag.TimelineFrame.load_plugins() failed to load plugin "%s":\n%s' %
722 (plugin_descriptor.plugin_id(), e))
725 view = plugin.get_view_class()
726 except Exception
as e:
728 'rqt_bag.TimelineFrame.load_plugins() failed to get view ' 729 'from plugin "%s":\n%s' % (plugin_descriptor.plugin_id(), e))
732 timeline_renderer =
None 734 timeline_renderer = plugin.get_renderer_class()
735 except AttributeError:
737 except Exception
as e:
739 'rqt_bag.TimelineFrame.load_plugins() failed to get renderer ' 740 'from plugin "%s":\n%s' % (plugin_descriptor.plugin_id(), e))
744 msg_types = plugin.get_message_types()
745 except AttributeError:
747 except Exception
as e:
749 'rqt_bag.TimelineFrame.load_plugins() failed to get message types ' 750 'from plugin "%s":\n%s' % (plugin_descriptor.plugin_id(), e))
754 'rqt_bag.TimelineFrame.load_plugins() plugin "%s" declares ' 755 'no message types.' % (plugin_descriptor.plugin_id()))
757 for msg_type
in msg_types:
758 self._viewer_types.setdefault(msg_type, []).append(view)
759 if timeline_renderer:
762 qDebug(
'rqt_bag.TimelineFrame.load_plugins() loaded plugin "%s"' %
763 plugin_descriptor.plugin_id())
769 :returns: a list of the currently loaded renderers for the plugins 775 renderer = self._timeline_renderers.get(datatype)
776 if renderer
is not None:
777 renderers.append((topic, renderer))
791 self._rendered_topics.add(topic)
793 self._rendered_topics.clear()
794 self.scene().update()
800 self._rendered_topics.add(topic)
804 self._rendered_topics.remove(topic)
805 self.scene().update()
811 Updates the cache of message timestamps for the given topic. 812 :return: number of messages added to the index cache 829 if len(topic_cache) == 0:
832 start_time = rospy.Time.from_sec(max(0.0, topic_cache[-1]))
836 topic_cache_len = len(topic_cache)
838 for entry
in self.scene().get_entries(topic, start_time, end_time):
839 topic_cache.append(entry.time.to_sec())
842 self.invalidated_caches.remove(topic)
844 return len(topic_cache) - topic_cache_len
848 Group timestamps into regions connected by timestamps less than max_interval secs apart 849 :param start_stamp: a list of stamps, ''list'' 850 :param stamp_step: seconds between each division, ''int'' 852 region_start, prev_stamp =
None,
None 855 if stamp - prev_stamp > max_interval:
856 region_end = prev_stamp
857 yield (region_start, region_end)
864 if region_start
and prev_stamp:
865 yield (region_start, prev_stamp)
869 Generate visible stamps every stamp_step 870 :param start_stamp: beginning of timeline stamp, ''int'' 871 :param stamp_step: seconds between each division, ''int'' 876 stamp = start_stamp + \
877 int((self.
_stamp_left - start_stamp) / stamp_step) * stamp_step + stamp_step
885 :param division: number of seconds in a division, ''int'' 886 :param elapsed: seconds from the beginning, ''int'' 887 :returns: relevent time elapsed string, ''str'' 889 secs = int(elapsed) % 60
891 mins = int(elapsed) / 60
896 if division >= 7 * 24 * 60 * 60:
898 elif division >= 24 * 60 * 60:
900 elif division >= 60 * 60:
902 elif division >= 5 * 60:
905 return '%dm%02ds' % (mins, secs)
906 elif division >= 0.1:
907 return '%d.%ss' % (secs, str(int(10.0 * (elapsed - int(elapsed)))))
908 elif division >= 0.01:
909 return '%d.%02ds' % (secs, int(100.0 * (elapsed - int(elapsed))))
911 return '%d.%03ds' % (secs, int(1000.0 * (elapsed - int(elapsed))))
916 converts a pixel x value to a stamp 917 :param x: pixel value to be converted, ''int'' 918 :param clamp_to_visible: 919 disallow values that are greater than the current timeline bounds,''bool'' 920 :returns: timestamp, ''int'' 927 elif fraction >= 1.0:
934 converts a distance in pixel space to a distance in stamp space 935 :param dx: distance in pixel space to be converted, ''int'' 936 :returns: distance in stamp space, ''float'' 942 converts a timestamp to the x value where that stamp exists in the timeline 943 :param stamp: timestamp to be converted, ''int'' 944 :param clamp_to_visible: 945 disallow values that are greater than the current timeline bounds,''bool'' 946 :returns: # of pixels from the left boarder, ''int'' 953 fraction = min(1.0, max(0.0, fraction))
963 if y > topic_y
and y <= topic_y + topic_h:
986 self.scene().update()
997 if start_stamp
is None:
1000 if (end_stamp - start_stamp) < rospy.Duration.from_sec(5.0):
1001 end_stamp = start_stamp + rospy.Duration.from_sec(5.0)
1004 self.scene().update()
1023 if not new_interval:
1026 new_range = new_interval[1] - new_interval[0]
1028 actual_zoom = new_range / curr_range
1030 if desired_zoom < 1.0:
1031 return actual_zoom < 0.95
1033 return actual_zoom > 1.05
1042 self.scene().update()
1047 @requires: left & right zoom interval sizes. 1054 center = self.playhead.to_sec()
1055 center_frac = (center - self.
_stamp_left) / stamp_interval
1057 new_stamp_interval = zoom * stamp_interval
1058 if new_stamp_interval == 0:
1067 left = center - center_frac * new_stamp_interval
1068 right = left + new_stamp_interval
1070 return (left, right)
1077 self._bag_timeline.resume()
1092 if event.modifiers() == Qt.ShiftModifier:
1095 x = self._clicked_pos.x()
1096 y = self._clicked_pos.y()
1101 if playhead_secs <= 0.0:
1104 self.
playhead = rospy.Time.from_sec(playhead_secs)
1105 self.scene().update()
1113 self.scene().update()
1124 self.scene().update()
1127 self.scene().views()[0].setCursor(QCursor(Qt.ClosedHandCursor))
1133 _SelectionMode.LEFT_MARKED,
1134 _SelectionMode.MOVE_LEFT,
1135 _SelectionMode.MOVE_RIGHT,
1136 _SelectionMode.SHIFTING]:
1141 self.scene().views()[0].setCursor(QCursor(Qt.ArrowCursor))
1142 self.scene().update()
1146 delta = event.angleDelta().y()
1147 except AttributeError:
1148 delta = event.delta()
1159 if event.buttons() == Qt.NoButton:
1162 _SelectionMode.MARKED,
1163 _SelectionMode.MOVE_LEFT,
1164 _SelectionMode.MOVE_RIGHT,
1165 _SelectionMode.SHIFTING]:
1172 self.scene().views()[0].setCursor(QCursor(Qt.SizeHorCursor))
1176 self.scene().views()[0].setCursor(QCursor(Qt.SizeHorCursor))
1178 elif x > left_x
and x < right_x:
1180 self.scene().views()[0].setCursor(QCursor(Qt.OpenHandCursor))
1184 self.scene().views()[0].setCursor(QCursor(Qt.ArrowCursor))
1187 if event.buttons() == Qt.MidButton
or event.modifiers() == Qt.ShiftModifier:
1189 dx_drag, dy_drag = x - self._dragged_pos.x(), y - self._dragged_pos.y()
1193 if (dx_drag == 0
and abs(dy_drag) > 0)
or \
1194 (dx_drag != 0
and abs(float(dy_drag) / dx_drag) > 0.2
and abs(dy_drag) > 1):
1200 self.scene().views()[0].setCursor(QCursor(Qt.ClosedHandCursor))
1201 elif event.buttons() == Qt.LeftButton:
1203 clicked_x = self._clicked_pos.x()
1204 clicked_y = self._clicked_pos.y()
1215 self.scene().update()
1219 self.scene().update()
1223 self.scene().update()
1226 dx_drag = x - self._dragged_pos.x()
1230 self._start_stamp.to_sec(),
1233 self._start_stamp.to_sec(),
1235 self.scene().update()
1245 self.
playhead = rospy.Time.from_sec(x_stamp)
1246 self.scene().update()
def _draw_topic_names(self, painter)
def on_left_down(self, event)
def paint(self, painter, option, widget)
_selected_region_outline_ends_color
def translate_timeline_left(self)
def on_mouse_up(self, event)
def _set_playhead(self, playhead)
def map_y_to_topic(self, y)
def translate_timeline_right(self)
_history_background_color
def translate_timeline(self, dstamp)
def _get_stamps(self, start_stamp, stamp_step)
def has_selected_region(self)
def can_zoom(self, desired_zoom)
def _draw_topic_histories(self, painter)
def on_middle_down(self, event)
def set_renderers_active(self, active)
def toggle_renderers(self)
def map_stamp_to_x(self, stamp, clamp_to_visible=True)
_selected_region_outline_top_color
def _get_label(self, division, elapsed)
def _draw_major_divisions(self, painter, stamps, start_stamp, division)
def on_mousewheel(self, event)
_active_message_line_width
def get_viewer_types(self, datatype)
def _update_index_cache(self, topic)
def is_renderer_active(self, topic)
def _draw_topic_history(self, painter, topic)
def _draw_playhead(self, painter)
def _draw_time_divisions(self, painter)
def map_x_to_stamp(self, x, clamp_to_visible=True)
def __init__(self, bag_timeline)
def _find_regions(self, stamps, max_interval)
def map_dx_to_dstamp(self, dx)
def _draw_minor_divisions(self, painter, stamps, start_stamp, division)
def _draw_history_border(self, painter)
def set_timeline_view(self, stamp_left, stamp_right)
def get_zoom_interval(self, zoom, center=None)
def _draw_bag_ends(self, painter)
def get_datatype(bag, topic)
_history_background_color_alternate
def on_mouse_move(self, event)
def _draw_topic_dividers(self, painter)
_major_divisions_label_indent
def emit_play_region(self)
def _draw_selected_region(self, painter)
def _trimmed_topic_name(self, topic_name)
def zoom_timeline(self, zoom, center=None)
def _qfont_width(self, name)
def set_renderer_active(self, topic, active)
def map_dstamp_to_dx(self, dstamp)