class AERPF(Block): """ Simple particle filter """ Block.alias('aer_pf') Block.input('track_log') Block.output('particles', 'All particles') Block.output('hps', 'A list of coherent hypotheses') Block.config('min_track_dist', 'Minimum distance between tracks') Block.config('max_vel', 'Maximum velocity') Block.config('max_bound', 'Largest size of the uncertainty') Block.config('max_hp', 'Maximum number of hypotheses to produce.') def init(self): params = dict(max_vel=self.config.max_vel, min_track_dist=self.config.min_track_dist, max_bound=self.config.max_bound) self.pdm = ParticleTrackerMultiple(**params) def update(self): tracks = self.input.track_log self.pdm.add_observations(tracks) particles = self.pdm.get_all_particles() if len(particles) > 0: self.output.particles = particles max_hp = self.config.max_hp hps = self.pdm.get_coherent_hypotheses(max_hp) self.output.hps = hps
class Bounce(Block): Block.alias('bounce') Block.config('width', 'Image dimension', default=320) Block.config('height', 'Image dimension', default=240) Block.config('transparent', 'If true, outputs a RGBA image instead of RGB.', default=False) Block.config('tight', 'Uses "tight" option for creating png (Matplotlib>=1.1).', default=False) Block.input('tick') Block.output('rgb') def init(self): self.plot_generic = PlotGeneric(width=self.config.width, height=self.config.height, transparent=self.config.transparent, tight=self.config.tight) def update(self): self.output.rgb = self.plot_generic.get_rgb(self.plot) def plot(self, pylab): t = self.get_input_timestamp(0) t0 = t t1 = t + 2 x = np.linspace(t0, t1, 1000) y = np.cos(x) pylab.plot(x, y) pylab.axis((t0, t1, -1.2, +1.2))
class MyBlockOK(Block): Block.config('x', 'description') Block.config('y', 'description 2', default=True) Block.config('z') Block.input('x') Block.input('y') Block.output('x')
class HDFwrite(Block): ''' This block writes the incoming signals to a file in HDF_ format. The HDF format is organized as follows: :: / (root) /procgraph (group with name procgraph) /procgraph/signal1 (table) /procgraph/signal2 (table) ... Each table has the following fields: time (float64 timestamp) value (the datatype of the signal) If a signal changes datatype, then an exception is raised. ''' Block.alias('hdfwrite') Block.input_is_variable('Signals to be written', min=1) Block.config('file', 'HDF file to write') Block.config('compress', 'Whether to compress the hdf table.', 1) Block.config('complib', 'Compression library (zlib, bzip2, blosc, lzo).', default='zlib') Block.config('complevel', 'Compression level (0-9)', 9) def init(self): self.writer = PGHDFLogWriter(self.config.file, compress=self.config.compress, complevel=self.config.complevel, complib=self.config.complib) def update(self): signals = self.get_input_signals_names() for signal in signals: if self.input_update_available(signal): self.log_signal(signal) def log_signal(self, signal): timestamp = self.get_input_timestamp(signal) value = self.get_input(signal) # only do something if we have something if value is None: return assert timestamp is not None if not isinstance(value, numpy.ndarray): # TODO: try converting try: value = numpy.array(value) except: msg = 'I can only log numpy arrays, not %r' % value.__class__ raise BadInput(msg, self, signal) self.writer.log_signal(timestamp, signal, value) def finish(self): self.writer.finish()
class DPDDSPredict(Block): Block.alias('dp_discdds_predict') Block.config('id_discdds') Block.config('plan') Block.config('config_dir', default=[]) Block.input('rgb') Block.output('prediction') def init(self): id_discdds = self.config.id_discdds dp_config = get_dp_config() dp_config.load(self.config.config_dir) self.discdds = dp_config.discdds.instance(id_discdds) plan = self.config.plan self.action = self.discdds.plan2action(plan) def update(self): rgb0 = self.input.rgb H, W = self.discdds.get_shape() rgb = resize(rgb0, width=W, height=H) y0 = UncertainImage(rgb) y1 = self.action.predict(y0) pred = y1.get_rgba_fill() pred2 = resize(pred, height=rgb0.shape[0], width=rgb0.shape[1]) self.output.prediction = pred2[:, :, :3]
class CVCapture(Generator): Block.alias('cv_capture') Block.config('cam', default=0) Block.config('width', default=160) Block.config('height', default=120) Block.config('fps', default=10) Block.output('rgb') def init(self): cam = self.config.cam width = self.config.width height = self.config.height fps = self.config.fps self.info('Capturing cam=%d %dx%d @%.1f fps' % (cam, width, height, fps)) self.capture = cv.CaptureFromCAM(cam) cv.SetCaptureProperty(self.capture, cv.CV_CAP_PROP_FRAME_WIDTH, width) cv.SetCaptureProperty(self.capture, cv.CV_CAP_PROP_FRAME_HEIGHT, height) cv.SetCaptureProperty(self.capture, cv.CV_CAP_PROP_FPS, fps) def update(self): t = time.time() img = cv.QueryFrame(self.capture) rgb = cv_to_numpy(img) self.set_output('rgb', value=rgb, timestamp=t) def next_data_status(self): return (True, None)
class Compose(Block): ''' Compose several images in the same canvas. You should probably use :ref:`block:grid` in many situations. Example configuration: :: compose.positions = {y: [0,0], ys: [320,20]} ''' Block.alias('compose') Block.config('width', 'Dimension in pixels.') Block.config('height', 'Dimension in pixels.') Block.config( 'positions', 'A structure giving the position of each signal in the canvas.') Block.input_is_variable('Images to compose.') Block.output('canvas', 'RGB image') def update(self): width = self.get_config('width') height = self.get_config('height') canvas = np.zeros((height, width, 3), dtype='uint8') positions = self.get_config('positions') if not isinstance(positions, dict): raise Exception('I expected a dict, not "%s"' % positions) for signal, position in positions.items(): if not self.is_valid_input_name(signal): raise Exception('Unknown input "%s" in %s.' % (signal, self)) rgb = self.get_input(signal) # TODO check if rgb is not None: assert_rgb_image(rgb, 'input %s to compose block' % signal) place_at(canvas, rgb, position[0], position[1]) #print "Writing image %s" % signal else: print "Ignoring image %s because not ready.\n" % signal self.set_output(0, canvas)
class AERTrackerLogReader(TextLog): Block.alias('aer_tracker_log') Block.config('file') Block.output('track') def init(self): self.last = -1 self.buffer = [] TextLog.init(self) def parse_format(self, line): # return none if we want more data x = aer_track_parse_line(line) if x is None: return None self.buffer.append(x) # If it was the last in the sequence: if x['peak'] == x['npeaks'] - 1: res = np.array(self.buffer, self.buffer[0].dtype) self.buffer = [] t = res[0]['timestamp'] if t <= self.last: t = self.last + 0.0000001 self.last = t return t, [(0, res)]
class FPSDataLimit(Block): ''' This block limits the output update to a certain framerate. ''' Block.alias('fps_data_limit') Block.config('fps', 'Maximum framerate.') Block.input_is_variable('Signals to decimate.', min=1) Block.output_is_variable('Decimated signals.') def init(self): self.state.last_timestamp = None def update(self): should_update = False last = self.state.last_timestamp current = max(self.get_input_signals_timestamps()) if last is None: should_update = True self.state.last_timestamp = current else: fps = self.config.fps delta = 1.0 / fps difference = current - last if difference > delta: should_update = True self.state.last_timestamp = current if not should_update: return # Just copy the input to the output for i in range(self.num_input_signals()): self.set_output(i, self.get_input(i), self.get_input_timestamp(i))
class LowPass(Block): ''' Implements simple low-pass filtering. Formula used: :: y[k] = alpha * u[k] + (1-alpha) * y[k-1] ''' # TODO: make a serious low-pass block Block.alias('low_pass') Block.config('alpha', 'Innovation rate') Block.input('value', 'Any numpy array.') Block.output('lowpass', 'The lowpass version.') def init(self): self.state.y = None def update(self): u = self.input[0] alpha = self.config.alpha if self.state.y is None: self.state.y = u else: self.state.y = self.state.y * (1 - alpha) + alpha * u self.output[0] = self.state.y
class AERTrackPlotter(Block): Block.alias('aer_track_plotter') Block.config('width', 'Image dimension', default=128) Block.input('tracks') Block.output('rgb') def init(self): self.plot_generic = PlotGeneric(width=self.config.width, height=self.config.width, transparent=False, tight=False) def update(self): self.output.rgb = self.plot_generic.get_rgb(self.plot) def plot(self, pylab): tracks = self.input.tracks plot_tracks(pylab, tracks, base_markersize=10, alpha=0.5) T = self.get_input_timestamp(0) pylab.title('Raw detections') time = 'T = %.1f ms' % (T * 1000) pylab.text(3, 3, time) set_viewport_style(pylab)
class TimeSlice(Block): ''' This block collects the history of a quantity for a given interval length, and it outputs a list of values when the buffer is full. Then it resets the buffer. See also :ref:`block:historyt` and :ref:`block:last_n_samples`. ''' Block.alias('time_slice') Block.config('interval', 'Length of the interval to record.', default=10) Block.input('values', 'Any signal.') Block.output('grouped', 'List of the values in a given interval.') def init(self): self.x = [] self.t = [] def update(self): sample = self.get_input(0) timestamp = self.get_input_timestamp(0) self.x.append(sample) self.t.append(timestamp) delta = self.t[-1] - self.t[0] # self.info('Delta: %.10f sec n = %6d' % (delta, len(self.t))) if np.abs(delta) >= self.config.interval: self.set_output(0, value=self.x, timestamp=self.t[0]) self.x = [] self.t = []
class AERAltPlotter(Block): Block.alias('aer_alt_plotter') Block.config('width', 'Image dimension', default=128) Block.input('alts') Block.output('rgb') def init(self): self.plot_generic = PlotGeneric(width=self.config.width, height=self.config.width, transparent=False, tight=False) self.max_q = 0 def update(self): self.output.rgb = self.plot_generic.get_rgb(self.plot) def plot(self, pylab): alts = self.input.alts markers = ['s', 'o', 'x'] quality = ['%f' % x.score for x in alts] for i, alt in enumerate(alts): subset = alt.subset # get the last ones tracks = get_last(subset) marker = markers[i % len(markers)] plot_tracks(pylab, tracks, base_markersize=10, marker=marker) pylab.text(10, 10, quality)
class Wait(Block): ''' This block waits a given number of updates before transmitting the output signal. ''' Block.alias('wait') Block.config('n', 'Number of updates to wait at the beginning.') Block.input_is_variable('Arbitrary signals.') Block.output_is_variable('Copy of the signals, minus the first ' '*n* updates.') def init(self): self.state.count = 0 def update(self): count = self.state.count # make something happen after we have waited enough if count >= self.config.n: # Just copy the input to the output for i in range(self.num_input_signals()): self.set_output(i, self.get_input(i), self.get_input_timestamp(i)) self.state.count = count + 1
class Sieve(Block): ''' This block decimates the data in time by transmitting only one in ``n`` updates. ''' Block.alias('sieve') Block.config('n', 'Decimation level; ``n = 3`` means transmit one in three.') Block.input('data', 'Arbitrary input signals.') Block.output('decimated', 'Decimated signals.') def init(self): self.state.count = 0 def update(self): # make something happen after we have waited enough if 0 == self.state.count % self.config.n: # Just copy the input to the output # XXX: using only one signal? for i in range(self.num_input_signals()): self.set_output(i, self.get_input(i), self.get_input_timestamp(i)) self.state.count += 1
class History(Block): ''' This block collects the history of a quantity, and outputs two signals ``x`` and ``t``. See also :ref:`block:historyt` and :ref:`block:last_n_samples`. ''' Block.alias('history') Block.config('interval', 'Length of the interval to record.', default=10) Block.input('values', 'Any signal.') Block.output('x', 'Sequence of values.') Block.output('t', 'Sequence of timestamps.') def init(self): self.history = HistoryInterval(self.config.interval) def update(self): sample = self.get_input(0) timestamp = self.get_input_timestamp(0) self.history.push(timestamp, sample) ts, xs = self.history.get_ts_xs() self.output.x = xs self.output.t = ts
class Solid(Block): Block.alias('solid') Block.output('rgb') Block.config('width') Block.config('height') Block.config('color', default=[1, 1, 1]) def init(self): self.info('init') def update(self): rgb = solid(self.config.width, self.config.height, self.config.color) self.info('updating solid ts : %s' % ETERNITY) # self.set_output(0, rgb, timestamp=ETERNITY) warnings.warn('Eternity is not respected') self.set_output(0, rgb, timestamp=0)
class HistoryT(Block): ''' This block collects the signals samples of a signals, and outputs *one* signal containing a tuple ``(t,x)``. See also :ref:`block:last_n_samples` and :ref:`block:history`. If ``natural`` is true, it uses the time from the beginning of the log. ''' Block.alias('historyt') Block.config('interval', 'Length of interval (seconds).', default=10) Block.config('natural', 'If true, set 0 to be timestamp of the log ' 'beginning. This allows to have prettier graphs', default=True) Block.input('x', 'Any signal.') Block.output('history', 'Tuple ``(t,x)`` containing two arrays.') def init(self): self.state.x = [] self.state.t = [] self.state.first_timestamp = None def update(self): sample = self.get_input(0) timestamp = self.get_input_timestamp(0) if self.state.first_timestamp is None: self.state.first_timestamp = timestamp if self.config.natural: timestamp = timestamp - self.state.first_timestamp x = self.state.x t = self.state.t x.append(sample) t.append(timestamp) while abs(t[0] - t[-1]) > self.config.interval: t.pop(0) x.pop(0) self.output.history = (t, x)
class Clock(Generator): Block.alias('clock') Block.config('interval', 'Delta between ticks.', default=1) Block.output('clock', 'Clock signal.') Block.config('length', 'Total interval', default=None) def init(self): self.state.clock = 0.0 def update(self): self.set_output('clock', self.state.clock, timestamp=self.state.clock) self.state.clock += self.config.interval def next_data_status(self): if self.config.length is not None and self.state.clock > self.config.length: return (False, None) else: return (True, self.state.clock + self.config.interval)
class PickleLoad(Block): ''' Dumps the input as a :py:mod:`pickle` file. ''' Block.alias('pickle_load') Block.config('file', 'File to read from.') Block.output('x', 'Object read from file') def update(self): with open(self.config.file, 'rb') as f: x = pickle.load(f) self.set_output(0, x, timestamp=ETERNITY)
class OrganicScale(Block): ''' A (almost failed!) attempt to scale a signal into [-1,1] according to the history. This one is a mess. ''' Block.alias('organic_scale') Block.input('value') Block.output('value_scaled') Block.config('skim', default=5) Block.config('skim_hist', default=5) Block.config('hist', 'How many steps of history to use.', default=100) Block.config('tau', default=0.1) def init(self): self.state.max = [] self.state.M = None def update(self): y = numpy.array(self.input.value) # first skim it y = skim_top_and_bottom(y, self.config.skim) # put the max in the history self.state.max.insert(0, max(abs(y))) # prune the history self.state.max = self.state.max[:self.config.hist] # get the history percent M = max(skim_top(numpy.array(self.state.max), self.config.skim_hist)) if self.state.M is None: self.state.M = M else: self.state.M += self.config.tau * (M - self.state.M) y = minimum(y, self.state.M) y = maximum(y, -self.state.M) y = y / self.state.M self.output.value_scaled = y
class AERResolver(Block): Block.alias('aer_resolver') # Block.config('ntracks') Block.input('track_log') Block.output('hps') Block.config('min_track_dist', default=10) Block.config('max_vel', default=1000.0) Block.config('history', default=0.005) Block.config('max_hp', 'Maximum number of hypotheses to show', default=10) def init(self): motion_model = MaxVelMotion(self.config.max_vel, self.config.min_track_dist) self.resolver = Resolver(motion_model=motion_model, history=self.config.history, min_track_dist=self.config.min_track_dist) def update(self): self.resolver.push(self.input.track_log) alts = self.resolver.compute_alternatives() if False: # check correct alts2 = self.resolver.compute_alternatives_combinatorial() alternatives_print(alts2, 'normal', n=4) alternatives_print(alts, 'fast', n=4) hps = alts N = self.config.max_hp if len(hps) > N: hps = hps[:N] # alternatives_print(hps) self.output.hps = hps
class ImageGrid(Block): ''' A block that creates a larger image by arranging them in a grid. The output is rgb, uint8. Inputs are passed through the "torgb" function. ''' Block.alias('grid') Block.config('cols', 'Columns in the grid.', default=None) Block.config('bgcolor', 'Background color.', default=[0, 0, 0]) Block.config('pad', 'Padding for each cell', default=0) Block.input_is_variable('Images to arrange in a grid.', min=1) Block.output('grid', 'Images arranged in a grid.') def update(self): if not self.all_input_signals_ready(): return n = self.num_input_signals() for i in range(n): input_check_convertible_to_rgb(self, i) cols = self.config.cols if cols is not None and not isinstance(cols, int): raise BadConfig('Expected an integer.', self, 'cols') images = [self.get_input(i) for i in range(n)] images = map(torgb, images) canvas = make_images_grid(images, cols=self.config.cols, pad=self.config.pad, bgcolor=self.config.bgcolor) self.set_output(0, canvas)
class Gain(Block): ''' A simple example of a gain block. ''' Block.alias('gain') Block.config('k', 'Multiplicative gain') Block.input('in', 'Input value') Block.output('out', 'Output multiplied by k.') def update(self): self.output[0] = self.input[0] * self.config.k
class JitteryClock(Generator): Block.alias('jittery_clock') Block.config('interval', 'Delta between ticks.', default=0.1) Block.config('noise', 'Jitter.', default=0.02) Block.output('clock', 'Clock signal.') Block.config('length', 'Total interval', default=None) def init(self): self.state.clock = 0 def update(self): self.set_output('clock', 'orig: %1.3f' % self.state.clock, timestamp=self.state.clock) dt = self.config.interval + np.random.rand() * self.config.noise self.state.clock += dt def next_data_status(self): if self.config.length is not None and self.state.clock > self.config.length: return (False, None) else: return (True, self.state.clock + self.config.interval)
class Border(Block): ''' Adds a block around the input image. ''' Block.alias('border') Block.input('rgb', 'Input image.') Block.output('rgb', 'Image with borders added around.') Block.config('color', 'border color (0-1 rgb)', default=[0, 0, 0]) Block.config('width', default=1) Block.config('left', 'pixel length for left border', default=None) Block.config('right', 'pixel length for right border', default=None) Block.config('top', 'pixel length for top border', default=None) Block.config('bottom', 'pixel length for bottom border', default=None) def update(self): check_rgb(self, 'rgb') def df(x): if x is None: return self.config.width else: return x # TODO: check color self.output.rgb = image_border(self.input.rgb, left=df(self.config.left), right=df(self.config.right), top=df(self.config.top), bottom=df(self.config.bottom), color=df(self.config.color))
class Display(Block): Block.alias('cv_display') Block.config('name', default=None) Block.config('position', default=None) Block.input('rgb') nimages = 0 def init(self): name = self.config.name if name is None: name = 'display%d' % Display.nimages self.name = name Display.nimages += 1 cv.NamedWindow(self.name, 1) if self.config.position is not None: x, y = self.config.position else: cols = 4 w, h = 320, 320 u = Display.nimages % cols v = int(np.floor(Display.nimages / cols)) x = u * w y = v * h cv.MoveWindow(self.name, x, y) def update(self): rgb = self.input.rgb img = numpy_to_cv(rgb) cv.ShowImage(self.name, img) def finish(self): warnings.warn('to fix') cv.DestroyAllWindows()
class AERPFHPPlotter(Block): Block.alias('aer_pf_hp_plotter') Block.config('width', 'Image dimension', default=128) Block.config('title', default=None) Block.input('alts') Block.output('rgb') def init(self): self.plot_generic = PlotGeneric(width=self.config.width, height=self.config.width, transparent=False, tight=False) self.max_q = 0 def update(self): self.output.rgb = self.plot_generic.get_rgb(self.plot) def plot(self, pylab): alts = self.input.alts markers = ['s', 'o', 'x'] for i, alt in enumerate(alts): marker = markers[i % len(markers)] self.plot_hp(pylab, alt, marker) # only draw if small... if len(alts) <= 2: scores = ",".join(['%g' % x.score for x in alts]) pylab.text(3, 3, 'score: %s' % scores) set_viewport_style(pylab) title = self.config.title if title is not None: pylab.title(title) def plot_hp(self, pylab, alt, marker): particles = alt.subset track2color = get_track_colors(particles) for id_track, particles in enumerate_id_track(particles): color = track2color[id_track] plot_particles(pylab, particles, color)
class AERSmoother(Block): Block.alias('aer_smoother') Block.config('ntracks') Block.input('track_log') Block.output('tracks') def init(self): self.smoother = Smoother(self.config.ntracks) def update(self): self.smoother.push(self.input.track_log) if self.smoother.is_complete(): self.set_output('tracks', self.smoother.get_values())
class BagWrite(Block): ''' This block writes the incoming signals to a ROS bag file. By default, the signals are organized as follows: :: / (root) /procgraph (group with name procgraph) /procgraph/signal1 (table) /procgraph/signal2 (table) ... Note that the input should be ROS messages; no conversion is done. ''' Block.alias('bagwrite') Block.input_is_variable('Signals to be written', min=1) Block.config('file', 'Bag file to write') def init(self): from ros import rosbag # @UnresolvedImport self.info('Writing to bag %r.' % self.config.file) make_sure_dir_exists(self.config.file) if os.path.exists(self.config.file): os.unlink(self.config.file) self.tmp_file = self.config.file + '.active' self.bag = rosbag.Bag(self.tmp_file, 'w', compression='bz2') self.signal2last_timestamp = {} def finish(self): self.bag.close() os.rename(self.tmp_file, self.config.file) def update(self): import rospy signals = self.get_input_signals_names() for signal in signals: msg = self.get_input(signal) timestamp = self.get_input_timestamp(signal) if ((signal in self.signal2last_timestamp) and (timestamp == self.signal2last_timestamp[signal])): continue self.signal2last_timestamp[signal] = timestamp ros_timestamp = rospy.Time.from_sec(timestamp) self.bag.write('/%s' % signal, msg, ros_timestamp)