class ForwardDifference12(Block): ''' Computes ``x[t+1] - x[t]`` normalized with timestamp. ''' Block.alias('two_step_difference') Block.input('x12', 'An array with the last 2 values of x.') Block.input('t12', 'An array with the last 2 values of the timestamp.') Block.output('x_dot', 'Derivative of x') def update(self): x = self.input.x12 t = self.input.t12 if not isiterable(x) or len(x) != 2: raise BadInput('Expected arrays of 2 elements', self, 'x') if not isiterable(t) or len(t) != 2: raise BadInput('Expected arrays of 2 elements', self, 't') delta = t[1] - t[0] if not delta > 0: raise BadInput('Bad timestamp sequence % s' % t, self, 't') # if this is a sequence of bytes, let's promove them to floats if x[0].dtype == numpy.dtype('uint8'): diff = x[1].astype('float32') - x[0].astype('float32') else: diff = x[1] - x[0] time = t[0] x_dot = diff / numpy.float32(delta) self.set_output('x_dot', x_dot, timestamp=time)
class ForwardDifference(Block): ''' Computes ``x[t+1] - x[t-1]`` normalized with timestamp. You want to attach this to :ref:`block:last_n_samples`. ''' Block.alias('forward_difference') Block.input('x123', 'An array with the last 3 values of x.') Block.input('t123', 'An array with the last 3 values of the timestamp.') Block.output('x_dot', 'Derivative of x') def update(self): x = self.input.x123 t = self.input.t123 if not isiterable(x) or len(x) != 3: raise BadInput('Expected arrays of 3 elements', self, 'x') if not isiterable(t) or len(t) != 3: raise BadInput('Expected arrays of 3 elements', self, 't') delta = t[2] - t[0] if not delta > 0: raise BadInput('Bad timestamp sequence % s' % t, self, 't') # if this is a sequence of bytes, let's promove them to floats if x[0].dtype == numpy.dtype('uint8'): diff = x[2].astype('float32') - x[0].astype('float32') else: diff = x[2] - x[0] time = t[1] x_dot = diff / numpy.float32(delta) self.set_output('x_dot', x_dot, timestamp=time)
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 Pose2velocity(Block): ''' Block used by :ref:`block:pose2commands`. ''' Block.alias('pose2vel_') Block.input('q12', 'Last two poses.') Block.input('t12', 'Last two timestamps.') Block.output('commands', 'Estimated commands ``[vx,vy,omega]``.') def update(self): q = self.get_input('q12') t = self.get_input('t12') if not (len(q) == 2 and len(t) == 2): raise BadInput('Bad input received.', self) pose1 = g.SE2_from_xytheta(q[0]) pose2 = g.SE2_from_xytheta(q[1]) delta = t[1] - t[0] if not delta > 0: raise BadInput('Bad timestamp sequence %s' % t, self, 't') _, vel = g.SE2.velocity_from_points(pose1, pose2, delta) linear, angular = g.linear_angular_from_se2(vel) commands = [linear[0], linear[1], angular] self.set_output('commands', commands, timestamp=t[0])
class vel_from_SE2_seq(Block): ''' Computes velocity in se2 represented as vx,vy,omega from a sequence of poses in SE2. ''' Block.alias('vel_from_SE2_seq') Block.input('pose', 'Pose as an element of SE2', dtype=SE2) Block.output('velocity', 'Velocity as vx,vy,omega.', dtype=np.dtype(('float', 3))) def init(self): self.state.prev = None def update(self): q2 = self.get_input(0) t2 = self.get_input_timestamp(0) if self.state.prev is not None: t1, q1 = self.state.prev vel = velocity_from_poses(t1, q1, t2, q2, S=SE2) # Convertion from se2 to R3 v, omega = linear_angular_from_se2(vel) out = np.array([v[0], v[1], omega]) self.set_output(0, out, timestamp=t2) self.state.prev = t2, q2
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 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 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 JitteryDisplay(Block): Block.alias('jittery_display') Block.input('clock') Block.output('rgb') def init(self): self.plot_generic = PlotGeneric(width=320, height=240, transparent=False, tight=False, keep=True) self.first_timestamp = None self.plot_anim = PlotAnim() self.nframes = 0 def update(self): if self.first_timestamp is None: self.first_timestamp = self.get_input_timestamp(0) self.time_since_start = self.get_input_timestamp(0) - self.first_timestamp self.nframes += 1 self.output.rgb = self.plot_generic.get_rgb(self.plot) def plot(self, pylab): self.plot_anim.set_pylab(pylab) self.plot_anim.text('clock', 0, 1, '%5.2f' % self.time_since_start) self.plot_anim.text('frames', 0, 0.5, '%d' % self.nframes) self.plot_anim.text('value', 0, 0.24, self.input.clock) pylab.axis((-0.2, 1.1, -0.1, 1.1))
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 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 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 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 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 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 ExpectationNorm(Block): Block.alias('expectation_norm') Block.input('x', 'Any numpy array.') Block.output('Ex', 'Expectation of input.') def init(self): self.value = None self.mass = None def update(self): x = self.input.x if self.value is None: self.value = x self.weight = np.zeros(x.shape) else: if False: w = np.abs(x - self.last_x) else: w = np.abs(x - self.last_x).sum() self.weight += w self.value += w * x self.weight[self.weight == 0] = np.inf res = self.value / self.weight self.output.Ex = res self.last_x = x.copy()
class use_simulation_time(Block): Block.input('state') Block.output('state') def update(self): state = self.input.state timestamp = state['timestamp'] self.set_output(0, state, timestamp)
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 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 TransparentImageAverageBlock(Block): Block.alias('trans_avg') Block.input('rgba') # Block.output('rgb') Block.output('rgba') def init(self): self.tia = TransparentImageAverage() def update(self): self.tia.update(self.input.rgba) # self.output.rgb = self.tia.get_rgb() self.output.rgba = self.tia.get_rgba()
class AERPFQualityPlotter(Block): Block.alias('aer_pf_quality_plot') Block.config('width', 'Image dimension', default=384) 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) self.min_score = None self.max_score = None def update(self): self.output.rgb = self.plot_generic.get_rgb(self.plot) def plot(self, pylab): tracks = self.input.tracks track2color = get_track_colors(tracks) for id_track, particles in enumerate_id_track(self.input.tracks): color = track2color[id_track] bound = particles['bound'] score = particles['score'] pylab.scatter(bound, np.log(score), marker='s', color=color) min_score = np.min(particles['score']) max_score = np.max(particles['score']) if self.min_score is None: self.min_score = min_score self.max_score = max_score self.min_score = min(self.min_score, min_score) self.max_score = min(self.max_score, max_score) # a = pylab.axis() # pylab.axis((-1, 30, a[2], a[3])) M = 0.1 y0 = np.log(self.min_score) y1 = np.log(self.max_score) delta = y1 - y0 y0 -= M * delta y1 += M * delta pylab.axis((-1, 30, y0, y1)) pylab.xlabel('spatial uncertainty (pixels)') pylab.ylabel('score') pylab.title('Particles statistics')
class SE2_relative_pose(Block): """ Lets the first pose be the identity """ Block.input('pose') Block.output('rel_pose') def init(self): self.state.pose0 = None def update(self): pose = self.input.pose if self.state.pose0 is None: self.state.pose0 = pose rel_pose = SE2.multiply(SE2.inverse(self.state.pose0), pose) self.output.rel_pose = rel_pose
class ToFile(Block): ''' Prints the input line by line to a given file.''' Block.alias('to_file') Block.config('file', 'File to write.') Block.input('values', 'Anything you wish to print to file.') def init(self): self.file = open(self.config.file, 'w') def update(self): s = str(self.input[0]) self.file.write(s) self.file.write('\n') self.file.flush()
class Pickle(Block): ''' Dumps the input as a :py:mod:`pickle` file. ''' Block.alias('pickle') Block.config('file', 'File to write to.') Block.input('x', 'Anything pickable.') def write(self, x, filename): make_sure_dir_exists(filename) with open(filename, 'wb') as f: pickle.dump(x, f, protocol=pickle.HIGHEST_PROTOCOL) def update(self): self.write(self.input.x, self.config.file + '.part') def finish(self): self.write(self.input.x, self.config.file)
class Psychedelic(Block): Block.alias('psychedelic') Block.input('rgb', 'An RGB image.') Block.output('processed', 'The processed image.') def init(self): self.channel = 0 def update(self): self.channel = (self.channel + 1) % 3 rgb = self.input.rgb.copy() for i in [0, 1, 2]: if i != self.channel: rgb[:, :, i] = 0 self.output.processed = rgb
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 Minimum(Block): ''' Computes the minimum of a signal over time. ''' Block.alias('minimum_over_time') Block.input('x', 'Any numpy array.') Block.output('min_x', 'Minimum of input.') def init(self): self.state.min_x = None def update(self): # TODO: check shape did not change if self.state.min_x is None: self.state.min_x = self.input.x else: self.state.min_x = np.minimum(self.state.min_x, self.input.x) self.output.min_x = self.state.min_x
class Retime(Block): ''' Multiplies timestamps by give factor ''' Block.alias('retime') Block.config('factor', 'Factor') Block.input('x') Block.output('y') def init(self): pass def update(self): value = self.get_input(0) t = self.get_input_timestamp(0) t_ = t * self.config.factor self.set_output(0, value, t_)
class VehiclesDisplay(VehiclesCairoDisplay): ''' Produces a top-down plot of a circular arena. ''' Block.alias('vehicles_cairo_display_all') Block.config('format', 'pdf|png', default='pdf') Block.config('file', 'Output file (pdf)', default=None) Block.output('rgb', 'RGB data (png)') Block.config('transparent', 'Outputs RGB with transparent bg', default=False) Block.config('width', 'Image width in points.', default=600) Block.config('height', 'Image height in points.', default=600) Block.config('trace', 'Trace the path', default=False) Block.config('plotting_params', 'Configuration to pass to vehicles_cairo_display_all()', default={}) Block.config('swf', 'Converts PDF to SWF using pdf2swf', default=True) Block.input('boot_obs') def get_shape(self): w = self.config.width h = self.config.height return (w, h) def draw_everything(self, cr): sim_state = self.input.boot_obs map_width = self.config.width map_height = self.config.height plotting_params = self.config.plotting_params with cairo_save(cr): cr.rectangle(0, 0, map_width, map_height) cr.clip() # TODO: implement trace vehicles_cairo_display_all(cr, map_width, map_height, sim_state, **plotting_params)