def acceleration(timestamp, speed):
"""Calculate acceleration from speed stream.
This node calculates the acceleration values from speed values. It is useful in cases where no
acceleration data from a sensor is available.
Args:
timestamp: Message stream with timestamps of distances.
speed: Message stream with speed values.
Yields:
Message stream with acceleration values.
"""
msg_ts, msg_speed = yield marv.pull_all(timestamp, speed)
if msg_ts is None or msg_speed is None:
return
msg_ts, msg_speed = yield marv.pull_all(timestamp, speed)
if msg_ts is None or msg_speed is None:
return
yield marv.set_header(title=timestamp.title)
yield marv.push({'value': 0})
yield marv.push({'value': 0})
pts = msg_ts
psp = msg_speed.value
while (msg_ts :=
(yield marv.pull(timestamp))) and (msg_speed :=
(yield marv.pull(speed))):
def distance_xyz(pos):
"""Calculate distance from Cartesian positions.
Args:
pos: Message stream with Cartesian position messages.
Yields:
Message stream with distances.
"""
msg = yield marv.pull(pos)
if msg is None:
return
yield marv.set_header(title=pos.title)
yield marv.push({'value': 0})
prev = msg
while msg := (yield marv.pull(pos)):
diffe = msg['x'] - prev['x']
diffn = msg['y'] - prev['y']
diffa = msg['z'] - prev['z']
dist = (diffe**2 + diffn**2 + diffa**2)**0.5
yield marv.push({'value': dist})
prev = msg
def orientations(imus, navsatorients):
while True:
tmp = yield marv.pull(imus)
if tmp is None:
break
yield marv.push(tmp)
while True:
tmp = yield marv.pull(navsatorients)
if tmp is None:
break
yield marv.push(tmp)
def collect(chatter):
pytype = get_message_type(chatter)
rosmsg = pytype()
msg = yield marv.pull(chatter)
assert msg is not None
rosmsg.deserialize(msg.data)
yield marv.push(rosmsg.data)
while True:
msg = yield marv.pull(chatter)
if msg is None:
return
rosmsg.deserialize(msg.data)
yield marv.push(rosmsg.data)
def images(cam):
"""Extract images from input stream to jpg files.
Args:
cam: Input stream of raw rosbag messages.
Returns:
File instances for images of input stream.
"""
# Set output stream title and pull first message
yield marv.set_header(title=cam.topic)
# Fetch and process first 20 image messages
name_template = '%s-{}.jpg' % cam.topic.replace('/', ':')[1:]
while True:
idx, msg = yield marv.pull(cam, enumerate=True)
if msg is None or idx >= 20:
break
# Deserialize raw ros message
pytype = get_message_type(cam)
rosmsg = pytype()
rosmsg.deserialize(msg.data)
# Write image to jpeg and push it to output stream
img = imgmsg_to_cv2(rosmsg, 'rgb8')
name = name_template.format(idx)
imgfile = yield marv.make_file(name)
cv2.imwrite(imgfile.path, img)
yield marv.push(imgfile)
def navsatfix(stream):
yield marv.set_header(title=stream.topic)
log = yield marv.get_logger()
deserialize = make_deserialize(stream)
get_timestamp = make_get_timestamp(log, stream)
erroneous = 0
while True:
msg = yield marv.pull(stream)
if msg is None:
break
rosmsg = deserialize(msg.data)
if not hasattr(rosmsg, 'status') or \
np.isnan(rosmsg.longitude) or \
np.isnan(rosmsg.latitude) or \
np.isnan(rosmsg.altitude):
erroneous += 1
continue
yield marv.push({
'status': rosmsg.status.status,
'lon': rosmsg.longitude,
'lat': rosmsg.latitude,
'timestamp': get_timestamp(rosmsg, msg),
})
if erroneous:
log.warning('skipped %d erroneous messages', erroneous)
def nooutput(stream):
yield marv.set_header()
while True:
msg = yield marv.pull(stream)
if msg is None:
return
yield marv.push(msg)
def speed(timestamp, distance):
"""Calculate speed from distance stream.
This node calculates the speed values from distance values. It is useful in cases where no speed
data from a sensor is available.
Args:
timestamp: Message stream with timestamps of distances.
distance: Message stream with distance values.
Yields:
Message stream with speed values.
"""
msg_ts, msg_dist = yield marv.pull_all(timestamp, distance)
if msg_ts is None or msg_dist is None:
return
yield marv.set_header(title=timestamp.title)
yield marv.push({'value': 0})
pts = msg_ts
while (msg_ts :=
(yield marv.pull(timestamp))) and (msg_dist :=
(yield marv.pull(distance))):
def gallery_section(images, title):
"""Create detail section with gallery.
Args:
title (str): Title to be displayed for detail section.
images: stream of marv image files
Returns:
One detail section.
"""
# pull all images
imgs = []
while True:
img = yield marv.pull(images)
if img is None:
break
imgs.append({'src': img.relpath})
if not imgs:
return
# create gallery widget and section containing it
widget = {'title': images.title, 'gallery': {'images': imgs}}
section = {'title': title, 'widgets': [widget]}
yield marv.push(section)
def filesize_plot(filesizes):
# Pull all filesizes
sizes = []
while True:
size = yield marv.pull(filesizes)
if size is None:
break
sizes.append(size)
# plot with plotly
fig = go.Figure(data=go.Scatter(y=sizes))
# save plotly figure to file
plotfile = yield marv.make_file('filesizes_plotly.json')
Path(plotfile.path).write_text(fig.to_json())
# create plotly widget referencing file
widget_plotly = {
'title': 'Filesizes (plotly)',
'plotly': f'marv-partial:{plotfile.relpath}',
}
# plot with matplotlib
fig = plt.figure()
axis = fig.add_subplot(1, 1, 1)
axis.plot(sizes, 'bo')
# save mpld3 figure to file
plotfile = yield marv.make_file('filesizes_mpld3.json')
Path(plotfile.path).write_text(json.dumps(mpld3.fig_to_dict(fig)))
# create mpld3 plot widget referencing file
widget_mpld3 = {
'title': 'Filesizes (mpld3)',
'mpld3': f'marv-partial:{plotfile.relpath}',
}
# save plot as image
plotfile = yield marv.make_file('filesizes.jpg')
fig.savefig(plotfile.path)
# create image widget referencing file
widget_image = {
'title': 'Filesizes (image)',
'image': {
'src': plotfile.relpath
},
}
# Let the user choose which widget to show with a dropdown
yield marv.push({
'title': 'Filesize plots',
'dropdown': {
'widgets': [
widget_plotly,
widget_mpld3,
widget_image,
],
},
})
def filesize_plot_fixed(filesizes):
# set_header() helps marv to schedule nodes
yield marv.set_header()
# Pull all filesizes
sizes = []
while True:
size = yield marv.pull(filesizes)
if size is None:
break
sizes.append(size)
# plot
fig = plt.figure()
axis = fig.add_subplot(1, 1, 1)
axis.plot(sizes, 'bo')
# axis.set_xlabel('foo')
# axis.set_ylabel('bat')
# save figure to file
plotfile = yield marv.make_file('filesizes.json')
with open(plotfile.path, 'w') as f:
json.dump(mpld3.fig_to_dict(fig), f)
# create plot widget referencing file
widget = {
'title': 'Filesizes',
'mpld3': f'marv-partial:{plotfile.relpath}',
}
yield marv.push(widget)
def image(cam):
"""Extract first image of input stream to jpg file.
Args:
cam: Input stream of raw rosbag messages.
Returns:
File instance for first image of input stream.
"""
# Set output stream title and pull first message
yield marv.set_header(title=cam.topic)
msg = yield marv.pull(cam)
if msg is None:
return
# Deserialize raw ros message
pytype = get_message_type(cam)
rosmsg = pytype()
rosmsg.deserialize(msg.data)
# Write image to jpeg and push it to output stream
name = f"{cam.topic.replace('/', ':')[1:]}.jpg"
imgfile = yield marv.make_file(name)
img = imgmsg_to_cv2(rosmsg, 'rgb8')
cv2.imwrite(imgfile.path, img, (cv2.IMWRITE_JPEG_QUALITY, 60))
yield marv.push(imgfile)
def metadata(dataset):
dataset = yield marv.pull(dataset)
dataset_yaml = next((x for x in dataset.files if x.path.endswith('dataset.yaml')), None)
if dataset_yaml is None:
return
dct = yaml.safe_load(Path(dataset_yaml.path).read_text())
yield marv.push(dct)
def rosmsg_imgstream(stream):
"""Convert stream of raw ros messages into stream of deserialized messages."""
deserialize = make_deserialize(stream)
while True:
msg = yield marv.pull(stream)
if msg is None:
break
rosmsg = deserialize(msg.data)
yield marv.push(rosmsg)
def connections_section(bagmeta, dataset, title):
"""Section displaying information about ROS connections."""
dataset, bagmeta = yield marv.pull_all(dataset, bagmeta)
if not bagmeta.topics:
raise marv.Abort()
columns = [
{
'title': 'Topic'
},
{
'title': 'Type'
},
{
'title': 'MD5'
},
{
'title': 'Latching'
},
{
'title': 'Message count',
'align': 'right'
},
]
rows = [{
'id':
idx,
'cells': [
{
'text': con.topic
},
{
'text': con.datatype
},
{
'text': con.md5sum
},
{
'bool': con.latching
},
{
'uint64': con.msg_count
},
]
} for idx, con in enumerate(bagmeta.connections)]
widgets = [{'table': {'columns': columns, 'rows': rows}}]
# TODO: Add text widget explaining what can be seen here: ROS bag
# files store connections. There can be multiple connections for
# one topic with the same or different message types and message
# types with the same name might have different md5s. For
# simplicity connections with the same topic, message type and md5
# are treated as one, within one bag file as well as across bags
# of one set. If one of such connections is latching, the
# aggregated connection will be latching.
yield marv.push({'title': title, 'widgets': widgets})
def objects(dataset):
dataset = yield marv.pull(dataset)
tracklet_file = next((x for x in dataset.files if x.path.endswith('tracklet_labels.xml')), None)
if tracklet_file is None:
return
fixme
root = ET.parse(tracklet_file.path).getroot()
cars = root.findall(".//*[objectType='Car']")
yield marv.push({'cars': len(cars)})
def summary_keyval(dataset): # pylint: disable=redefined-outer-name
dataset = yield marv.pull(dataset)
if len(dataset.files) < 2:
return
yield marv.push({'keyval': {
'items': [
{'title': 'size', 'formatter': 'filesize', 'list': False,
'cell': {'uint64': sum(x.size for x in dataset.files)}},
{'title': 'files', 'list': False,
'cell': {'uint64': len(dataset.files)}},
],
}})
def bbox(stream):
"""Create videofile from input sream."""
func, catmap = make_detect_function(
'/opt/marv/centernet_hg104_512x512_coco17_tpu-8',
'/opt/marv/mscoco_label_map.pbtxt')
while True:
img = yield marv.pull(stream)
if img is None:
break
detections = detect(img, func)
vizimg = visualize(img, detections, catmap)
yield marv.push(vizimg)
def galleries(stream):
"""Galleries for all images streams.
Used by marv_robotics.detail.images_section.
"""
yield marv.set_header(title=stream.title)
images = []
while True:
img = yield marv.pull(stream)
if img is None:
break
images.append({'src': img.relpath})
yield marv.push({'title': stream.title, 'gallery': {'images': images}})
def images_section(galleries, title):
"""Section with galleries of images for each images stream."""
tmp = []
while True:
msg = yield marv.pull(galleries)
if msg is None:
break
tmp.append(msg)
galleries = tmp
galleries = sorted(galleries, key=lambda x: x.title)
widgets = yield marv.pull_all(*galleries)
if widgets:
yield marv.push({'title': title, 'widgets': widgets})
def summary_keyval(dataset, bagmeta):
"""Keyval widget summarizing bag metadata.
Useful for detail_summary_widgets.
"""
dataset, bagmeta = yield marv.pull_all(dataset, bagmeta)
yield marv.push({
'keyval': {
'items': [
{
'title': 'size',
'formatter': 'filesize',
'list': False,
'cell': {
'uint64': sum(x.size for x in dataset.files)
}
},
{
'title': 'files',
'list': False,
'cell': {
'uint64': len(dataset.files)
}
},
{
'title': 'start time',
'formatter': 'datetime',
'list': False,
'cell': {
'timestamp': bagmeta.start_time
}
},
{
'title': 'end time',
'formatter': 'datetime',
'list': False,
'cell': {
'timestamp': bagmeta.end_time
}
},
{
'title': 'duration',
'formatter': 'timedelta',
'list': False,
'cell': {
'timedelta': bagmeta.duration
}
},
],
}
})
def fulltext_per_topic(stream):
yield marv.set_header(title=stream.topic)
words = set()
deserialize = make_deserialize(stream)
while True:
msg = yield marv.pull(stream)
if msg is None:
break
rosmsg = deserialize(msg.data)
words.update(WSNULL.split(rosmsg.data))
if not words:
raise marv.Abort()
yield marv.push({'words': list(words)})
def distance_gps(pos):
"""Calculate distance from GPS positions.
Args:
pos: Message stream with GPS position messages.
Yields:
Message stream with distances.
"""
msg = yield marv.pull(pos)
if msg is None:
return
yield marv.set_header(title=pos.title)
yield marv.push({'value': 0})
lat_p = radians(msg['lat'])
lon_p = radians(msg['lon'])
alt_p = msg['alt']
while msg := (yield marv.pull(pos)):
lat = radians(msg['lat'])
lon = radians(msg['lon'])
alt = msg['alt']
lat_d = lat_p - lat
lon_d = lon_p - lon
alt_d = alt_p - alt
dis = sin(lat_d * 0.5)**2 + cos(lat) * cos(lat_p) * sin(lon_d * 0.5)**2
dis = 2 * 6371008.8 * asin(sqrt(dis))
if not math.isnan(alt_d):
dis = sqrt(
dis**2 +
alt_d**2) # euclidean approx taking altitude into account
yield marv.push({'value': dis})
lat_p = lat
lon_p = lon
alt_p = alt
def fulltext(streams):
"""Extract all text from bag file and store for fulltext search."""
tmp = []
while True:
stream = yield marv.pull(streams)
if stream is None:
break
tmp.append(stream)
streams = tmp
if not streams:
raise marv.Abort()
msgs = yield marv.pull_all(*streams)
words = {x for msg in msgs for x in msg.words}
yield marv.push({'words': sorted(words)})
def filesizes(images):
"""Stat filesize of files.
Args:
images: stream of marv image files
Returns:
Stream of filesizes
"""
# Pull each image and push its filesize
while True:
img = yield marv.pull(images)
if img is None:
break
yield marv.push(img.size)
def video_section(src, title):
"""Section displaying a video player."""
videofile = yield marv.pull(src)
if videofile is None:
return
yield marv.push({
'title':
title,
'widgets': [{
'title': 'Camera',
'video': {
'src': videofile.relpath
},
}],
})
def imgsrc(stream):
"""Convert ROS sensor_msgs/Image stream into cv2 image stream."""
while True:
rosmsg = yield marv.pull(stream)
if rosmsg is None:
break
try:
img = imgmsg_to_cv2(rosmsg, 'bgr8')
except (ImageFormatError, ImageConversionError) as err:
log = yield marv.get_logger()
log.error('could not convert image from topic %s: %s ',
stream.topic, err)
raise marv.Abort()
yield marv.push(img)
def positions(stream):
yield marv.set_header(title=stream.topic)
log = yield marv.get_logger()
deserialize = make_deserialize(stream)
get_timestamp = make_get_timestamp(log, stream)
erroneous = 0
e_offset = None
n_offset = None
u_offset = None
positions = []
while True:
msg = yield marv.pull(stream)
if msg is None:
break
rosmsg = deserialize(msg.data)
if not hasattr(rosmsg, 'status') or \
np.isnan(rosmsg.longitude) or \
np.isnan(rosmsg.latitude) or \
np.isnan(rosmsg.altitude) or \
np.isnan(rosmsg.position_covariance[0]):
erroneous += 1
continue
e, n, _, _ = utm.from_latlon(rosmsg.latitude, rosmsg.longitude)
if e_offset is None:
e_offset = e
n_offset = n
u_offset = rosmsg.altitude
e = e - e_offset
n = n - n_offset
u = rosmsg.altitude - u_offset
positions.append([get_timestamp(rosmsg, msg) / 1e9,
rosmsg.latitude,
rosmsg.longitude,
rosmsg.altitude,
e, n, u,
rosmsg.status.status,
np.sqrt(rosmsg.position_covariance[0])])
if erroneous:
log = yield marv.get_logger()
log.warning('skipped %d erroneous messages', erroneous)
if positions:
yield marv.push({'values': positions})
def trajectory(navsatfixes):
# Only one topic for now
navsatfix = yield marv.pull(navsatfixes) # pylint: disable=redefined-outer-name
if not navsatfix:
raise marv.Abort()
yield marv.set_header(title=navsatfix.title)
features = []
quality = None
coords = []
timestamps = []
while True:
msg = yield marv.pull(navsatfix)
if msg is None:
break
timestamps.append(msg['timestamp'])
# Whether to output an augmented fix is determined by both the fix
# type and the last time differential corrections were received. A
# fix is valid when status >= STATUS_FIX.
# STATUS_NO_FIX = -1 -> unable to fix position -> color id 0 = red
# STATUS_FIX = 0 -> unaugmented fix -> color id 1 = orange
# STATUS_SBAS_FIX = 1 -> satellite-based augmentation -> color id 2 = blue
# STATUS_GBAS_FIX = 2 -> ground-based augmentation -> color id 3 = green
# -> unknown status id -> color id 4 = black
if -1 <= msg['status'] <= 2:
new_quality = msg['status'] + 1
else:
new_quality = 4
# start new feature if quality changed
if quality != new_quality:
if coords:
features.append(_create_feature(coords, quality, timestamps))
quality = new_quality
coords = []
timestamps = []
coords.append((msg['lon'], msg['lat']))
if coords:
features.append(_create_feature(coords, quality, timestamps))
if features:
out = {'feature_collection': {'features': features}}
yield marv.push(out)
def video_sink(stream):
"""Create videofile from input sream."""
framerate = 10
encoder = None
while True:
img = yield marv.pull(stream)
if img is None:
break
if not encoder:
video = yield marv.make_file('camera.webm')
height, width = img.shape[0:2]
ffargs = [
'ffmpeg',
'-f',
'rawvideo',
'-pixel_format',
'bgr24',
'-video_size',
f'{width}x{height}',
'-framerate',
f'{framerate}',
'-i',
'-',
'-c:v',
'libvpx-vp9',
'-pix_fmt',
'yuv420p',
'-loglevel',
'error',
'-threads',
'8',
'-speed',
'4',
'-y',
video.path,
]
encoder = popen(ffargs, stdin=PIPE)
encoder.stdin.write(img)
if encoder:
encoder.stdin.close()
encoder.wait()
yield marv.push(video)
