def main():
parser = argparse.ArgumentParser(
description=
'Merge one or more bag files with the possibilities of filtering topics.'
)
parser.add_argument('outputbag',
help='Output bag file with topics merged.')
parser.add_argument('inputbag', nargs='+', help='Input bag files.')
parser.add_argument('-v',
'--verbose',
action="store_true",
default=False,
help='Verbose output.')
parser.add_argument(
'-t',
'--topics',
default="*",
help=
'String interpreted as a list of topics (wildcards \'*\' and \'?\' allowed) to include in the merged bag file.'
)
args = parser.parse_args()
topics = args.topics.split(' ')
total_included_count = 0
total_skipped_count = 0
if (args.verbose):
print("Writing bag file: " + args.outputbag)
print("Matching topics against patters: '%s'" % ' '.join(topics))
with Bag(args.outputbag, 'w') as o:
for ifile in args.inputbag:
matchedtopics = []
included_count = 0
skipped_count = 0
if (args.verbose):
print("> Reading bag file: " + ifile)
with Bag(ifile, 'r') as ib:
for topic, msg, t in ib:
if any(fnmatchcase(topic, pattern) for pattern in topics):
if not topic in matchedtopics:
matchedtopics.append(topic)
if (args.verbose):
print("Including matched topic '%s'" % topic)
o.write(topic, msg, t)
included_count += 1
else:
skipped_count += 1
total_included_count += included_count
total_skipped_count += skipped_count
if (args.verbose):
print("< Included %d messages and skipped %d" %
(included_count, skipped_count))
if (args.verbose):
print("Total: Included %d messages and skipped %d" %
(total_included_count, total_skipped_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 merge():
# Setting arguments
parser = argparse.ArgumentParser(description='Merge one or more bag files with the possibilities of filtering topics.')
parser.add_argument('outputbag', help='output bag file with topics merged')
parser.add_argument('inputbag', nargs='+', help='input bag files')
parser.add_argument('-v', '--verbose', action="store_true", default=False, help='verbose output')
parser.add_argument('-t', '--topics', default="*", help='string interpreted as a list of topics (wildcards \'*\' and \'?\' allowed) to include in the merged/filtered bag file')
parser.add_argument('-nt', '--new_topic', default=False, help='store all the potentially found topics with this "new_topic" name in the merged/filtered bag file')
args = parser.parse_args()
# Varialbe initialisation
topics = args.topics.split(' ')
total_included_count = 0
total_skipped_count = 0
if (args.verbose):
print("Writing bag file: " + args.outputbag)
print("Matching topics against patters: '%s'" % ' '.join(topics))
# Start the process
with Bag(args.outputbag, 'w') as o:
for ifile in args.inputbag:
matchedtopics = []
included_count = 0
skipped_count = 0
if (args.verbose):
print("> Reading bag file: " + ifile)
# Read each potentially available bag file
with Bag(ifile, 'r') as ib:
for topic, msg, t in ib:
if any(fnmatchcase(topic, pattern) for pattern in topics):
if not topic in matchedtopics:
matchedtopics.append(topic)
if (args.verbose):
print("Including matched topic '%s'" % topic)
if args.new_topic: # If required, store with new topic name
o.write(args.new_topic, msg, t)
else: # otherwise, continue as it is
o.write(topic, msg, t)
included_count += 1
else:
skipped_count += 1
total_included_count += included_count
total_skipped_count += skipped_count
if (args.verbose):
print("< Included %d messages and skipped %d" % (included_count, skipped_count))
if (args.verbose):
print("Total: Included %d messages and skipped %d" % (total_included_count, total_skipped_count))
def remap(input_bag_path, output_bag_path):
"""
Remaps the topics of a single bag
:param input_bag_path: the bag with the topic that should be renamed
:param output_bag_path: the path where the output bag should be created
"""
with Bag(output_bag_path, 'w') as output_bag: # create output bag
# write all topics into the output bag and rename the specified one:
for topic, msg, t in Bag(input_bag_path):
if topic == '/image_raw':
output_bag.write('/camera/image_proc', msg, t)
else:
output_bag.write(topic, msg, t)
def RenameTopics(bag_file, location, collect_type, car_name):
print("Renaming topics...")
new_bag_file = bag_file + "_" + location + "_" + collect_type + "_" + car_name + ".bag"
velodyne_name = '/vlp16_1/velodyne_points'
new_velodyne_name = '/points_raw'
with Bag(new_bag_file, 'w') as Y:
for topic, msg, t in Bag(bag_file + ".bag"):
if topic == velodyne_name:
Y.write(new_velodyne_name, msg, t)
else:
Y.write(topic, msg, t)
def main():
ifile = './mvsec_data_bags/outdoor_day2_events.bag'
write_bag = Bag(ifile, 'w')
print('write bag opened')
ofile = './mvsec_data_bags/outdoor_day2_data.bag'
read_bag = Bag(ofile, 'r')
print('read bag opened')
iteration = 1
for topic, msg, t in read_bag:
if ('events' in topic):
write_bag.write(topic, msg, t)
iteration = iteration + 1
print(iteration, topic)
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 load_trajectory_from_bag(file, fast=True, fast_only=False):
traj = []
fast_traj = []
bag = Bag(file)
normal_topics = ['raven_state', 'raven_state/array']
normal_topics = normal_topics + ['/' + topic for topic in normal_topics]
fast_topic = ['raven_state/1000Hz']
#fast_topic = fast_topic + ['/' + topic for topic in fast_topic]
if fast_only:
for topic, msg, t in bag.read_messages(topics=fast_topic):
fast_traj.append(msg)
return get_trajectory(fast_traj)
for topic, msg, t in bag.read_messages(topics=normal_topics):
traj.append(msg)
if fast:
for topic, msg, t in bag.read_messages(topics=fast_topic):
fast_traj.append(msg)
if fast_traj:
return get_trajectory(fast_traj, traj)
else:
return get_trajectory(traj)
def process_bag_file(file: Union[str, BinaryIO],
topic_to_cb: Mapping[str, Callable[[ROSMsgT], DataPoint]]) \
-> DistTrace:
"""
Process messages from bag files
Notes
-----
Only the timestamp of messages from the same topic is guaranteed to be monotonically increasing.
Be careful when merging messages from different topics.
"""
ret_dist_trace = {topic: []
for topic in topic_to_cb.keys()
} # type: Dict[str, List[StampedDataPoint]]
with Bag(file) as bag:
for topic, msg, t in bag.read_messages(topics=topic_to_cb.keys()):
if hasattr(msg, "header"):
t = msg.header.stamp # Use sender's timestamp if available
t_nsec = t.to_nsec() # type: int
quo, rem = divmod(t_nsec, (5**6))
if t_nsec > MAX_PRECISE_INT or rem != 0:
raise RuntimeWarning(
"Timestamp %s nano-sec may lose precision when converted to milli-sec"
"in floating point." % str(t))
t_msec_f = float(quo) / (2**6)
if topic in topic_to_cb:
dp = topic_to_cb[topic](msg)
ret_dist_trace[topic].append(StampedDataPoint(t_msec_f, dp))
else:
continue # Ignore other topics
return ret_dist_trace
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 main():
parser = argparse.ArgumentParser()
parser.add_argument("--bag",
dest="bag",
help="Path to ROS bag.",
required=True)
parser.add_argument("--left_image_0",
help="Path to 'left_image_00000.png'")
parser.add_argument("--right_image_0",
help="Path to 'right_image_00000.png'")
parser.add_argument("--start_time",
help="A start time to check",
type=float,
default=None)
args = parser.parse_args()
bridge = CvBridge()
bag = Bag(args.bag)
t_start = bag.get_start_time()
n_msgs = 0
if args.left_image_0 is None and args.right_image_0 is None:
raise ValueError(
"You must provide either left_image_0 or right_image_0")
isLeft = args.left_image_0 is not None
ref_img = args.left_image_0 if isLeft else args.right_image_0
print("Using ref image %s" % ref_img)
ref_image_0 = cv2.imread(ref_img)[:, :, 0]
assert ref_image_0 is not None
topic = '/davis/left/image_raw' if isLeft else '/davis/right/image_raw'
if args.start_time:
topic, msg, t = next(
bag.read_messages(topics=[topic],
start_time=rospy.Time(args.start_time +
t_start)))
width = msg.width
height = msg.height
image = np.asarray(bridge.imgmsg_to_cv2(msg, msg.encoding))
image = np.reshape(image, (height, width))
print("Correct? {}".format(bool(np.all(np.isclose(image,
ref_image_0)))))
else:
for topic, msg, t in bag.read_messages(topics=[topic]):
n_msgs += 1
width = msg.width
height = msg.height
image = np.asarray(bridge.imgmsg_to_cv2(msg, msg.encoding))
image = np.reshape(image, (height, width))
if bool(np.all(np.isclose(image, ref_image_0))):
print("Ref image found at {} ({} from start)".format(
t.to_sec(),
t.to_sec() - t_start))
return
print("Processed {} images, ref not found!".format(n_msgs))
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')
topic_rules, tf_rules = RenameRule.parse(conf_file_fn)
messages = bag_in.read_messages(return_connection_header=True)
for topic, msg, t, conn_header in messages:
if topic == '/tf' or topic == '/tf_static':
new_topic = topic
new_msg = modify_tf(msg, tf_rules)
else:
new_topic = modify_topic(topic, topic_rules)
# Modify the frame_id in header if header exists
new_msg = modify_msg(msg, tf_rules)
bag_out.write(new_topic, new_msg, t, connection_header=conn_header)
bag_in.close()
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 _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()
}
def ipfs_download(multihash):
rospy.wait_for_service('/liability/infochan/ipfs/get_file')
download = rospy.ServiceProxy('/liability/infochan/ipfs/get_file',
IpfsDownloadFile)
with NamedTemporaryFile(delete=False) as tmpfile:
res = download(multihash, Filepath(tmpfile.name))
if not res.success:
raise Exception(res.error_msg)
messages = {}
for topic, msg, timestamp in Bag(tmpfile.name, 'r').read_messages():
messages[topic] = msg
return messages
def _get_objective(self, lia: Liability) -> dict:
mhash = lia.objective.multihash
tempdir = gettempdir()
os.chdir(tempdir)
# temp_obj = NamedTemporaryFile(delete=False)
self.ipfs.get(mhash)
messages = {}
for topic, msg, timestamp in Bag(mhash, 'r').read_messages():
messages[topic] = msg
return messages
def ipfs_download(multihash):
tempdir = gettempdir()
os.chdir(tempdir)
temp_obj = NamedTemporaryFile(delete=False)
res = ipfs_download_file(connect(), multihash.multihash, temp_obj.name)
if not res:
raise Exception("Can't download objective")
messages = {}
for topic, msg, timestamp in Bag(temp_obj.name, 'r').read_messages():
messages[topic] = msg
return messages
def ipfs_upload(messages):
rospy.wait_for_service('/liability/infochan/ipfs/add_file')
upload = rospy.ServiceProxy('/liability/infochan/ipfs/add_file',
IpfsUploadFile)
with NamedTemporaryFile(delete=False) as tmpfile:
recorder = Bag(tmpfile.name, 'w')
for topic in messages:
recorder.write('/{}'.format(topic), messages[topic])
recorder.close()
res = upload(Filepath(tmpfile.name))
if not res.success:
raise Exception(res.error_msg)
return res.ipfs_address
def main():
parser = argparse.ArgumentParser(
description=
'Align timestamps of several topics to that of a specified topic.')
parser.add_argument('inputbag', help='input bag file')
parser.add_argument('outputbag', help='output bag file')
parser.add_argument('correct_timestamp_topic',
help='topics with timestamps we wish to align to')
parser.add_argument('-ta',
'--topics_to_align',
nargs='+',
help='topics to align')
args = parser.parse_args()
print(args.topics_to_align)
count = 0
correct_timestamps = []
with Bag(args.outputbag, 'w') as ob:
for topic, msg, t in Bag(args.inputbag, 'r').read_messages():
if topic == args.correct_timestamp_topic:
correct_timestamps.append(t)
# for topic_unaligned in topics_to_align:
print("Reading", topic, t)
for topic, msg, t in Bag(args.inputbag, 'r').read_messages():
# print("Writing", topic, t)
if topic in args.topics_to_align:
print("Writing", topic, t, " aligning timestamp from",
msg.header.stamp, " to", correct_timestamps[count])
msg.header.stamp = correct_timestamps[count]
ob.write(topic, msg, correct_timestamps[count])
count += 1
else:
ob.write(topic, msg, t)
def create_objective(period: str, stream_id: str, email: str) -> str:
ipfs = connect()
tempdir = gettempdir()
os.chdir(tempdir)
with NamedTemporaryFile(delete=False) as f:
bag = Bag(f.name, 'w')
bag.write('/period', String(period))
bag.write('/stream_id', String(stream_id))
bag.write('/email', String(email))
bag.close()
res = ipfs.add(f.name)
rospy.loginfo("Hash of the objective is {}".format(res['Hash']))
return res['Hash']
def analyze_time_stamps(bag_name, freq):
print 'finding adjust time and bad time stamps ...'
ts = defaultdict(list)
with Bag(bag_name, 'r') as inbag:
min_time_diff = rospy.Duration(1e10)
t0 = time.time()
start_time = inbag.get_start_time()
end_time = inbag.get_end_time()
total_time = end_time - start_time
last_percent = 0
print "total time span to be processed: %.2fs" % total_time
for topic, msg, t in inbag.read_messages():
percent_done = (t.to_sec() - start_time) / total_time * 100
if (percent_done - last_percent > 5.0):
last_percent = percent_done
t1 = time.time()
time_left = (100 - percent_done) * (t1 - t0) / percent_done
print ("percent done: %5.0f, expected time " + \
"remaining: %5.0fs") % (percent_done, time_left)
if hasattr(msg, 'header'):
ts[topic].append(msg.header.stamp)
dt = t - msg.header.stamp
if dt < min_time_diff:
min_time_diff = dt
print 'min time diff: ', min_time_diff.to_sec()
if rospy.is_shutdown():
break
print 'time shift: ', min_time_diff.to_sec()
bad_stamps = []
bad_diffs = []
for tp in ('/fla/ovc_node/left/image_raw',
'/fla/ovc_node/right/image_raw'):
tss = np.asarray(ts[tp])
tsd = np.diff(tss)
idx = np.where((tsd > rospy.Duration(1.5 * 1.0 / freq))
| (tsd < rospy.Duration(0.1 * 1.0 / freq)))
# add 1 to index to skip *subsequent* frame
idxp = [i + 1 for i in idx]
bad_stamps = bad_stamps + (tss[idxp]).tolist()
bad_diffs = bad_diffs + (tsd[idx]).tolist()
for i in range(0, len(bad_stamps)):
print 'bad time stamp: %f dt = %f' % (bad_stamps[i].to_sec(),
bad_diffs[i].to_sec())
return min_time_diff, sorted(bad_stamps)
def ipfs_download(multihash: Multihash) -> (dict, Bag):
rospy.wait_for_service('/ipfs/get_file')
download = rospy.ServiceProxy('/ipfs/get_file', IpfsDownloadFile)
tmpfile = NamedTemporaryFile(delete=False)
res = download(multihash, Filepath(tmpfile.name))
tmpfile.close()
if not res.success:
raise Exception(res.error_msg)
messages = {}
bag = Bag(tmpfile.name, 'r')
for topic, msg, timestamp in bag.read_messages():
if topic not in messages:
messages[topic] = [msg]
else:
messages[topic].append(msg)
os.unlink(tmpfile.name)
return (messages, bag)
def writepc2frombag2file(input_bagfile, pc2_topic, out_file_path,start_time=None, end_time=None, msg_count=None):
try:
output_file_name = get_output_file_name(input_bagfile) + '_xyz.txt'
if out_file_path.endswith('/'):
output_file = out_file_path + output_file_name
else:
output_file = out_file_path+'/' + output_file_name
output_file_fh = open(output_file,'w')
print('writing output_file at', output_file)
# check start and end time condition
if start_time is not None and end_time is not None:
assert (end_time - start_time) > 0 , "end_time should be higher than start time"
if msg_count is None:
use_msg_count = False
else:
use_msg_count = True
# msg_count is already static casted in arg parse, just checking again for sanity ad if function is re used individially
msg_count = int(msg_count)
assert msg_count > 0, "should have positive msg_count"
input_bag = Bag(input_bagfile,'r')
print('bag load success')
count = 0;
for topic, msg, ts in input_bag.read_messages(topics=[pc2_topic],start_time=start_time, end_time=end_time):
output_file_fh.write('msg_timestamp: %f\n' % msg.header.stamp.to_sec())
output_file_fh.write('%d\n' %msg.width)
for data_pts in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
output_file_fh.write('%f %f %f %d\n' % (data_pts[0], data_pts[1], data_pts[2], data_pts[3]))
count +=1
print(count,msg_count)
if use_msg_count and count >= msg_count:
break
input_bag.close()
output_file_fh.close()
except Exception as e_init:
print(traceback.format_exc(e_init))
def pinata_rosbag(ipfs_hash: str) -> str:
data = requests.get(f'https://gateway.pinata.cloud/ipfs/{ipfs_hash}')
rospy.loginfo(data.status_code)
if data.status_code != 200:
rospy.loginfo(f'Error downloading rosbag! {data.content}')
return
else:
tmpfile = NamedTemporaryFile(delete=False)
tmpfile.write(data.content)
bag = Bag(tmpfile.name)
messages = {}
for topic, msg, timestamp in bag.read_messages():
if topic not in messages:
messages[topic] = [msg]
else:
messages[topic].append(msg)
os.unlink(tmpfile.name)
return (messages, bag)
#!/usr/bin/env python
#------------------------------------------------------------------------------
#
# append one bag to another one. This is faster than merge, but riskier
#
import rospy
import argparse
from rosbag import Bag
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='append bag2 to bag1.')
parser.add_argument('bagfile1')
parser.add_argument('bagfile2')
args = parser.parse_args(rospy.myargv()[1:])
with Bag(args.bagfile1, 'a') as inbag1:
with Bag(args.bagfile2, 'r') as inbag2:
for topic, msg, t in inbag2.read_messages():
inbag1.write(topic, msg, t)
inbag2.close()
inbag1.close()
action='store',
default=None,
type=int,
help='chunk threshold in bytes.')
parser.add_argument('bagfile')
args = parser.parse_args()
rospy.init_node('compute_point_cloud')
depth_topic = args.sensor_topic + '/image_depth'
points_topic = args.sensor_topic + '/points'
camera_info = make_caminfo_msg.from_ros(read_yaml(args.camera_info))
adj = PointCloudAdjuster(camera_info)
with Bag(args.bagfile, 'r') as inbag:
cthresh = args.chunk_threshold \
if args.chunk_threshold else inbag.chunk_threshold
print "using chunk threshold: ", cthresh
with Bag(args.outbag, mode='w', chunk_threshold=cthresh) as outbag:
t0 = time.time()
start_time = inbag.get_start_time()
end_time = inbag.get_end_time()
total_time = end_time - start_time
time_to_data = defaultdict(dict)
last_percent = 0
print "total time span to be processed: %.2fs" % total_time
for topic, msg, t in inbag.read_messages():
percent_done = (t.to_sec() - start_time) / total_time * 100
if (percent_done - last_percent > 5.0):
last_percent = percent_done
nargs='+',
help='A list of input bag files',
)
# add an argument for the optional output file
PARSER.add_argument(
'--output_file',
'-o',
type=str,
help='An optional output file to dump to instead of the command line.',
default=None,
required=False,
)
try:
# get the arguments from the argument parser
ARGS = PARSER.parse_args()
# open the input bag with an automatically closing context
with Bag(ARGS.input_bag, 'r') as input_bag:
# if there is an output file path, open the file
if ARGS.output_file is not None:
ARGS.output_file = open(ARGS.output_file, 'w')
# stream the input bag to the output bag
dump(input_bag, ARGS.topics, ARGS.output_file)
# if there was an output file, close it
if ARGS.output_file is not None:
ARGS.output_file.close()
except KeyboardInterrupt:
pass
# explicitly define the outward facing API of this module
__all__ = [dump.__name__]
def on_enter(self, userdata):
self._failed = False
try:
# Initialization
# userdata
self._bag_filename = userdata.bag_filename
self._trajectories_command = userdata.trajectories_command
# joint definitions
l_arm_range = range(16, 23)
r_arm_range = range(23, 30)
atlasJointNames = [
'back_bkz', 'back_bky', 'back_bkx', 'neck_ry', 'l_leg_hpz',
'l_leg_hpx', 'l_leg_hpy', 'l_leg_kny', 'l_leg_aky',
'l_leg_akx', 'r_leg_hpz', 'r_leg_hpx', 'r_leg_hpy',
'r_leg_kny', 'r_leg_aky', 'r_leg_akx', 'l_arm_shz',
'l_arm_shx', 'l_arm_ely', 'l_arm_elx', 'l_arm_wry',
'l_arm_wrx', 'l_arm_wry2', 'r_arm_shz', 'r_arm_shx',
'r_arm_ely', 'r_arm_elx', 'r_arm_wry', 'r_arm_wrx',
'r_arm_wry2'
]
joint_position_cmd = [0] * len(l_arm_range)
# starting and stopping time of measurement data to be taken into account for cailbration
time_start = 0
time_end = 0
time_tf = 0
# gravity vector in world frame
g_world = np.resize(np.array([0, 0, -9.81]), (3, 1))
# take this number of data points for each pose.
max_data_points_per_pose = 100
# rotation of the force torque sensor in world frame (quaternions)
ft_rotation = [0, 0, 0, 0]
# loop over all ft sensors to calibrate
for chain in self._calibration_chain:
tf_chain = ['/pelvis', 'ltorso', 'mtorso', 'utorso']
if chain == 'left_arm':
joint_range = l_arm_range
tfname = 'l_hand'
tf_chain.extend([
'l_clav', 'l_scap', 'l_uarm', 'l_larm', 'l_ufarm',
'l_lfarm', 'l_hand'
])
elif chain == 'right_arm':
joint_range = r_arm_range
tf_chain.extend([
'r_clav', 'r_scap', 'r_uarm', 'r_larm', 'r_ufarm',
'r_lfarm', 'r_hand'
])
tfname = 'r_hand'
else:
Logger.logwarn(
'CalculateForceTorqueCalibration: Undefined chain %s',
chain)
# initialize transformations
tf_data = [
] # frame to frame transformation with numerical indexes from the tf_chain
tf_data_cum = [
] # world to frame transformation with numerical indexes from the tf_chain
for i in range(len(tf_chain)):
tf_data.append([0, 0, 0, 0])
tf_data_cum.append([0, 0, 0, 0])
# get number of poses from the commanded trajectories. 2 Points per Pose
number_of_poses = len(
self._trajectories_command[chain].points) / 2
# define information matrix for calibration
InfMat = np.zeros(
(6 * max_data_points_per_pose * number_of_poses, 10))
InfMat_i = 0 # Index in this Matrix
# definie measurement vector for calibration
MeasVec = np.zeros(
(6 * max_data_points_per_pose * number_of_poses, 1))
# commanded joint trajectory for current chain
# all commanded positions in this trajectory
current_traj_cmd = self._trajectories_command[chain]
# flag if the time interval was already set:
timesetflag = False
point_index_cmd = 0 # start with index 0
transformation_available = False
# read time series from bag file
# check in desired input trajectory, at which time a position is reached
# take the data in a defined time period after the commanded new position
bag_from_robot = Bag(os.path.expanduser(
self._bag_filename)) # open bag file
Logger.loginfo(
'CalculateForceTorqueCalibration: Calibrate %s from %s. Using %d different positions from trajectory command. Expecting 2 commanded positions per pose'
%
(chain, self._bag_filename, len(current_traj_cmd.points)))
for topic, msg, t in bag_from_robot.read_messages(topics=[
'/flor/controller/atlas_state', '/tf',
'/flor/controller/joint_command'
]):
# Check if all desired poses have been reached
if point_index_cmd > len(current_traj_cmd.points) - 1:
break
current_position_cmd = current_traj_cmd.points[
point_index_cmd].positions
####################################################
# Check tf message: remember last transformation from world to ft sensor
if topic == '/tf':
# loop over all transformations inside this tf message
for j in range(len(msg.transforms)):
data = msg.transforms[j]
tr = data.transform # current transformation
header = data.header
# check if frame matches on of the saved frames
for i in range(len(tf_chain)):
if data.child_frame_id == tf_chain[i]:
tf_data[i] = np.array([
tr.rotation.x, tr.rotation.y,
tr.rotation.z, tr.rotation.w
])
# calculate the newest transformation to the force torque sensor
tf_data_cum[0] = tf_data[0]
for i in range(1, len(tf_chain)):
tf_data_cum[
i] = q = tf.transformations.quaternion_multiply(
tf_data_cum[i - 1], tf_data[i])
if i == len(tf_chain) - 1:
time_tf = msg.transforms[
0].header.stamp.to_sec()
# nothing to do with tf message
# check if all transformations are available. (up to the last link)
if np.any(
tf_data_cum[-1]
): # real part of quaternion unequal to zero: data exists
transformation_available = True
continue
else:
time_msg = msg.header.stamp.to_sec()
####################################################
# check if timestamp is interesting. Then check the
if not (time_msg > time_start
and time_msg < time_end) and not timesetflag:
# the timestamp is not in evaluation interval. Look for reaching of the pose
# record data (see below)
if topic == '/flor/controller/joint_command':
# get the commanded position
for i in range(len(joint_range)):
osrf_ndx = joint_range[i]
joint_position_cmd[i] = msg.position[osrf_ndx]
# Check if position command matches the currently expected commanded position
pos_reached = True
for i in range(len(joint_range)):
if abs(joint_position_cmd[i] -
current_position_cmd[i]) > 0.001:
pos_reached = False
break
if pos_reached:
# end time of the values regarded for calibration data: take the latest time possible for this pose
time_end = msg.header.stamp.to_sec(
) + self._settlingtime
# starting time for calibration: Take 100ms
time_start = time_end - 0.1
data_points_for_this_pose = 0
timesetflag = True
# take the next point next time. Each pose consists of two trajectory points (one for reaching, one for settling).
# take the second one
point_index_cmd = point_index_cmd + 2
if not pos_reached:
# The commanded position has not been reached. Skip
continue
continue # continue here, because data aquisition is triggered by the time_start, time_end
if time_msg > time_end:
timesetflag = False # prepare for new evaluation interval
####################################################
# check if enough datapoints for this pose have been collected
if data_points_for_this_pose > max_data_points_per_pose:
# already enough data points for this pose
continue
####################################################
# Check if message is atlas_state
# IF this is the case, fill information matrix
if topic != '/flor/controller/atlas_state':
continue
# Extract measured force and torque
if chain == 'left_arm':
FT = [
msg.l_hand.force.x, msg.l_hand.force.y,
msg.l_hand.force.z, msg.l_hand.torque.x,
msg.l_hand.torque.y, msg.l_hand.torque.z
]
elif chain == 'right_arm':
FT = [
msg.r_hand.force.x, msg.r_hand.force.y,
msg.r_hand.force.z, msg.r_hand.torque.x,
msg.r_hand.torque.y, msg.r_hand.torque.z
]
# calculate gravitation vector
if not transformation_available:
Logger.logwarn(
'No tf messages available at time %1.4f.' %
time_msg)
continue
R = tf.transformations.quaternion_matrix(tf_data_cum[-1])
g = np.dot((R[0:3, 0:3]).transpose(), g_world)
gx = g[0]
gy = g[1]
gz = g[2]
# fill information matrix for this data point. Source: [1], equ. (7)
M = np.zeros((6, 10))
M[0, 0] = gx
M[1, 0] = gy
M[2, 0] = gz
M[3, 2] = gz
M[3, 3] = -gy
M[4, 1] = -gz
M[4, 3] = gx
M[5, 1] = gy
M[5, 2] = -gx
M[0, 4] = 1.0
M[1, 5] = 1.0
M[2, 6] = 1.0
M[3, 7] = 1.0
M[4, 8] = 1.0
M[5, 9] = 1.0
# fill big information matrix and vector (stack small information matrizes)
for i in range(6):
for j in range(10):
InfMat[InfMat_i * 6 + i, j] = M[i, j]
MeasVec[InfMat_i * 6 + i, 0] = FT[i]
InfMat_i = InfMat_i + 1 # increase index
data_points_for_this_pose = data_points_for_this_pose + 1
# shorten big information matrix
if InfMat_i < max_data_points_per_pose * number_of_poses:
InfMat_calc = InfMat[0:(6 * InfMat_i) - 1, :]
MeasVec_calc = MeasVec[0:(6 * InfMat_i - 1), :]
else:
InfMat_calc = InfMat
MeasVec_calc = MeasVec
# calculate calibration data
if chain in self._static_calibration_data.keys(
): # calculate calibration with given static parameters
# bring colums with first parameters on the other side of the equation
if len(self._static_calibration_data[chain]) != 4:
Logger.logwarn(
"CalculateForceTorqueCalibration: Given static calibration data for %s has length %d. Required 4 entries."
% (chain, len(k_fix)))
self._failed = True
return
# convert physical parameters to identification parameters (mass, 1st moment)
m = self._static_calibration_data[chain][0]
if m == 0:
mom_x = 0
mom_y = 0
mom_z = 0
elif m > 0:
mom_x = self._static_calibration_data[chain][1] / m
mom_y = self._static_calibration_data[chain][2] / m
mom_z = self._static_calibration_data[chain][3] / m
else:
Logger.logwarn(
"CalculateForceTorqueCalibration: Negative mass (%f) for calibration requested. Abort."
% mass)
self._failed = True
return
k_fix = np.resize(np.array([m, mom_x, mom_y, mom_z]),
(4, 1))
Logger.loginfo(
"CalculateForceTorqueCalibration:static calibration data for %s given: %s. Reduce equation system"
% (chain, str(self._static_calibration_data[chain])))
MeasVec_calc_corr = np.subtract(
np.array(MeasVec_calc),
np.dot(InfMat_calc[:, 0:4], k_fix))
InfMat_calc_corr = InfMat_calc[:, 4:
10] # only the last 6 colums which correspond to the sensor offsets
k = np.linalg.lstsq(
InfMat_calc_corr,
MeasVec_calc_corr)[0] # solve reduced equation system
k_calibration = self._static_calibration_data[chain]
k_calibration.extend(k)
else: # calculate normally with all parameters unknown
k = np.linalg.lstsq(InfMat_calc, MeasVec_calc)[0]
# convert to physical parameters (first moment -> mass)
if k[0] > 0:
k_calibration = [
k[0], k[1] / k[0], k[2] / k[0], k[3] / k[0], k[4],
k[5], k[6], k[7], k[8], k[9]
]
else:
Logger.loginfo(
"CalculateForceTorqueCalibration:Calibration brought negative mass %f"
% k[0])
k_calibration = [
0.0, 0.0, 0.0, 0.0, k[4], k[5], k[6], k[7], k[8],
k[9]
]
Logger.loginfo(
"CalculateForceTorqueCalibration:calibration data for %s" %
chain)
Logger.loginfo("CalculateForceTorqueCalibration:mass: %f" %
float(k_calibration[0]))
Logger.loginfo(
"CalculateForceTorqueCalibration:center of mass: %f %f %f"
% (k_calibration[1], k_calibration[2], k_calibration[3]))
Logger.loginfo(
"CalculateForceTorqueCalibration:F offset: %f %f %f" %
(k_calibration[4], k_calibration[5], k_calibration[6]))
Logger.loginfo(
"CalculateForceTorqueCalibration:M offset: %f %f %f" %
(k_calibration[7], k_calibration[8], k_calibration[9]))
self._ft_calib_data[chain] = k_calibration
bag_from_robot.close()
userdata.ft_calib_data = self._ft_calib_data
Logger.loginfo(
'CalculateForceTorqueCalibration:Calibration finished')
self._done = True
except Exception as e:
Logger.logwarn(
'CalculateForceTorqueCalibration:Unable to calculate calibration:\n%s'
% str(e))
self._failed = True
def main():
parser = argparse.ArgumentParser(
description=("Extracts grayscale and event images from a ROS bag and "
"saves them as TFRecords for training in TensorFlow."))
parser.add_argument("--bag", dest="bag",
help="Path to ROS bag.",
required=True)
parser.add_argument("--prefix", dest="prefix",
help="Output file prefix.",
required=True)
parser.add_argument("--output_folder", dest="output_folder",
help="Output folder.",
required=True)
parser.add_argument("--max_aug", dest="max_aug",
help="Maximum number of images to combine for augmentation.",
type=int,
default=6)
parser.add_argument("--n_skip", dest="n_skip",
help="Maximum number of images to combine for augmentation.",
type=int,
default=1)
parser.add_argument("--start_time", dest="start_time",
help="Time to start in the bag.",
type=float,
default=0.0)
parser.add_argument("--end_time", dest="end_time",
help="Time to end in the bag.",
type=float,
default=-1.0)
parser.add_argument("--save_rgb_images", default=True,
const=False, nargs="?")
parser.add_argument("--debug", default=False,
const=True, nargs="?")
parser.add_argument("--whitelist_imageids_txt",
type=str,
default=None)
args = parser.parse_args()
bridge = CvBridge()
n_msgs = 0
left_start_event_offset = 0
right_start_event_offset = 0
left_event_image_iter = 0
right_event_image_iter = 0
left_image_iter = 0
right_image_iter = 0
first_left_image_time = -1
first_right_image_time = -1
left_events = []
right_events = []
left_images = []
right_images = []
left_image_times = []
right_image_times = []
left_event_count_images = []
left_event_time_images = []
left_event_image_times = []
right_event_count_images = []
right_event_time_images = []
right_event_image_times = []
left_image_event_start_idx = []
left_image_event_end_idx = []
right_image_event_start_idx = []
right_image_event_end_idx = []
whitelist_imageids = None
if args.whitelist_imageids_txt is not None:
with open(args.whitelist_imageids_txt, 'r') as fp:
whitelist_imageids = fp.read().splitlines()
whitelist_imageids = [int(l) for l in whitelist_imageids]
cols = 346
rows = 260
print("Processing bag")
bag = Bag(args.bag)
h5_left, h5_right = None, None
if args.debug:
import h5py
h5_file = h5py.File(args.bag[:-len("bag")]+"hdf5")
h5_left = h5_file['davis']['left']['events']
h5_right = h5_file['davis']['right']['events']
options = tf.python_io.TFRecordOptions(tf.python_io.TFRecordCompressionType.GZIP)
left_tf_writer = tf.python_io.TFRecordWriter(
os.path.join(args.output_folder, args.prefix, "left_event_images.tfrecord"),
options=options)
right_tf_writer = tf.python_io.TFRecordWriter(
os.path.join(args.output_folder, args.prefix, "right_event_images.tfrecord"),
options=options)
# Get actual time for the start of the bag.
t_start = bag.get_start_time()
t_start_ros = rospy.Time(t_start)
# Set the time at which the bag reading should end.
if args.end_time == -1.0:
t_end = bag.get_end_time()
else:
t_end = t_start + args.end_time
for topic, msg, t in bag.read_messages(
topics=['/davis/left/image_raw',
'/davis/right/image_raw',
'/davis/left/events',
'/davis/right/events'],
# **** MOD **** #
# NOTE: we always start reading from the start in order
# to count the number of events that have to be
# discarded in the HDF5 file
# start_time=rospy.Time(args.start_time + t_start),
end_time=rospy.Time(t_end)):
# Check to make sure we're working with stereo messages.
if not ('left' in topic or 'right' in topic):
print('ERROR: topic {} does not contain left or right, is this stereo?'
'If not, you will need to modify the topic names in the code.'.
format(topic))
return
n_msgs += 1
if n_msgs % 500 == 0:
print("Processed {} msgs, {} images, time is {}.".format(n_msgs,
left_event_image_iter,
t.to_sec() - t_start))
isLeft = 'left' in topic
# **** MOD **** # /*start
# If we are still not reading the part
# we are interested in, we just count
# the number of events
if t.to_sec() < args.start_time + t_start:
if 'events' in topic and msg.events:
if isLeft:
left_start_event_offset += len(msg.events)
else:
right_start_event_offset += len(msg.events)
continue # read the next msg
# **** MOD **** # end*/
if 'image' in topic:
width = msg.width
height = msg.height
if width != cols or height != rows:
print("Image dimensions are not what we expected: set: ({} {}) vs got:({} {})"
.format(cols, rows, width, height))
return
time = msg.header.stamp
image = np.asarray(bridge.imgmsg_to_cv2(msg, msg.encoding))
image = np.reshape(image, (height, width))
if isLeft:
if whitelist_imageids is None or left_image_iter in whitelist_imageids:
if args.save_rgb_images:
cv2.imwrite(os.path.join(args.output_folder,
args.prefix,
"left_image{:05d}.png".format(left_image_iter)),
image)
if left_image_iter > 0:
left_image_times.append(time)
else:
first_left_image_time = time
left_event_image_times.append(time.to_sec())
left_image_iter += 1
else:
if whitelist_imageids is None or right_image_iter in whitelist_imageids:
if args.save_rgb_images:
cv2.imwrite(os.path.join(args.output_folder,
args.prefix,
"right_image{:05d}.png".format(right_image_iter)),
image)
if right_image_iter > 0:
right_image_times.append(time)
else:
first_right_image_time = time
right_event_image_times.append(time.to_sec())
right_image_iter += 1
elif 'events' in topic and msg.events:
for event in msg.events:
ts = event.ts
event = [event.x,
event.y,
(ts - t_start_ros).to_sec(),
(float(event.polarity) - 0.5) * 2]
if isLeft:
if first_left_image_time != -1 and ts > first_left_image_time:
left_events.append(event)
else:
left_start_event_offset += 1
else:
if first_right_image_time != -1 and ts > first_right_image_time:
right_events.append(event)
else:
right_start_event_offset += 1
if isLeft:
if len(left_image_times) >= args.max_aug and \
left_events[-1][2] > (left_image_times[args.max_aug - 1] - t_start_ros).to_sec():
left_event_image_iter, consumed = _save_events(left_events,
left_image_times,
left_event_count_images,
left_event_time_images,
left_event_image_times,
left_start_event_offset,
left_image_event_start_idx,
left_image_event_end_idx,
rows,
cols,
args.max_aug,
args.n_skip,
left_event_image_iter,
args.prefix,
'left',
left_tf_writer,
t_start_ros,
h5_left,
whitelist_imageids)
left_start_event_offset += consumed
else:
if len(right_image_times) >= args.max_aug and \
right_events[-1][2] > (right_image_times[args.max_aug - 1] - t_start_ros).to_sec():
right_event_image_iter, consumed = _save_events(right_events,
right_image_times,
right_event_count_images,
right_event_time_images,
right_event_image_times,
right_start_event_offset,
right_image_event_start_idx,
right_image_event_end_idx,
rows,
cols,
args.max_aug,
args.n_skip,
right_event_image_iter,
args.prefix,
'right',
right_tf_writer,
t_start_ros,
h5_right,
whitelist_imageids)
right_start_event_offset += consumed
left_tf_writer.close()
right_tf_writer.close()
image_counter_file = open(os.path.join(args.output_folder, args.prefix, "n_images.txt"), 'w')
image_counter_file.write("{} {}".format(left_event_image_iter, right_event_image_iter))
image_counter_file.close()
