Example #1
0
class HDFread(Generator):
    ''' 
        This block reads a log written with :ref:`block:hdfwrite`.
    
    '''
    Block.alias('hdfread')
    Block.output_is_defined_at_runtime('The signals read from the log.')
    Block.config('file', 'HDF file to read')
    Block.config('signals', 'Which signals to output (and in what order). '
                 'Should be a comma-separated list. If you do not specify it '
                 ' will be all signals (TODO: in the original order).',
                 default=None)

    Block.config('quiet',
                 'If true, disables advancements status messages.',
                 default=False)

    def get_output_signals(self):
        self.reader = self.config.file
        all_signals = self.reader.get_all_signals()

        if self.config.signals is None:
            self.signals = all_signals
        else:
            signal_list = filter(lambda x: x, self.config.signals.split(','))
            if not signal_list:
                msg = 'Bad format: %r.' % self.config.signals
                raise BadConfig(msg, self, 'signals')
            for s in signal_list:
                if not s in all_signals:
                    msg = ('Signal %r not present in log (available: %r)' %
                           (s, all_signals))
                    raise BadConfig(msg, self, 'signals')
                self.signals.append(s)

        return self.signals

    def init(self):
        # let's do the rest of the initialization

        # signal -> table
        self.signal2table = {}
        # signal -> index in the table (or None)
        self.signal2index = {}

        for signal in self.signals:
            self.signal2table[signal] = self.log_group._f_getChild(signal)
            if len(self.signal2table[signal]) > 0:
                self.signal2index[signal] = 0
            else:
                self.signal2index[signal] = None

    def _choose_next_signal(self):
        ''' Returns a tuple (name,timestamp) of the signal that produces 
            the next event, or (None,None) if we finished the log. '''
        # array of tuples (signal, timestamp)
        status = []
        for signal in self.signals:
            index = self.signal2index[signal]
            if index is not None:
                table = self.signal2table[signal]
                timestamp = table[index]['time']
                status.append((signal, timestamp))

        if not status:
            return (None, None)
        else:
            sorted_status = sorted(status, key=operator.itemgetter(1))
            return sorted_status[0]

    def next_data_status(self):
        next_signal, next_timestamp = self._choose_next_signal()
        if next_signal is None:
            return (False, None)
        else:
            return (True, next_timestamp)

    def update(self):
        next_signal, next_timestamp = self._choose_next_signal()

        table = self.signal2table[next_signal]
        index = self.signal2index[next_signal]
        assert next_timestamp == table[index]['time']
        value = table[index]['value']

        self.set_output(next_signal, value=value, timestamp=next_timestamp)

        # update index
        if index + 1 == len(table):
            # finished
            self.signal2index[next_signal] = None
        else:
            self.signal2index[next_signal] = index + 1

        # write status message if not quiet
        if next_signal == self.signals[0] and not self.config.quiet:
            self.write_update_message(index, len(table), next_signal)

    def write_update_message(self, index, T, signal, nintervals=10):
        interval = int(numpy.floor(T * 1.0 / nintervals))
        if (index > 0 and index != interval * (nintervals)
                and index % interval == 0):
            percentage = index * 100.0 / T
            T = str(T)
            index = str(index).rjust(len(T))
            self.debug('%s read %.0f%% (%s/%s) (tracking signal %r).' %
                       (self.config.file, percentage, index, T, signal))

    def finish(self):
        tc_close(self.hf)
