class CBCDisplayDist(Block): Block.alias('cbc_display_dist') Block.config('width', 'Image dimension', default=320) Block.config('height', 'Image dimension', default=320) Block.input('res', 'Results dictionary') Block.output('rgb') def init(self): self.plot_generic = PlotGeneric(width=self.config.width, height=self.config.height, transparent=False, tight=False, keep=True) self.plot_anim = PlotAnim() def update(self): self.output.rgb = self.plot_generic.get_rgb(self.plot) def plot(self, pylab): res = self.input.res S = res['S'] self.info(S.shape) Sx = S[0, :] Sy = S[1, :] # Sz = S[2, :] self.plot_anim.set_pylab(pylab) self.plot_anim.plot('Sxy', Sx, Sy, 'ko') pylab.axis((-1, +1, -1, +1)) turn_off_all_axes(pylab)
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
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 ServoStatus(Block): Block.alias('servo_status') Block.config('width', 'Image dimension', default=320) Block.config('height', 'Image dimension', default=240) Block.config('style', 'Style (0=lines, 1=points)', default=0) Block.input('y') Block.input('y_goal') Block.input('state') Block.output('rgb') def init(self): self.plot_generic = PlotGeneric(width=self.config.width, height=self.config.height, transparent=False, tight=False, keep=True) self.first_timestamp = None self.plot_anim = PlotAnim() 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.output.rgb = self.plot_generic.get_rgb(self.plot) def plot(self, pylab): y = data_from_msg(self.input.y) y_goal = data_from_msg(self.input.y_goal) sensels = np.array(range(y.size)) self.plot_anim.set_pylab(pylab) M = 0.1 y_min = 0 y_max = 1 if self.config.style == 0: y_style = 'k-' y_goal_style = 'g-' elif self.config.style == 1: y_style = 'ko' y_goal_style = 'go' else: raise ValueError(self.config.style) self.plot_anim.plot('y_goal', sensels, y_goal, y_goal_style) self.plot_anim.plot('y', sensels, y, y_style) n = y.size border = n / 100.0 pylab.axis((-border, n - 1 + border, y_min - M, y_max + M)) turn_off_all_axes(pylab) # state = self.input.state.data # self.plot_anim.text('state', 1, 0.7, state) self.plot_anim.text('clock', 0, 1, '%5.2f' % self.time_since_start)
def init(self): self.plot_generic = PlotGeneric(width=self.config.width, height=self.config.height, transparent=False, tight=False, keep=True) self.plot_anim = PlotAnim()
def init(self): self.plot_generic = PlotGeneric(width=self.config.width, height=self.config.height, transparent=False, tight=False, keep=True) self.plot_anim = PlotAnim() self.timestamps = [] self.ys = [] self.y_goals = [] self.y_goal = None self.servo_state = STATE_WAIT self.first_timestamp = None self.plot_line = None
class ServoError(Block): """ Captures the error since the start of the data. """ Block.alias('servo_error') Block.config('width', 'Image dimension', default=320) Block.config('height', 'Image dimension', default=240) Block.config('use_first_y_goal', default=True) Block.input('y') Block.input('y_goal') Block.input('state') Block.output('rgb') def init(self): self.plot_generic = PlotGeneric(width=self.config.width, height=self.config.height, transparent=False, tight=False, keep=True) self.plot_anim = PlotAnim() self.timestamps = [] self.ys = [] self.y_goals = [] self.y_goal = None self.servo_state = STATE_WAIT self.first_timestamp = None self.plot_line = None def update_values(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 if isinstance(self.input.state, str): state = self.input.state else: state = self.input.state.data assert state in [STATE_WAIT, STATE_SERVOING] self.y_goal = data_from_msg(self.input.y_goal) y = data_from_msg(self.input.y) assert self.y_goal.shape == y.shape timestamp = self.get_input_timestamp(0) if self.servo_state == STATE_WAIT and state == STATE_SERVOING: # we started now self.timestamps = [] self.ys = [] self.y_goals = [] self.info('%s: Started servoing' % self.time_since_start) if self.servo_state == STATE_SERVOING and state == STATE_WAIT: self.info('%s: Stopped servoing' % self.time_since_start) self.timestamps = [] self.ys = [] self.y_goals = [] # only add values if we are servoing if state == STATE_SERVOING: self.timestamps.append(timestamp) self.ys.append(y) self.y_goals.append(self.y_goal) self.servo_state = state self.timestamp = timestamp def get_relative_times(self): T = np.array(self.timestamps) if T.size > 0: T -= T[0] return T @contract(y0='array[N]', y1='array[N]') def metric(self, y0, y1): return np.linalg.norm(y0 - y1) def get_errors(self): if self.config.use_first_y_goal: es = [self.metric(self.y_goal, y) for y in self.ys] else: es = [self.metric(y_goal, y) for y_goal, y in zip(self.y_goals, self.ys)] return np.array(es) def get_relative_errors(self): es = self.get_errors() if es.size > 0: es = es / es[0] return es def update(self): self.update_values() self.output.rgb = self.plot_generic.get_rgb(self.plot) def get_T(self, duration, chunk, min_time): duration = max(duration, min_time) T = np.ceil(duration / chunk) * chunk return T def plot(self, pylab): ts = self.get_relative_times() es = self.get_relative_errors() assert ts.size == es.size self.plot_anim.set_pylab(pylab) if ts.size > 0: T = self.get_T(duration=(ts[-1] - ts[0]), chunk=5, min_time=15) assert T >= ts[-1] else: T = 10 if self.plot_line is None: ax1 = pylab.gca() ax1.axes.get_xaxis().set_visible(False) ax1.axes.get_yaxis().set_visible(False) self.plot_line = True border = 2 if self.config.use_first_y_goal: pylab.axis((-1, T + border, -0.1, 1.3)) else: x_axis_set(pylab, -1, T + border) if es.size > 0: y_axis_set(pylab, -0.1, np.max(es) * 1.3) # s = '%s %s' % (self.servo_state, self.timestamp) # self.plot_anim.text('status', 0.7 * T, 1.2, s) # self.plot_anim.text('clock', 0.7 * T, 1.2, '%5.2f' % self.time_since_start) self.plot_anim.plot('error', ts, es, 'k-') if es.size > 0: self.plot_anim.plot('error1', ts[0], es[0], 'rs') self.plot_anim.plot('error2', ts[-1], es[-1], 'rs') else: self.plot_anim.plot('error1', [], [], 'ro') self.plot_anim.plot('error2', [], [], 'ro') self.plot_anim.plot('zero', [0, T], [0, 0], 'k--') if self.servo_state == STATE_SERVOING: # self.info('Servoing') pass else: pass