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 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 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 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 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 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 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 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 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 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 images(stream, image_width, max_frames, convert_32FC1_scale,
convert_32FC1_offset):
"""Extract max_frames equidistantly spread images from each sensor_msgs/Image stream.
Args:
stream: sensor_msgs/Image stream
image_width (int): Scale to image_width, keeping aspect ratio.
max_frames (int): Maximum number of frames to extract.
convert_32FC1_scale (float): Scale factor for FC image values.
convert_32FC1_offset (float): Offset for FC image values.
"""
# pylint: disable=invalid-name,too-many-locals
yield marv.set_header(title=stream.topic)
deserialize = make_deserialize(stream)
interval = int(math.ceil(stream.msg_count / max_frames))
digits = int(math.ceil(math.log(stream.msg_count) / math.log(10)))
name_template = '%s-{:0%sd}.jpg' % (stream.topic.replace('/',
':')[1:], digits)
counter = count()
while True:
msg = yield marv.pull(stream)
if msg is None:
break
idx = next(counter)
if idx % interval:
continue
rosmsg = deserialize(msg.data)
try:
img = ros2cv(rosmsg, convert_32FC1_scale, convert_32FC1_offset)
except (ImageFormatError, ImageConversionError) as err:
log = yield marv.get_logger()
log.error('could not convert image from topic %s: %s ',
stream.topic, err)
return
height = int(round(image_width * img.shape[0] / img.shape[1]))
scaled_img = cv2.resize(img, (image_width, height),
interpolation=cv2.INTER_AREA)
name = name_template.format(idx)
imgfile = yield marv.make_file(name)
cv2.imwrite(imgfile.path, scaled_img, (cv2.IMWRITE_JPEG_QUALITY, 60))
yield imgfile
def position_xyz(stream):
"""Extract position from Pose.
Args:
stream: ROS message stream with Pose messages.
Yields:
Message stream with Cartesian positions.
"""
stream = yield marv.pull(stream)
if not stream:
return
yield marv.set_header(title=stream.topic)
deserialize = make_deserialize(stream)
while msg := (yield marv.pull(stream)):
rosmsg = deserialize(msg.data)
pos = rosmsg.pose.position
yield marv.push({'x': pos.x, 'y': pos.y, 'z': pos.z})
def motion_timestamp(stream):
"""Extract timestamps for motion events.
Args:
stream: ROS message stream with Pose or GPS messages.
Yields:
Message stream with timestamps.
"""
log = yield marv.get_logger()
stream = yield marv.pull(stream) # take first matching connection
if not stream:
return
yield marv.set_header(title=stream.topic)
deserialize = make_deserialize(stream)
get_timestamp = make_get_timestamp(log, stream)
while msg := (yield marv.pull(stream)):
rosmsg = deserialize(msg.data)
yield marv.push(get_timestamp(rosmsg, msg))
def imus(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
imus = []
while True:
msg = yield marv.pull(stream)
if msg is None:
break
rosmsg = deserialize(msg.data)
if np.isnan(rosmsg.orientation.x):
erroneous += 1
continue
imus.append([get_timestamp(rosmsg, msg) / 1e9,
yaw_angle(rosmsg.orientation)])
if erroneous:
log = yield marv.get_logger()
log.warning('skipped %d erroneous messages', erroneous)
yield marv.push({'values': imus})
def position_gps(stream):
"""Extract position from GPS.
Args:
stream: ROS message stream with GPS messages.
Yields:
Message stream with GPS positions.
"""
stream = yield marv.pull(stream)
if not stream:
return
yield marv.set_header(title=stream.topic)
deserialize = make_deserialize(stream)
while msg := (yield marv.pull(stream)):
rosmsg = deserialize(msg.data)
yield marv.push({
'lat': rosmsg.latitude,
'lon': rosmsg.longitude,
'alt': rosmsg.altitude
})
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 easting_northing(stream):
"""Extract easting and northing.
Args:
stream: ROS message stream with Pose or GPS messages.
Yields:
Message stream with easting and northing.
"""
stream = yield marv.pull(stream) # take first matching connection
if not stream:
return
yield marv.set_header(title=stream.topic)
deserialize = make_deserialize(stream)
while msg := (yield marv.pull(stream)):
rosmsg = deserialize(msg.data)
if stream.msg_type == 'sensor_msgs/NavSatFix':
easting, northing, _, _ = utm.from_latlon(rosmsg.latitude,
rosmsg.longitude)
else:
easting, northing = rosmsg.pose.position.x, rosmsg.pose.position.y
yield marv.push({'e': easting, 'n': northing})
def gnss_plots(gps, orientation):
# pylint: disable=too-many-locals,too-many-statements
# TODO: framework does not yet support multiple foreach
# pick only first combination for now
log = yield marv.get_logger()
gps, orientation = yield marv.pull_all(gps, orientation)
if gps is None:
log.error('No gps messages')
raise marv.Abort()
gtitle = gps.title
gps = yield marv.pull(gps) # There is only one message
# Check whether there are any valid messages left
if gps is None:
log.error('No valid gps messages')
raise marv.Abort()
gps = gps['values']
if orientation is not None:
otitle = orientation.title
orientation = yield marv.pull(orientation)
if orientation is None:
log.warning('No orientations found')
otitle = 'none'
orientation = []
else:
orientation = orientation['values']
name = '__'.join(x.replace('/', ':')[1:] for x in [gtitle, otitle]) + '.jpg'
title = f'{gtitle} with {otitle}'
yield marv.set_header(title=title)
plotfile = yield marv.make_file(name)
fig = plt.figure()
fig.subplots_adjust(wspace=0.3)
ax1 = fig.add_subplot(1, 3, 1) # e-n plot
ax2 = fig.add_subplot(2, 3, 2) # orientation plot
ax3 = fig.add_subplot(2, 3, 3) # e-time plot
ax4 = fig.add_subplot(2, 3, 5) # up plot
ax5 = fig.add_subplot(2, 3, 6) # n-time plot
# masking for finite values
gps = np.array(gps)
gps = gps[np.isfinite(gps[:, 1])]
# precompute plot vars
c = cm.prism(gps[:, 7]/2) # pylint: disable=no-member
ax1.scatter(gps[:, 4], gps[:, 5], c=c, edgecolor='none', s=3,
label='green: RTK\nyellow: DGPS\nred: Single')
xfmt = md.DateFormatter('%H:%M:%S')
ax3.xaxis.set_major_formatter(xfmt)
ax4.xaxis.set_major_formatter(xfmt)
ax5.xaxis.set_major_formatter(xfmt)
if orientation:
ax2.xaxis.set_major_formatter(xfmt)
orientation = np.array(orientation)
ax2.plot([datetime.fromtimestamp(x) for x in orientation[:, 0]], # noqa: DTZ
orientation[:, 1])
ax3.plot([datetime.fromtimestamp(x) for x in gps[:, 0]], gps[:, 4]) # noqa: DTZ
ax4.plot([datetime.fromtimestamp(x) for x in gps[:, 0]], gps[:, 6]) # noqa: DTZ
ax5.plot([datetime.fromtimestamp(x) for x in gps[:, 0]], gps[:, 5]) # noqa: DTZ
fig.autofmt_xdate()
ax1.legend(loc='upper right', title='')
ax1.set_ylabel('GNSS northing [m]')
ax1.set_xlabel('GNSS easting [m]')
ax2.set_ylabel('Heading over time [rad]')
ax3.set_ylabel('GNSS easting over time [m]')
ax4.set_ylabel('GNSS height over time [m]')
ax5.set_ylabel('GNSS northing over time [m]')
fig.set_size_inches(16, 9)
try:
fig.savefig(plotfile.path)
finally:
plt.close()
yield plotfile
def filter_pos(timestamp, pos, pvar, qvar, rvar, keys): # pylint: disable=too-many-arguments,too-many-locals
"""Kalman filter input stream using simple linear motion model.
Args:
timestamp: Message stream with timestamps corresponding to pos.
pos: Message stream with positional data.
pvar: Model uncertainty.
qvar: Process uncertainty.
rvar: Measurement uncertainty.
keys: Keynames of positional data.
Yields:
Message stream with filtered positions.
"""
# pylint: disable=invalid-name
msg_ts, msg_pos = yield marv.pull_all(timestamp, pos)
if msg_ts is None or msg_pos is None:
return
yield marv.set_header(title=timestamp.title)
yield marv.push(msg_pos)
F = numpy.eye(6)
x = numpy.array([
msg_pos[keys[0]],
0.,
msg_pos[keys[1]],
0.,
msg_pos[keys[2]],
0.,
])
P = numpy.eye(6) * pvar
Q = numpy.eye(6)
R = numpy.eye(3) * rvar
H = numpy.array([
[1, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0],
])
last_ts = msg_ts
while True:
msg_ts, msg_pos = yield marv.pull_all(timestamp, pos)
if msg_ts is None or msg_pos is None:
return
dt = (msg_ts - last_ts) / 1e9
last_ts = msg_ts
F[0, 1] = dt
F[2, 3] = dt
F[4, 5] = dt
G = numpy.array([[.5 * dt**2, dt]]).T
subQ = G.dot(G.T) * qvar
Q[0:2, 0:2] = subQ
Q[2:4, 2:4] = subQ
Q[4:6, 4:6] = subQ
x = F.dot(x)
P = F.dot(P).dot(F.T) + Q
z = numpy.array([msg_pos[x] for x in keys])
K = P.dot(H.T).dot(numpy.linalg.inv(H.dot(P).dot(H.T) + R))
x = x + K.dot(z - H.dot(x))
P = P - K.dot(H).dot(P)
res = H.dot(x)
yield marv.push({x: float(res[i]) for i, x in enumerate(keys)})
def ffmpeg(stream, speed, convert_32FC1_scale, convert_32FC1_offset): # pylint: disable=invalid-name
"""Create video for each sensor_msgs/Image topic with ffmpeg."""
# pylint: disable=too-many-locals
yield marv.set_header(title=stream.topic)
name = f"{stream.topic.replace('/', '_')[1:]}.webm"
video = yield marv.make_file(name)
duration = (stream.end_time - stream.start_time) * 1e-9
framerate = (stream.msg_count / duration) if duration else 1
deserialize = make_deserialize(stream)
with ExitStack() as stack:
encoder = None
dims = None
while True:
msg = yield marv.pull(stream)
if msg is None:
break
rosmsg = deserialize(msg.data)
try:
img = ros2cv(rosmsg, convert_32FC1_scale, convert_32FC1_offset)
except (ImageFormatError, ImageConversionError) as err:
log = yield marv.get_logger()
log.error('could not convert image from topic %s: %s ',
stream.topic, err)
return
if not encoder:
dims = img.shape[:2]
ffargs = [
'ffmpeg',
'-f',
'rawvideo',
'-pixel_format',
'gray' if len(img.shape) == 2 else 'bgr24',
'-video_size',
f'{img.shape[1]}x{img.shape[0]}',
'-framerate',
str(framerate),
'-i',
'-',
'-c:v',
'libvpx-vp9',
'-keyint_min',
'25',
'-g',
'25',
'-tile-columns',
'4',
'-frame-parallel',
'1',
'-an',
'-f',
'webm',
'-dash',
'1',
'-pix_fmt',
'yuv420p',
'-loglevel',
'error',
'-threads',
'8',
'-speed',
str(speed),
'-y',
video.path,
]
encoder = stack.enter_context(popen(ffargs, stdin=PIPE))
if dims != img.shape[:2]:
log = yield marv.get_logger()
log.warning(
f'could not encode, image size changed {dims} != {img.shape[:2]}'
)
return
encoder.stdin.write(img)
yield video
def motion_section(timestamp, easting_northing, distance, speed, acceleration): # pylint: disable=too-many-arguments,too-many-locals
"""Create motion section.
Args:
timestamp: Message stream of timestamps.
easting_northing: Message stream of easting/northing coordinates.
distance: Message stream of distances.
speed: Message stream of speeds.
acceleration: Message stream of accelerations.
Yields:
Motion section for frontend.
"""
yield marv.set_header()
traces = {
name: empty_trace(name, 'scatter')
for name in ['en', 'distance', 'speed', 'acceleration']
}
plots = {
name: empty_plotly_widget(trace, name)
for name, trace in traces.items()
}
# customize individual plots
del plots['en']['layout']['xaxis']['type']
plots['en']['layout']['xaxis']['title'] = 'filtered easting (m)'
plots['en']['layout']['yaxis']['title'] = 'filtered northing (m)'
plots['en']['layout']['yaxis']['scaleanchor'] = 'x'
plots['en']['layout']['yaxis']['scaleratio'] = 1
plots['distance']['layout']['yaxis']['title'] = 'distance driven (m)'
plots['speed']['layout']['yaxis']['title'] = 'speed (m/s)'
del plots['acceleration']['layout']['margin']
plots['acceleration']['layout']['yaxis']['title'] = 'acceleration (m/s²)'
firste = None
firstn = None
distsum = 0
while True:
msg_ts, msg_en, msg_distance, msg_speed, msg_acceleration = yield marv.pull_all(
timestamp, easting_northing, distance, speed, acceleration)
if msg_ts is None or msg_en is None or msg_distance is None or msg_speed is None or \
msg_acceleration is None:
break
if firste is None:
firste = msg_en['e']
firstn = msg_en['n']
rele = 0
reln = 0
else:
rele = msg_en['e'] - firste
reln = msg_en['n'] - firstn
traces['en']['x'].append(rele)
traces['en']['y'].append(reln)
tsval = int(msg_ts / 1e6)
traces['distance']['x'].append(tsval)
traces['speed']['x'].append(tsval)
traces['acceleration']['x'].append(tsval)
distsum += msg_distance.value
traces['distance']['y'].append(distsum)
traces['speed']['y'].append(msg_speed.value)
traces['acceleration']['y'].append(msg_acceleration.value)
if traces['distance']['x']:
file_en = yield marv.make_file('easting_northing.json')
Path(file_en.path).write_text(json.dumps(plots['en']))
file_dist = yield marv.make_file('distance.json')
Path(file_dist.path).write_text(json.dumps(plots['distance']))
file_speed = yield marv.make_file('speed.json')
Path(file_speed.path).write_text(json.dumps(plots['speed']))
file_accel = yield marv.make_file('acceleration.json')
Path(file_accel.path).write_text(json.dumps(plots['acceleration']))
yield marv.push({
'title':
'Motion plots',
'widgets': [
{
'title': '',
'plotly': f'marv-partial:{file_en.relpath}'
},
{
'title': '',
'plotly': f'marv-partial:{file_dist.relpath}'
},
{
'title': '',
'plotly': f'marv-partial:{file_speed.relpath}'
},
{
'title': '',
'plotly': f'marv-partial:{file_accel.relpath}'
},
]
})