Example #2
0
class HDFread_many(Generator):
    ''' 
        This block is a variation on :ref:`block:hdfread` that can 
        concatenate the output of logs stored in multiple HDF files.
        
        The files are specified using a wildcard: for example, ``dir/*.h5``.
        
        A difference with :ref:`block:hdfread` is that all signals
        must be specified explicitly (:ref:`block:hdfread` can guess); the
        reason is that we want to avoid reading unrelated logs with different
        signals.
        
        We check that all files specified have all required signals.
        
        The logfiles are read in the order compatible with their timestamp.
    '''
    Block.alias('hdfread_many')
    Block.output_is_defined_at_runtime('The signals read from the logs.')

    Block.config('files', 'HDF files to read; you can use the wildcard ``*``.')
    Block.config(
        'signals', 'Which signals to output (and in what order). '
        'Should be a comma-separated list. ')

    Block.config('quiet',
                 'If true, disables advancements status messages.',
                 default=False)

    def get_output_signals(self):
        self.signals = filter(lambda x: x, self.config.signals.split(','))
        if not self.signals:
            raise BadConfig('No signals specified.', self, 'signals')
        return self.signals

    def init(self):
        # Find the required files
        files = glob.glob(self.config.files)

        if len(files) == 0:
            raise BadConfig(
                'No files correspond to the pattern %r.' % self.config.files,
                self, 'files')

        # Open all logs; make sure they have the required signals and
        # note their initial timestamp.

        # list(tuple(file, timestamp, signal2table))
        logs = []
        for f in files:
            hf = tc_open_for_reading(f)
            check_is_procgraph_log(hf)

            log_group = hf.root._f_getChild(PROCGRAPH_LOG_GROUP)
            log_signals = list(log_group._v_children)

            signal2table = {}
            for s in self.signals:
                if not s in log_signals:
                    raise Exception('Log %r does not have signal %r '
                                    '(it has %s).' % (f, s, log_signals))
                signal2table[s] = log_group._f_getChild(s)

            timestamp = signal2table[s][0]['time']
            logs.append((hf, timestamp, signal2table))

        # Sort them by timestamp
        self.logs = sorted(logs, key=operator.itemgetter(1))
        self.current_log = None

        for log in logs:
            filename = log[0].filename
            length = len(list(log[2].values())[0])
            self.status('- queued log (%6d rows) from %r' % (length, filename))

        self.start_reading_next_log()

    def status(self, s):
        if not self.config.quiet:
            self.info(s)

    def start_reading_next_log(self):
        if self.current_log is not None:
            self.status('Just finished log %r.' % self.current_log[0].filename)
            tc_close(self.current_log[0])
            self.current_log = None

        if not self.logs:
            # we finished
            self.status('No logs remaining.')
            return

        self.current_log = self.logs.pop(0)
        self.signal2table = self.current_log[2]

        self.status('Now starting with %r.' % self.current_log[0].filename)

        # signal -> index in the table (or None)
        self.signal2index = {}

        for signal in self.signals:
            if len(self.signal2table[signal]) > 0:
                self.signal2index[signal] = 0
            else:
                self.signal2index[signal] = None

    def _choose_next_signal(self):
        ''' Returns a tuple (name,timestamp) of the signal that produces 
            the next event, or (None,None) if we finished the log. '''
        # array of tuples (signal, timestamp)
        status = []
        for signal in self.signals:
            index = self.signal2index[signal]
            if index is not None:
                table = self.signal2table[signal]
                timestamp = table[index]['time']
                status.append((signal, timestamp))

        if not status:
            return (None, None)
        else:
            sorted_status = sorted(status, key=operator.itemgetter(1))
            return sorted_status[0]

    def next_data_status(self):
        next_signal, next_timestamp = self._choose_next_signal()
        if next_signal is None:
            # one log is finished:
            if not self.logs:
                # no more logs
                return (False, None)
            else:
                self.start_reading_next_log()
                return self.next_data_status()
        else:
            return (True, next_timestamp)

    def update(self):
        next_signal, next_timestamp = self._choose_next_signal()
        assert next_signal is not None

        # get value
        table = self.signal2table[next_signal]
        index = self.signal2index[next_signal]
        assert next_timestamp == table[index]['time']
        value = table[index]['value']

        self.set_output(next_signal, value=value, timestamp=next_timestamp)

        # update index
        if index + 1 == len(table):
            # finished
            self.status('Finished reading signal %r.' % next_signal)
            self.signal2index[next_signal] = None
        else:
            self.signal2index[next_signal] = index + 1

        # Status messages for the first signal
        if next_signal == self.signals[0]:
            T = len(table)
            nintervals = 10
            interval = int(numpy.floor(T * 1.0 / nintervals))
            if (index > 0 and index != interval * (nintervals)
                    and index % interval == 0):
                percentage = index * 100.0 / T

                T = str(T)
                index = str(index).rjust(len(T))
                self.status('Read %.0f%% (%s/%s) of %r (tracking signal %r).' %
                            (percentage, index, T,
                             os.path.basename(
                                 self.current_log[0].filename), next_signal))

    def finish(self):
        self.start_reading_next_log()
