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)
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()
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()