class ImageTimelineRenderer(TimelineRenderer): """ Draws thumbnails of sensor_msgs/Image or sensor_msgs/CompressedImage in the timeline. """ def __init__(self, timeline, thumbnail_height=160): super(ImageTimelineRenderer, self).__init__(timeline, msg_combine_px=40.0) self.thumbnail_height = thumbnail_height self.thumbnail_combine_px = 20.0 # use cached thumbnail if it's less than this many pixels away self.min_thumbnail_width = 8 # don't display thumbnails if less than this many pixels across self.quality = Image.NEAREST # quality hint for thumbnail scaling self.thumbnail_cache = TimelineCache(self._load_thumbnail, lambda topic, msg_stamp, thumbnail: self.timeline.scene().update()) # TimelineRenderer implementation def get_segment_height(self, topic): return self.thumbnail_height def draw_timeline_segment(self, painter, topic, stamp_start, stamp_end, x, y, width, height): """ draws a stream of images for the topic :param painter: painter object, ''QPainter'' :param topic: topic to draw, ''str'' :param stamp_start: stamp to start drawing, ''rospy.Time'' :param stamp_end: stamp to end drawing, ''rospy.Time'' :param x: x to draw images at, ''int'' :param y: y to draw images at, ''int'' :param width: width in pixels of the timeline area, ''int'' :param height: height in pixels of the timeline area, ''int'' """ if x < self.timeline._history_left: width += x - self.timeline._history_left x = self.timeline._history_left max_interval_thumbnail = self.timeline.map_dx_to_dstamp(self.thumbnail_combine_px) max_interval_thumbnail = max(0.1, max_interval_thumbnail) thumbnail_gap = 6 thumbnail_x, thumbnail_y, thumbnail_height = x + 1, y + 1, height - 2 - thumbnail_gap # leave 1px border # set color to white draw rectangle over messages painter.setBrush(QBrush(Qt.white)) painter.drawRect(x, y, width, height - thumbnail_gap) thumbnail_width = None while True: available_width = (x + width) - thumbnail_x # Check for enough remaining to draw thumbnail if width > 1 and available_width < self.min_thumbnail_width: break # Try to display the thumbnail, if its right edge is to the right of the timeline's left side if not thumbnail_width or thumbnail_x + thumbnail_width >= self.timeline._history_left: stamp = self.timeline.map_x_to_stamp(thumbnail_x, clamp_to_visible=False) thumbnail_bitmap = self.thumbnail_cache.get_item(topic, stamp, max_interval_thumbnail) # Cache miss if not thumbnail_bitmap: thumbnail_details = (thumbnail_height,) self.thumbnail_cache.enqueue((topic, stamp, max_interval_thumbnail, thumbnail_details)) if not thumbnail_width: break else: thumbnail_width, _ = thumbnail_bitmap.size if width > 1: if available_width < thumbnail_width: thumbnail_width = available_width - 1 QtImage = ImageQt(thumbnail_bitmap) pixmap = QPixmap.fromImage(QtImage) painter.drawPixmap(thumbnail_x, thumbnail_y, thumbnail_width, thumbnail_height, pixmap) thumbnail_x += thumbnail_width if width == 1: break painter.setPen(QPen(Qt.black)) painter.setBrush(QBrush(Qt.transparent)) if width == 1: painter.drawRect(x, y, thumbnail_x - x, height - thumbnail_gap - 1) else: painter.drawRect(x, y, width, height - thumbnail_gap - 1) return True def close(self): if self.thumbnail_cache: self.thumbnail_cache.stop() self.thumbnail_cache.join() def _load_thumbnail(self, topic, stamp, thumbnail_details): """ Loads the thumbnail from the bag """ (thumbnail_height,) = thumbnail_details # Find position of stamp using index t = rospy.Time.from_sec(stamp) bag, entry = self.timeline.scene().get_entry(t, topic) if not entry: return None, None pos = entry.position # Not in the cache; load from the bag file with self.timeline.scene()._bag_lock: msg_topic, msg, msg_stamp = bag._read_message(pos) # Convert from ROS image to PIL image try: pil_image = image_helper.imgmsg_to_pil(msg) except Exception as ex: print('Error loading image on topic %s: %s' % (topic, str(ex)), file=sys.stderr) pil_image = None if not pil_image: print('Disabling renderer on %s' % topic, file=sys.stderr) self.timeline.set_renderer_active(topic, False) return None, None # Calculate width to maintain aspect ratio try: pil_image_size = pil_image.size thumbnail_width = int(round(thumbnail_height * (float(pil_image_size[0]) / pil_image_size[1]))) # Scale to thumbnail size thumbnail = pil_image.resize((thumbnail_width, thumbnail_height), self.quality) return msg_stamp, thumbnail except Exception as ex: print('Error loading image on topic %s: %s' % (topic, str(ex)), file=sys.stderr) raise return None, None
class ImageTimelineRenderer(TimelineRenderer): """ Draws thumbnails of sensor_msgs/Image or sensor_msgs/CompressedImage in the timeline. """ def __init__(self, timeline, thumbnail_height=160): super(ImageTimelineRenderer, self).__init__(timeline, msg_combine_px=40.0) self.thumbnail_height = thumbnail_height self.thumbnail_combine_px = 20.0 # use cached thumbnail if it's less than this many pixels away self.min_thumbnail_width = 8 # don't display thumbnails if less than this many pixels across self.quality = Image.NEAREST # quality hint for thumbnail scaling self.thumbnail_cache = TimelineCache(self._load_thumbnail, lambda topic, msg_stamp, thumbnail: self.timeline.scene().update()) # TimelineRenderer implementation def get_segment_height(self, topic): return self.thumbnail_height def draw_timeline_segment(self, painter, topic, stamp_start, stamp_end, x, y, width, height): """ draws a stream of images for the topic :param painter: painter object, ''QPainter'' :param topic: topic to draw, ''str'' :param stamp_start: stamp to start drawing, ''rospy.Time'' :param stamp_end: stamp to end drawing, ''rospy.Time'' :param x: x to draw images at, ''int'' :param y: y to draw images at, ''int'' :param width: width in pixels of the timeline area, ''int'' :param height: height in pixels of the timeline area, ''int'' """ if x < self.timeline._history_left: width += x - self.timeline._history_left x = self.timeline._history_left max_interval_thumbnail = self.timeline.map_dx_to_dstamp(self.thumbnail_combine_px) max_interval_thumbnail = max(0.1, max_interval_thumbnail) thumbnail_gap = 6 thumbnail_x, thumbnail_y, thumbnail_height = x + 1, y + 1, height - 2 - thumbnail_gap # leave 1px border # set color to white draw rectangle over messages painter.setBrush(QBrush(Qt.white)) painter.drawRect(x, y, width, height - thumbnail_gap) thumbnail_width = None while True: available_width = (x + width) - thumbnail_x # Check for enough remaining to draw thumbnail if width > 1 and available_width < self.min_thumbnail_width: break # Try to display the thumbnail, if its right edge is to the right of the timeline's left side if not thumbnail_width or thumbnail_x + thumbnail_width >= self.timeline._history_left: stamp = self.timeline.map_x_to_stamp(thumbnail_x, clamp_to_visible=False) thumbnail_bitmap = self.thumbnail_cache.get_item(topic, stamp, max_interval_thumbnail) # Cache miss if not thumbnail_bitmap: thumbnail_details = (thumbnail_height,) self.thumbnail_cache.enqueue((topic, stamp, max_interval_thumbnail, thumbnail_details)) if not thumbnail_width: break else: thumbnail_width, _ = thumbnail_bitmap.size if width > 1: if available_width < thumbnail_width: thumbnail_width = available_width - 1 QtImage = ImageQt(thumbnail_bitmap) pixmap = QPixmap.fromImage(QtImage) painter.drawPixmap(thumbnail_x, thumbnail_y, thumbnail_width, thumbnail_height, pixmap) thumbnail_x += thumbnail_width if width == 1: break painter.setPen(QPen(QBrush(Qt.black))) painter.setBrush(QBrush(Qt.transparent)) if width == 1: painter.drawRect(x, y, thumbnail_x - x, height - thumbnail_gap - 1) else: painter.drawRect(x, y, width, height - thumbnail_gap - 1) return True def close(self): if self.thumbnail_cache: self.thumbnail_cache.stop() self.thumbnail_cache.join() def _load_thumbnail(self, topic, stamp, thumbnail_details): """ Loads the thumbnail from the bag """ (thumbnail_height,) = thumbnail_details # Find position of stamp using index t = rospy.Time.from_sec(stamp) bag, entry = self.timeline.scene().get_entry(t, topic) if not entry: return None, None pos = entry.position # Not in the cache; load from the bag file with self.timeline.scene()._bag_lock: msg_topic, msg, msg_stamp = bag._read_message(pos) # Convert from ROS image to PIL image try: pil_image = image_helper.imgmsg_to_pil(msg) except Exception, ex: print >> sys.stderr, 'Error loading image on topic %s: %s' % (topic, str(ex)) pil_image = None if not pil_image: print >> sys.stderr, 'Disabling renderer on %s' % topic self.timeline.set_renderer_active(topic, False) return None, None # Calculate width to maintain aspect ratio try: pil_image_size = pil_image.size thumbnail_width = int(round(thumbnail_height * (float(pil_image_size[0]) / pil_image_size[1]))) # Scale to thumbnail size thumbnail = pil_image.resize((thumbnail_width, thumbnail_height), self.quality) return msg_stamp, thumbnail except Exception, ex: print >> sys.stderr, 'Error loading image on topic %s: %s' % (topic, str(ex)) raise return None, None