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)