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}' }, ] })