def shift(
input_file: Bag,
output_file: Bag,
shift_topic: list,
shift_sec: float,
) -> None:
"""
Shift a topic in a bag forward or backwards in time.
Args:
input_file: the input bag to stream from
output_file: the output bag to write data to
topic: the topic to shift in time
shift_sec: the amount of time to shift by
Returns:
None
"""
# create a progress bar for iterating over the messages in the bag
with tqdm(total=input_file.get_message_count()) as prog:
# iterate over the messages in this input bag
for topic, msg, time in input_file:
# if the topic is the sentinel topic, increase the count
if topic == shift_topic:
# adjust time by the shift value
time += rospy.Duration(shift_sec)
# update the message header with the new time stamp
msg.header.stamp = time
# update the progress bar with a single iteration
prog.update(1)
# write the message
output_file.write(topic, msg, time)
def stream(input_file: Bag, output_file: Bag, topics: list) -> None:
"""
Stream data from an input bag to an output bag.
Args:
input_file: the input bag to stream from
output_file: the output bag to write data to
topics: a list of the topics to include
Returns:
None
"""
included = 0
skipped = 0
# create a progress bar for iterating over the messages in the bag
with tqdm(total=input_file.get_message_count(), unit='message') as prog:
# iterate over the messages in this input bag
for topic, msg, time in input_file:
# check for matches between the topics filter and this topic
if any(fnmatchcase(topic, pattern) for pattern in topics):
# write this message to the output bag
output_file.write(topic, msg, time)
# increment the counter of included messages
included += 1
else:
# increment the counter of excluded messages
skipped += 1
# update the progress bar with a single iteration
prog.update(1)
# update the progress bar post fix text with statistics
prog.set_postfix(included=included, skipped=skipped)
def write_bag(bag_origin, bag_write: rosbag.Bag, t_start):
count = 0
for topic, msg, t in bag_origin.read_messages("/tf", t_start):
time_stamp = rospy.Time(0) + (t - t_start)
msg.transforms[0].header.stamp = time_stamp
bag_write.write(topic, msg, time_stamp)
count += 1
bag_write.close()
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 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 decompress(
input_file: Bag,
output_file: Bag,
camera_info: list,
compressed_camera: list,
raw_camera: list,
) -> None:
"""
Decompress a compressed image into a raw image.
Args:
input_file: the input bag to stream from
output_file: the output bag to write data to
camera_info: a list of info topics for each compressed image topic
compressed_camera: a list of compressed image topics
raw_camera: a list of raw image topics to publish
Returns:
None
"""
# get the dims for each compressed camera info topic
dims = [get_camera_dimensions(input_file, info) for info in camera_info]
# convert the list of dims to a dictionary of compressed topics to dims
dims = dict(zip(compressed_camera, dims))
# setup a dictionary mapping compressed topics to raw camera topics
raw_camera = dict(zip(compressed_camera, raw_camera))
# get the number of camera info topics to leave out
camera_info_total = input_file.get_message_count(topic_filters=camera_info)
# get the total number of output messages as total minus camera info topics
total = input_file.get_message_count() - camera_info_total
# create a progress bar for iterating over the messages in the output bag
with tqdm(total=total) as prog:
# iterate over the messages in this input bag
for topic, msg, time in input_file:
# don't write the camera info to the bag
if topic in camera_info:
continue
# if the topic is the compress image topic, decompress the image
if topic in compressed_camera:
# get the image from the compressed topic
img = get_camera_image(msg.data, dims[topic])
# create a raw image message and replace the message
msg = image_msg(img, time, dims[topic], 'rgb8')
# reset the topic
topic = raw_camera[topic]
# update the progress bar with a single iteration
prog.update(1)
# write the message
output_file.write(topic, msg, time)
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 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 clip(
input_file: Bag,
output_file: Bag,
sentinel_topic: list,
start: int,
stop: int,
) -> None:
"""
Clip a bag using a range of messages for a given topic.
Args:
input_file: the input bag to stream from
output_file: the output bag to write data to
sentinel_topic: the topic to watch for starting and stopping the clip
start: the starting message for the clip
stop: the stopping message for the clip
Returns:
None
"""
# initialize a counter for the sentinel topic messages
count = 0
# create a progress bar for iterating over the messages in the bag
with tqdm(total=stop - start, unit='sentinel') as prog:
# iterate over the messages in this input bag
for topic, msg, time in input_file:
# if the topic is the sentinel topic, increase the count
if topic == sentinel_topic:
count += 1
# if the count hasn't reached the starting point, continue
if count < start:
continue
# if the count has exceeded the stopping point, break
if count >= stop:
break
# update the progress bar with a single iteration
if topic == sentinel_topic:
prog.update(1)
# write the message
output_file.write(topic, msg, time)
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']
from rosbag import Bag
from std_msgs.msg import String
bag = Bag("objective.bag", "w")
bag.write("/liability", String("0xaaEc8f06cd803Daf8807C5aB10660b95d385a0dC"))
bag.write("/log_hash",
String("QmRTsJ4cT3Qax8bfAepTEne1rbFiPkgUApemtWCj9FhEgd"))
bag.close()
def video_to_bag(
video_path: str,
output_file: Bag,
topics: set = {'/image_raw', '/image_raw/compressed'},
compression: str = 'png',
base: str = None,
time_zone: int = 6,
frame_skip: int = 1,
) -> None:
"""
Convert a video file to a ROS bag file.
Args:
video_path: the path to the video to convert to a bag
output_file: the bag file to publish the camera topics to
topics: the camera topics to publish
compression: the compression to use as either 'png' or 'jpeg'
base: the base for the camera topics to publish (i.e., '/foo')
time_zone: the hour offset to adjust for UTC time zone
frame_skip: the number of frames to skip between output messages
Returns:
None
"""
# open the video file
video = cv2.VideoCapture(video_path)
# read the start time of the video from the file
start = os.path.getmtime(video_path) + time_zone * 3600
# get the dimensions of frames in the video
height = int(video.get(cv2.CAP_PROP_FRAME_HEIGHT))
width = int(video.get(cv2.CAP_PROP_FRAME_WIDTH))
dims = height, width
# set the base end point to an empty string if it's None
base = '' if base is None else '{}'.format(base)
# get the number of frames in the video
frame_count = int(video.get(cv2.CAP_PROP_frame_count))
# write the camera metadata to the bag if compression is in the topics
if '/image_raw/compressed' in topics:
ros_stamp = rospy.rostime.Time.from_seconds(start)
topic = '{}/camera_info'.format(base)
msg = camera_info_msg(ros_stamp, dims)
output_file.write(topic, msg, ros_stamp)
# write the images to the bag
for idx in tqdm(range(frame_count), unit='frame'):
# get the frame's point in time relative to the origin frame
sec = video.get(cv2.CAP_PROP_POS_MSEC) / 1000.0
# create a timestamp for this frame
ros_stamp = rospy.rostime.Time.from_seconds(start + sec)
# read an image and the flag for a next image being available
_, image = video.read()
# skip the frame if it's not aligned with the skip
if idx % frame_skip != 0:
continue
# write a raw image if the argument is set
if '/image_raw' in topics:
topic = '{}/image_raw'.format(base)
msg = image_msg(image, ros_stamp, dims, ENCODING)
output_file.write(topic, msg, ros_stamp)
# write a compressed image if the argument is set
if '/image_raw/compressed' in topics:
topic = '{}/image_raw/compressed'.format(base)
msg = image_compressed_msg(image, ros_stamp, compression)
output_file.write(topic, msg, ros_stamp)
# release the video file
video.release()
from rosbag import Bag
from std_msgs.msg import String
bag = Bag("objective.bag", "w")
bag.write('/email', String('*****@*****.**'))
bag.write('/pilot_name', String('Vadim Manaenko'))
bag.write('/pilot_reg', String('123456'))
bag.write('/id_serial', String('INTCJ123-4567-890'))
bag.write('/id_reg', String('FA123456789'))
bag.write('/drone_type', String('VTOL'))
bag.write('/drone_make', String('DJI'))
bag.write('/drone_model', String('Matrice 100'))
bag.close()
def semantic_segment(
metadata: str,
input_bag: Bag,
model: 'keras.models.Model',
predict: str,
output_bag: Bag = None,
output_dir: str = None,
base: str = None,
num_samples: int = 200,
encoding: str = 'rgb',
) -> None:
"""
Predict a stream of images from an input ROSbag.
Args:
metadata: the metadata about the semantic segmentations from the model
input_bag: the input bag to predict targets from a topic
model: the semantic segmentation model to use to make predictions
predict: the topic to get a priori estimates from
output_bag: the output bag to write the a priori estimates to
output_dir: the output directory to write image pairs to
base: the base-name for the prediction image topic
num_samples: the number of image pairs to sample for output directory
encoding: the encoding for the images to write
Returns:
None
"""
# create the base endpoint for the topics
base = '' if base is None else '{}'.format(base)
# setup the output directories
if output_dir is not None:
x_dir = os.path.join(output_dir, 'X', 'data')
if not os.path.isdir(x_dir):
os.makedirs(x_dir)
y_dir = os.path.join(output_dir, 'y', 'data')
if not os.path.isdir(y_dir):
os.makedirs(y_dir)
# read the RGB map and vectorized method from the metadata file
rgb_map, unmap_rgb = read_rgb_map(metadata)
# write the color map metadata to the output bag
if output_bag is not None:
ros_stamp = rospy.rostime.Time(input_bag.get_start_time())
msg = String(repr(rgb_map))
output_bag.write('{}/rgb_map'.format(rgb_map), msg, ros_stamp)
# open a Window to play the video
x_window = Window('img', model.input_shape[1], model.input_shape[2])
y_window = Window('sem-seg', model.output_shape[1], model.output_shape[2])
# create a progress bar for iterating over the messages in the bag
total_messages = input_bag.get_message_count(topic_filters=predict)
with tqdm(total=total_messages, unit='message') as prog:
# iterate over the messages in this input bag
for _, msg, time in input_bag.read_messages(topics=predict):
# update the progress bar with a single iteration
prog.update(1)
if np.random.random() > num_samples / total_messages:
continue
# create a tensor from the raw pixel data
pixels = get_camera_image(msg.data,
(msg.height, msg.width))[..., :3]
# flip the BGR image to RGB
if encoding == 'bgr':
pixels = pixels[..., ::-1]
# resize the pixels to the shape of the model
_pixels = resize(
pixels,
model.input_shape[1:],
anti_aliasing=False,
mode='symmetric',
clip=False,
preserve_range=True,
).astype('uint8')
# pass the frame through the model
y_pred = model.predict(_pixels[None, ...])[0]
y_pred = np.stack(unmap_rgb(y_pred.argmax(axis=-1)), axis=-1)
y_pred = y_pred.astype('uint8')
# show the pixels on the windows
x_window.show(_pixels)
y_window.show(y_pred)
# create an Image message and write it to the output ROSbag
if output_bag is not None:
msg = image_msg(y_pred, msg.header.stamp, y_pred.shape[:2],
'rgb8')
output_bag.write('{}/image_raw'.format(base), msg,
msg.header.stamp)
# sample a number and write the image pair to disk
if output_dir is not None:
x_file = os.path.join(x_dir, '{}.png'.format(time))
Image.fromarray(pixels).save(x_file)
y_file = os.path.join(y_dir, '{}.png'.format(time))
y_pred = resize(
y_pred,
pixels.shape[:2],
anti_aliasing=False,
mode='symmetric',
clip=False,
preserve_range=True,
).astype('uint8')
Image.fromarray(y_pred).save(y_file)
#!/usr/bin/env python
from rosbag import Bag
import rospy
from sensor_msgs.msg import (PointCloud2, PointField)
import sensor_msgs.point_cloud2 as point_cloud2
from std_msgs.msg import Header
header = Header(frame_id='/base')
point_fields = [
PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1),
PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1),
PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1),
PointField(name="intensity", offset=12, datatype=PointField.UINT8,
count=1),
]
points = [[0.3, 0.0, 0.0, 0], [0.2, 0.8, 0.9, 120], [1.4, 2.2, 5.1, 90]]
bag = Bag("point_cloud.bag", "w")
point_cloud = point_cloud2.create_cloud(header, point_fields, points)
bag.write("points", point_cloud)
bag.close()
