def cairo_plot_sensor_data(cr, vehicle_state, scale=1.0, compact=True): for attached in vehicle_state['sensors']: sensor = attached['sensor'] observations = attached['current_observations'] sensor_pose = SE2_from_SE3(SE3.from_yaml(attached['current_pose'])) sensor_type = sensor['type'] if sensor_type == VehiclesConstants.SENSOR_TYPE_RANGEFINDER: if compact: with cairo_rototranslate(cr, sensor_pose): cr.scale(scale, scale) plot_ranges_compact(cr=cr, directions=np.array(sensor['directions']), valid=np.array(observations['valid']), readings=np.array(observations['readings']) ) else: plot_ranges(cr=cr, pose=sensor_pose, directions=np.array(sensor['directions']), valid=np.array(observations['valid']), readings=np.array(observations['readings']) ) elif sensor_type == VehiclesConstants.SENSOR_TYPE_PHOTORECEPTORS: if compact: with cairo_rototranslate(cr, sensor_pose): cr.scale(scale, scale) plot_photoreceptors_compact(cr=cr, directions=np.array(sensor['directions']), valid=np.array(observations['valid']), luminance=np.array(observations['luminance'])) else: plot_photoreceptors(cr=cr, pose=sensor_pose, directions=np.array(sensor['directions']), valid=np.array(observations['valid']), readings=np.array(observations['readings']), luminance=np.array(observations['luminance'])) elif sensor_type == VehiclesConstants.SENSOR_TYPE_FIELDSAMPLER: if True: with cairo_rototranslate(cr, sensor_pose): plot_fieldsampler_fancy(cr=cr, positions=np.array(sensor['positions']), sensels=np.array(observations['sensels'])) else: plot_fieldsampler(cr=cr, pose=sensor_pose, positions=np.array(sensor['positions']), sensels=np.array(observations['sensels'])) elif sensor_type == 'RandomSensor': # XXX: how can we visualize this? pass else: logger.warning('Unknown sensor type %r.' % sensor['type']) pass
def cairo_plot_sensor_skins(cr, vehicle_state, scale=1.0): for attached in vehicle_state['sensors']: sensor = attached['sensor'] sensor_pose = SE2_from_SE3(SE3.from_yaml(attached['current_pose'])) extra = attached.get('extra', {}) sensor_skin = extra.get('skin', None) if sensor_skin is None: sensor_skin = sensor['type'] skins_library = get_conftools_skins() if not sensor_skin in skins_library: logger.warning('Could not find skin %r' % sensor_skin) else: skin = skins_library.instance(sensor_skin) with cairo_rototranslate(cr, sensor_pose): cr.scale(scale, scale) skin.draw(cr)