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 trajectory(navsatfixes): navsatfix = yield marv.pull(navsatfixes) # Only one topic for now if not navsatfix: raise marv.Abort() yield marv.set_header(title=navsatfix.title) features = [] prev_quality = None timestamps = [] while True: msg = yield marv.pull(navsatfix) if msg is None: break dt = msg['timestamp'] timestamps.append(int(dt * 1e9)) # 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: quality = msg['status'] + 1 else: quality = 4 if quality != prev_quality: color = ( (1., 0., 0., 1.), # rgba (1., 0.65, 0., 1.), (0., 0., 1., 1.), (0., 1., 0., 1.))[quality] coords = [] feat = { 'properties': { 'color': color, 'width': 4., 'timestamps': timestamps, 'markervertices': [c * 30 for c in (0., 0., -1., .3, -1., -.3)] }, 'geometry': { 'line_string': { 'coordinates': coords } } } features.append(feat) prev_quality = quality coords.append((msg['lon'], msg['lat'])) if features: out = {'feature_collection': {'features': features}} yield marv.push(out)
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)})
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': list(words)})
def trajectory(navsatfixes): navsatfix = yield marv.pull(navsatfixes) # Only one topic for now 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 dt = msg['timestamp'] timestamps.append(int(dt * 1e9)) # 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_section(videos, title): """Section displaying one video player per image stream.""" tmps = [] while True: tmp = yield marv.pull(videos) if tmp is None: break tmps.append(tmp) videos = sorted(tmps, key=lambda x: x.title) if not videos: raise marv.Abort() videofiles = yield marv.pull_all(*videos) widgets = [{'title': video.title, 'video': {'src': videofile.relpath}} for video, videofile in zip(videos, videofiles)] assert len(set(x['title'] for x in widgets)) == len(widgets) if widgets: yield marv.push({'title': title, 'widgets': widgets})
def trajectory_section(geojson, title, minzoom, maxzoom, tile_server_protocol): """Section displaying trajectory on a map. Args: tile_server_protocol (str): Set to ``https:`` if you host marv behind http and prefer the tile requests to be secured. """ geojson = yield marv.pull(geojson) if not geojson: raise marv.Abort() layers = [ {'title': 'Background', 'tiles': [ {'title': 'Roadmap', 'url': '%s//[abc].osm.ternaris.com/styles/osm-bright/rendered/{z}/{x}/{y}.png' % tile_server_protocol, 'attribution': '© <a href="http://openstreetmap.org/copyright">OpenStreetMap</a> contributors', 'retina': 3, 'zoom': {'min': 0, 'max': 20}}, {'title': 'Satellite', 'url': '%s//server.arcgisonline.com/ArcGIS/rest/services/World_Imagery/MapServer/tile/{z}/{y}/{x}.png' % tile_server_protocol, 'attribution': 'Sources: Esri, DigitalGlobe, GeoEye, Earthstar Geographics, CNES/Airbus DS, USDA, USGS, AeroGRID, IGN, and the GIS User Community', 'zoom': {'min': 0, 'max': 18}}, ]}, {'title': 'Trajectory', 'color': (0., 1., 0., 1.), 'geojson': geojson}, ] dct = make_map_dict({ 'layers': layers, 'zoom': {'min': minzoom, 'max': maxzoom}, }) jsonfile = yield marv.make_file('data.json') with open(jsonfile.path, 'w') as f: json.dump(dct, f, sort_keys=True) yield marv.push({'title': title, 'widgets': [{'map_partial': 'marv-partial:{}'.format(jsonfile.relpath)}]})
def gnss_plots(gps, orientation): # 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.warn('No orientations found') otitle = 'none' orientation = [] else: orientation = orientation['values'] name = '__'.join(x.replace('/', ':')[1:] for x in [gtitle, otitle]) + '.jpg' title = '{} with {}'.format(gtitle, 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) 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]], orientation[:, 1]) ax3.plot([datetime.fromtimestamp(x) for x in gps[:, 0]], gps[:, 4]) ax4.plot([datetime.fromtimestamp(x) for x in gps[:, 0]], gps[:, 6]) ax5.plot([datetime.fromtimestamp(x) for x in gps[:, 0]], gps[:, 5]) 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) except: plt.close() yield plotfile