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)