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 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 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 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})