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)
