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 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': 'marv-partial:{}'.format(plotfile.relpath), } yield marv.push(widget)
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 = '{}.jpg'.format(cam.topic.replace('/', ':')[1:]) 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) 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 continue # TODO: namedtuple? out = { 'status': rosmsg.status.status, 'lon': rosmsg.longitude, 'lat': rosmsg.latitude, 'timestamp': rosmsg.header.stamp.to_time() } yield marv.push(out) if erroneous: log = yield marv.get_logger() log.warn('skipped %d erroneous messages', erroneous)
def nooutput(stream): yield marv.set_header() while True: msg = yield marv.pull(stream) if msg is None: return yield marv.push(msg)
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 while True: msg = yield marv.pull(stream) if msg is None: break rosmsg.deserialize(msg.data) try: mono, img = ros2cv(rosmsg, convert_32FC1_scale, convert_32FC1_offset) except cv_bridge.CvBridgeError as e: log = yield marv.get_logger() log.error('could not convert image %s', e) return if not encoder: ffargs = [ 'ffmpeg', '-f', 'rawvideo', '-pixel_format', '%s' % 'gray' if mono else 'bgr24', '-video_size', '%dx%d' % (rosmsg.width, rosmsg.height), '-framerate', '%s' % framerate, '-i', '-', '-c:v', 'libvpx-vp9', '-pix_fmt', 'yuv420p', '-loglevel', 'error', '-threads', '8', '-speed', str(speed), '-y', video.path, ] encoder = subprocess.Popen(ffargs, stdin=subprocess.PIPE) encoder.stdin.write(img) encoder.stdin.close() encoder.wait() yield video
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 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 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 while True: msg = yield marv.pull(stream) if msg is None: break rosmsg.deserialize(msg.data) if not encoder: ffargs = [ 'ffmpeg', '-f', 'rawvideo', '-pixel_format', '%s' % {'mono8': 'gray', '32FC1': 'gray', '8UC1': 'gray'}.get(rosmsg.encoding, 'rgb24'), '-video_size', '%dx%d' % (rosmsg.width, rosmsg.height), '-framerate', '%s' % framerate, '-i', '-', '-c:v', 'libvpx-vp9', '-pix_fmt', 'yuv420p', '-loglevel', 'error', '-threads', '8', '-speed', str(speed), '-y', video.path, ] encoder = subprocess.Popen(ffargs, stdin=subprocess.PIPE) if rosmsg.encoding == 'mono8': data = rosmsg.data elif rosmsg.encoding == '32FC1': data = cv2.convertScaleAbs(numpy.nan_to_num(imgmsg_to_cv2(rosmsg, 'passthrough')), None, convert_32FC1_scale, convert_32FC1_offset) elif rosmsg.encoding == '8UC1': data = imgmsg_to_cv2(rosmsg).tobytes() else: data = imgmsg_to_cv2(rosmsg, 'rgb8').tobytes() encoder.stdin.write(data) encoder.stdin.close() encoder.wait() yield video
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 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 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 \ 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 # TODO: why do we accumulate? positions.append([rosmsg.header.stamp.to_sec(), 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.warn('skipped %d erroneous messages', erroneous) if positions: yield marv.push({'values': positions})
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. """ yield marv.set_header(title=stream.topic) pytype = get_message_type(stream) rosmsg = pytype() 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 = counter.next() if idx % interval: continue rosmsg.deserialize(msg.data) try: mono, img = ros2cv(rosmsg, convert_32FC1_scale, convert_32FC1_offset) except cv_bridge.CvBridgeError as e: log = yield marv.get_logger() log.error('could not convert image %s', e) 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 imus(stream): yield marv.set_header(title=stream.topic) pytype = get_message_type(stream) rosmsg = pytype() 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 # TODO: why do we accumulate? imus.append([rosmsg.header.stamp.to_sec(), yaw_angle(rosmsg.orientation)]) if erroneous: log = yield marv.get_logger() log.warn('skipped %d erroneous messages', erroneous) yield marv.push({'values': imus})
def navsatorients(stream): log = yield marv.get_logger() yield marv.set_header(title=stream.topic) pytype = get_message_type(stream) rosmsg = pytype() erroneous = 0 navsatorients = [] while True: msg = yield marv.pull(stream) if msg is None: break rosmsg.deserialize(msg.data) if np.isnan(rosmsg.yaw): erroneous += 1 continue # TODO: why do we accumulate? navsatorients.append([rosmsg.header.stamp.to_sec(), rosmsg.yaw]) if erroneous: log.warn('skipped %d erroneous messages', erroneous) yield marv.push({'values': navsatorients})
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. """ yield marv.set_header(title=stream.topic) pytype = get_message_type(stream) rosmsg = pytype() 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 = counter.next() if idx % interval: continue rosmsg.deserialize(msg.data) if rosmsg.encoding == '32FC1': img = cv2.convertScaleAbs(numpy.nan_to_num(imgmsg_to_cv2(rosmsg, 'passthrough')), None, convert_32FC1_scale, convert_32FC1_offset) elif rosmsg.encoding == '8UC1': img = imgmsg_to_cv2(rosmsg) else: img = imgmsg_to_cv2(rosmsg, "rgb8") 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 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