def read_from_bag(self, bagfile): bag_info = rosbag_info(bagfile) known = topics_in_bag(bag_info) self._match_topics_with_known(known) source = read_bag_stats(bagfile, topics=self.resolved) show_progress = read_bag_stats_progress(source, logger, interval=5) # Topic to last message self._topic2last = {} self.iterator = show_progress.next self.read_one = True
def read(self, topics, start=None, stop=None, use_stamp_if_available=False): source = read_bag_stats(self.bag, topics, logger=None, start_time=start, stop_time=stop) source = read_bag_stats_progress(source, logger, interval=2) oldt = None for topic, msg, t, _ in source: name = topic value = msg if use_stamp_if_available: try: stamp = msg.header.stamp except: timestamp = t.to_sec() else: timestamp = stamp.to_sec() if timestamp == 0: timestamp = t.to_sec() else: timestamp = t.to_sec() if oldt is not None: if oldt == timestamp: logger.error('Sequence with invalid timestamp? %s' % timestamp) if start is not None: if timestamp < start: logger.warn('Skipping %s < %s because not included in interval.' % (timestamp, start)) continue if stop is not None: if timestamp > stop: logger.warn('Skipping %s > %s because not included in interval.' % (timestamp, stop)) continue if start is not None: assert start <= timestamp if stop is not None: assert stop >= timestamp yield timestamp, (name, value) oldt = timestamp
def read_all(self, topics): bagfile = self.get_bagfile() return read_bag_stats(bagfile, topics=topics, logger=logger)