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 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 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 AllReady(Block):
'''
This block outputs the inputs, unchanged.
'''
Block.alias('all_ready')
Block.input_is_variable('Input signals.', min=1)
Block.output_is_variable('Output signals, equal to input.')
def update(self):
if not self.all_input_signals_ready():
return
for i in range(self.num_input_signals()):
if self.input_update_available(i):
t, value = self.get_input_ts_and_value(i)
self.set_output(i, value, t)
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 Any(Block):
'''
Joins the stream of multiple signals onto one output signal.
'''
Block.alias('any')
Block.input_is_variable('Signals to be put on the same stream.', min=1)
Block.output('stream', 'Unified stream.')
def init(self):
self.last_ts = {}
self.buffer = []
def update(self):
# DO NOT USE NAMES
# TODO: check other blocks for this bug
nsignals = self.num_input_signals()
for i in range(nsignals):
value = self.get_input(i)
ts = self.get_input_timestamp(i)
if value is None:
continue
if not i in self.last_ts or ts != self.last_ts[i]:
self.buffer.append((ts, value))
self.last_ts[i] = ts
# t = [x[0] for x in self.buffer]
# self.debug(' after1: %s' % t)
#
self.buffer = sorted(self.buffer, key=lambda x: x[0])
# t = [x[0] for x in self.buffer]
# self.debug(' sort: %s' % t)
# Make sure we saw every signal before outputing one
# if len(self.last_ts) == nsignals:
# FIXME: bug we will send one sample of the last stream
if self.buffer:
ts, value = self.buffer.pop(0)
self.set_output(0, value, ts)
class Info(Block):
'''
Prints more compact information about the inputs
than :ref:`block:print`.
For numpy arrays it prints their shape and dtype
instead of their values.
'''
Block.alias('info')
Block.input_is_variable('Signals to describe.', min=1)
def init(self):
self.first = {}
self.counter = {}
def update(self):
# Just copy the input to the output
for i in range(self.num_input_signals()):
name = self.canonicalize_input(i)
val = self.get_input(i)
ts = self.get_input_timestamp(i)
if ts is None:
continue
if not i in self.first:
self.first[i] = ts
self.counter[i] = 0
friendly = ts - self.first[i]
# if isinstance(val, numpy.ndarray):
# s = "%s %s" % (str(val.shape), str(val.dtype))
# else:
s = str(val)
if len(s) > 40:
s = s[:40]
s = s.replace('\n', '|')
date = datetime.fromtimestamp(ts).isoformat(' ')[:-4]
ts = "%.2f" % ts
self.debug('%s (%8.2fs) %12s %5d %s' %
(date, friendly, name, self.counter[i], s))
self.counter[i] += 1
class AERRawStream(IteratorGenerator):
'''
'''
Block.alias('aer_raw_stream')
Block.config('filename', 'File.')
Block.output('events', 'Event stream')
def get_iterator(self):
filename = self.config.filename
if not os.path.exists(filename):
raise BadConfig(self, 'Not existent file %r.' % filename,
'filename')
raw_events = aer_load_log_generic(filename)
events2 = aer_raw_relative_timestamp(raw_events)
for e in events2:
yield 0, e['timestamp'], e
class FPSPrint(Block):
''' Prints the fps count for the input signals. '''
Block.alias('fps_print')
Block.input_is_variable('Any signal.', min=1)
def init(self):
self.state.last_timestamp = None
def update(self):
current = max(self.get_input_signals_timestamps())
last = self.state.last_timestamp
if last is not None:
difference = current - last
fps = 1.0 / difference
self.info("FPS %s %.1f" % (self.canonicalize_input(0), fps))
self.state.last_timestamp = current
class Expectation(Block):
''' Computes the sample expectation of a signal. '''
Block.alias('expectation')
Block.input('x', 'Any numpy array.')
Block.output('Ex', 'Expectation of input.')
def init(self):
self.state.num_samples = 0
def update(self):
N = self.state.num_samples
if N == 0:
self.state.Ex = self.input.x.copy()
else:
self.state.Ex = (self.state.Ex * N + self.input.x) / float(N + 1)
self.state.num_samples += 1
self.output.Ex = self.state.Ex
class Constant(Block):
''' Output a numerical constant that never changes.
Example: ::
|constant value=42| -> ...
'''
Block.alias('constant')
Block.config('value', 'Constant value to output.')
Block.output('constant', 'The constant value.')
def update(self):
# XXX: are you sure we need ETERNITY?
self.set_output(0, self.config.value, timestamp=ETERNITY)
def __repr__(self):
return 'Constant(%s)' % self.config.value
class ApplyDiffeoAction(Block):
'''
Applies a diffeomorphism.
The diffeo is created on the fly.
'''
Block.alias('apply_diffeo_action')
Block.config('id_symdiffeo')
Block.input('rgb', 'Input image (either gray or RGB)')
Block.output('rgba', 'Output image')
def init(self):
self.diffeo_action = None
@contract(shape='seq[2](>=1)')
def init_diffeo(self, shape):
id_symdiffeo = self.config.id_symdiffeo
symdiffeo = get_dp_config().symdiffeos.instance(id_symdiffeo)
label = 'tmp'
original_cmd = [np.zeros(1)]
self.info('creating diffeo_action')
contracts.disable_all()
self.diffeo_action = \
diffeo_action_from_symdiffeo(symdiffeo, shape,
label, original_cmd)
self.info('..done')
def update(self):
rgb = self.input.rgb
if self.diffeo_action is None:
self.init_diffeo(shape=rgb.shape[:2])
y0 = UncertainImage(rgb)
y1 = self.diffeo_action.predict(y0)
rgb1 = y1.get_rgba()
self.output.rgba = rgb1
class StaticImage(Generator):
Block.alias('static_image')
Block.config('file', 'Static image to read')
Block.output('rgb', 'Image rgb')
def init(self):
self.done = False
def next_data_status(self):
# print('Status')
if self.done:
# print ('Return False, none')
return (False, None)
else:
# print ('Return True, none')
return (True, 0) # XXX: not sure
def update(self):
# print('update')
self.set_output('rgb', imread(self.config.file), 0)
self.done = True
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 ReadDiffeoStreamItems(IteratorGenerator):
""" Reads a sequence of LogItems, separating in y0,u,y1 """
Block.alias('read_diffeo_stream_components')
Block.config('stream', 'Stream to read')
Block.output('y0')
Block.output('u')
Block.output('y1')
def init_iterator(self):
""" Must return an iterator yielding signal, timestamp, values """
return self.read_stream(self.config.stream)
def read_stream(self, id_stream):
stream = get_conftools_streams().instance(id_stream)
for i, log_item in enumerate(stream.read_all()):
timestamp = i * 1.0
yield 'y0', timestamp, log_item.y0
yield 'y1', timestamp, log_item.y1
yield 'u', timestamp, log_item.u
class RewriteTimestamps(Block):
'''
Retims the timestamps equally spaced.
[0, interval, interval*2, interval*3, ...]
'''
Block.alias('rewrite_timestamps')
Block.config('interval', 'interval')
Block.input('x')
Block.output('y')
def init(self):
self.i = 1
def update(self):
value = self.get_input(0)
t = self.i * self.config.interval
self.i += 1
self.set_output(0, value, t)
class AsJSON(Block):
''' Converts the input into a JSON string.
TODO: add example
'''
Block.alias('as_json')
Block.input_is_variable('Inputs to transcribe as JSON.')
Block.output('json', 'JSON string.')
def update(self):
data = {}
data['timestamp'] = max(self.get_input_signals_timestamps())
for i in range(self.num_input_signals()):
name = self.canonicalize_input(i)
value = self.input[name]
data[name] = value
self.output.json = json.dumps(data)
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 FPSLimit(Block):
''' This block limits the output update to a certain *realtime* framerate.
Note that this uses realtime wall clock time -- not the data time!
This is mean for real-time applications, such as visualization.'''
Block.alias('fps_limit')
Block.config('fps', 'Realtime fps limit.')
Block.input_is_variable('Arbitrary signals.')
Block.output_is_variable('Arbitrary signals with limited framerate.')
def init(self):
self.state.last_timestamp = None
def update(self):
should_update = False
last = self.state.last_timestamp
current = time.time()
if last is None:
should_update = True
self.state.last_timestamp = current
else:
fps = self.config.fps
delta = 1.0 / fps
difference = current - last
#print "difference: %s ~ %s" % (difference, delta)
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 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 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 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 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 YAMLLogReader(Generator):
'''
Reads the Vehicles log format (YAML)
'''
Block.alias('yaml_log_reader')
Block.output('state')
Block.config('file', 'YAML file to read')
def init(self):
self.f = open(self.config.file)
self.iterator = yaml.load_all(self.f, Loader=Loader)
self._load_next()
def _load_next(self):
try:
self.next_value = self.iterator.next()
self.next_timestamp = self.next_value['timestamp']
self.has_next = True
except StopIteration:
self.has_next = False
def next_data_status(self):
if self.has_next:
return ('state', self.next_timestamp)
else:
return (False, None)
def update(self):
if not self.has_next:
return # XXX: error here?
self.set_output(0,
value=self.next_value,
timestamp=self.next_timestamp)
self._load_next()
def finish(self):
self.f.close()
class se2_from_SE2_seq(Block):
''' Computes velocity in se2 given poses in SE2. '''
Block.alias('se2_from_SE2_seq')
Block.input('pose', 'Pose as an element of SE2', dtype=SE2)
Block.output('velocity',
'Velocity as an element of se(2).',
dtype=np.dtype(('float', (3, 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)
self.set_output(0, vel, timestamp=t2)
self.state.prev = t2, q2
class DepthBuffer(Block):
Block.alias('depth_buffer')
Block.input('rgba')
Block.output('rgba')
Block.output('line')
Block.output('depth')
def init(self):
self.depth = None
def update(self):
rgba = self.input.rgba
if self.depth is None:
H, W = rgba.shape[0:2]
self.depth = np.zeros((H, W))
self.depth.fill(0)
d = get_depth(rgba)
mask = rgba[:, :, 3] > 0
closer = np.logical_and(self.depth < d, mask)
farther = np.logical_not(closer)
self.depth[closer] = d
rgba = rgba.copy()
rgba[farther, 3] = 0
with_line = rgba[:, :, 0:3].copy()
with_line[d, :, 0] = 255
with_line[d, :, 1] = 55
depth = self.depth.copy()
depth[depth == 0] = np.nan
self.output.rgba = rgba
self.output.line = with_line
self.output.depth = depth
class Extract(Block):
'''
This block extracts some of the components of a vector.
Example: Extracts the first and third component of x. ::
x -> |extract index=[0,2]| -> x_part
'''
Block.alias('extract')
Block.input('vector', 'Any numpy array')
Block.output('part', 'The part extracted')
Block.config('index', 'Index (or indices) to extract.')
def update(self):
index = self.config.index
vector = np.array(self.input.vector)
part = vector[index]
self.output.part = part
class Join(Block):
'''
This block joins multiple signals into one.
'''
Block.alias('join')
Block.input_is_variable('Signals to be joined together.')
Block.output('joined', 'Joined signals.')
def init(self):
sizes = {}
names = self.get_input_signals_names()
for signal in names:
sizes[signal] = None
self.state.sizes = sizes
def update(self):
sizes = self.state.sizes
result = []
for name in self.get_input_signals_names():
value = self.get_input(name)
# workaround for scalar values
value = np.reshape(value, np.size(value))
size = len(value)
if value is None:
return
if sizes[name] is None:
sizes[name] = size
else:
if size != sizes[name]:
raise Exception('Signal %s changed size from %s to %s.' %
(name, sizes[name], size))
result.extend(value)
self.output[0] = np.array(result)