def get_info(bag_file, topic_filter=None):
bag = Bag(bag_file)
topics = bag.get_type_and_topic_info().topics
for topic in topics:
if topic_filter and topics[topic].msg_type not in topic_filter:
continue
print("{}: {} Hz".format(topic, round(topics[topic].frequency, 3)))
print(topics[topic].message_count)
times = np.ones(shape=bag.get_message_count(topic_filters=topic))
n = 0
for _, msg, t in bag.read_messages(topics=topic):
times[n] = msg.header.stamp.to_sec()
n += 1
times = 1 / np.gradient(times)
times = times[np.where((times > np.percentile(times, 10)) & (times < np.percentile(times, 90)))]
print("mean: {}, median: {}".format(np.mean(times), np.median(times)))
print("min: {}, max: {}".format(np.min(times), np.max(times)))
# plt.scatter(times, np.gradient(times))
plt.hist(times)
plt.yscale("log")
plt.title("{}: {}".format(os.path.basename(bag_file), topic))
if not os.path.exists("images"):
os.makedirs("images")
plt.savefig(os.path.join("images/", "{}.{}.png".format(os.path.basename(bag_file), topic.replace("/", ""))))
plt.cla()
def d8n_get_all_images_topic_bag(bag: rosbag.Bag, min_messages: int = 0) -> List[Tuple[str, type]]:
"""
Returns the (name, type) of all topics that look like images
and that have nonzero message count.
"""
tat = bag.get_type_and_topic_info()
consider_images = [
"sensor_msgs/Image",
"sensor_msgs/CompressedImage",
]
all_types = set()
found = []
topics = tat.topics
for t, v in list(topics.items()):
msg_type = v.msg_type
all_types.add(msg_type)
message_count = v.message_count
if msg_type in consider_images:
# quick fix: ignore image_raw if we have image_compressed version
if "raw" in t:
other = t.replace("raw", "compressed")
if other in topics:
continue
if message_count < min_messages:
# print('ignoring topic %r because message_count = 0' % t)
continue
found.append((t, msg_type))
return found
def incoming_ask(ask_msg):
rospy.loginfo('Incoming ask:\n' + str(ask_msg))
if ask_msg.model == self.model and ask_msg.token == self.token:
rospy.loginfo('Incoming ask with right model and token')
with TemporaryDirectory() as tmpdir:
rospy.logdebug('Temporary directory created: %s', tmpdir)
os.chdir(tmpdir)
rospy.loginfo('Waiting for objective file from IPFS...')
try:
self.ipfs.get(ask_msg.objective, timeout=60)
rospy.logdebug('Objective is written to %s', tmpdir + '/' + ask_msg.objective)
bag = Bag(str(tmpdir + '/' + ask_msg.objective))
msg_type = list(bag.get_type_and_topic_info()[1].values())[0][0]
if msg_type != Mission()._type:
rospy.logwarn('Wrong message type in objective topic')
return
except ipfsapi.exceptions.TimeoutError:
rospy.logwarn('Timeout of waiting objective file: ' + str(ask_msg.objective))
rospy.logwarn('Skip incoming ask')
return
self.make_bid(ask_msg)
else:
rospy.logwarn('Incoming ask with wrong model and token, skip')
def _assert_bag_valid(self,
filename,
topics=None,
start_time=None,
stop_time=None):
'''
Open the bagfile at the specified filename and read it to ensure topic limits were
enforced and the optional topic list and start/stop times are also enforced.
'''
bag = Bag(filename)
topics_dict = bag.get_type_and_topic_info()[1]
bag_topics = set(topics_dict.keys())
param_topics = set(self.topic_limits.keys())
if topics:
self.assertEqual(bag_topics, set(topics))
self.assertTrue(bag_topics.issubset(param_topics))
for topic in topics_dict:
size = topics_dict[
topic].message_count * 8 # Calculate stored message size as each message is 8 bytes
count = topics_dict[topic].message_count
gen = bag.read_messages(topics=topic)
_, _, first_time = next(gen)
last_time = first_time # in case the next for loop does not execute
if start_time:
self.assertGreaterEqual(first_time, start_time)
for _, _, last_time in gen: # Read through all messages so last_time is valid
pass
if stop_time:
self.assertLessEqual(last_time, stop_time)
duration = last_time - first_time
self._assert_limits_enforced(topic, duration, size, count)
def process_bag(bag_in_fn, bag_out_fn, conf_file_fn):
bag_in = Bag(bag_in_fn)
bag_out = Bag(bag_out_fn, 'w')
include_rules, exclude_rules, time_rules = read_rules(conf_file_fn)
topic_rules = include_rules + exclude_rules
# Set the time UNIX time at the start of the bag file
for r in topic_rules:
r.set_begin_time(bag_in.get_start_time())
for r in time_rules:
r.set_begin_time(bag_in.get_start_time())
# Find start and end times that are actually required
t_start, t_end = get_begin_end(time_rules, bag_in)
# Find topics that are actually required
bag_topics = bag_in.get_type_and_topic_info().topics.keys()
topics = get_topics(topic_rules, bag_topics)
messages = bag_in.read_messages(topics=topics,
start_time=t_start,
end_time=t_end,
return_connection_header=True)
for topic, msg, t, conn_header in messages:
# Check default enforcement for this message
if FilterRule.is_tf_topic(topic):
default = FilterRule.DEFAULT_ENFORCEMENT_TF
else:
default = FilterRule.DEFAULT_ENFORCEMENT_TOPIC
# When default is to include, only check whether the exclusion
# rules are satisfied, and if all of them are ok, write it out
if default == FilterRule.INCLUDE:
# Check exclusions
ok = True
for r in exclude_rules:
if r.msg_match(topic, msg, t):
ok = False
# When default is to exclude, check if the message matches any
# of the inclusion rules and write it out if it does
else:
# Check inclusions
ok = False
for r in include_rules:
if r.msg_match(topic, msg, t):
ok = True
# Time rules
time_ok = True
for r in time_rules:
if not r.is_ok_with(topic, msg, t):
time_ok = False
# Write to file
if ok and time_ok:
bag_out.write(topic, msg, t, connection_header=conn_header)
bag_out.close()
def import_schema(self, schema, srv_options, options, restriction_type,
restricts):
Bag = import_bag(srv_options)
pcid_str = options.pop('pcid', srv_options.pop('pcid', 0))
pcid = int(pcid_str)
patch_column = options.pop('patch_column',
srv_options.pop('patch_column', 'points'))
patch_columns = options.pop('patch_columns', '*').strip()
patch_columns = [
col.strip() for col in patch_columns.split(',') if col.strip()
]
filename = srv_options.pop('rosbag_path', "") + options.pop(
'rosbag_path', "") + schema
bag = Bag(filename, 'r')
tablecols = []
topics = bag.get_type_and_topic_info().topics
pcid_for_topic = {k: pcid + 1 + i for i, k in enumerate(topics.keys())}
pointcloud_formats = True
if restriction_type is 'limit':
topics = {k: v for k, v in topics.items() if k in restricts}
pointcloud_formats = 'pointcloud_formats' in restricts
elif restriction_type is 'except':
topics = {k: v for k, v in topics.items() if k not in restricts}
pointcloud_formats = 'pointcloud_formats' not in restricts
tabledefs = []
if pointcloud_formats:
tablecols = [
ColumnDefinition('pcid', type_name='integer'),
ColumnDefinition('srid', type_name='integer'),
ColumnDefinition('schema', type_name='text'),
ColumnDefinition('format', type_name='text'),
ColumnDefinition('rostype', type_name='text'),
ColumnDefinition('columns', type_name='text[]'),
ColumnDefinition('ply_header', type_name='text'),
]
tableopts = {
'metadata': 'true',
'rosbag': schema,
'pcid': pcid_str
}
tabledefs.append(
TableDefinition("pointcloud_formats",
columns=tablecols,
options=tableopts))
for topic, infos in topics.items():
pcid = pcid_for_topic[topic]
columns, _, _, _, _, _ = get_columns(bag, topic, infos, pcid,
patch_column, patch_columns)
tablecols = [get_column_def(k, *v) for k, v in columns.items()]
tableopts = {'topic': topic, 'rosbag': schema, 'pcid': str(pcid)}
tabledefs.append(
TableDefinition(topic, columns=tablecols, options=tableopts))
return tabledefs
def process_bag(self, bag: rosbag.Bag) -> None:
info = bag.get_type_and_topic_info()
bag_topics = set(info.topics.keys())
print('Bag Name: {}'.format(bag.filename))
size = len(max(self.topic_set.keys(), key=len)) + 1
for mode, topics in self.topic_set.items():
if bag_topics.issubset(topics):
print(('{:<' + str(size) + '} --SUBSET by {} topics').format(
mode + ':', len(topics - bag_topics), size=size))
elif bag_topics.issuperset(topics):
print(('{:<' + str(size) + '} ++SUPERSET').format(mode + ':',
size=size))
else:
print(('{:<' + str(size) + '} off by {}').format(
mode + ':', len(topics - bag_topics), size=size))
print('-------------------')
def import_schema(self, schema, srv_options, options,
restriction_type, restricts):
Bag = import_bag(srv_options)
pcid_str = options.pop('pcid', srv_options.pop('pcid', 0))
pcid = int(pcid_str)
patch_column = options.pop('patch_column', srv_options.pop('patch_column', 'points'))
patch_columns = options.pop('patch_columns', '*').strip()
patch_columns = [col.strip() for col in patch_columns.split(',') if col.strip()]
filename = srv_options.pop('rosbag_path', "") + options.pop('rosbag_path', "") + schema
bag = Bag(filename, 'r')
tablecols = []
topics = bag.get_type_and_topic_info().topics
pcid_for_topic = {k: pcid+1+i for i, k in enumerate(topics.keys())}
pointcloud_formats = True
if restriction_type is 'limit':
topics = {k: v for k, v in topics.items() if k in restricts}
pointcloud_formats = 'pointcloud_formats' in restricts
elif restriction_type is 'except':
topics = {k: v for k, v in topics.items() if k not in restricts}
pointcloud_formats = 'pointcloud_formats' not in restricts
tabledefs = []
if pointcloud_formats:
tablecols = [
ColumnDefinition('pcid', type_name='integer'),
ColumnDefinition('srid', type_name='integer'),
ColumnDefinition('schema', type_name='text'),
ColumnDefinition('format', type_name='text'),
ColumnDefinition('rostype', type_name='text'),
ColumnDefinition('columns', type_name='text[]'),
ColumnDefinition('ply_header', type_name='text'),
]
tableopts = {'metadata': 'true', 'rosbag': schema, 'pcid': pcid_str}
tabledefs.append(TableDefinition("pointcloud_formats", columns=tablecols,
options=tableopts))
for topic, infos in topics.items():
pcid = pcid_for_topic[topic]
columns, _, _, _, _, _ = get_columns(bag, topic, infos, pcid,
patch_column, patch_columns)
tablecols = [get_column_def(k, *v) for k, v in columns.items()]
tableopts = {'topic': topic, 'rosbag': schema, 'pcid': str(pcid)}
tabledefs.append(TableDefinition(topic, columns=tablecols, options=tableopts))
return tabledefs
def _read_info(self):
"""Read some metadata from inside this rosbag."""
from rosbag import Bag, ROSBagUnindexedException, ROSBagException
try:
b = Bag(self.path)
except ROSBagUnindexedException:
b = Bag(self.path, allow_unindexed=True)
print('Reindexing', self.filename)
b.reindex()
try:
duration = b.get_end_time() - b.get_start_time()
except ROSBagException:
duration = 0
return {
'n_messages': b.get_message_count(),
'duration': duration,
'topic_list': b.get_type_and_topic_info()[1].keys()
}
class Rosbag(ForeignDataWrapper):
def __init__(self, options, columns=None):
super(Rosbag, self).__init__(options, columns)
Bag = import_bag(options)
self.filename = options.pop('rosbag_path', "") + options.pop('rosbag')
self.topic = options.pop('topic', None)
pointcloud_formats = strtobool(options.pop('metadata', 'false'))
self.patch_column = options.pop('patch_column', 'points').strip()
self.patch_columns = options.pop('patch_columns', '*').strip()
self.patch_columns = [col.strip() for col in self.patch_columns.split(',') if col.strip()]
self.patch_count_default = int(options.pop('patch_count_default', 1000))
# 0 => 1 patch per message
self.patch_count_pointcloud = int(options.pop('patch_count_pointcloud', 0))
assert(self.patch_count_default > 0)
assert(self.patch_count_pointcloud >= 0)
self.pcid = int(options.pop('pcid', 0))
self.bag = Bag(self.filename, 'r')
self.topics = self.bag.get_type_and_topic_info().topics
self.pointcloud_formats = None
if pointcloud_formats:
self.pointcloud_formats = []
topics = self.topic.split(',') if self.topic else self.topics
for i, topic in enumerate(self.topics):
if topic not in topics:
continue
infos = self.topics[topic]
columns, patch_schema, patch_ply_header, _, patch_columns, patch_srid = \
get_columns(self.bag, topic, infos, self.pcid+i+1, self.patch_column,
self.patch_columns)
self.pointcloud_formats.append({
'pcid': self.pcid+i+1,
'srid': patch_srid,
'schema': patch_schema,
'format': columns[self.patch_column][3],
'rostype': infos.msg_type,
'columns': patch_columns,
'ply_header': patch_ply_header,
})
return
if not self.topic:
log_to_postgres('"topic" option is required', ERROR)
self.infos = self.topics[self.topic]
(self.columns, self.patch_schema, self.patch_ply_header, self.endianness,
self.patch_columns, self.patch_srid) = \
get_columns(self.bag, self.topic, self.infos, self.pcid, self.patch_column,
self.patch_columns)
if columns:
missing = set(columns) - set(self.columns.keys())
columns = list(c for c in self.columns.keys() if c in columns)
if missing:
missing = ", ".join(sorted(missing))
support = ", ".join(sorted(self.columns.keys()))
log_to_postgres(
"extra unsupported columns : {}".format(missing), WARNING,
hint="supported columns : {}".format(support))
self.columns = {col: self.columns[col] for col in columns}
if options:
log_to_postgres("extra unsupported options : {}".format(
options.keys()), WARNING)
@classmethod
def import_schema(self, schema, srv_options, options,
restriction_type, restricts):
Bag = import_bag(srv_options)
pcid_str = options.pop('pcid', srv_options.pop('pcid', 0))
pcid = int(pcid_str)
patch_column = options.pop('patch_column', srv_options.pop('patch_column', 'points'))
patch_columns = options.pop('patch_columns', '*').strip()
patch_columns = [col.strip() for col in patch_columns.split(',') if col.strip()]
filename = srv_options.pop('rosbag_path', "") + options.pop('rosbag_path', "") + schema
bag = Bag(filename, 'r')
tablecols = []
topics = bag.get_type_and_topic_info().topics
pcid_for_topic = {k: pcid+1+i for i, k in enumerate(topics.keys())}
pointcloud_formats = True
if restriction_type is 'limit':
topics = {k: v for k, v in topics.items() if k in restricts}
pointcloud_formats = 'pointcloud_formats' in restricts
elif restriction_type is 'except':
topics = {k: v for k, v in topics.items() if k not in restricts}
pointcloud_formats = 'pointcloud_formats' not in restricts
tabledefs = []
if pointcloud_formats:
tablecols = [
ColumnDefinition('pcid', type_name='integer'),
ColumnDefinition('srid', type_name='integer'),
ColumnDefinition('schema', type_name='text'),
ColumnDefinition('format', type_name='text'),
ColumnDefinition('rostype', type_name='text'),
ColumnDefinition('columns', type_name='text[]'),
ColumnDefinition('ply_header', type_name='text'),
]
tableopts = {'metadata': 'true', 'rosbag': schema, 'pcid': pcid_str}
tabledefs.append(TableDefinition("pointcloud_formats", columns=tablecols,
options=tableopts))
for topic, infos in topics.items():
pcid = pcid_for_topic[topic]
columns, _, _, _, _, _ = get_columns(bag, topic, infos, pcid,
patch_column, patch_columns)
tablecols = [get_column_def(k, *v) for k, v in columns.items()]
tableopts = {'topic': topic, 'rosbag': schema, 'pcid': str(pcid)}
tabledefs.append(TableDefinition(topic, columns=tablecols, options=tableopts))
return tabledefs
def execute(self, quals, columns):
if self.pointcloud_formats is not None:
for f in self.pointcloud_formats:
yield f
return
self.patch_data = ''
from rospy.rostime import Time
tmin = None
tmax = None
for qual in quals:
if qual.field_name == "time":
t = int(qual.value)
t = Time(t / 1000000000, t % 1000000000)
if qual.operator in ['=', '>', '>=']:
tmin = t
if qual.operator in ['=', '<', '<=']:
tmax = t
for topic, msg, t in self.bag.read_messages(
topics=self.topic, start_time=tmin, end_time=tmax):
for row in self.get_rows(topic, msg, t, columns):
yield row
# flush leftover patch data
if self.patch_data and self.last_row:
count = int((len(self.patch_data) / self.point_size))
# in replicating mode, a single leftover point must not be reported
if count > 1 or self.patch_step_size == self.patch_size:
res = self.last_row
if self.patch_column in columns:
res[self.patch_column] = hexlify(
pack('=b3I', self.endianness, self.pcid, 0, count) + self.patch_data)
if self.patch_ply_header and 'ply' in columns:
self.ply_info['count'] = count
res['ply'] = self.patch_ply_header.format(**self.ply_info) + self.patch_data
yield res
def get_rows(self, topic, msg, t, columns, toplevel=True):
if toplevel and len(msg.__slots__) == 1:
attr = getattr(msg, msg.__slots__[0])
if isinstance(attr, list):
for msg in attr:
for row in self.get_rows(topic, msg, t, columns, False):
yield row
return
res = {}
data_columns = set(columns)
if self.patch_column in columns:
data_columns = data_columns.union(self.patch_columns) - set([self.patch_column])
if "filename" in data_columns:
res["filename"] = self.filename
if "topic" in data_columns:
res["topic"] = topic
if "time" in data_columns:
res["time"] = t.to_nsec()
if self.infos.msg_type == 'sensor_msgs/PointCloud2':
self.patch_count = self.patch_count_pointcloud or (msg.width*msg.height)
self.point_size = msg.point_step
self.patch_size = self.patch_count * self.point_size
self.patch_step_size = self.patch_size
self.endianness = 0 if msg.is_bigendian else 1
data_columns = data_columns - set(['ply', self.patch_column])
self.patch_data += msg.data
data_columns = data_columns - set(res.keys())
for column in data_columns:
attr = msg
for col in column.split('.'):
if isinstance(attr, list):
attr = tuple(getattr(a, col) for a in attr)
else:
attr = getattr(attr, col)
if hasattr(attr, "to_nsec"):
attr = attr.to_nsec()
elif hasattr(attr, "x"):
if hasattr(attr, "w"):
attr = (attr.x, attr.y, attr.z, attr.w)
else:
attr = (attr.x, attr.y, attr.z)
elif isinstance(attr, str):
fmt = self.columns[column][3]
if fmt:
attr = unpack(fmt, attr)
res[column] = attr
if self.patch_column in columns and not self.infos.msg_type == 'sensor_msgs/PointCloud2':
fmt = self.columns[self.patch_column][3]
self.patch_count = self.patch_count_default
self.point_size = calcsize(fmt)
self.patch_size = self.patch_count * self.point_size
self.patch_step_size = self.patch_size - self.point_size
self.patch_data += get_point_data(res, self.patch_columns, fmt)
res = {k: v for k, v in res.items() if k not in self.patch_columns}
if not self.patch_data:
yield res
else:
# todo: ensure current res and previous res are equal if there is some leftover
# patch_data
while len(self.patch_data) >= self.patch_size:
data = self.patch_data[0:self.patch_size]
count = int(self.patch_size / self.point_size)
res[self.patch_column] = hexlify(
pack('=b3I', self.endianness, self.pcid, 0, count) + data)
if self.patch_ply_header and 'ply' in columns:
self.ply_info = {
'endianness': 'big' if self.endianness else 'little',
'filename': self.filename,
'topic': self.topic,
'count': count
}
res['ply'] = self.patch_ply_header.format(**self.ply_info) + data
self.patch_data = self.patch_data[self.patch_step_size:]
yield res
self.last_row = res
class Rosbag(ForeignDataWrapper):
def __init__(self, options, columns=None):
super(Rosbag, self).__init__(options, columns)
Bag = import_bag(options)
self.filename = options.pop('rosbag_path', "") + options.pop('rosbag')
self.topic = options.pop('topic', None)
pointcloud_formats = strtobool(options.pop('metadata', 'false'))
self.patch_column = options.pop('patch_column', 'points').strip()
self.patch_columns = options.pop('patch_columns', '*').strip()
self.patch_columns = [
col.strip() for col in self.patch_columns.split(',')
if col.strip()
]
self.patch_count_default = int(options.pop('patch_count_default',
1000))
# 0 => 1 patch per message
self.patch_count_pointcloud = int(
options.pop('patch_count_pointcloud', 0))
assert (self.patch_count_default > 0)
assert (self.patch_count_pointcloud >= 0)
self.pcid = int(options.pop('pcid', 0))
self.bag = Bag(self.filename, 'r')
self.topics = self.bag.get_type_and_topic_info().topics
self.pointcloud_formats = None
if pointcloud_formats:
self.pointcloud_formats = []
topics = self.topic.split(',') if self.topic else self.topics
for i, topic in enumerate(self.topics):
if topic not in topics:
continue
infos = self.topics[topic]
columns, patch_schema, patch_ply_header, _, patch_columns, patch_srid = \
get_columns(self.bag, topic, infos, self.pcid+i+1, self.patch_column,
self.patch_columns)
self.pointcloud_formats.append({
'pcid':
self.pcid + i + 1,
'srid':
patch_srid,
'schema':
patch_schema,
'format':
columns[self.patch_column][3],
'rostype':
infos.msg_type,
'columns':
patch_columns,
'ply_header':
patch_ply_header,
})
return
if not self.topic:
log_to_postgres('"topic" option is required', ERROR)
self.infos = self.topics[self.topic]
(self.columns, self.patch_schema, self.patch_ply_header, self.endianness,
self.patch_columns, self.patch_srid) = \
get_columns(self.bag, self.topic, self.infos, self.pcid, self.patch_column,
self.patch_columns)
if columns:
missing = set(columns) - set(self.columns.keys())
columns = list(c for c in self.columns.keys() if c in columns)
if missing:
missing = ", ".join(sorted(missing))
support = ", ".join(sorted(self.columns.keys()))
log_to_postgres(
"extra unsupported columns : {}".format(missing),
WARNING,
hint="supported columns : {}".format(support))
self.columns = {col: self.columns[col] for col in columns}
if options:
log_to_postgres(
"extra unsupported options : {}".format(options.keys()),
WARNING)
@classmethod
def import_schema(self, schema, srv_options, options, restriction_type,
restricts):
Bag = import_bag(srv_options)
pcid_str = options.pop('pcid', srv_options.pop('pcid', 0))
pcid = int(pcid_str)
patch_column = options.pop('patch_column',
srv_options.pop('patch_column', 'points'))
patch_columns = options.pop('patch_columns', '*').strip()
patch_columns = [
col.strip() for col in patch_columns.split(',') if col.strip()
]
filename = srv_options.pop('rosbag_path', "") + options.pop(
'rosbag_path', "") + schema
bag = Bag(filename, 'r')
tablecols = []
topics = bag.get_type_and_topic_info().topics
pcid_for_topic = {k: pcid + 1 + i for i, k in enumerate(topics.keys())}
pointcloud_formats = True
if restriction_type is 'limit':
topics = {k: v for k, v in topics.items() if k in restricts}
pointcloud_formats = 'pointcloud_formats' in restricts
elif restriction_type is 'except':
topics = {k: v for k, v in topics.items() if k not in restricts}
pointcloud_formats = 'pointcloud_formats' not in restricts
tabledefs = []
if pointcloud_formats:
tablecols = [
ColumnDefinition('pcid', type_name='integer'),
ColumnDefinition('srid', type_name='integer'),
ColumnDefinition('schema', type_name='text'),
ColumnDefinition('format', type_name='text'),
ColumnDefinition('rostype', type_name='text'),
ColumnDefinition('columns', type_name='text[]'),
ColumnDefinition('ply_header', type_name='text'),
]
tableopts = {
'metadata': 'true',
'rosbag': schema,
'pcid': pcid_str
}
tabledefs.append(
TableDefinition("pointcloud_formats",
columns=tablecols,
options=tableopts))
for topic, infos in topics.items():
pcid = pcid_for_topic[topic]
columns, _, _, _, _, _ = get_columns(bag, topic, infos, pcid,
patch_column, patch_columns)
tablecols = [get_column_def(k, *v) for k, v in columns.items()]
tableopts = {'topic': topic, 'rosbag': schema, 'pcid': str(pcid)}
tabledefs.append(
TableDefinition(topic, columns=tablecols, options=tableopts))
return tabledefs
def execute(self, quals, columns):
if self.pointcloud_formats is not None:
for f in self.pointcloud_formats:
yield f
return
self.patch_data = ''
from rospy.rostime import Time
tmin = None
tmax = None
for qual in quals:
if qual.field_name == "time":
t = int(qual.value)
t = Time(t / 1000000000, t % 1000000000)
if qual.operator in ['=', '>', '>=']:
tmin = t
if qual.operator in ['=', '<', '<=']:
tmax = t
for topic, msg, t in self.bag.read_messages(topics=self.topic,
start_time=tmin,
end_time=tmax):
for row in self.get_rows(topic, msg, t, columns):
yield row
# flush leftover patch data
if self.patch_data and self.last_row:
count = int((len(self.patch_data) / self.point_size))
# in replicating mode, a single leftover point must not be reported
if count > 1 or self.patch_step_size == self.patch_size:
res = self.last_row
if self.patch_column in columns:
res[self.patch_column] = hexlify(
pack('=b3I', self.endianness, self.pcid, 0, count) +
self.patch_data)
if self.patch_ply_header and 'ply' in columns:
self.ply_info['count'] = count
res['ply'] = self.patch_ply_header.format(
**self.ply_info) + self.patch_data
yield res
def get_rows(self, topic, msg, t, columns, toplevel=True):
if toplevel and len(msg.__slots__) == 1:
attr = getattr(msg, msg.__slots__[0])
if isinstance(attr, list):
for msg in attr:
for row in self.get_rows(topic, msg, t, columns, False):
yield row
return
res = {}
data_columns = set(columns)
if self.patch_column in columns:
data_columns = data_columns.union(self.patch_columns) - set(
[self.patch_column])
if "filename" in data_columns:
res["filename"] = self.filename
if "topic" in data_columns:
res["topic"] = topic
if "time" in data_columns:
res["time"] = t.to_nsec()
if self.infos.msg_type == 'sensor_msgs/PointCloud2':
self.patch_count = self.patch_count_pointcloud or (msg.width *
msg.height)
self.point_size = msg.point_step
self.patch_size = self.patch_count * self.point_size
self.patch_step_size = self.patch_size
self.endianness = 0 if msg.is_bigendian else 1
data_columns = data_columns - set(['ply', self.patch_column])
self.patch_data += msg.data
data_columns = data_columns - set(res.keys())
for column in data_columns:
attr = msg
for col in column.split('.'):
if isinstance(attr, list):
attr = tuple(getattr(a, col) for a in attr)
else:
attr = getattr(attr, col)
if hasattr(attr, "to_nsec"):
attr = attr.to_nsec()
elif hasattr(attr, "x"):
if hasattr(attr, "w"):
attr = (attr.x, attr.y, attr.z, attr.w)
else:
attr = (attr.x, attr.y, attr.z)
elif isinstance(attr, str):
fmt = self.columns[column][3]
if fmt:
attr = unpack(fmt, attr)
res[column] = attr
if self.patch_column in columns and not self.infos.msg_type == 'sensor_msgs/PointCloud2':
fmt = self.columns[self.patch_column][3]
self.patch_count = self.patch_count_default
self.point_size = calcsize(fmt)
self.patch_size = self.patch_count * self.point_size
self.patch_step_size = self.patch_size - self.point_size
self.patch_data += get_point_data(res, self.patch_columns, fmt)
res = {k: v for k, v in res.items() if k not in self.patch_columns}
if not self.patch_data:
yield res
else:
# todo: ensure current res and previous res are equal if there is some leftover
# patch_data
while len(self.patch_data) >= self.patch_size:
data = self.patch_data[0:self.patch_size]
count = int(self.patch_size / self.point_size)
res[self.patch_column] = hexlify(
pack('=b3I', self.endianness, self.pcid, 0, count) + data)
if self.patch_ply_header and 'ply' in columns:
self.ply_info = {
'endianness': 'big' if self.endianness else 'little',
'filename': self.filename,
'topic': self.topic,
'count': count
}
res['ply'] = self.patch_ply_header.format(
**self.ply_info) + data
self.patch_data = self.patch_data[self.patch_step_size:]
yield res
self.last_row = res
