def sensor_process(self, sensor_data):
self.image_rgb = np.transpose(
image_converter.to_rgb_array(sensor_data['CameraRGB']).swapaxes(
0, 1), (1, 0, 2))
self.image_depth = np.transpose(
image_converter.depth_to_logarithmic_grayscale(
sensor_data['CameraDepth']).swapaxes(0, 1), (1, 0, 2))
self.image_depth = np.array(self.image_depth, dtype=np.uint8)
self.image_seg = np.transpose(
image_converter.labels_to_cityscapes_palette(
sensor_data['CameraSemSeg']).swapaxes(0, 1), (1, 0, 2))
self.image_seg = np.array(self.image_seg, dtype=np.uint8)
if self.image_rgb is not None:
image_rgb_frame = CvBridge().cv2_to_imgmsg(self.image_rgb, "rgb8")
self.image_rgb_pub.publish(image_rgb_frame)
if self.image_depth is not None:
image_depth_frame = CvBridge().cv2_to_imgmsg(
self.image_depth, "rgb8")
self.image_depth_pub.publish(image_depth_frame)
if self.image_seg is not None:
image_seg_frame = CvBridge().cv2_to_imgmsg(self.image_seg, "rgb8")
self.image_seg_pub.publish(image_seg_frame)
def _on_render(self):
gap_x = (WINDOW_WIDTH - 2 * MINI_WINDOW_WIDTH) / 3
mini_image_y = WINDOW_HEIGHT - MINI_WINDOW_HEIGHT - gap_x
if self._main_image is not None:
array = image_converter.to_rgb_array(self._main_image)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface, (0, 0))
if self._mini_view_image1 is not None:
array = image_converter.depth_to_logarithmic_grayscale(
self._mini_view_image1)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface, (gap_x, mini_image_y))
if self._mini_view_image2 is not None:
array = image_converter.labels_to_cityscapes_palette(
self._mini_view_image2)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface,
(2 * gap_x + MINI_WINDOW_WIDTH, mini_image_y))
if self._map_view is not None:
array = self._map_view
array = array[:, :, :3]
new_window_width = (float(WINDOW_HEIGHT) / float(
self._map_shape[0])) * float(self._map_shape[1])
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
w_pos = int(self._position[0] *
(float(WINDOW_HEIGHT) / float(self._map_shape[0])))
h_pos = int(self._position[1] *
(new_window_width / float(self._map_shape[1])))
pygame.draw.circle(surface, [255, 0, 0, 255], (w_pos, h_pos), 6, 0)
for agent in self._agent_positions:
if agent.HasField('vehicle'):
agent_position = self._map.get_position_on_map([
agent.vehicle.transform.location.x,
agent.vehicle.transform.location.y,
agent.vehicle.transform.location.z
])
w_pos = int(
agent_position[0] *
(float(WINDOW_HEIGHT) / float(self._map_shape[0])))
h_pos = int(agent_position[1] *
(new_window_width / float(self._map_shape[1])))
pygame.draw.circle(surface, [255, 0, 255, 255],
(w_pos, h_pos), 4, 0)
self._display.blit(surface, (WINDOW_WIDTH, 0))
pygame.display.flip()
def process_sensor_data(self, sensor_data):
_main_image = sensor_data.get('CameraRGB', None)
_mini_view_image1 = sensor_data.get('CameraDepth', None)
_mini_view_image2 = sensor_data.get('CameraSemSeg', None)
t1 = image_converter.to_rgb_array(_main_image)
t2 = np.max(
image_converter.depth_to_logarithmic_grayscale(_mini_view_image1),
axis=2,
keepdims=True)
t3 = np.max(image_converter.to_rgb_array(_mini_view_image2), axis=2)
return [t1, t2, t3]
def _get_camera_images(self):
sensor_data = self._sensor_data
image_object = {
"rgb_center": ic.to_bgra_array(sensor_data.get("RGBCameraCenter", None)),
"rgb_left": ic.to_bgra_array(sensor_data.get("RGBCameraLeft", None)),
"rgb_right": ic.to_bgra_array(sensor_data.get("RGBCameraRight", None)),
"depth": ic.depth_to_logarithmic_grayscale(
sensor_data.get("DepthCamera", None)
),
"sem_seg": ic.labels_to_cityscapes_palette(
sensor_data.get("SemSegCamera", None)
),
}
return image_object
def _on_render(self):
gap_x = (WINDOW_WIDTH - 2 * MINI_WINDOW_WIDTH) / 3
mini_image_y = WINDOW_HEIGHT - MINI_WINDOW_HEIGHT - gap_x
if self._main_image is not None:
array = image_converter.to_rgb_array(self._main_image)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface, (0, 0))
if self._mini_view_image1 is not None:
array = image_converter.depth_to_logarithmic_grayscale(
self._mini_view_image1)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface, (gap_x, mini_image_y))
if self._mini_view_image2 is not None:
array = image_converter.labels_to_cityscapes_palette(
self._mini_view_image2)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface,
(2 * gap_x + MINI_WINDOW_WIDTH, mini_image_y))
if self._lidar_measurement is not None:
lidar_data = np.array(self._lidar_measurement.data[:, :2])
lidar_data *= 2.0
lidar_data += 100.0
lidar_data = np.fabs(lidar_data)
lidar_data = lidar_data.astype(np.int32)
lidar_data = np.reshape(lidar_data, (-1, 2))
#draw lidar
lidar_img_size = (200, 200, 3)
lidar_img = np.zeros(lidar_img_size)
lidar_img[tuple(lidar_data.T)] = (255, 255, 255)
surface = pygame.surfarray.make_surface(lidar_img)
self._display.blit(surface, (10, 10))
if self._map_view is not None:
array = self._map_view
array = array[:, :, :3]
new_window_width = \
(float(WINDOW_HEIGHT) / float(self._map_shape[0])) * \
float(self._map_shape[1])
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
w_pos = int(self._position[0] *
(float(WINDOW_HEIGHT) / float(self._map_shape[0])))
h_pos = int(self._position[1] *
(new_window_width / float(self._map_shape[1])))
pygame.draw.circle(surface, [255, 0, 0, 255], (w_pos, h_pos), 6, 0)
for agent in self._agent_positions:
if agent.HasField('vehicle'):
agent_position = self._map.convert_to_pixel([
agent.vehicle.transform.location.x,
agent.vehicle.transform.location.y,
agent.vehicle.transform.location.z
])
w_pos = int(
agent_position[0] *
(float(WINDOW_HEIGHT) / float(self._map_shape[0])))
h_pos = int(agent_position[1] *
(new_window_width / float(self._map_shape[1])))
pygame.draw.circle(surface, [255, 0, 255, 255],
(w_pos, h_pos), 4, 0)
if agent.HasField('pedestrian'):
agent_position = self._map.convert_to_pixel([
agent.pedestrian.transform.location.x,
agent.pedestrian.transform.location.y,
agent.pedestrian.transform.location.z
])
w_pos = int(
agent_position[0] *
(float(WINDOW_HEIGHT) / float(self._map_shape[0])))
h_pos = int(agent_position[1] *
(new_window_width / float(self._map_shape[1])))
pygame.draw.circle(surface, [0, 0, 0, 0], (w_pos, h_pos),
4, 0)
self._display.blit(surface, (WINDOW_WIDTH, 0))
pygame.display.flip()
def _on_render(self, meas):
gap_x = (WINDOW_WIDTH - 2 * MINI_WINDOW_WIDTH) / 3
mini_image_y = WINDOW_HEIGHT - MINI_WINDOW_HEIGHT - gap_x
#meas.pl
if self._main_image is not None:
array = image_converter.to_rgb_array(self._main_image)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface, (0, 0))
if mixed_reality:
if self._mini_view_image2 is not None:
array1 = image_converter.labels_to_cityscapes_palette(
self._mini_view_image2)
surface1 = pygame.surfarray.make_surface(array1.swapaxes(0, 1))
surface1.set_alpha(128)
self._display.blit(surface1, (0, 0))
else:
if self._mini_view_image1 is not None:
array = image_converter.depth_to_logarithmic_grayscale(
self._mini_view_image1)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface, (gap_x, mini_image_y))
if self._mini_view_image2 is not None:
array = image_converter.labels_to_cityscapes_palette(
self._mini_view_image2)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(
surface, (2 * gap_x + MINI_WINDOW_WIDTH, mini_image_y))
myfont = pygame.font.SysFont("sans", 25, bold=True)
label = myfont.render(("Current Speed: %f km/h" %
meas.player_measurements.forward_speed), 1,
(255, 255, 0))
self._display.blit(label, (WINDOW_WIDTH / 2, WINDOW_HEIGHT - 40))
neighborAgents = [agent for agent in meas.non_player_agents if scipy.spatial.distance.cdist([[meas.player_measurements.transform.location.x, \
meas.player_measurements.transform.location.y]], \
[[agent.vehicle.transform.location.x,agent.vehicle.transform.location.y]]) < 50e2]
myfont = pygame.font.SysFont("sans", 20, bold=True)
for agent in neighborAgents:
if agent.HasField('vehicle'):
# print(agent.vehicle.transform.location)
label = myfont.render(("Vehicle nearby"), 1, (255, 0, 255))
self._display.blit(label, (WINDOW_WIDTH / 2, 40))
if agent.HasField('pedestrian'):
# print(agent.vehicle.transform.location)
label = myfont.render(("Pedestrian nearby"), 1, (255, 0, 255))
self._display.blit(label, (WINDOW_WIDTH / 2, 80))
if self._map_view is not None:
array = self._map_view
array = array[:, :, :3]
new_window_width = (float(WINDOW_HEIGHT) / float(
self._map_shape[0])) * float(self._map_shape[1])
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
w_pos = int(self._position[0] *
(float(WINDOW_HEIGHT) / float(self._map_shape[0])))
h_pos = int(self._position[1] *
(new_window_width / float(self._map_shape[1])))
pygame.draw.circle(surface, [255, 0, 0, 255], (w_pos, h_pos), 6, 0)
for agent in self._agent_positions:
if agent.HasField('vehicle'):
print("found non-player agent")
agent_position = self._map.get_position_on_map([
agent.vehicle.transform.location.x,
agent.vehicle.transform.location.y,
agent.vehicle.transform.location.z
])
w_pos = int(
agent_position[0] *
(float(WINDOW_HEIGHT) / float(self._map_shape[0])))
h_pos = int(agent_position[1] *
(new_window_width / float(self._map_shape[1])))
pygame.draw.circle(surface, [255, 0, 255, 255],
(w_pos, h_pos), 4, 0)
self._display.blit(surface, (WINDOW_WIDTH, 0))
pygame.display.flip()
def _on_render(self):
gap_x = (WINDOW_WIDTH - 2 * MINI_WINDOW_WIDTH) / 3
mini_image_y = WINDOW_HEIGHT - MINI_WINDOW_HEIGHT - gap_x
if self._main_image is not None:
array = image_converter.to_rgb_array(self._main_image)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface, (0, 0))
if self._mini_view_image1 is not None:
array = image_converter.depth_to_logarithmic_grayscale(self._mini_view_image1)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface, (gap_x, mini_image_y))
if self._mini_view_image2 is not None:
array = image_converter.labels_to_cityscapes_palette(
self._mini_view_image2)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(
surface, (2 * gap_x + MINI_WINDOW_WIDTH, mini_image_y))
if self._lidar_measurement is not None:
lidar_points = np.array(self._lidar_measurement.data['points'][:, :, :2])
lidar_points /= 50.0
lidar_points += 100.0
lidar_points = np.fabs(lidar_points)
lidar_points = lidar_points.astype(np.int32)
lidar_points = np.reshape(lidar_points, (-1, 2))
lidar_points_labels = self._lidar_measurement.data['labels']
lidar_points_labels = np.reshape(lidar_points_labels, (-1))
#draw lidar
lidar_img_size = (200, 200, 3)
lidar_img = np.zeros(lidar_img_size)
for class_id, color in image_converter.semantic_segmentation_classes_to_colors.items():
lidar_img[tuple(lidar_points[lidar_points_labels == class_id].T)] = color
surface = pygame.surfarray.make_surface(
lidar_img
)
self._display.blit(surface, (10, 10))
if self._map_view is not None:
array = self._map_view
array = array[:, :, :3]
new_window_width =(float(WINDOW_HEIGHT)/float(self._map_shape[0]))*float(self._map_shape[1])
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
w_pos = int(self._position[0]*(float(WINDOW_HEIGHT)/float(self._map_shape[0])))
h_pos =int(self._position[1] *(new_window_width/float(self._map_shape[1])))
pygame.draw.circle(surface, [255, 0, 0, 255], (w_pos,h_pos), 6, 0)
for agent in self._agent_positions:
if agent.HasField('vehicle'):
agent_position = self._map.get_position_on_map([
agent.vehicle.transform.location.x,
agent.vehicle.transform.location.y,
agent.vehicle.transform.location.z])
w_pos = int(agent_position[0]*(float(WINDOW_HEIGHT)/float(self._map_shape[0])))
h_pos =int(agent_position[1] *(new_window_width/float(self._map_shape[1])))
pygame.draw.circle(surface, [255, 0, 255, 255], (w_pos,h_pos), 4, 0)
self._display.blit(surface, (WINDOW_WIDTH, 0))
pygame.display.flip()
def get_obs(self):
self._timer.tick()
measurements, sensor_data = self.client.read_data()
self.auto_control = measurements.player_measurements.autopilot_control
self._main_image = sensor_data.get('CameraForHuman', None)
rgb_image = sensor_data.get('CameraRGB', None)
depth_image = sensor_data.get('CameraDepth', None)
rgb_image = image_converter.to_rgb_array(rgb_image)
rgb_image = (rgb_image.astype(np.float32) - 128) / 128
depth_image = (
np.max(image_converter.depth_to_logarithmic_grayscale(depth_image),
axis=2,
keepdims=True) - 128) / 128
image = np.concatenate([rgb_image, depth_image], axis=2)
if self.prev_image is None:
self.prev_image = image
now_image = image
image = np.concatenate([self.prev_image, image], axis=2)
self.prev_image = now_image
collision = self.get_collision(measurements)
control, reward = self._get_keyboard_control(pygame.key.get_pressed())
reward, _ = self.calculate_reward(measurements, reward, collision)
# Print measurements every second.
if self._timer.elapsed_seconds_since_lap() > 1.0:
if self._city_name is not None:
# Function to get car position on map.
map_position = self._map.convert_to_pixel([
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
])
# Function to get orientation of the road car is in.
lane_orientation = self._map.get_lane_orientation([
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
])
self._print_player_measurements_map(
measurements.player_measurements, map_position,
lane_orientation, reward)
else:
self._print_player_measurements(
measurements.player_measurements)
# Plot position on the map as well.
self._timer.lap()
# Set the player position
if self._city_name is not None:
self._position = self._map.convert_to_pixel([
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
])
self._agent_positions = measurements.non_player_agents
self.step += 1
done = False
if self.step > STEP_LIMIT:
done = True
self.step = 0
return ((image,
[measurements.player_measurements.forward_speed * 3.6 / 100]),
reward, done)
def _on_render(self):
gap_x = (WINDOW_WIDTH - 2 * MINI_WINDOW_WIDTH) / 3
mini_image_y = WINDOW_HEIGHT - MINI_WINDOW_HEIGHT - gap_x
if self._main_image is not None:
array = image_converter.to_rgb_array(self._main_image)
################################################################################################ SAVING IMAGE DATA IN %SELF._DATA%
#print(self._velocity)
#print("\n")
if self._velocity > 5 and self._val3 == 1: ## YOU CAN CHANGE THIS ACCORDING TO YOU
print("\nimage is saved\n")
self._data['data{}_angle_{}_throttle_{}_speed_{}'.format(
self._i, format(self._val1, '.2f'),
format(self._val2, '.2f'), format(self._velocity,
'.2f'))] = array
self._i += 1 ##FRAME NUMBER INCREASE BY ONE
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface, (0, 0))
if self._mini_view_image1 is not None:
array = image_converter.depth_to_logarithmic_grayscale(
self._mini_view_image1)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface, (gap_x, mini_image_y))
if self._map_view is not None:
array = self._map_view
array = array[:, :, :3]
new_window_width = \
(float(WINDOW_HEIGHT) / float(self._map_shape[0])) * \
float(self._map_shape[1])
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
w_pos = int(self._position[0] *
(float(WINDOW_HEIGHT) / float(self._map_shape[0])))
h_pos = int(self._position[1] *
(new_window_width / float(self._map_shape[1])))
pygame.draw.circle(surface, [255, 0, 0, 255], (w_pos, h_pos), 6, 0)
for agent in self._agent_positions:
if agent.HasField('vehicle'):
agent_position = self._map.convert_to_pixel([
agent.vehicle.transform.location.x,
agent.vehicle.transform.location.y,
agent.vehicle.transform.location.z
])
w_pos = int(
agent_position[0] *
(float(WINDOW_HEIGHT) / float(self._map_shape[0])))
h_pos = int(agent_position[1] *
(new_window_width / float(self._map_shape[1])))
pygame.draw.circle(surface, [255, 0, 255, 255],
(w_pos, h_pos), 4, 0)
self._display.blit(surface, (WINDOW_WIDTH, 0))
pygame.display.flip()
def record_train_data(measurements,sensor_data):
## Collect sensordata->sensors
rgb_array = np.uint8(sensor_data['CameraRGB'].data) # (88*200*3)(宽×高×通道)
rgb_array = rgb_array[115:510,:]
rgb_array = scipy.misc.imresize(rgb_array,[88,200])
seg_array = sensor_data.get('CameraSemSeg', None).data
depth_array = sensor_data.get('CameraDepth', None)
depth_array = image_converter.depth_to_logarithmic_grayscale(depth_array)
depth_array = depth_array[:,:,0]
lidar_measurement = sensor_data.get('Lidar32', None)
lidar_data = np.array(lidar_measurement.data[:, :2])
lidar_data *= 2.0
lidar_data += 100.0
lidar_data = np.fabs(lidar_data)
lidar_data = lidar_data.astype(np.int32)
lidar_data = np.reshape(lidar_data, (-1, 2))
# draw lidar
lidar_img_size = (200, 200)
lidar_img = np.zeros(lidar_img_size)
lidar_img[tuple(lidar_data.T)] = 255
sensors = {'rgb':rgb_array, 'CameraSemSeg':seg_array, 'CameraDepth':depth_array,'Lidar32':lidar_img}
## collect measurementdata->targets
player_measurements = measurements.player_measurements
control = measurements.player_measurements.autopilot_control
steer = control.steer
throttle = control.throttle
brake = control.brake
hand_brake = control.hand_brake
reverse = control.reverse
steer_noise = 0
gas_noise = 0
brake_noise = 0
pos_x = player_measurements.transform.location.x
pos_y = player_measurements.transform.location.y
speed = player_measurements.forward_speed * 3.6 # m/s -> km/h
col_other = player_measurements.collision_other
col_ped = player_measurements.collision_pedestrians
col_cars = player_measurements.collision_vehicles
other_lane = 100 * player_measurements.intersection_otherlane
offroad = 100 * player_measurements.intersection_offroad
acc_x = player_measurements.acceleration.x
acc_y = player_measurements.acceleration.y
acc_z = player_measurements.acceleration.z
platform_time = measurements.platform_timestamp
game_time = measurements.game_timestamp
orientation_x = player_measurements.transform.orientation.x
orientation_y = player_measurements.transform.orientation.y
orientation_z = player_measurements.transform.orientation.z
command = 2.0
noise = 0
camera = 0
angle = 0
targets = [steer,throttle,brake,hand_brake,reverse,steer_noise,gas_noise,brake_noise,
pos_x,pos_y,speed,col_other,col_ped,col_cars,other_lane,offroad,
acc_x,acc_y,acc_z,platform_time,game_time,orientation_x,orientation_y,orientation_z,
command,noise,camera,angle]
return sensors,targets
def _on_render(self):
measurements, sensor_data = self.client.read_data()
temp_array = np.zeros((240, 320, 1))
gap_x = (WINDOW_WIDTH - 2 * MINI_WINDOW_WIDTH) / 3
mini_image_y = WINDOW_HEIGHT - MINI_WINDOW_HEIGHT - gap_x
# array = image_converter.labels_to_cityscapes_palette(self._mini_view_image2)
# sem_return = array
# print("sem return is " + str(np.shape(np.array(sem_return))))
if self._main_image is not None:
array = image_converter.to_rgb_array(self._main_image)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface, (0, 0))
if self._mini_view_image1 is not None:
array = image_converter.depth_to_logarithmic_grayscale(
self._mini_view_image1)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface, (gap_x, mini_image_y))
if self._mini_view_image2 is not None:
# print("Entered")
array = image_converter.labels_to_cityscapes_palette(
self._mini_view_image2)
# temp_array = array[:,:,0]
# plt.imshow(temp_array[:,:,0])
if self._enable_imitation:
print("Size of input is " + str(np.shape(np.asarray(array))))
control = measurements.player_measurements.autopilot_control
# img = np.asarray(array[:,:,0])
img = array
# np.reshape(img, img.shape + (1,))
print("Shape of img is " + str(np.shape(img)))
# plt.imshow(img[:,:,0])
# plt.show()
img = cv2.resize(img[:, :, :], (320, 240))
img = img.reshape(1, 240, 320, 3)
print("Shape of img is " + str(np.shape(img)))
img = img[:, :, :, 0]
img = img.reshape(1, 240, 320, 1)
print("Shape of img is " + str(np.shape(img)))
plt.imshow(img[0, :, :, 0])
# # try:
control.steer = float(
self.model.predict(img[:, :, :], batch_size=1)) / 10.0
control.throttle = 0.5
# # except:
# # control.steer = 0.0
print("Steer is " + str(control.steer))
self.client.send_control(control)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface,
(2 * gap_x + MINI_WINDOW_WIDTH, mini_image_y))
if self._lidar_measurement is not None:
lidar_data = np.array(self._lidar_measurement.data[:, :2])
lidar_data *= 2.0
lidar_data += 100.0
lidar_data = np.fabs(lidar_data)
lidar_data = lidar_data.astype(np.int32)
lidar_data = np.reshape(lidar_data, (-1, 2))
#draw lidar
lidar_img_size = (200, 200, 3)
lidar_img = np.zeros(lidar_img_size)
lidar_img[tuple(lidar_data.T)] = (255, 255, 255)
surface = pygame.surfarray.make_surface(lidar_img)
self._display.blit(surface, (10, 10))
if self._map_view is not None:
array = self._map_view
array = array[:, :, :3]
new_window_width = \
(float(WINDOW_HEIGHT) / float(self._map_shape[0])) * \
float(self._map_shape[1])
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
w_pos = int(self._position[0] *
(float(WINDOW_HEIGHT) / float(self._map_shape[0])))
h_pos = int(self._position[1] *
(new_window_width / float(self._map_shape[1])))
pygame.draw.circle(surface, [255, 0, 0, 255], (w_pos, h_pos), 6, 0)
for agent in self._agent_positions:
if agent.HasField('vehicle'):
agent_position = self._map.convert_to_pixel([
agent.vehicle.transform.location.x,
agent.vehicle.transform.location.y,
agent.vehicle.transform.location.z
])
w_pos = int(
agent_position[0] *
(float(WINDOW_HEIGHT) / float(self._map_shape[0])))
h_pos = int(agent_position[1] *
(new_window_width / float(self._map_shape[1])))
pygame.draw.circle(surface, [255, 0, 255, 255],
(w_pos, h_pos), 4, 0)
self._display.blit(surface, (WINDOW_WIDTH, 0))
pygame.display.flip()
def record_train_data(measurements, sensor_data):
## Collect sensordata->sensors
rgb_array = np.uint8(sensor_data['CameraRGB'].data) # (88*200*3)(宽×高×通道)
rgb_array = rgb_array[115:510, :]
rgb_array = scipy.misc.imresize(rgb_array, [88, 200])
seg_array = sensor_data.get('CameraSemSeg', None).data
depth_array = sensor_data.get('CameraDepth', None)
depth_array = image_converter.depth_to_logarithmic_grayscale(depth_array)
depth_array = depth_array[:, :, 0]
lidar_measurement = sensor_data.get('Lidar32', None)
lidar_data = np.array(lidar_measurement.data[:, :2])
lidar_data *= 2.0
lidar_data += 100.0
lidar_data = np.fabs(lidar_data)
lidar_data = lidar_data.astype(np.int32)
lidar_data = np.reshape(lidar_data, (-1, 2))
# draw lidar
lidar_img_size = (200, 200)
lidar_img = np.zeros(lidar_img_size)
lidar_img[tuple(lidar_data.T)] = 255
sensors = {
'rgb': rgb_array,
'CameraSemSeg': seg_array,
'CameraDepth': depth_array,
'Lidar32': lidar_img
}
## collect measurementdata->targets
player_measurements = measurements.player_measurements
control = measurements.player_measurements.autopilot_control
steer = control.steer
throttle = control.throttle
brake = control.brake
hand_brake = control.hand_brake
reverse = control.reverse
steer_noise = 0
gas_noise = 0
brake_noise = 0
pos_x = player_measurements.transform.location.x
pos_y = player_measurements.transform.location.y
speed = player_measurements.forward_speed * 3.6 # m/s -> km/h
col_other = player_measurements.collision_other
col_ped = player_measurements.collision_pedestrians
col_cars = player_measurements.collision_vehicles
other_lane = 100 * player_measurements.intersection_otherlane
offroad = 100 * player_measurements.intersection_offroad
acc_x = player_measurements.acceleration.x
acc_y = player_measurements.acceleration.y
acc_z = player_measurements.acceleration.z
platform_time = measurements.platform_timestamp
game_time = measurements.game_timestamp
orientation_x = player_measurements.transform.orientation.x
orientation_y = player_measurements.transform.orientation.y
orientation_z = player_measurements.transform.orientation.z
command = 2.0
noise = 0
camera = 0
angle = 0
targets = [
steer, throttle, brake, hand_brake, reverse, steer_noise, gas_noise,
brake_noise, pos_x, pos_y, speed, col_other, col_ped, col_cars,
other_lane, offroad, acc_x, acc_y, acc_z, platform_time, game_time,
orientation_x, orientation_y, orientation_z, command, noise, camera,
angle
]
return sensors, targets
array = image_converter.to_rgb_array(imagem_camera1)
surface = pygame.surfarray.make_surface(
array.swapaxes(0, 1))
display.blit(surface, (0, 70))
imagem_camera2 = sensor_data.get("Camera2", None)
if imagem_camera2 is not None:
array = image_converter.labels_to_cityscapes_palette(
imagem_camera2)
surface = pygame.surfarray.make_surface(
array.swapaxes(0, 1))
display.blit(surface, (500, 70))
imagem_camera3 = sensor_data.get("Camera3", None)
if imagem_camera3 is not None:
array = image_converter.depth_to_logarithmic_grayscale(
imagem_camera3)
surface = pygame.surfarray.make_surface(
array.swapaxes(0, 1))
display.blit(surface, (0, 570))
lidar_measurement = sensor_data.get('Lidar32', None)
if lidar_measurement is not None:
lidar_data = np.array(lidar_measurement.data[:, :2])
lidar_data *= 2.0
lidar_data += 100.0
lidar_data = np.fabs(lidar_data)
lidar_data = lidar_data.astype(np.int32)
lidar_data = np.reshape(lidar_data, (-1, 2))
#draw lidar
lidar_img_size = (500, 500, 3)
lidar_img = np.zeros(lidar_img_size)
