timeline_frame.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2012, 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 
00034 from python_qt_binding.QtCore import qDebug, QPointF, QRectF, Qt, qWarning, Signal
00035 from python_qt_binding.QtGui import QBrush, QCursor, QColor, QFont, \
00036                                     QFontMetrics, QPen, QPolygonF
00037 from python_qt_binding.QtWidgets import QGraphicsItem
00038 import rospy
00039 
00040 import bisect
00041 import threading
00042 
00043 from .index_cache_thread import IndexCacheThread
00044 from .plugins.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
00068     (time delimiters, labels, topic names and backgrounds).
00069     Also handles mouse callbacks since they interact closely with the drawn elements
00070     """
00071 
00072     def __init__(self, bag_timeline):
00073         super(TimelineFrame, self).__init__()
00074         self._bag_timeline = bag_timeline
00075         self._clicked_pos = None
00076         self._dragged_pos = None
00077 
00078         # Timeline boundries
00079         self._start_stamp = None  # earliest of all stamps
00080         self._end_stamp = None  # latest of all stamps
00081         self._stamp_left = None  # earliest currently visible timestamp on the timeline
00082         self._stamp_right = None  # latest currently visible timestamp on the timeline
00083         self._history_top = 30
00084         self._history_left = 0
00085         self._history_width = 0
00086         self._history_bottom = 0
00087         self._history_bounds = {}
00088         self._margin_left = 4
00089         self._margin_right = 20
00090         self._margin_bottom = 20
00091         self._history_top = 30
00092 
00093         # Background Rendering
00094         self._bag_end_color = QColor(0, 0, 0, 25)  # color of background of timeline before first message and after last
00095         self._history_background_color_alternate = QColor(179, 179, 179, 25)
00096         self._history_background_color = QColor(204, 204, 204, 102)
00097 
00098         # Timeline Division Rendering
00099         # Possible time intervals used between divisions
00100         # 1ms, 5ms, 10ms, 50ms, 100ms, 500ms
00101         # 1s, 5s, 15s, 30s
00102         # 1m, 2m, 5m, 10m, 15m, 30m
00103         # 1h, 2h, 3h, 6h, 12h
00104         # 1d, 7d
00105         self._sec_divisions = [0.001, 0.005, 0.01, 0.05, 0.1, 0.5,
00106                                1, 5, 15, 30,
00107                                1 * 60, 2 * 60, 5 * 60, 10 * 60, 15 * 60, 30 * 60,
00108                                1 * 60 * 60, 2 * 60 * 60, 3 * 60 * 60, 6 * 60 * 60, 12 * 60 * 60,
00109                                1 * 60 * 60 * 24, 7 * 60 * 60 * 24]
00110         self._minor_spacing = 15
00111         self._major_spacing = 50
00112         self._major_divisions_label_indent = 3  # padding in px between line and label
00113         self._major_division_pen = QPen(QBrush(Qt.black), 0, Qt.DashLine)
00114         self._minor_division_pen = QPen(QBrush(QColor(153, 153, 153, 128)), 0, Qt.DashLine)
00115         self._minor_division_tick_pen = QPen(QBrush(QColor(128, 128, 128, 128)), 0)
00116 
00117         # Topic Rendering
00118         self.topics = []
00119         self._topics_by_datatype = {}
00120         self._topic_font_height = None
00121         self._topic_name_sizes = None
00122         self._topic_name_spacing = 3  # minimum pixels between end of topic name and start of history
00123         self._topic_font_size = 10.0
00124         self._topic_font = QFont("cairo")
00125         self._topic_font.setPointSize(self._topic_font_size)
00126         self._topic_font.setBold(False)
00127         self._topic_vertical_padding = 4
00128         self._topic_name_max_percent = 25.0  # percentage of the horiz space that can be used for topic display
00129 
00130         # Time Rendering
00131         self._time_tick_height = 5
00132         self._time_font_height = None
00133         self._time_font_size = 10.0
00134         self._time_font = QFont("cairo")
00135         self._time_font.setPointSize(self._time_font_size)
00136         self._time_font.setBold(False)
00137 
00138         # Defaults
00139         self._default_brush = QBrush(Qt.black, Qt.SolidPattern)
00140         self._default_pen = QPen(Qt.black)
00141         self._default_datatype_color = QColor(0, 0, 102, 204)
00142         self._datatype_colors = {
00143             'sensor_msgs/CameraInfo': QColor(0, 0, 77, 204),
00144             'sensor_msgs/Image': QColor(0, 77, 77, 204),
00145             'sensor_msgs/LaserScan': QColor(153, 0, 0, 204),
00146             'pr2_msgs/LaserScannerSignal': QColor(153, 0, 0, 204),
00147             'pr2_mechanism_msgs/MechanismState': QColor(0, 153, 0, 204),
00148             'tf/tfMessage': QColor(0, 153, 0, 204),
00149         }
00150         self._default_msg_combine_px = 1.0  # minimum number of pixels allowed between two bag messages before they are combined
00151         self._active_message_line_width = 3
00152 
00153         # Selected Region Rendering
00154         self._selected_region_color = QColor(0, 179, 0, 21)
00155         self._selected_region_outline_top_color = QColor(0.0, 77, 0.0, 51)
00156         self._selected_region_outline_ends_color = QColor(0.0, 77, 0.0, 102)
00157         self._selecting_mode = _SelectionMode.NONE
00158         self._selected_left = None
00159         self._selected_right = None
00160         self._selection_handle_width = 3.0
00161 
00162         # Playhead Rendering
00163         self._playhead = None  # timestamp of the playhead
00164         self._paused = False
00165         self._playhead_pointer_size = (6, 6)
00166         self._playhead_line_width = 1
00167         self._playhead_color = QColor(255, 0, 0, 191)
00168 
00169         # Zoom
00170         self._zoom_sensitivity = 0.005
00171         self._min_zoom_speed = 0.5
00172         self._max_zoom_speed = 2.0
00173         self._min_zoom = 0.0001  # max zoom out (in px/s)
00174         self._max_zoom = 50000.0  # max zoom in  (in px/s)
00175 
00176         # Plugin management
00177         self._viewer_types = {}
00178         self._timeline_renderers = {}
00179         self._rendered_topics = set()
00180         self.load_plugins()
00181 
00182         # Bag indexer for rendering the default message views on the timeline
00183         self.index_cache_cv = threading.Condition()
00184         self.index_cache = {}
00185         self.invalidated_caches = set()
00186         self._index_cache_thread = IndexCacheThread(self)
00187 
00188     # TODO the API interface should exist entirely at the bag_timeline level. Add a "get_draw_parameters()" at the bag_timeline level to access these
00189     # Properties, work in progress API for plugins:
00190 
00191     # property: playhead
00192     def _get_playhead(self):
00193         return self._playhead
00194 
00195     def _set_playhead(self, playhead):
00196         """
00197         Sets the playhead to the new position, notifies the threads and updates the scene so it will redraw
00198         :signal: emits status_bar_changed_signal if the playhead is successfully set
00199         :param playhead: Time to set the playhead to, ''rospy.Time()''
00200         """
00201         with self.scene()._playhead_lock:
00202             if playhead == self._playhead:
00203                 return
00204 
00205             self._playhead = playhead
00206             if self._playhead != self._end_stamp:
00207                 self.scene().stick_to_end = False
00208 
00209             playhead_secs = playhead.to_sec()
00210             if playhead_secs > self._stamp_right:
00211                 dstamp = playhead_secs - self._stamp_right + (self._stamp_right - self._stamp_left) * 0.75
00212                 if dstamp > self._end_stamp.to_sec() - self._stamp_right:
00213                     dstamp = self._end_stamp.to_sec() - self._stamp_right
00214                 self.translate_timeline(dstamp)
00215 
00216             elif playhead_secs < self._stamp_left:
00217                 dstamp = self._stamp_left - playhead_secs + (self._stamp_right - self._stamp_left) * 0.75
00218                 if dstamp > self._stamp_left - self._start_stamp.to_sec():
00219                     dstamp = self._stamp_left - self._start_stamp.to_sec()
00220                 self.translate_timeline(-dstamp)
00221 
00222             # Update the playhead positions
00223             for topic in self.topics:
00224                 bag, entry = self.scene().get_entry(self._playhead, topic)
00225                 if entry:
00226                     if topic in self.scene()._playhead_positions and self.scene()._playhead_positions[topic] == (bag, entry.position):
00227                         continue
00228                     new_playhead_position = (bag, entry.position)
00229                 else:
00230                     new_playhead_position = (None, None)
00231                 with self.scene()._playhead_positions_cvs[topic]:
00232                     self.scene()._playhead_positions[topic] = new_playhead_position
00233                     self.scene()._playhead_positions_cvs[topic].notify_all()  # notify all message loaders that a new message needs to be loaded
00234             self.scene().update()
00235             self.scene().status_bar_changed_signal.emit()
00236 
00237     playhead = property(_get_playhead, _set_playhead)
00238 
00239     # TODO add more api variables here to allow plugin access
00240     @property
00241     def _history_right(self):
00242         return self._history_left + self._history_width
00243 
00244     @property
00245     def has_selected_region(self):
00246         return self._selected_left is not None and self._selected_right is not None
00247 
00248     @property
00249     def play_region(self):
00250         if self.has_selected_region:
00251             return (rospy.Time.from_sec(self._selected_left), rospy.Time.from_sec(self._selected_right))
00252         else:
00253             return (self._start_stamp, self._end_stamp)
00254 
00255     def emit_play_region(self):
00256         play_region = self.play_region
00257         if(play_region[0] is not None and play_region[1] is not None):
00258             self.scene().selected_region_changed.emit(*play_region)
00259 
00260     @property
00261     def start_stamp(self):
00262         return self._start_stamp
00263 
00264     @property
00265     def end_stamp(self):
00266         return self._end_stamp
00267 
00268     # QGraphicsItem implementation
00269     def boundingRect(self):
00270         return QRectF(0, 0, self._history_left + self._history_width + self._margin_right, self._history_bottom + self._margin_bottom)
00271 
00272     def paint(self, painter, option, widget):
00273         if self._start_stamp is None:
00274             return
00275 
00276         self._layout()
00277         self._draw_topic_dividers(painter)
00278         self._draw_selected_region(painter)
00279         self._draw_time_divisions(painter)
00280         self._draw_topic_histories(painter)
00281         self._draw_bag_ends(painter)
00282         self._draw_topic_names(painter)
00283         self._draw_history_border(painter)
00284         self._draw_playhead(painter)
00285     # END QGraphicsItem implementation
00286 
00287     # Drawing Functions
00288 
00289     def _qfont_width(self, name):
00290         return QFontMetrics(self._topic_font).width(name)
00291 
00292     def _trimmed_topic_name(self, topic_name):
00293         """
00294         This function trims the topic name down to a reasonable percentage of the viewable scene area
00295         """
00296         allowed_width = self._scene_width * (self._topic_name_max_percent / 100.0)
00297         allowed_width = allowed_width - self._topic_name_spacing - self._margin_left
00298         trimmed_return = topic_name
00299         if allowed_width < self._qfont_width(topic_name):
00300             #  We need to trim the topic
00301             trimmed = ''
00302             split_name = topic_name.split('/')
00303             split_name = filter(lambda a: a != '', split_name)
00304             #  Save important last element of topic name provided it is small
00305             popped_last = False
00306             if self._qfont_width(split_name[-1]) < .5 * allowed_width:
00307                 popped_last = True
00308                 last_item = split_name[-1]
00309                 split_name = split_name[:-1]
00310                 allowed_width = allowed_width - self._qfont_width(last_item)
00311             # Shorten and add remaining items keeping lenths roughly equal
00312             for item in split_name:
00313                 if self._qfont_width(item) > allowed_width / float(len(split_name)):
00314                     trimmed_item = item[:-3] + '..'
00315                     while self._qfont_width(trimmed_item) > allowed_width / float(len(split_name)):
00316                         if len(trimmed_item) >= 3:
00317                             trimmed_item = trimmed_item[:-3] + '..'
00318                         else:
00319                             break
00320                     trimmed = trimmed + '/' + trimmed_item
00321                 else:
00322                     trimmed = trimmed + '/' + item
00323             if popped_last:
00324                 trimmed = trimmed + '/' + last_item
00325             trimmed = trimmed[1:]
00326             trimmed_return = trimmed
00327         return trimmed_return
00328 
00329     def _layout(self):
00330         """
00331         Recalculates the layout of the of the timeline to take into account any changes that have occured
00332         """
00333         # Calculate history left and history width
00334         self._scene_width = self.scene().views()[0].size().width()
00335 
00336         max_topic_name_width = -1
00337         for topic in self.topics:
00338             topic_width = self._qfont_width(self._trimmed_topic_name(topic))
00339             if max_topic_name_width <= topic_width:
00340                 max_topic_name_width = topic_width
00341 
00342         # Calculate font height for each topic
00343         self._topic_font_height = -1
00344         for topic in self.topics:
00345             topic_height = QFontMetrics(self._topic_font).height()
00346             if self._topic_font_height <= topic_height:
00347                 self._topic_font_height = topic_height
00348 
00349         # Update the timeline boundries
00350         new_history_left = self._margin_left + max_topic_name_width + self._topic_name_spacing
00351         new_history_width = self._scene_width - new_history_left - self._margin_right
00352         self._history_left = new_history_left
00353         self._history_width = new_history_width
00354 
00355         # Calculate the bounds for each topic
00356         self._history_bounds = {}
00357         y = self._history_top
00358         for topic in self.topics:
00359             datatype = self.scene().get_datatype(topic)
00360 
00361             topic_height = None
00362             if topic in self._rendered_topics:
00363                 renderer = self._timeline_renderers.get(datatype)
00364                 if renderer:
00365                     topic_height = renderer.get_segment_height(topic)
00366             if not topic_height:
00367                 topic_height = self._topic_font_height + self._topic_vertical_padding
00368 
00369             self._history_bounds[topic] = (self._history_left, y, self._history_width, topic_height)
00370 
00371             y += topic_height
00372 
00373 #        new_history_bottom = max([y + h for (x, y, w, h) in self._history_bounds.values()]) - 1
00374         new_history_bottom = max([y + h for (_, y, _, h) in self._history_bounds.values()]) - 1
00375         if new_history_bottom != self._history_bottom:
00376             self._history_bottom = new_history_bottom
00377 
00378     def _draw_topic_histories(self, painter):
00379         """
00380         Draw all topic messages
00381         :param painter: allows access to paint functions,''QPainter''
00382         """
00383         for topic in sorted(self._history_bounds.keys()):
00384             self._draw_topic_history(painter, topic)
00385 
00386     def _draw_topic_history(self, painter, topic):
00387         """
00388         Draw boxes corrisponding to message regions on the timeline.
00389         :param painter: allows access to paint functions,''QPainter''
00390         :param topic: the topic for which message boxes should be drawn, ''str''
00391         """
00392 
00393 #        x, y, w, h = self._history_bounds[topic]
00394         _, y, _, h = self._history_bounds[topic]
00395 
00396         msg_y = y + 2
00397         msg_height = h - 2
00398 
00399         datatype = self.scene().get_datatype(topic)
00400 
00401         # Get the renderer and the message combine interval
00402         renderer = None
00403         msg_combine_interval = None
00404         if topic in self._rendered_topics:
00405             renderer = self._timeline_renderers.get(datatype)
00406             if not renderer is None:
00407                 msg_combine_interval = self.map_dx_to_dstamp(renderer.msg_combine_px)
00408         if msg_combine_interval is None:
00409             msg_combine_interval = self.map_dx_to_dstamp(self._default_msg_combine_px)
00410 
00411         # Get the cache
00412         if topic not in self.index_cache:
00413             return
00414         all_stamps = self.index_cache[topic]
00415 
00416 #        start_index = bisect.bisect_left(all_stamps, self._stamp_left)
00417         end_index = bisect.bisect_left(all_stamps, self._stamp_right)
00418         # Set pen based on datatype
00419         datatype_color = self._datatype_colors.get(datatype, self._default_datatype_color)
00420         # Iterate through regions of connected messages
00421         width_interval = self._history_width / (self._stamp_right - self._stamp_left)
00422 
00423         # Draw stamps
00424         for (stamp_start, stamp_end) in self._find_regions(all_stamps[:end_index], self.map_dx_to_dstamp(self._default_msg_combine_px)):
00425             if stamp_end < self._stamp_left:
00426                 continue
00427 
00428             region_x_start = self._history_left + (stamp_start - self._stamp_left) * width_interval
00429             if region_x_start < self._history_left:
00430                 region_x_start = self._history_left  # Clip the region
00431             region_x_end = self._history_left + (stamp_end - self._stamp_left) * width_interval
00432             region_width = max(1, region_x_end - region_x_start)
00433 
00434             painter.setBrush(QBrush(datatype_color))
00435             painter.setPen(QPen(datatype_color, 1))
00436             painter.drawRect(region_x_start, msg_y, region_width, msg_height)
00437 
00438         # Draw active message
00439         if topic in self.scene()._listeners:
00440             curpen = painter.pen()
00441             oldwidth = curpen.width()
00442             curpen.setWidth(self._active_message_line_width)
00443             painter.setPen(curpen)
00444             playhead_stamp = None
00445             playhead_index = bisect.bisect_right(all_stamps, self.playhead.to_sec()) - 1
00446             if playhead_index >= 0:
00447                 playhead_stamp = all_stamps[playhead_index]
00448                 if playhead_stamp > self._stamp_left and playhead_stamp < self._stamp_right:
00449                     playhead_x = self._history_left + (all_stamps[playhead_index] - self._stamp_left) * width_interval
00450                     painter.drawLine(playhead_x, msg_y, playhead_x, msg_y + msg_height)
00451             curpen.setWidth(oldwidth)
00452             painter.setPen(curpen)
00453 
00454         # Custom renderer
00455         if renderer:
00456             # Iterate through regions of connected messages
00457             for (stamp_start, stamp_end) in self._find_regions(all_stamps[:end_index], msg_combine_interval):
00458                 if stamp_end < self._stamp_left:
00459                     continue
00460 
00461                 region_x_start = self._history_left + (stamp_start - self._stamp_left) * width_interval
00462                 region_x_end = self._history_left + (stamp_end - self._stamp_left) * width_interval
00463                 region_width = max(1, region_x_end - region_x_start)
00464                 renderer.draw_timeline_segment(painter, topic, stamp_start, stamp_end, region_x_start, msg_y, region_width, msg_height)
00465 
00466         painter.setBrush(self._default_brush)
00467         painter.setPen(self._default_pen)
00468 
00469     def _draw_bag_ends(self, painter):
00470         """
00471         Draw markers to indicate the area the bag file represents within the current visible area.
00472         :param painter: allows access to paint functions,''QPainter''
00473         """
00474         x_start, x_end = self.map_stamp_to_x(self._start_stamp.to_sec()), self.map_stamp_to_x(self._end_stamp.to_sec())
00475         painter.setBrush(QBrush(self._bag_end_color))
00476         painter.drawRect(self._history_left, self._history_top, x_start - self._history_left, self._history_bottom - self._history_top)
00477         painter.drawRect(x_end, self._history_top, self._history_left + self._history_width - x_end, self._history_bottom - self._history_top)
00478         painter.setBrush(self._default_brush)
00479         painter.setPen(self._default_pen)
00480 
00481     def _draw_topic_dividers(self, painter):
00482         """
00483         Draws horizontal lines between each topic to visually separate the messages
00484         :param painter: allows access to paint functions,''QPainter''
00485         """
00486         clip_left = self._history_left
00487         clip_right = self._history_left + self._history_width
00488 
00489         row = 0
00490         for topic in self.topics:
00491             (x, y, w, h) = self._history_bounds[topic]
00492 
00493             if row % 2 == 0:
00494                 painter.setPen(Qt.lightGray)
00495                 painter.setBrush(QBrush(self._history_background_color_alternate))
00496             else:
00497                 painter.setPen(Qt.lightGray)
00498                 painter.setBrush(QBrush(self._history_background_color))
00499             left = max(clip_left, x)
00500             painter.drawRect(left, y, min(clip_right - left, w), h)
00501             row += 1
00502         painter.setBrush(self._default_brush)
00503         painter.setPen(self._default_pen)
00504 
00505     def _draw_selected_region(self, painter):
00506         """
00507         Draws a box around the selected region
00508         :param painter: allows access to paint functions,''QPainter''
00509         """
00510         if self._selected_left is None:
00511             return
00512 
00513         x_left = self.map_stamp_to_x(self._selected_left)
00514         if self._selected_right is not None:
00515             x_right = self.map_stamp_to_x(self._selected_right)
00516         else:
00517             x_right = self.map_stamp_to_x(self.playhead.to_sec())
00518 
00519         left = x_left
00520         top = self._history_top - self._playhead_pointer_size[1] - 5 - self._time_font_size - 4
00521         width = x_right - x_left
00522         height = self._history_top - top
00523 
00524         painter.setPen(self._selected_region_color)
00525         painter.setBrush(QBrush(self._selected_region_color))
00526         painter.drawRect(left, top, width, height)
00527 
00528         painter.setPen(self._selected_region_outline_ends_color)
00529         painter.setBrush(Qt.NoBrush)
00530         painter.drawLine(left, top, left, top + height)
00531         painter.drawLine(left + width, top, left + width, top + height)
00532 
00533         painter.setPen(self._selected_region_outline_top_color)
00534         painter.setBrush(Qt.NoBrush)
00535         painter.drawLine(left, top, left + width, top)
00536 
00537         painter.setPen(self._selected_region_outline_top_color)
00538         painter.drawLine(left, self._history_top, left, self._history_bottom)
00539         painter.drawLine(left + width, self._history_top, left + width, self._history_bottom)
00540 
00541         painter.setBrush(self._default_brush)
00542         painter.setPen(self._default_pen)
00543 
00544     def _draw_playhead(self, painter):
00545         """
00546         Draw a line and 2 triangles to denote the current position being viewed
00547         :param painter: ,''QPainter''
00548         """
00549         px = self.map_stamp_to_x(self.playhead.to_sec())
00550         pw, ph = self._playhead_pointer_size
00551 
00552         # Line
00553         painter.setPen(QPen(self._playhead_color))
00554         painter.setBrush(QBrush(self._playhead_color))
00555         painter.drawLine(px, self._history_top - 1, px, self._history_bottom + 2)
00556 
00557         # Upper triangle
00558         py = self._history_top - ph
00559         painter.drawPolygon(QPolygonF([QPointF(px, py + ph), QPointF(px + pw, py), QPointF(px - pw, py)]))
00560 
00561         # Lower triangle
00562         py = self._history_bottom + 1
00563         painter.drawPolygon(QPolygonF([QPointF(px, py), QPointF(px + pw, py + ph), QPointF(px - pw, py + ph)]))
00564 
00565         painter.setBrush(self._default_brush)
00566         painter.setPen(self._default_pen)
00567 
00568     def _draw_history_border(self, painter):
00569         """
00570         Draw a simple black rectangle frame around the timeline view area
00571         :param painter: ,''QPainter''
00572         """
00573         bounds_width = min(self._history_width, self.scene().width())
00574         x, y, w, h = self._history_left, self._history_top, bounds_width, self._history_bottom - self._history_top
00575 
00576         painter.setBrush(Qt.NoBrush)
00577         painter.setPen(Qt.black)
00578         painter.drawRect(x, y, w, h)
00579         painter.setBrush(self._default_brush)
00580         painter.setPen(self._default_pen)
00581 
00582     def _draw_topic_names(self, painter):
00583         """
00584         Calculate positions of existing topic names and draw them on the left, one for each row
00585         :param painter: ,''QPainter''
00586         """
00587         topics = self._history_bounds.keys()
00588         coords = [(self._margin_left, y + (h / 2) + (self._topic_font_height / 2)) for (_, y, _, h) in self._history_bounds.values()]
00589 
00590         for text, coords in zip([t.lstrip('/') for t in topics], coords):
00591             painter.setBrush(self._default_brush)
00592             painter.setPen(self._default_pen)
00593             painter.setFont(self._topic_font)
00594             painter.drawText(coords[0], coords[1], self._trimmed_topic_name(text))
00595 
00596     def _draw_time_divisions(self, painter):
00597         """
00598         Draw vertical grid-lines showing major and minor time divisions.
00599         :param painter: allows access to paint functions,''QPainter''
00600         """
00601         x_per_sec = self.map_dstamp_to_dx(1.0)
00602         major_divisions = [s for s in self._sec_divisions if x_per_sec * s >= self._major_spacing]
00603         if len(major_divisions) == 0:
00604             major_division = max(self._sec_divisions)
00605         else:
00606             major_division = min(major_divisions)
00607 
00608         minor_divisions = [s for s in self._sec_divisions if x_per_sec * s >= self._minor_spacing and major_division % s == 0]
00609         if len(minor_divisions) > 0:
00610             minor_division = min(minor_divisions)
00611         else:
00612             minor_division = None
00613 
00614         start_stamp = self._start_stamp.to_sec()
00615 
00616         major_stamps = list(self._get_stamps(start_stamp, major_division))
00617         self._draw_major_divisions(painter, major_stamps, start_stamp, major_division)
00618 
00619         if minor_division:
00620             minor_stamps = [s for s in self._get_stamps(start_stamp, minor_division) if s not in major_stamps]
00621             self._draw_minor_divisions(painter, minor_stamps, start_stamp, minor_division)
00622 
00623     def _draw_major_divisions(self, painter, stamps, start_stamp, division):
00624         """
00625         Draw black hashed vertical grid-lines showing major time divisions.
00626         :param painter: allows access to paint functions,''QPainter''
00627         """
00628         label_y = self._history_top - self._playhead_pointer_size[1] - 5
00629         for stamp in stamps:
00630             x = self.map_stamp_to_x(stamp, False)
00631 
00632             label = self._get_label(division, stamp - start_stamp)
00633             label_x = x + self._major_divisions_label_indent
00634             if label_x + self._qfont_width(label) < self.scene().width():
00635                 painter.setBrush(self._default_brush)
00636                 painter.setPen(self._default_pen)
00637                 painter.setFont(self._time_font)
00638                 painter.drawText(label_x, label_y, label)
00639 
00640             painter.setPen(self._major_division_pen)
00641             painter.drawLine(x, label_y - self._time_tick_height - self._time_font_size, x, self._history_bottom)
00642 
00643         painter.setBrush(self._default_brush)
00644         painter.setPen(self._default_pen)
00645 
00646     def _draw_minor_divisions(self, painter, stamps, start_stamp, division):
00647         """
00648         Draw grey hashed vertical grid-lines showing minor time divisions.
00649         :param painter: allows access to paint functions,''QPainter''
00650         """
00651         xs = [self.map_stamp_to_x(stamp) for stamp in stamps]
00652         painter.setPen(self._minor_division_pen)
00653         for x in xs:
00654             painter.drawLine(x, self._history_top, x, self._history_bottom)
00655 
00656         painter.setPen(self._minor_division_tick_pen)
00657         for x in xs:
00658             painter.drawLine(x, self._history_top - self._time_tick_height, x, self._history_top)
00659 
00660         painter.setBrush(self._default_brush)
00661         painter.setPen(self._default_pen)
00662 
00663     # Close function
00664 
00665     def handle_close(self):
00666         for renderer in self._timeline_renderers.values():
00667             renderer.close()
00668         self._index_cache_thread.stop()
00669 
00670     # Plugin interaction functions
00671 
00672     def get_viewer_types(self, datatype):
00673         return [RawView] + self._viewer_types.get('*', []) + self._viewer_types.get(datatype, [])
00674 
00675     def load_plugins(self):
00676         from rqt_gui.rospkg_plugin_provider import RospkgPluginProvider
00677         self.plugin_provider = RospkgPluginProvider('rqt_bag', 'rqt_bag::Plugin')
00678 
00679         plugin_descriptors = self.plugin_provider.discover(None)
00680         for plugin_descriptor in plugin_descriptors:
00681             try:
00682                 plugin = self.plugin_provider.load(plugin_descriptor.plugin_id(), plugin_context=None)
00683             except Exception as e:
00684                 qWarning('rqt_bag.TimelineFrame.load_plugins() failed to load plugin "%s":\n%s' % (plugin_descriptor.plugin_id(), e))
00685                 continue
00686             try:
00687                 view = plugin.get_view_class()
00688             except Exception as e:
00689                 qWarning('rqt_bag.TimelineFrame.load_plugins() failed to get view from plugin "%s":\n%s' % (plugin_descriptor.plugin_id(), e))
00690                 continue
00691 
00692             timeline_renderer = None
00693             try:
00694                 timeline_renderer = plugin.get_renderer_class()
00695             except AttributeError:
00696                 pass
00697             except Exception as e:
00698                 qWarning('rqt_bag.TimelineFrame.load_plugins() failed to get renderer from plugin "%s":\n%s' % (plugin_descriptor.plugin_id(), e))
00699 
00700             msg_types = []
00701             try:
00702                 msg_types = plugin.get_message_types()
00703             except AttributeError:
00704                 pass
00705             except Exception as e:
00706                 qWarning('rqt_bag.TimelineFrame.load_plugins() failed to get message types from plugin "%s":\n%s' % (plugin_descriptor.plugin_id(), e))
00707             finally:
00708                 if not msg_types:
00709                     qWarning('rqt_bag.TimelineFrame.load_plugins() plugin "%s" declares no message types:\n%s' % (plugin_descriptor.plugin_id(), e))
00710 
00711             for msg_type in msg_types:
00712                 self._viewer_types.setdefault(msg_type, []).append(view)
00713                 if timeline_renderer:
00714                     self._timeline_renderers[msg_type] = timeline_renderer(self)
00715 
00716             qDebug('rqt_bag.TimelineFrame.load_plugins() loaded plugin "%s"' % plugin_descriptor.plugin_id())
00717 
00718     # Timeline renderer interaction functions
00719 
00720     def get_renderers(self):
00721         """
00722         :returns: a list of the currently loaded renderers for the plugins
00723         """
00724         renderers = []
00725 
00726         for topic in self.topics:
00727             datatype = self.scene().get_datatype(topic)
00728             renderer = self._timeline_renderers.get(datatype)
00729             if renderer is not None:
00730                 renderers.append((topic, renderer))
00731         return renderers
00732 
00733     def is_renderer_active(self, topic):
00734         return topic in self._rendered_topics
00735 
00736     def toggle_renderers(self):
00737         idle_renderers = len(self._rendered_topics) < len(self.topics)
00738 
00739         self.set_renderers_active(idle_renderers)
00740 
00741     def set_renderers_active(self, active):
00742         if active:
00743             for topic in self.topics:
00744                 self._rendered_topics.add(topic)
00745         else:
00746             self._rendered_topics.clear()
00747         self.scene().update()
00748 
00749     def set_renderer_active(self, topic, active):
00750         if active:
00751             if topic in self._rendered_topics:
00752                 return
00753             self._rendered_topics.add(topic)
00754         else:
00755             if not topic in self._rendered_topics:
00756                 return
00757             self._rendered_topics.remove(topic)
00758         self.scene().update()
00759 
00760     # Index Caching functions
00761 
00762     def _update_index_cache(self, topic):
00763         """
00764         Updates the cache of message timestamps for the given topic.
00765         :return: number of messages added to the index cache
00766         """
00767         if self._start_stamp is None or self._end_stamp is None:
00768             return 0
00769 
00770         if topic not in self.index_cache:
00771             # Don't have any cache of messages in this topic
00772             start_time = self._start_stamp
00773             topic_cache = []
00774             self.index_cache[topic] = topic_cache
00775         else:
00776             topic_cache = self.index_cache[topic]
00777 
00778             # Check if the cache has been invalidated
00779             if topic not in self.invalidated_caches:
00780                 return 0
00781 
00782             if len(topic_cache) == 0:
00783                 start_time = self._start_stamp
00784             else:
00785                 start_time = rospy.Time.from_sec(max(0.0, topic_cache[-1]))
00786 
00787         end_time = self._end_stamp
00788 
00789         topic_cache_len = len(topic_cache)
00790 
00791         for entry in self.scene().get_entries(topic, start_time, end_time):
00792             topic_cache.append(entry.time.to_sec())
00793 
00794         if topic in self.invalidated_caches:
00795             self.invalidated_caches.remove(topic)
00796 
00797         return len(topic_cache) - topic_cache_len
00798 
00799     def _find_regions(self, stamps, max_interval):
00800         """
00801         Group timestamps into regions connected by timestamps less than max_interval secs apart
00802         :param start_stamp: a list of stamps, ''list''
00803         :param stamp_step: seconds between each division, ''int''
00804         """
00805         region_start, prev_stamp = None, None
00806         for stamp in stamps:
00807             if prev_stamp:
00808                 if stamp - prev_stamp > max_interval:
00809                     region_end = prev_stamp
00810                     yield (region_start, region_end)
00811                     region_start = stamp
00812             else:
00813                 region_start = stamp
00814 
00815             prev_stamp = stamp
00816 
00817         if region_start and prev_stamp:
00818             yield (region_start, prev_stamp)
00819 
00820     def _get_stamps(self, start_stamp, stamp_step):
00821         """
00822         Generate visible stamps every stamp_step
00823         :param start_stamp: beginning of timeline stamp, ''int''
00824         :param stamp_step: seconds between each division, ''int''
00825         """
00826         if start_stamp >= self._stamp_left:
00827             stamp = start_stamp
00828         else:
00829             stamp = start_stamp + int((self._stamp_left - start_stamp) / stamp_step) * stamp_step + stamp_step
00830 
00831         while stamp < self._stamp_right:
00832             yield stamp
00833             stamp += stamp_step
00834 
00835     def _get_label(self, division, elapsed):
00836         """
00837         :param division: number of seconds in a division, ''int''
00838         :param elapsed: seconds from the beginning, ''int''
00839         :returns: relevent time elapsed string, ''str''
00840         """
00841         secs = int(elapsed) % 60
00842 
00843         mins = int(elapsed) / 60
00844         hrs = mins / 60
00845         days = hrs / 24
00846         weeks = days / 7
00847 
00848         if division >= 7 * 24 * 60 * 60:  # >1wk divisions: show weeks
00849             return '%dw' % weeks
00850         elif division >= 24 * 60 * 60:  # >24h divisions: show days
00851             return '%dd' % days
00852         elif division >= 60 * 60:  # >1h divisions: show hours
00853             return '%dh' % hrs
00854         elif division >= 5 * 60:  # >5m divisions: show minutes
00855             return '%dm' % mins
00856         elif division >= 1:  # >1s divisions: show minutes:seconds
00857             return '%dm%02ds' % (mins, secs)
00858         elif division >= 0.1:  # >0.1s divisions: show seconds.0
00859             return '%d.%ss' % (secs, str(int(10.0 * (elapsed - int(elapsed)))))
00860         elif division >= 0.01:  # >0.1s divisions: show seconds.0
00861             return '%d.%02ds' % (secs, int(100.0 * (elapsed - int(elapsed))))
00862         else:  # show seconds.00
00863             return '%d.%03ds' % (secs, int(1000.0 * (elapsed - int(elapsed))))
00864 
00865     # Pixel location/time conversion functions
00866     def map_x_to_stamp(self, x, clamp_to_visible=True):
00867         """
00868         converts a pixel x value to a stamp
00869         :param x: pixel value to be converted, ''int''
00870         :param clamp_to_visible: disallow values that are greater than the current timeline bounds,''bool''
00871         :returns: timestamp, ''int''
00872         """
00873         fraction = float(x - self._history_left) / self._history_width
00874 
00875         if clamp_to_visible:
00876             if fraction <= 0.0:
00877                 return self._stamp_left
00878             elif fraction >= 1.0:
00879                 return self._stamp_right
00880 
00881         return self._stamp_left + fraction * (self._stamp_right - self._stamp_left)
00882 
00883     def map_dx_to_dstamp(self, dx):
00884         """
00885         converts a distance in pixel space to a distance in stamp space
00886         :param dx: distance in pixel space to be converted, ''int''
00887         :returns: distance in stamp space, ''float''
00888         """
00889         return float(dx) * (self._stamp_right - self._stamp_left) / self._history_width
00890 
00891     def map_stamp_to_x(self, stamp, clamp_to_visible=True):
00892         """
00893         converts a timestamp to the x value where that stamp exists in the timeline
00894         :param stamp: timestamp to be converted, ''int''
00895         :param clamp_to_visible: disallow values that are greater than the current timeline bounds,''bool''
00896         :returns: # of pixels from the left boarder, ''int''
00897         """
00898         if self._stamp_left is None:
00899             return None
00900         fraction = (stamp - self._stamp_left) / (self._stamp_right - self._stamp_left)
00901 
00902         if clamp_to_visible:
00903             fraction = min(1.0, max(0.0, fraction))
00904 
00905         return self._history_left + fraction * self._history_width
00906 
00907     def map_dstamp_to_dx(self, dstamp):
00908         return (float(dstamp) * self._history_width) / (self._stamp_right - self._stamp_left)
00909 
00910     def map_y_to_topic(self, y):
00911         for topic in self._history_bounds:
00912             x, topic_y, w, topic_h = self._history_bounds[topic]
00913             if y > topic_y and y <= topic_y + topic_h:
00914                 return topic
00915         return None
00916 
00917     # View port manipulation functions
00918     def reset_timeline(self):
00919         self.reset_zoom()
00920 
00921         self._selected_left = None
00922         self._selected_right = None
00923         self._selecting_mode = _SelectionMode.NONE
00924 
00925         self.emit_play_region()
00926 
00927         if self._stamp_left is not None:
00928             self.playhead = rospy.Time.from_sec(self._stamp_left)
00929 
00930     def set_timeline_view(self, stamp_left, stamp_right):
00931         self._stamp_left = stamp_left
00932         self._stamp_right = stamp_right
00933 
00934     def translate_timeline(self, dstamp):
00935         self.set_timeline_view(self._stamp_left + dstamp, self._stamp_right + dstamp)
00936         self.scene().update()
00937 
00938     def translate_timeline_left(self):
00939         self.translate_timeline((self._stamp_right - self._stamp_left) * -0.05)
00940 
00941     def translate_timeline_right(self):
00942         self.translate_timeline((self._stamp_right - self._stamp_left) * 0.05)
00943 
00944     # Zoom functions
00945     def reset_zoom(self):
00946         start_stamp, end_stamp = self._start_stamp, self._end_stamp
00947         if start_stamp is None:
00948             return
00949 
00950         if (end_stamp - start_stamp) < rospy.Duration.from_sec(5.0):
00951             end_stamp = start_stamp + rospy.Duration.from_sec(5.0)
00952 
00953         self.set_timeline_view(start_stamp.to_sec(), end_stamp.to_sec())
00954         self.scene().update()
00955 
00956     def zoom_in(self):
00957         self.zoom_timeline(0.5)
00958 
00959     def zoom_out(self):
00960         self.zoom_timeline(2.0)
00961 
00962     def can_zoom_in(self):
00963         return self.can_zoom(0.5)
00964 
00965     def can_zoom_out(self):
00966         return self.can_zoom(2.0)
00967 
00968     def can_zoom(self, desired_zoom):
00969         if not self._stamp_left or not self.playhead:
00970             return False
00971 
00972         new_interval = self.get_zoom_interval(desired_zoom)
00973         if not new_interval:
00974             return False
00975 
00976         new_range = new_interval[1] - new_interval[0]
00977         curr_range = self._stamp_right - self._stamp_left
00978         actual_zoom = new_range / curr_range
00979 
00980         if desired_zoom < 1.0:
00981             return actual_zoom < 0.95
00982         else:
00983             return actual_zoom > 1.05
00984 
00985     def zoom_timeline(self, zoom, center=None):
00986         interval = self.get_zoom_interval(zoom, center)
00987         if not interval:
00988             return
00989 
00990         self._stamp_left, self._stamp_right = interval
00991 
00992         self.scene().update()
00993 
00994     def get_zoom_interval(self, zoom, center=None):
00995         """
00996         @rtype: tuple
00997         @requires: left & right zoom interval sizes.
00998         """
00999         if self._stamp_left is None:
01000             return None
01001 
01002         stamp_interval = self._stamp_right - self._stamp_left
01003         if center is None:
01004             center = self.playhead.to_sec()
01005         center_frac = (center - self._stamp_left) / stamp_interval
01006 
01007         new_stamp_interval = zoom * stamp_interval
01008         if new_stamp_interval == 0:
01009             return None
01010         # Enforce zoom limits
01011         px_per_sec = self._history_width / new_stamp_interval
01012         if px_per_sec < self._min_zoom:
01013             new_stamp_interval = self._history_width / self._min_zoom
01014         elif px_per_sec > self._max_zoom:
01015             new_stamp_interval = self._history_width / self._max_zoom
01016 
01017         left = center - center_frac * new_stamp_interval
01018         right = left + new_stamp_interval
01019 
01020         return (left, right)
01021 
01022     def pause(self):
01023         self._paused = True
01024 
01025     def resume(self):
01026         self._paused = False
01027         self._bag_timeline.resume()
01028 
01029     # Mouse event handlers
01030     def on_middle_down(self, event):
01031         self._clicked_pos = self._dragged_pos = event.pos()
01032         self.pause()
01033 
01034     def on_left_down(self, event):
01035         if self.playhead == None:
01036             return
01037 
01038         self._clicked_pos = self._dragged_pos = event.pos()
01039 
01040         self.pause()
01041 
01042         if event.modifiers() == Qt.ShiftModifier:
01043             return
01044 
01045         x = self._clicked_pos.x()
01046         y = self._clicked_pos.y()
01047         if x >= self._history_left and x <= self._history_right:
01048             if y >= self._history_top and y <= self._history_bottom:
01049                 # Clicked within timeline - set playhead
01050                 playhead_secs = self.map_x_to_stamp(x)
01051                 if playhead_secs <= 0.0:
01052                     self.playhead = rospy.Time(0, 1)
01053                 else:
01054                     self.playhead = rospy.Time.from_sec(playhead_secs)
01055                 self.scene().update()
01056 
01057             elif y <= self._history_top:
01058                 # Clicked above timeline
01059                 if self._selecting_mode == _SelectionMode.NONE:
01060                     self._selected_left = None
01061                     self._selected_right = None
01062                     self._selecting_mode = _SelectionMode.LEFT_MARKED
01063                     self.scene().update()
01064                     self.emit_play_region()
01065 
01066                 elif self._selecting_mode == _SelectionMode.MARKED:
01067                     left_x = self.map_stamp_to_x(self._selected_left)
01068                     right_x = self.map_stamp_to_x(self._selected_right)
01069                     if x < left_x - self._selection_handle_width or x > right_x + self._selection_handle_width:
01070                         self._selected_left = None
01071                         self._selected_right = None
01072                         self._selecting_mode = _SelectionMode.LEFT_MARKED
01073                         self.scene().update()
01074                     self.emit_play_region()
01075                 elif self._selecting_mode == _SelectionMode.SHIFTING:
01076                     self.scene().views()[0].setCursor(QCursor(Qt.ClosedHandCursor))
01077 
01078     def on_mouse_up(self, event):
01079         self.resume()
01080 
01081         if self._selecting_mode in [_SelectionMode.LEFT_MARKED, _SelectionMode.MOVE_LEFT, _SelectionMode.MOVE_RIGHT, _SelectionMode.SHIFTING]:
01082             if self._selected_left is None:
01083                 self._selecting_mode = _SelectionMode.NONE
01084             else:
01085                 self._selecting_mode = _SelectionMode.MARKED
01086         self.scene().views()[0].setCursor(QCursor(Qt.ArrowCursor))
01087         self.scene().update()
01088 
01089     def on_mousewheel(self, event):
01090         try:
01091             delta = event.angleDelta().y()
01092         except AttributeError:
01093             delta = event.delta()
01094         dz = delta / 120.0
01095         self.zoom_timeline(1.0 - dz * 0.2)
01096 
01097     def on_mouse_move(self, event):
01098         if not self._history_left:  # TODO: need a better notion of initialized
01099             return
01100 
01101         x = event.pos().x()
01102         y = event.pos().y()
01103 
01104         if event.buttons() == Qt.NoButton:
01105             # Mouse moving
01106             if self._selecting_mode in [_SelectionMode.MARKED, _SelectionMode.MOVE_LEFT, _SelectionMode.MOVE_RIGHT, _SelectionMode.SHIFTING]:
01107                 if y <= self._history_top and self._selected_left is not None:
01108                     left_x = self.map_stamp_to_x(self._selected_left)
01109                     right_x = self.map_stamp_to_x(self._selected_right)
01110 
01111                     if abs(x - left_x) <= self._selection_handle_width:
01112                         self._selecting_mode = _SelectionMode.MOVE_LEFT
01113                         self.scene().views()[0].setCursor(QCursor(Qt.SizeHorCursor))
01114                         return
01115                     elif abs(x - right_x) <= self._selection_handle_width:
01116                         self._selecting_mode = _SelectionMode.MOVE_RIGHT
01117                         self.scene().views()[0].setCursor(QCursor(Qt.SizeHorCursor))
01118                         return
01119                     elif x > left_x and x < right_x:
01120                         self._selecting_mode = _SelectionMode.SHIFTING
01121                         self.scene().views()[0].setCursor(QCursor(Qt.OpenHandCursor))
01122                         return
01123                     else:
01124                         self._selecting_mode = _SelectionMode.MARKED
01125                 self.scene().views()[0].setCursor(QCursor(Qt.ArrowCursor))
01126         else:
01127             # Mouse dragging
01128             if event.buttons() == Qt.MidButton or event.modifiers() == Qt.ShiftModifier:
01129                 # Middle or shift: zoom and pan
01130                 dx_drag, dy_drag = x - self._dragged_pos.x(), y - self._dragged_pos.y()
01131 
01132                 if dx_drag != 0:
01133                     self.translate_timeline(-self.map_dx_to_dstamp(dx_drag))
01134                 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):
01135                     zoom = min(self._max_zoom_speed, max(self._min_zoom_speed, 1.0 + self._zoom_sensitivity * dy_drag))
01136                     self.zoom_timeline(zoom, self.map_x_to_stamp(x))
01137 
01138                 self.scene().views()[0].setCursor(QCursor(Qt.ClosedHandCursor))
01139             elif event.buttons() == Qt.LeftButton:
01140                 # Left: move selected region and move selected region boundry
01141                 clicked_x = self._clicked_pos.x()
01142                 clicked_y = self._clicked_pos.y()
01143 
01144                 x_stamp = self.map_x_to_stamp(x)
01145 
01146                 if y <= self._history_top:
01147                     if self._selecting_mode == _SelectionMode.LEFT_MARKED:
01148                         # Left and selecting: change selection region
01149                         clicked_x_stamp = self.map_x_to_stamp(clicked_x)
01150 
01151                         self._selected_left = min(clicked_x_stamp, x_stamp)
01152                         self._selected_right = max(clicked_x_stamp, x_stamp)
01153                         self.scene().update()
01154 
01155                     elif self._selecting_mode == _SelectionMode.MOVE_LEFT:
01156                         self._selected_left = x_stamp
01157                         self.scene().update()
01158 
01159                     elif self._selecting_mode == _SelectionMode.MOVE_RIGHT:
01160                         self._selected_right = x_stamp
01161                         self.scene().update()
01162 
01163                     elif self._selecting_mode == _SelectionMode.SHIFTING:
01164                         dx_drag = x - self._dragged_pos.x()
01165                         dstamp = self.map_dx_to_dstamp(dx_drag)
01166 
01167                         self._selected_left = max(self._start_stamp.to_sec(), min(self._end_stamp.to_sec(), self._selected_left + dstamp))
01168                         self._selected_right = max(self._start_stamp.to_sec(), min(self._end_stamp.to_sec(), self._selected_right + dstamp))
01169                         self.scene().update()
01170                     self.emit_play_region()
01171 
01172                 elif clicked_x >= self._history_left and clicked_x <= self._history_right and clicked_y >= self._history_top and clicked_y <= self._history_bottom:
01173                     # Left and clicked within timeline: change playhead
01174                     if x_stamp <= 0.0:
01175                         self.playhead = rospy.Time(0, 1)
01176                     else:
01177                         self.playhead = rospy.Time.from_sec(x_stamp)
01178                     self.scene().update()
01179             self._dragged_pos = event.pos()


rqt_bag
Author(s): Aaron Blasdel, Tim Field
autogenerated on Thu Aug 17 2017 02:19:27