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)