Exemplo n.º 1
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)
Exemplo n.º 2
0
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))
Exemplo n.º 3
0
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)
Exemplo n.º 4
0
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)
Exemplo n.º 5
0
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
Exemplo n.º 6
0
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_)
Exemplo n.º 7
0
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)
Exemplo n.º 8
0
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
Exemplo n.º 9
0
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
Exemplo n.º 10
0
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
Exemplo n.º 11
0
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
Exemplo n.º 12
0
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
Exemplo n.º 13
0
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
Exemplo n.º 14
0
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
Exemplo n.º 15
0
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)
Exemplo n.º 16
0
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
Exemplo n.º 17
0
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)
Exemplo n.º 18
0
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)
Exemplo n.º 19
0
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()
Exemplo n.º 20
0
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))
Exemplo n.º 21
0
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)
Exemplo n.º 22
0
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()
Exemplo n.º 23
0
 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')
Exemplo n.º 24
0
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]
Exemplo n.º 25
0
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)
Exemplo n.º 26
0
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()
Exemplo n.º 27
0
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
Exemplo n.º 28
0
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
Exemplo n.º 29
0
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
Exemplo n.º 30
0
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)