def show_sensor_data(pylab, vehicle, robot_pose=None, col='r'):
if robot_pose is None:
robot_pose = SE3.from_yaml(vehicle['pose'])
for attached in vehicle['sensors']:
sensor_pose = from_yaml(attached['current_pose'])
sensor_t, sensor_theta = \
translation_angle_from_SE2(SE2_from_SE3(sensor_pose))
print('robot: %s' % SE2.friendly(SE2_from_SE3(robot_pose)))
print(' sens: %s' % SE2.friendly(SE2_from_SE3(sensor_pose)))
# sensor_theta = -sensor_theta
sensor = attached['sensor']
if sensor['type'] == 'Rangefinder':
directions = np.array(sensor['directions'])
observations = attached['current_observations']
readings = np.array(observations['readings'])
valid = np.array(observations['valid'])
# directions = directions[valid]
# readings = readings[valid]
x = []
y = []
rho_min = 0.05
for theta_i, rho_i in zip(directions, readings):
print('theta_i: %s' % theta_i)
x.append(sensor_t[0] +
np.cos(sensor_theta + theta_i) * rho_min)
y.append(sensor_t[1] +
np.sin(sensor_theta + theta_i) * rho_min)
x.append(sensor_t[0] +
np.cos(sensor_theta + theta_i) * rho_i)
y.append(sensor_t[1] +
np.sin(sensor_theta + theta_i) * rho_i)
x.append(None)
y.append(None)
pylab.plot(x, y, color=col, markersize=0.5, zorder=2000)
elif sensor['type'] == 'Photoreceptors':
directions = np.array(sensor['directions'])
observations = attached['current_observations']
readings = np.array(observations['readings'])
luminance = np.array(observations['luminance'])
valid = np.array(observations['valid'])
readings[np.logical_not(valid)] = 0.6
rho_min = 0.5
for theta_i, rho_i, lum in zip(directions, readings, luminance):
x = []
y = []
x.append(sensor_t[0] +
np.cos(sensor_theta + theta_i) * rho_min)
y.append(sensor_t[1] +
np.sin(sensor_theta + theta_i) * rho_min)
x.append(sensor_t[0] +
np.cos(sensor_theta + theta_i) * rho_i)
y.append(sensor_t[1] +
np.sin(sensor_theta + theta_i) * rho_i)
pylab.plot(x, y, color=(lum, lum, lum),
markersize=0.5, zorder=2000)
else:
print('Unknown sensor type %r' % sensor['type'])
def from_yaml(self, yaml_structure):
''' Recovers a value from a Yaml structure. '''
# TODO: explicit check
# TODO: add testing
from geometry.yaml import from_yaml
return from_yaml(yaml_structure)
