# Copyright 2016 - 2018 Ternaris.
# SPDX-License-Identifier: AGPL-3.0-only
from __future__ import absolute_import, division, print_function
import marv
import marv_node.testing
from marv_node.testing import make_dataset, make_sink, run_nodes, temporary_directory
from marv_store import Store
from pkg_resources import resource_filename
from marv_robotics.bag import messages
@marv.node()
@marv.input('stream', default=marv.select(messages, '/non-existent'))
def nooutput(stream):
yield marv.set_header()
while True:
msg = yield marv.pull(stream)
if msg is None:
return
yield marv.push(msg)
@marv.node()
@marv.input('nooutput', default=nooutput)
@marv.input('chatter', default=marv.select(messages, '/chatter'))
def collect(nooutput, chatter):
msg = yield marv.pull(nooutput)
assert msg is None
# -*- coding: utf-8 -*-
#
# Copyright 2016 - 2018 Ternaris.
# SPDX-License-Identifier: AGPL-3.0-only
from __future__ import absolute_import, division, print_function
import marv
from marv.types import Words
from .bag import get_message_type, messages
@marv.node(Words)
@marv.input('stream', foreach=marv.select(messages, '*:std_msgs/String'))
def fulltext_per_topic(stream):
yield marv.set_header() # TODO: workaround
words = set()
pytype = get_message_type(stream)
rosmsg = pytype()
while True:
msg = yield marv.pull(stream)
if msg is None:
break
rosmsg.deserialize(msg.data)
words.update(rosmsg.data.split())
if not words:
raise marv.Abort()
yield marv.push({'words': list(words)})
@marv.node(Words)
# Work around cv_bridge bug
if msg.encoding == '8UC1':
msg.encoding = 'mono8'
elif msg.encoding == '16UC1':
msg.encoding = 'mono16'
if msg.encoding[2:4] == 'FC':
passimg = numpy.nan_to_num(imgmsg_to_cv2(msg, 'passthrough'))
valscaled = cv2.convertScaleAbs(passimg, None, scale, offset)
return (mono, valscaled)
return (mono, imgmsg_to_cv2(msg, 'mono8' if mono else 'bgr8'))
@marv.node(File)
@marv.input('stream', foreach=marv.select(messages, '*:sensor_msgs/Image'))
@marv.input('speed', default=4)
@marv.input('convert_32FC1_scale', default=1)
@marv.input('convert_32FC1_offset', default=0)
def ffmpeg(stream, speed, convert_32FC1_scale, convert_32FC1_offset):
"""Create video for each sensor_msgs/Image topic with ffmpeg"""
yield marv.set_header(title=stream.topic)
name = '{}.webm'.format(stream.topic.replace('/', '_')[1:])
video = yield marv.make_file(name)
duration = (stream.end_time - stream.start_time) * 1e-9
framerate = stream.msg_count / duration
pytype = get_message_type(stream)
rosmsg = pytype()
encoder = None
# -*- coding: utf-8 -*-
#
# Copyright 2016 - 2018 Ternaris.
# SPDX-License-Identifier: AGPL-3.0-only
from __future__ import absolute_import, division, print_function
import numpy as np
import marv
from marv.types import File, GeoJson
from .bag import get_message_type, messages
@marv.node()
@marv.input('stream', foreach=marv.select(messages, '*:sensor_msgs/NavSatFix'))
def navsatfix(stream):
yield marv.set_header(title=stream.topic)
pytype = get_message_type(stream)
rosmsg = pytype()
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
rot[0, 0] = 1 - 2 * q2 * q2 - 2 * q3 * q3
rot[0, 1] = 2 * (q1 * q2 - q3 * q4)
rot[0, 2] = 2 * (q1 * q3 + q2 * q4)
rot[1, 0] = 2 * (q1 * q2 + q3 * q4)
rot[1, 1] = 1 - 2 * q1 * q1 - 2 * q3 * q3
rot[1, 2] = 2 * (q2 * q3 - q1 * q4)
rot[2, 0] = 2 * (q1 * q3 - q2 * q4)
rot[2, 1] = 2 * (q1 * q4 + q2 * q3)
rot[2, 2] = 1 - 2 * q1 * q1 - 2 * q2 * q2
vec = np.dot(rot, [1, 0, 0])
return np.arctan2(vec[1], vec[0])
@marv.node()
@marv.input('stream', foreach=marv.select(messages, '*:sensor_msgs/NavSatFix'))
def positions(stream):
yield marv.set_header(title=stream.topic)
pytype = get_message_type(stream)
rosmsg = pytype()
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 \
# SPDX-License-Identifier: AGPL-3.0-only
from __future__ import absolute_import, division, print_function
import marv
import marv_node.testing
from marv_node.testing import make_dataset, make_sink, run_nodes, temporary_directory
from marv_store import Store
from pkg_resources import resource_filename
from marv_robotics.bag import messages
from marv_robotics.fulltext import fulltext
@marv.node()
@marv.input('chatter', default=marv.select(messages, '/chatter'))
def collect(chatter):
msg = yield marv.pull(chatter)
assert msg is not None
yield marv.push(msg.data)
while True:
msg = yield marv.pull(chatter)
if msg is None:
return
yield marv.push(msg.data)
class TestCase(marv_node.testing.TestCase):
# TODO: Generate bags instead, but with connection info!
BAGS = [
resource_filename('marv_robotics.tests', 'data/test_0.bag'),
matplotlib.use('Agg')
import matplotlib.pyplot as plt
import mpld3
imgmsg_to_cv2 = cv_bridge.CvBridge().imgmsg_to_cv2
import marv
from marv_nodes.types_capnp import File
from marv_detail.types_capnp import Section, Widget
from marv_robotics.bag import get_message_type, raw_messages
TOPIC = '/wide_stereo/left/image_rect_throttle'
@marv.node(File)
@marv.input('cam', marv.select(raw_messages, TOPIC))
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