Example #3
0
class BagRead(Generator):
    '''
        This block reads a bag file (ROS logging format).
    '''
    Block.alias('bagread')
    Block.output_is_defined_at_runtime('The signals read from the log.')
    Block.config('file', 'Bag file to read')
    Block.config('limit',
                 'Limit in seconds on how much data we want. (0=no limit)',
                 default=0)
    Block.config('t0', 'Relative start time', default=None)
    Block.config('t1', 'Relative start time', default=None)

    Block.config('topics', 'Which signals to output (and in what order). '
                 'Should be a comma-separated list. If you do not specify it '
                 '(or if empty) it will be all signals.',
                 default=[])

    Block.config('quiet',
                 'If true, disables advancements status messages.',
                 default=False)

    @contract(returns='list(str)')
    def get_topics(self):
        bagfile = self.config.file
        if self.config.topics is not None:
            given_topics = self.config.topics.strip()
        else:
            given_topics = None

        if given_topics:
            topics = given_topics.split(',')
        else:
            all_topics = [c.topic for c in self.bag._get_connections()]
            topics = sorted(set(all_topics))

        self.baginfo = rosbag_info_cached(bagfile)
        res, _, asked2resolved = resolve_topics(self.baginfo, topics)
        self.info('Resolving:\n%s' % pformat(asked2resolved))
        return res

    def get_output_signals(self):
        import rosbag
        self.bag = rosbag.Bag(self.config.file)

        self.topics = self.get_topics()

        self.topic2signal = {}
        signals = []
        for t in self.topics:
            self.info(t)
            if ':' in t:
                tokens = t.split(':')
                assert len(tokens) == 2
                t = tokens[0]
                signal_name = tokens[1]
            else:
                signal_name = str(t).split('/')[-1]
            if signal_name in signals:
                self.error('Warning: repeated name %s' % signal_name)
                signal_name = ("%s" % t).replace('/', '_')
                self.error('Using long form %r.' % signal_name)
            signals.append(signal_name)
            self.topic2signal[t] = signal_name

        topics = self.topic2signal.keys()

        self.info(self.topic2signal)

        limit = self.config.limit
        if not isinstance(limit, (float, int)):
            msg = 'I require a number; 0 for none.'
            raise BadConfig(msg, self, 'limit')

        if self.config.t0 is not None or self.config.t1 is not None:
            t0 = self.config.t0
            t1 = self.config.t1
            start_time, end_time = self._get_start_end_time_t0_t1(t0, t1)
        else:
            start_time, end_time = self._get_start_end_time(limit)

        print('t0: %s' % self.config.t0)
        print('t1: %s' % self.config.t1)
        print('start_time: %s' % start_time)
        print('end_time: %s' % end_time)
        print('start_stamp: %s' % self.start_stamp)
        print('end_stamp: %s' % self.end_stamp)

        params = dict(topics=topics, start_time=start_time, end_time=end_time)

        self.iterator = self.bag.read_messages(**params)

        return signals

    @contract(limit='None|number')
    def _get_start_end_time(self, limit):
        """
            Returns the start and end time to use (rospy.Time).

            also sets self.start_stamp, self.end_stamp

        """
        from rospy.rostime import Time  #@UnresolvedImport
        self.info('limit: %r' % limit)
        warnings.warn('Make better code for dealing with unindexed')
        if limit is not None and limit != 0:
            # try:
            chunks = self.bag.__dict__['_chunks']
            self.start_stamp = chunks[0].start_time.to_sec()
            self.end_stamp = chunks[-1].end_time.to_sec()
            start_time = Time.from_sec(self.start_stamp)
            end_time = Time.from_sec(self.start_stamp + limit)
            return start_time, end_time
            # except Exception as e:
            #     self.error('Perhaps unindexed bag?')
            #     self.error(traceback.format_exc(e))
            #     raise
            #     start_time = None
            #     end_time = None
            #
            # self.info('start_stamp: %s' % self.start_stamp)
            # self.info('end_stamp: %s' % self.end_stamp)
        else:
            self.start_stamp = None
            self.end_stamp = None
            return None, None

    @contract(limit='None|number')
    def _get_start_end_time_t0_t1(self, t0, t1):
        """
            Returns the start and end time to use (rospy.Time).

            also sets self.start_stamp, self.end_stamp

        """
        warnings.warn('Make better code for dealing with unindexed')
        from rospy.rostime import Time  #@UnresolvedImport
        if t0 is not None or t1 is not None:
            # try:
            chunks = self.bag.__dict__['_chunks']
            self.start_stamp = chunks[0].start_time.to_sec()
            self.end_stamp = chunks[-1].end_time.to_sec()
            if t0 is not None:
                start_time = self.start_stamp + t0
            else:
                start_time = self.start_stamp
            if t1 is not None:
                end_time = self.start_stamp + t1
            else:
                end_time = self.end_stamp
            start_time = Time.from_sec(start_time)
            end_time = Time.from_sec(end_time)
            return start_time, end_time
            # except Exception as e:
            #     self.error('Perhaps unindexed bag?')
            #     self.error(traceback.format_exc(e))
            #     raise
            #     start_time = None
            #     end_time = None
            #
            # self.info('start_stamp: %s' % self.start_stamp)
            # self.info('end_stamp: %s' % self.end_stamp)
        else:
            self.start_stamp = None
            self.end_stamp = None
            return None, None

    def init(self):
        self._load_next()

    def _load_next(self):
        try:
            topic, msg, timestamp = self.iterator.next()
            self.next_timestamp = timestamp.to_sec()
            self.next_value = msg
            self.next_topic = topic
            self.next_signal = self.topic2signal[topic]
            self.has_next = True
        except StopIteration:
            self.has_next = False

    def next_data_status(self):
        if self.has_next:
            return (self.next_signal, self.next_timestamp)
        else:
            return (False, None)

    def update(self):
        if not self.has_next:
            return  # XXX: error here?

        self.set_output(self.next_signal,
                        value=self.next_value,
                        timestamp=self.next_timestamp)

        self._load_next()

    def finish(self):
        self.bag.close()