class LogSim(Generator):
Block.alias('log_sim')
Block.output('stream')
def init(self):
line = self.config.line
self.queue = []
for i, char in enumerate(line):
t = i + 1
if char == ' ':
continue
elif char == '*':
self.queue.insert(0, (t, -1))
elif char.isdigit():
v = int(char)
self.queue.insert(0, (t, v))
else:
raise Exception('Bad input "%s" at char %d of "%s".' % \
(char, t, line))
def next_data_status(self):
if self.queue:
return (True, self.queue[-1][0])
else:
return (False, None)
def update(self):
assert self.queue
timestamp, value = self.queue.pop()
self.set_output(0, value, timestamp)
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 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 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 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 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 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 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 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 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 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 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 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 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 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 ASync(Block):
'''
The first signal is the "master".
Waits that all signals are perceived once.
Then it creates one event every time the master arrives.
'''
Block.alias('async')
Block.input_is_variable(
'Signals to (a)synchronize. The first is the master.', min=2)
Block.output_is_variable('Synchronized signals.')
def init(self):
self.state.last_sent_timestamp = None
def update(self):
self.state.last_timestamp = None
# No signal until everybody is ready
if not self.all_input_signals_ready():
return
# No signal unless the master is ready
master_timestamp = self.get_input_timestamp(0)
if self.state.last_timestamp == master_timestamp:
return
self.state.last_timestamp = master_timestamp
# Write all signals using master's timestamp
for s in self.get_input_signals_names():
self.set_output(s, self.get_input(s), master_timestamp)
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 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 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 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 Blend(Block):
'''
Blend two or more images.
RGB images are interpreted as having full alpha (opaque).
All images must have the same width and height.
'''
Block.alias('blend')
Block.input_is_variable('images to blend', min=2)
Block.output('rgb', 'The output is a RGB image (no alpha)')
def update(self):
# TODO: check images
result = None
for signal in self.get_input_signals_names():
image = self.get_input(signal)
if result is None:
result = image
continue
result = blend(result, image)
self.output.rgb = to_rgb(result)
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 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 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 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 Identity(Block):
'''
This block outputs the inputs, unchanged.
'''
Block.alias('identity')
Block.input_is_variable('Input signals.', min=1)
Block.output_is_variable('Output signals, equal to input.')
def update(self):
# 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 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)