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.to_rgb_array(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.to_rgb_array(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.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 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 _prepare_video_images(self):
speed = int(self._measurements.player_measurements.forward_speed * 3.6)
speed = "{} km/h".format(speed)
speed_limit = "{} km/h".format(self._current_speed_limit)
traffic_light = self._current_traffic_light[0].name
current_hlc = (
self._current_hlc.name if self._drive_model_enabled else "Disabled"
)
info = [speed, speed_limit, traffic_light, current_hlc]
self._video_info.append(info)
self._video_images[0].append(ic.to_rgb_array(self._game_image))
self._video_images[1].append(ic.to_rgb_array(self._game_image_3p))
def _on_render(self):
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))
pygame.display.flip()
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 update_distances(self, msg):
rgbd_frame = to_rgb_array(msg.data)
normalized_distances = np.dot(
rgbd_frame,
[1.0, 256.0, 256.0 * 256.0]) / (256.0 * 256.0 * 256.0 - 1.0)
# Compute distances in meters.
distances = self._rgbd_max_range * normalized_distances
self._distances.append((msg.timestamp, distances))
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._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 _on_render(self):
gap_x = (WINDOW_WIDTH - 2 * MINI_WINDOW_WIDTH) / 3
mini_image_y = WINDOW_HEIGHT - MINI_WINDOW_HEIGHT - gap_x
global COUNT
global frame
#COUNT+=1
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((COUNT%frame)==0):
pygame.image.save(surface,"E:/RGB/rgb"+format(COUNT)+".png")
# print("RGB saved")
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))
if(COUNT%frame==0):
pygame.image.save(surface,"E:/Semanticdata/Semantic"+format(COUNT)+".png")
# print("Semantic saved")
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.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 _render_pygame(self):
if self._game_image is not None:
array = ic.to_rgb_array(self._game_image)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
if self._game_state == GameState.WRITING:
self._render_progressbar(
surface, 300, 40, self._disk_writer_thread.progress
)
self._pygame_display.blit(surface, (0, 0))
self._render_HUD()
pygame.display.flip()
def _save_training_files(self, datapoints, point_cloud):
logging.info(
"Attempting to save at timer step {}, frame no: {}".format(
self._timer.step, self.captured_frame_no))
groundplane_fname = GROUNDPLANE_PATH.format(self.captured_frame_no)
lidar_fname = LIDAR_PATH.format(self.captured_frame_no)
kitti_fname = LABEL_PATH.format(self.captured_frame_no)
img_fname = IMAGE_PATH.format(self.captured_frame_no)
calib_filename = CALIBRATION_PATH.format(self.captured_frame_no)
save_groundplanes(groundplane_fname,
self._measurements.player_measurements,
LIDAR_HEIGHT_POS)
save_ref_files(OUTPUT_FOLDER, self.captured_frame_no)
save_image_data(img_fname,
image_converter.to_rgb_array(self._main_image))
save_kitti_data(kitti_fname, datapoints)
save_lidar_data(lidar_fname, point_cloud, LIDAR_HEIGHT_POS,
LIDAR_DATA_FORMAT)
save_calibration_matrices(calib_filename, self._intrinsic,
self._extrinsic)
def yang_annotate_images(self, sensor_data, directions):
if self._save_images:
out = {}
for name, image in sensor_data.items():
if isinstance(image, ImageSensorData):
numpy_image = image_converter.to_rgb_array(image)
txtdt = {2.0: "follow",
3.0: "left",
4.0: "right",
5.0: "straight",
0.0: "goal"}
viz = self.write_text_on_image(numpy_image, txtdt[directions], 10)
# convert the PIL image object to the ImageSensorData
viz = viz.convert("RGBA")
viz = ImageSensorData(image.frame_number,
image.width,
image.height,
image.type,
image.fov,
viz.tobytes())
out[name] = viz
else:
out[name] = image
return out
def run_carla_client(args):
with make_carla_client(args.host, args.port) as client:
print('CarlaClient connected')
while True:
settings = CarlaSettings()
settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=0,
NumberOfPedestrians=0,
WeatherId=3, #1, 3, 7, 8, 14
QualityLevel=args.quality_level)
settings.randomize_seeds()
# Now we want to add a couple of cameras to the player vehicle.
# We will collect the images produced by these cameras every
# frame.
# The default camera captures RGB images of the scene.
camera0 = sensor.Camera('CameraRGB', PostProcessing='SceneFinal')
# Set image resolution in pixels.
w = 800
h = 600
camera0.set_image_size(800, 600)
# Set its position relative to the car in meters.
camera0.set_position(0.30, 0, 4)
settings.add_sensor(camera0)
# Let's add another camera producing ground-truth depth.
depth_camera = sensor.Camera('CameraDepth', PostProcessing='Depth')
depth_camera.set_image_size(800, 600)
depth_camera.set_position(x=0.30, y=0, z=4)
settings.add_sensor(depth_camera)
if args.lidar:
lidar = Lidar('Lidar32')
lidar.set_position(0, 0, 2.50)
lidar.set_rotation(0, 0, 0)
lidar.set(Channels=32,
Range=50,
PointsPerSecond=100000,
RotationFrequency=10,
UpperFovLimit=10,
LowerFovLimit=-30)
settings.add_sensor(lidar)
scene = client.load_settings(settings)
# Choose one player start at random.
number_of_player_starts = len(scene.player_start_spots)
player_start = random.randint(0, max(0,
number_of_player_starts - 1))
print('Starting new episode at %r...' % scene.map_name)
client.start_episode(player_start)
action_model = roughPPO.actor_model((h, w, 3), (3, 1), (20, 2))
critic_model = roughPPO.critic_model((h, w, 3), (3, 1), (20, 2),
(2, 1))
actor_action = [0, 0]
critic_score = 0
ppo_check = 0
# Iterate every frame in the episode.
ppo_iter_count = 0
while True:
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
# print_measurements(measurements)
main_image = sensor_data.get("CameraRGB", None)
image_array = image_converter.to_rgb_array(main_image)
point_cloud = image_converter.depth_to_local_point_cloud(
sensor_data['CameraDepth'], image_array, max_depth=1000)
image_array = cv2.cvtColor(image_array, cv2.COLOR_RGB2BGR)
cv2.imshow("FrontCam View", image_array)
reverse = 0
try:
steer_val = 0
bez_mask, coord_trajectory, trajectory, world_x_left, world_x_right, world_z_left, world_z_right = floodfill(
image_array, point_cloud)
track_point_yaw = trajectory[-5]
yaw_error = np.arctan(
(0 - track_point_yaw[0]) / (track_point_yaw[1]))
state_update_long(trajectory)
state_update_lat(world_x_left, world_x_right, yaw_error)
if ppo_check == 1:
reward_score = roughPPO.reward(actor_action,
world_x_left,
world_x_right)
if len(SDC_data_store.throttle_queue) < 50:
SDC_data_store.throttle_queue.append(
actor_action[0])
SDC_data_store.steer_queue.append(actor_action[1])
SDC_data_store.rewards_queue.append(reward_score)
SDC_data_store.critic_score.append(critic_score)
else:
print("IDHAR!!!!!!!!!!")
SDC_data_store.throttle_queue.popleft()
SDC_data_store.throttle_queue.append(
actor_action[0])
SDC_data_store.steer_queue.popleft()
SDC_data_store.steer_queue.append(actor_action[1])
SDC_data_store.rewards_queue.popleft()
SDC_data_store.rewards_queue.append(reward_score)
SDC_data_store.critic_score.popleft()
SDC_data_store.critic_score.append(reward_score)
if (len(SDC_data_store.rewards_queue) > 1):
returns, advs = roughPPO.advantages(
list(SDC_data_store.critic_score),
list(SDC_data_store.rewards_queue))
loss = roughPPO.ppo_loss(
advs, SDC_data_store.rewards_queue,
SDC_data_store.critic_score)
action_model.compile(optimizer=Adam(lr=1e-3),
loss=loss)
critic_model.compile(optimizer=Adam(lr=1e-3),
loss='mse')
print("LOSS : " + str(loss))
ppo_check = 0
if ppo_iter_count % 20 == 0 and ppo_iter_count > 0 and ppo_check == 0:
ppo_check = 1
action_output, critic_output = ppo_input(
measurements, trajectory, image_array,
action_model, critic_model)
actor_action = action_output[0]
critic_score = critic_output[0][0]
throttle_a = actor_action[0]
steer_val = actor_action[1]
else:
throttle_a = PID_throttle(trajectory, reverse)
cv2.imshow("bez", bez_mask)
steer_val = PID_steer(trajectory, reverse,
world_x_left, world_x_right)
check = SDC_data_store.shadow_check
client.send_control(steer=steer_val * check,
throttle=throttle_a * check + 0.5 *
(1 - check),
brake=0.0,
hand_brake=False,
reverse=False)
SDC_data_store.turn_value = steer_val
except Exception as e:
print(e)
client.send_control(steer=SDC_data_store.turn_value *
check,
throttle=1,
brake=0.85,
hand_brake=True,
reverse=False)
cv2.waitKey(1)
if SDC_data_store.count % 10 == 0:
print("frame: ", SDC_data_store.count)
# if SDC_data_store.count%10==0:
# SDC_data_store.sum_cte = 0
SDC_data_store.count += 1
print("*********\n")
ppo_iter_count += 1
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_loop(self):
self._timer.tick()
measurements, sensor_data = self.client.read_data()
self._main_image = sensor_data.get('CameraRGB_main', None)
self._mini_view_image1 = sensor_data.get('CameraDepth', None)
self._mini_view_image2 = sensor_data.get('CameraSemSeg', None)
self._lidar_measurement = sensor_data.get('Lidar32', None)
self._h5_image = sensor_data.get('CameraRGB_h5', None)
timediff = self._timer.elapsed_seconds_since_lap()
# Print measurements every second.
if timediff > 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)
else:
self._print_player_measurements(measurements.player_measurements)
# Plot position on the map as well.
self._timer.lap()
control = self._get_keyboard_control(pygame.key.get_pressed(), measurements, sensor_data)
# 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
#print(control)
if control is None:
self._on_new_episode()
elif self._enable_autopilot:
self.client.send_control(measurements.player_measurements.autopilot_control)
else:
self.client.send_control(control)
### Saving on autopilot ###
if (self._enable_autopilot) and (timediff > 0.03) and (self._h5_image is not None):
steer = measurements.player_measurements.autopilot_control.steer
throttle = measurements.player_measurements.autopilot_control.steer
brake = measurements.player_measurements.autopilot_control.brake
hand_brake = measurements.player_measurements.autopilot_control.hand_brake
reverse = measurements.player_measurements.autopilot_control.reverse
s_image = image_converter.to_rgb_array(self._h5_image)
s_measurements = [
steer,
throttle,
brake,
hand_brake,
reverse,
0.0,
0.0,
0.0,
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.forward_speed,
measurements.player_measurements.collision_other,
measurements.player_measurements.collision_pedestrians,
measurements.player_measurements.collision_vehicles,
measurements.player_measurements.intersection_otherlane,
measurements.player_measurements.intersection_offroad,
measurements.player_measurements.acceleration.x,
measurements.player_measurements.acceleration.y,
measurements.player_measurements.acceleration.z,
measurements.platform_timestamp,
measurements.game_timestamp,
measurements.player_measurements.transform.orientation.x,
measurements.player_measurements.transform.orientation.y,
measurements.player_measurements.transform.orientation.z,
-1,
0.0,
0.0,
0.0,
]
if brake == 1.0:
self.brake_count += 1
else:
self.brake_count = 0
# Need to handle intersection data separately
# When stopping at stop light, labels can be random
# When turning, driving labels need to be populated retrospectively
# For lane following, just populate in large batch
if self._at_intersection:
if self._drivestate == 0:
self.saveh5s()
#print("Saved command {}".format(2))
self._drivestate = 1
self._save_holder.append([copy.deepcopy(s_image), copy.deepcopy(s_measurements)])
self._save_holder_stop.append([copy.deepcopy(s_image), copy.deepcopy(s_measurements)])
if self.brake_count > 3:
#print("Stopped at light")
self._save_holder[-1][1][24] = -2
self._save_holder_stop[-1][1][24] = -2
elif steer < -0.01:
#print("Turning left")
self._cmd = 3
elif steer > 0.01:
#print("Turning right")
self._cmd = 4
else:
#print("Going straight")
pass
else:
if self._drivestate == 1:
for i in range(len(self._save_holder)):
if self._save_holder[i][1][24] == -1:
self._save_holder[i][1][24] = self._cmd
self._save_holder_stop[i][1][24] = self._cmd
else:
self._save_holder[i][1][24] = self._cmd
self._save_holder_stop[i][1][24] = 6
self.saveh5s()
#print("Saved command {}".format(self._cmd))
self._cmd = 5
self._drivestate = 0
self._save_holder.append([copy.deepcopy(s_image), copy.deepcopy(s_measurements)])
self._save_holder_stop.append([copy.deepcopy(s_image), copy.deepcopy(s_measurements)])
#print("Following lane")
self._save_holder[-1][1][24] = 2
if self.brake_count > 3:
self._save_holder_stop[-1][1][24] = 6
else:
self._save_holder_stop[-1][1][24] = 2
def run_carla_client(args):
# Here we will run 3 episodes with 300 frames each.
number_of_episodes = 30000000
# frames_per_episode = 300
# We assume the CARLA server is already waiting for a client to connect at
# host:port. To create a connection we can use the `make_carla_client`
# context manager, it creates a CARLA client object and starts the
# connection. It will throw an exception if something goes wrong. The
# context manager makes sure the connection is always cleaned up on exit.
global client
with make_carla_client(args.host, args.port) as client:
print('CarlaClient connected')
for episode in range(0, number_of_episodes):
# Start a new episode.
if args.settings_filepath is None:
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=0,
NumberOfPedestrians=0,
# WeatherId=random.choice([1, 3, 7, 8, 14]),
WeatherId=1,
QualityLevel=args.quality_level)
settings.randomize_seeds()
# Now we want to add a couple of cameras to the player vehicle.
# We will collect the images produced by these cameras every
# frame.
# The default camera captures RGB images of the scene.
camera0 = Camera('CameraRGB')
# Set image resolution in pixels.
camera0.set_image_size(210, 160)
# Set its position relative to the car in meters.
camera0.set_position(0.30, 0, 1.30)
settings.add_sensor(camera0)
# Let's add another camera producing ground-truth depth.
camera1 = Camera('CameraDepth', PostProcessing='Depth')
camera1.set_image_size(210, 160)
camera1.set_position(0.30, 0, 1.30)
settings.add_sensor(camera1)
# Now we load these settings into the server. The server replies
# with a scene description containing the available start spots for
# the player. Here we can provide a CarlaSettings object or a
# CarlaSettings.ini file as string.
scene = client.load_settings(settings)
# Choose one player start at random.
# number_of_player_starts = len(scene.player_start_spots)
# player_start = random.randint(0, max(0, number_of_player_starts - 1))
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print('Starting new episode at %r...' % scene.map_name)
client.start_episode(0)
# Iterate every frame in the episode.
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
# Print some of the measurements.
player_measurements = measurements.player_measurements
global other_lane
global offroad
other_lane = 100 * player_measurements.intersection_otherlane
offroad = 100 * player_measurements.intersection_offroad
global done
done = False
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
global image_RGB_real
image_RGB_real = image_RGB.flatten()
re_image(image_RGB_real)
print_measurements(measurements)
global reward
reward = -other_lane - offroad + np.sqrt(
np.square(player_measurements.transform.location.x - 271.0) +
np.square(player_measurements.transform.location.y - 129.5))
col = player_measurements.collision_other
if offroad > 10 or other_lane > 10 or col > 0:
print(111111111111111111111)
done = True
def run_carla_client(host, port, far):
# Here we will run a single episode with 300 frames.
number_of_frames = 30
frame_step = 10 # Save one image every 100 frames
image_size = [800, 600]
camera_local_pos = [0.3, 0.0, 1.3] # [X, Y, Z]
camera_local_rotation = [0, 0, 0] # [pitch(Y), yaw(Z), roll(X)]
fov = 70
autopilot = False
control = VehicleControl()
for start_i in range(150):
output_folder = '/home2/mariap/Packages/CARLA_0.8.2/PythonClient/_out/pos_' + str(
start_i)
if not os.path.exists(output_folder):
os.mkdir(output_folder)
print("make " + str(output_folder))
# Connect with the server
with make_carla_client(host, port) as client:
print('CarlaClient connected')
for start_i in range(150):
output_folder = '/home2/mariap/Packages/CARLA_0.8.2/PythonClient/_out/pos_' + str(
start_i)
print(output_folder)
# Here we load the settings.
settings = CarlaSettings()
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=100,
NumberOfPedestrians=500,
WeatherId=random.choice([1, 3, 7, 8, 14]))
settings.randomize_seeds()
camera1 = Camera('CameraDepth', PostProcessing='Depth', FOV=fov)
camera1.set_image_size(*image_size)
camera1.set_position(*camera_local_pos)
camera1.set_rotation(*camera_local_rotation)
settings.add_sensor(camera1)
camera2 = Camera('CameraRGB', PostProcessing='SceneFinal', FOV=fov)
camera2.set_image_size(*image_size)
camera2.set_position(*camera_local_pos)
camera2.set_rotation(*camera_local_rotation)
settings.add_sensor(camera2)
camera3 = Camera('CameraSeg',
PostProcessing='SemanticSegmentation')
camera3.set_image_size(*image_size)
camera3.set_position(*camera_local_pos)
camera3.set_rotation(*camera_local_rotation)
settings.add_sensor(camera3)
client.load_settings(settings)
# Start at location index id '0'
client.start_episode(start_i)
cameras_dict = {}
pedestrians_dict = {}
cars_dict = {}
# Compute the camera transform matrix
camera_to_car_transform = camera2.get_unreal_transform()
# (Intrinsic) (3, 3) K Matrix
K = np.identity(3)
K[0, 2] = image_size[0] / 2.0
K[1, 2] = image_size[1] / 2.0
K[0,
0] = K[1,
1] = image_size[0] / (2.0 * np.tan(fov * np.pi / 360.0))
with open(output_folder + '/camera_intrinsics.p', 'w') as camfile:
pickle.dump(K, camfile)
# Iterate every frame in the episode except for the first one.
for frame in range(1, number_of_frames):
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
# Save one image every 'frame_step' frames
if not frame % frame_step:
for name, measurement in sensor_data.items():
filename = '{:s}/{:0>6d}'.format(name, frame)
measurement.save_to_disk(
os.path.join(output_folder, filename))
# Start transformations time mesure.
timer = StopWatch()
# RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]]
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
image_seg = np.tile(
labels_to_array(sensor_data['CameraSeg']), (1, 1, 3))
# 2d to (camera) local 3d
# We use the image_RGB to colorize each 3D point, this is optional.
# "max_depth" is used to keep only the points that are near to the
# camera, meaning 1.0 the farest points (sky)
point_cloud = depth_to_local_point_cloud(
sensor_data['CameraDepth'], image_RGB, max_depth=far)
point_cloud_seg = depth_to_local_point_cloud(
sensor_data['CameraDepth'], image_seg, max_depth=far)
# (Camera) local 3d to world 3d.
# Get the transform from the player protobuf transformation.
world_transform = Transform(
measurements.player_measurements.transform)
# Compute the final transformation matrix.
car_to_world_transform = world_transform * camera_to_car_transform
# Car to World transformation given the 3D points and the
# transformation matrix.
point_cloud.apply_transform(car_to_world_transform)
point_cloud_seg.apply_transform(car_to_world_transform)
Rt = car_to_world_transform.matrix
Rt_inv = car_to_world_transform.inverse(
).matrix # Rt_inv is the world to camera matrix !!
#R_inv=world_transform.inverse().matrix
cameras_dict[frame] = {}
cameras_dict[frame]['inverse_rotation'] = Rt_inv[:]
cameras_dict[frame]['rotation'] = Rt[:]
cameras_dict[frame]['translation'] = Rt_inv[0:3, 3]
cameras_dict[frame][
'camera_to_car'] = camera_to_car_transform.matrix
# Get non-player info
vehicles = {}
pedestrians = {}
for agent in measurements.non_player_agents:
# check if the agent is a vehicle.
if agent.HasField('vehicle'):
pos = agent.vehicle.transform.location
pos_vector = np.array([[pos.x], [pos.y], [pos.z],
[1.0]])
trnasformed_3d_pos = np.dot(Rt_inv, pos_vector)
pos2d = np.dot(K, trnasformed_3d_pos[:3])
# Normalize the point
norm_pos2d = np.array([
pos2d[0] / pos2d[2], pos2d[1] / pos2d[2],
pos2d[2]
])
# Now, pos2d contains the [x, y, d] values of the image in pixels (where d is the depth)
# You can use the depth to know the points that are in front of the camera (positive depth).
x_2d = image_size[0] - norm_pos2d[0]
y_2d = image_size[1] - norm_pos2d[1]
vehicles[agent.id] = {}
vehicles[agent.id]['transform'] = [
agent.vehicle.transform.location.x,
agent.vehicle.transform.location.y,
agent.vehicle.transform.location.z
]
vehicles[agent.id][
'bounding_box.transform'] = agent.vehicle.bounding_box.transform.location.z
vehicles[agent.id][
'yaw'] = agent.vehicle.transform.rotation.yaw
vehicles[agent.id]['bounding_box'] = [
agent.vehicle.bounding_box.extent.x,
agent.vehicle.bounding_box.extent.y,
agent.vehicle.bounding_box.extent.z
]
vehicle_transform = Transform(
agent.vehicle.bounding_box.transform)
pos = agent.vehicle.transform.location
bbox3d = agent.vehicle.bounding_box.extent
# Compute the 3D bounding boxes
# f contains the 4 points that corresponds to the bottom
f = np.array([[
pos.x + bbox3d.x, pos.y - bbox3d.y,
pos.z - bbox3d.z +
agent.vehicle.bounding_box.transform.location.z
],
[
pos.x + bbox3d.x,
pos.y + bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x - bbox3d.x,
pos.y + bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x - bbox3d.x,
pos.y - bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x + bbox3d.x,
pos.y - bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x + bbox3d.x,
pos.y + bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x - bbox3d.x,
pos.y + bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
],
[
pos.x - bbox3d.x,
pos.y - bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.
bounding_box.transform.location.z
]])
f_rotated = vehicle_transform.transform_points(f)
f_2D_rotated = []
vehicles[
agent.id]['bounding_box_coord'] = f_rotated
for i in range(f.shape[0]):
point = np.array([[f_rotated[i, 0]],
[f_rotated[i, 1]],
[f_rotated[i, 2]], [1]])
transformed_2d_pos = np.dot(
Rt_inv,
point) # 3d Position in camera space
pos2d = np.dot(
K, transformed_2d_pos[:3]
) # Conversion to camera frustum space
norm_pos2d = np.array([
pos2d[0] / pos2d[2], pos2d[1] / pos2d[2],
pos2d[2]
])
#print([image_size[0] - (pos2d[0] / pos2d[2]), image_size[1] - (pos2d[1] / pos2d[2])])
f_2D_rotated.append(
np.array([
image_size[0] - norm_pos2d[0],
image_size[1] - norm_pos2d[1]
]))
vehicles[
agent.id]['bounding_box_2D'] = f_2D_rotated
elif agent.HasField('pedestrian'):
pedestrians[agent.id] = {}
pedestrians[agent.id]['transform'] = [
agent.pedestrian.transform.location.x,
agent.pedestrian.transform.location.y,
agent.pedestrian.transform.location.z
]
pedestrians[agent.id][
'yaw'] = agent.pedestrian.transform.rotation.yaw
pedestrians[agent.id]['bounding_box'] = [
agent.pedestrian.bounding_box.extent.x,
agent.pedestrian.bounding_box.extent.y,
agent.pedestrian.bounding_box.extent.z
]
vehicle_transform = Transform(
agent.pedestrian.bounding_box.transform)
pos = agent.pedestrian.transform.location
bbox3d = agent.pedestrian.bounding_box.extent
# Compute the 3D bounding boxes
# f contains the 4 points that corresponds to the bottom
f = np.array(
[[pos.x + bbox3d.x, pos.y - bbox3d.y, pos.z],
[pos.x + bbox3d.x, pos.y + bbox3d.y, pos.z],
[pos.x - bbox3d.x, pos.y + bbox3d.y, pos.z],
[pos.x - bbox3d.x, pos.y - bbox3d.y, pos.z],
[
pos.x + bbox3d.x, pos.y - bbox3d.y,
pos.z + bbox3d.z
],
[
pos.x + bbox3d.x, pos.y + bbox3d.y,
pos.z + bbox3d.z
],
[
pos.x - bbox3d.x, pos.y + bbox3d.y,
pos.z + bbox3d.z
],
[
pos.x - bbox3d.x, pos.y - bbox3d.y,
pos.z + bbox3d.z
]])
f_rotated = vehicle_transform.transform_points(f)
pedestrians[
agent.id]['bounding_box_coord'] = f_rotated
f_2D_rotated = []
for i in range(f.shape[0]):
point = np.array([[f_rotated[i, 0]],
[f_rotated[i, 1]],
[f_rotated[i, 2]], [1]])
transformed_2d_pos = np.dot(
Rt_inv, point) # See above for cars
pos2d = np.dot(K, transformed_2d_pos[:3])
norm_pos2d = np.array([
pos2d[0] / pos2d[2], pos2d[1] / pos2d[2],
pos2d[2]
])
f_2D_rotated.append([
image_size[0] - norm_pos2d[0],
image_size[1] - norm_pos2d[1]
])
pedestrians[
agent.id]['bounding_box_2D'] = f_2D_rotated
cars_dict[frame] = vehicles
pedestrians_dict[frame] = pedestrians
# End transformations time mesure.
timer.stop()
# Save PLY to disk
# This generates the PLY string with the 3D points and the RGB colors
# for each row of the file.
point_cloud.save_to_disk(
os.path.join(output_folder,
'{:0>5}.ply'.format(frame)))
point_cloud_seg.save_to_disk(
os.path.join(output_folder,
'{:0>5}_seg.ply'.format(frame)))
print_message(timer.milliseconds(), len(point_cloud),
frame)
if autopilot:
client.send_control(
measurements.player_measurements.autopilot_control)
else:
control.hand_brake = True
client.send_control(control)
with open(output_folder + '/cameras.p', 'w') as camerafile:
pickle.dump(cameras_dict, camerafile)
print(output_folder + "cameras.p")
with open(output_folder + '/people.p', 'w') as peoplefile:
pickle.dump(pedestrians_dict, peoplefile)
with open(output_folder + '/cars.p', 'w') as carfile:
pickle.dump(cars_dict, carfile)
def run_carla_client(host, port, far):
# Here we will run a single episode with 300 frames.
number_of_frames = 3000
frame_step = 100 # Save one image every 100 frames
output_folder = '_out'
image_size = [800, 600]
camera_local_pos = [0.3, 0.0, 1.3] # [X, Y, Z]
camera_local_rotation = [0, 0, 0] # [pitch(Y), yaw(Z), roll(X)]
fov = 70
# Connect with the server
with make_carla_client(host, port) as client:
print('CarlaClient connected')
# Here we load the settings.
settings = CarlaSettings()
settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=20,
NumberOfPedestrians=40,
WeatherId=random.choice([1, 3, 7, 8, 14]))
settings.randomize_seeds()
camera1 = Camera('CameraDepth', PostProcessing='Depth', FOV=fov)
camera1.set_image_size(*image_size)
camera1.set_position(*camera_local_pos)
camera1.set_rotation(*camera_local_rotation)
settings.add_sensor(camera1)
camera2 = Camera('CameraRGB', PostProcessing='SceneFinal', FOV=fov)
camera2.set_image_size(*image_size)
camera2.set_position(*camera_local_pos)
camera2.set_rotation(*camera_local_rotation)
settings.add_sensor(camera2)
client.load_settings(settings)
# Start at location index id '0'
client.start_episode(0)
# Compute the camera transform matrix
camera_to_car_transform = camera2.get_unreal_transform()
# Iterate every frame in the episode except for the first one.
for frame in range(1, number_of_frames):
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
# Save one image every 'frame_step' frames
if not frame % frame_step:
# Start transformations time mesure.
timer = StopWatch()
# RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]]
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
# 2d to (camera) local 3d
# We use the image_RGB to colorize each 3D point, this is optional.
# "max_depth" is used to keep only the points that are near to the
# camera, meaning 1.0 the farest points (sky)
point_cloud = depth_to_local_point_cloud(
sensor_data['CameraDepth'],
image_RGB,
max_depth=far
)
# (Camera) local 3d to world 3d.
# Get the transform from the player protobuf transformation.
world_transform = Transform(
measurements.player_measurements.transform
)
# Compute the final transformation matrix.
car_to_world_transform = world_transform * camera_to_car_transform
# Car to World transformation given the 3D points and the
# transformation matrix.
point_cloud.apply_transform(car_to_world_transform)
# End transformations time mesure.
timer.stop()
# Save PLY to disk
# This generates the PLY string with the 3D points and the RGB colors
# for each row of the file.
point_cloud.save_to_disk(os.path.join(
output_folder, '{:0>5}.ply'.format(frame))
)
print_message(timer.milliseconds(), len(point_cloud), frame)
client.send_control(
measurements.player_measurements.autopilot_control
)
def exec_waypoint_nav_demo(args):
""" Executes waypoint navigation demo.
"""
with make_carla_client(args.host, args.port) as client:
print('Carla client connected.')
date = "2021_10_15"
trial = 1
mdl = 'model-' + date + '-' + str(trial)
model = load_model(os.path.join("models", mdl))
settings = make_carla_settings(args)
# Now we load these settings into the server. The server replies
# with a scene description containing the available start spots for
# the player. Here we can provide a CarlaSettings object or a
# CarlaSettings.ini file as string.
scene = client.load_settings(settings)
# Refer to the player start folder in the WorldOutliner to see the
# player start information
player_start = PLAYER_START_INDEX
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print('Starting new episode at %r...' % scene.map_name)
client.start_episode(player_start)
#############################################
# Load Configurations
#############################################
# Load configuration file (options.cfg) and then parses for the various
# options. Here we have two main options:
# live_plotting and live_plotting_period, which controls whether
# live plotting is enabled or how often the live plotter updates
# during the simulation run.
config = configparser.ConfigParser()
config.read(
os.path.join(os.path.dirname(os.path.realpath(__file__)),
'options.cfg'))
demo_opt = config['Demo Parameters']
# Get options
enable_live_plot = demo_opt.get('live_plotting', 'true').capitalize()
enable_live_plot = enable_live_plot == 'True'
live_plot_period = float(demo_opt.get('live_plotting_period', 0))
# Set options
live_plot_timer = Timer(live_plot_period)
#############################################
# Load Waypoints
#############################################
# Opens the waypoint file and stores it to "waypoints"
waypoints_file = WAYPOINTS_FILENAME
waypoints_np = None
with open(waypoints_file) as waypoints_file_handle:
waypoints = list(
csv.reader(waypoints_file_handle,
delimiter=',',
quoting=csv.QUOTE_NONNUMERIC))
waypoints_np = np.array(waypoints)
# Because the waypoints are discrete and our controller performs better
# with a continuous path, here we will send a subset of the waypoints
# within some lookahead distance from the closest point to the vehicle.
# Interpolating between each waypoint will provide a finer resolution
# path and make it more "continuous". A simple linear interpolation
# is used as a preliminary method to address this issue, though it is
# better addressed with better interpolation methods (spline
# interpolation, for example).
# More appropriate interpolation methods will not be used here for the
# sake of demonstration on what effects discrete paths can have on
# the controller. It is made much more obvious with linear
# interpolation, because in a way part of the path will be continuous
# while the discontinuous parts (which happens at the waypoints) will
# show just what sort of effects these points have on the controller.
# Can you spot these during the simulation? If so, how can you further
# reduce these effects?
# Linear interpolation computations
# Compute a list of distances between waypoints
wp_distance = [] # distance array
for i in range(1, waypoints_np.shape[0]):
wp_distance.append(
np.sqrt((waypoints_np[i, 0] - waypoints_np[i - 1, 0])**2 +
(waypoints_np[i, 1] - waypoints_np[i - 1, 1])**2))
wp_distance.append(0) # last distance is 0 because it is the distance
# from the last waypoint to the last waypoint
# Linearly interpolate between waypoints and store in a list
wp_interp = [] # interpolated values
# (rows = waypoints, columns = [x, y, v])
wp_interp_hash = [] # hash table which indexes waypoints_np
# to the index of the waypoint in wp_interp
interp_counter = 0 # counter for current interpolated point index
for i in range(waypoints_np.shape[0] - 1):
# Add original waypoint to interpolated waypoints list (and append
# it to the hash table)
wp_interp.append(list(waypoints_np[i]))
wp_interp_hash.append(interp_counter)
interp_counter += 1
# Interpolate to the next waypoint. First compute the number of
# points to interpolate based on the desired resolution and
# incrementally add interpolated points until the next waypoint
# is about to be reached.
num_pts_to_interp = int(np.floor(wp_distance[i] /\
float(INTERP_DISTANCE_RES)) - 1)
wp_vector = waypoints_np[i + 1] - waypoints_np[i]
wp_uvector = wp_vector / np.linalg.norm(wp_vector)
for j in range(num_pts_to_interp):
next_wp_vector = INTERP_DISTANCE_RES * float(j +
1) * wp_uvector
wp_interp.append(list(waypoints_np[i] + next_wp_vector))
interp_counter += 1
# add last waypoint at the end
wp_interp.append(list(waypoints_np[-1]))
wp_interp_hash.append(interp_counter)
interp_counter += 1
#############################################
# Controller 2D Class Declaration
#############################################
# This is where we take the controller2d.py class
# and apply it to the simulator
controller = controller2d.Controller2D(waypoints)
#############################################
# Determine simulation average timestep (and total frames)
#############################################
# Ensure at least one frame is used to compute average timestep
num_iterations = ITER_FOR_SIM_TIMESTEP
if (ITER_FOR_SIM_TIMESTEP < 1):
num_iterations = 1
# Gather current data from the CARLA server. This is used to get the
# simulator starting game time. Note that we also need to
# send a command back to the CARLA server because synchronous mode
# is enabled.
measurement_data, sensor_data = client.read_data()
sim_start_stamp = measurement_data.game_timestamp / 1000.0
# Send a control command to proceed to next iteration.
# This mainly applies for simulations that are in synchronous mode.
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
# Computes the average timestep based on several initial iterations
sim_duration = 0
for i in range(num_iterations):
# Gather current data
measurement_data, sensor_data = client.read_data()
# Send a control command to proceed to next iteration
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
# Last stamp
if i == num_iterations - 1:
sim_duration = measurement_data.game_timestamp / 1000.0 -\
sim_start_stamp
# Outputs average simulation timestep and computes how many frames
# will elapse before the simulation should end based on various
# parameters that we set in the beginning.
SIMULATION_TIME_STEP = sim_duration / float(num_iterations)
print("SERVER SIMULATION STEP APPROXIMATION: " + \
str(SIMULATION_TIME_STEP))
TOTAL_EPISODE_FRAMES = int((TOTAL_RUN_TIME + WAIT_TIME_BEFORE_START) /\
SIMULATION_TIME_STEP) + TOTAL_FRAME_BUFFER
#############################################
# Frame-by-Frame Iteration and Initialization
#############################################
# Store pose history starting from the start position
measurement_data, sensor_data = client.read_data()
start_x, start_y, start_yaw = get_current_pose(measurement_data)
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
x_history = [start_x]
y_history = [start_y]
yaw_history = [start_yaw]
time_history = [0]
speed_history = [0]
#############################################
# Vehicle Trajectory Live Plotting Setup
#############################################
# Uses the live plotter to generate live feedback during the simulation
# The two feedback includes the trajectory feedback and
# the controller feedback (which includes the speed tracking).
lp_traj = lv.LivePlotter(tk_title="Trajectory Trace")
lp_1d = lv.LivePlotter(tk_title="Controls Feedback")
###
# Add 2D position / trajectory plot
###
trajectory_fig = lp_traj.plot_new_dynamic_2d_figure(
title='Vehicle Trajectory',
figsize=(FIGSIZE_X_INCHES, FIGSIZE_Y_INCHES),
edgecolor="black",
rect=[PLOT_LEFT, PLOT_BOT, PLOT_WIDTH, PLOT_HEIGHT])
trajectory_fig.set_invert_x_axis() # Because UE4 uses left-handed
# coordinate system the X
# axis in the graph is flipped
trajectory_fig.set_axis_equal() # X-Y spacing should be equal in size
# Add waypoint markers
trajectory_fig.add_graph("waypoints",
window_size=waypoints_np.shape[0],
x0=waypoints_np[:, 0],
y0=waypoints_np[:, 1],
linestyle="-",
marker="",
color='g')
# Add trajectory markers
trajectory_fig.add_graph("trajectory",
window_size=TOTAL_EPISODE_FRAMES,
x0=[start_x] * TOTAL_EPISODE_FRAMES,
y0=[start_y] * TOTAL_EPISODE_FRAMES,
color=[1, 0.5, 0])
# Add lookahead path
trajectory_fig.add_graph("lookahead_path",
window_size=INTERP_MAX_POINTS_PLOT,
x0=[start_x] * INTERP_MAX_POINTS_PLOT,
y0=[start_y] * INTERP_MAX_POINTS_PLOT,
color=[0, 0.7, 0.7],
linewidth=4)
# Add starting position marker
trajectory_fig.add_graph("start_pos",
window_size=1,
x0=[start_x],
y0=[start_y],
marker=11,
color=[1, 0.5, 0],
markertext="Start",
marker_text_offset=1)
# Add end position marker
trajectory_fig.add_graph("end_pos",
window_size=1,
x0=[waypoints_np[-1, 0]],
y0=[waypoints_np[-1, 1]],
marker="D",
color='r',
markertext="End",
marker_text_offset=1)
# Add car marker
trajectory_fig.add_graph("car",
window_size=1,
marker="s",
color='b',
markertext="Car",
marker_text_offset=1)
###
# Add 1D speed profile updater
###
forward_speed_fig =\
lp_1d.plot_new_dynamic_figure(title="Forward Speed (m/s)")
forward_speed_fig.add_graph("forward_speed",
label="forward_speed",
window_size=TOTAL_EPISODE_FRAMES)
forward_speed_fig.add_graph("reference_signal",
label="reference_Signal",
window_size=TOTAL_EPISODE_FRAMES)
# Add throttle signals graph
throttle_fig = lp_1d.plot_new_dynamic_figure(title="Throttle")
throttle_fig.add_graph("throttle",
label="throttle",
window_size=TOTAL_EPISODE_FRAMES)
# Add brake signals graph
brake_fig = lp_1d.plot_new_dynamic_figure(title="Brake")
brake_fig.add_graph("brake",
label="brake",
window_size=TOTAL_EPISODE_FRAMES)
# Add steering signals graph
steer_fig = lp_1d.plot_new_dynamic_figure(title="Steer")
steer_fig.add_graph("steer",
label="steer",
window_size=TOTAL_EPISODE_FRAMES)
# live plotter is disabled, hide windows
if not enable_live_plot:
lp_traj._root.withdraw()
lp_1d._root.withdraw()
# Iterate the frames until the end of the waypoints is reached or
# the TOTAL_EPISODE_FRAMES is reached. The controller simulation then
# ouptuts the results to the controller output directory.
reached_the_end = False
skip_first_frame = True
closest_index = 0 # Index of waypoint that is currently closest to
# the car (assumed to be the first index)
closest_distance = 0 # Closest distance of closest waypoint to car
for frame in range(TOTAL_EPISODE_FRAMES):
# Gather current data from the CARLA server
measurement_data, sensor_data = client.read_data()
# Update pose, timestamp
current_x, current_y, current_yaw = \
get_current_pose(measurement_data)
current_speed = measurement_data.player_measurements.forward_speed
current_timestamp = float(measurement_data.game_timestamp) / 1000.0
# Wait for some initial time before starting the demo
if current_timestamp <= WAIT_TIME_BEFORE_START:
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
continue
else:
current_timestamp = current_timestamp - WAIT_TIME_BEFORE_START
# Store history
x_history.append(current_x)
y_history.append(current_y)
yaw_history.append(current_yaw)
speed_history.append(current_speed)
time_history.append(current_timestamp)
# Save the images to disk if requested.
if args.save_images_to_disk:
for name, measurement in sensor_data.items():
filename = args.out_filename_format.format(name, frame)
measurement.save_to_disk(filename)
###
# Controller update (this uses the controller2d.py implementation)
###
# To reduce the amount of waypoints sent to the controller,
# provide a subset of waypoints that are within some
# lookahead distance from the closest point to the car. Provide
# a set of waypoints behind the car as well.
# Find closest waypoint index to car. First increment the index
# from the previous index until the new distance calculations
# are increasing. Apply the same rule decrementing the index.
# The final index should be the closest point (it is assumed that
# the car will always break out of instability points where there
# are two indices with the same minimum distance, as in the
# center of a circle)
closest_distance = np.linalg.norm(
np.array([
waypoints_np[closest_index, 0] - current_x,
waypoints_np[closest_index, 1] - current_y
]))
new_distance = closest_distance
new_index = closest_index
while new_distance <= closest_distance:
closest_distance = new_distance
closest_index = new_index
new_index += 1
if new_index >= waypoints_np.shape[0]: # End of path
break
new_distance = np.linalg.norm(
np.array([
waypoints_np[new_index, 0] - current_x,
waypoints_np[new_index, 1] - current_y
]))
new_distance = closest_distance
new_index = closest_index
while new_distance <= closest_distance:
closest_distance = new_distance
closest_index = new_index
new_index -= 1
if new_index < 0: # Beginning of path
break
new_distance = np.linalg.norm(
np.array([
waypoints_np[new_index, 0] - current_x,
waypoints_np[new_index, 1] - current_y
]))
# Once the closest index is found, return the path that has 1
# waypoint behind and X waypoints ahead, where X is the index
# that has a lookahead distance specified by
# INTERP_LOOKAHEAD_DISTANCE
waypoint_subset_first_index = closest_index - 1
if waypoint_subset_first_index < 0:
waypoint_subset_first_index = 0
waypoint_subset_last_index = closest_index
total_distance_ahead = 0
while total_distance_ahead < INTERP_LOOKAHEAD_DISTANCE:
total_distance_ahead += wp_distance[waypoint_subset_last_index]
waypoint_subset_last_index += 1
if waypoint_subset_last_index >= waypoints_np.shape[0]:
waypoint_subset_last_index = waypoints_np.shape[0] - 1
break
# Use the first and last waypoint subset indices into the hash
# table to obtain the first and last indicies for the interpolated
# list. Update the interpolated waypoints to the controller
# for the next controller update.
new_waypoints = \
wp_interp[wp_interp_hash[waypoint_subset_first_index]:\
wp_interp_hash[waypoint_subset_last_index] + 1]
controller.update_waypoints(new_waypoints)
# Update the other controller values and controls
controller.update_values(current_x, current_y, current_yaw,
current_speed, current_timestamp, frame)
controller.update_controls()
cmd_throttle, cmd_steer, cmd_brake = controller.get_commands()
# Skip the first frame (so the controller has proper outputs)
if skip_first_frame and frame == 0:
pass
else:
# Update live plotter with new feedback
trajectory_fig.roll("trajectory", current_x, current_y)
trajectory_fig.roll("car", current_x, current_y)
# When plotting lookahead path, only plot a number of points
# (INTERP_MAX_POINTS_PLOT amount of points). This is meant
# to decrease load when live plotting
new_waypoints_np = np.array(new_waypoints)
path_indices = np.floor(
np.linspace(0, new_waypoints_np.shape[0] - 1,
INTERP_MAX_POINTS_PLOT))
trajectory_fig.update(
"lookahead_path",
new_waypoints_np[path_indices.astype(int), 0],
new_waypoints_np[path_indices.astype(int), 1],
new_colour=[0, 0.7, 0.7])
forward_speed_fig.roll("forward_speed", current_timestamp,
current_speed)
forward_speed_fig.roll("reference_signal", current_timestamp,
controller._desired_speed)
throttle_fig.roll("throttle", current_timestamp, cmd_throttle)
brake_fig.roll("brake", current_timestamp, cmd_brake)
steer_fig.roll("steer", current_timestamp, cmd_steer)
# Refresh the live plot based on the refresh rate
# set by the options
if enable_live_plot and \
live_plot_timer.has_exceeded_lap_period():
lp_traj.refresh()
lp_1d.refresh()
live_plot_timer.lap()
# Predict steering angle based on the Camera
image = sensor_data.get('CameraRGB', None)
# sensor_data['CameraRGB'].data
image = image_converter.to_rgb_array(image)
# np.save("camera.npy",image)
image = img_preprocess(image)
# cv2.imshow("CameraRGB",image)
image = np.array([image])
# print(image.shape)
steering_angle = float(model.predict(image))
print(steering_angle)
# Output controller command to CARLA server
send_control_command(client,
throttle=cmd_throttle,
steer=steering_angle,
brake=cmd_brake)
# Find if reached the end of waypoint. If the car is within
# DIST_THRESHOLD_TO_LAST_WAYPOINT to the last waypoint,
# the simulation will end.
dist_to_last_waypoint = np.linalg.norm(
np.array([
waypoints[-1][0] - current_x, waypoints[-1][1] - current_y
]))
if dist_to_last_waypoint < DIST_THRESHOLD_TO_LAST_WAYPOINT:
reached_the_end = True
if reached_the_end:
break
# End of demo - Stop vehicle and Store outputs to the controller output
# directory.
if reached_the_end:
print("Reached the end of path. Writing to controller_output...")
else:
print("Exceeded assessment time. Writing to controller_output...")
# Stop the car
send_control_command(client, throttle=0.0, steer=0.0, brake=1.0)
# Store the various outputs
store_trajectory_plot(trajectory_fig.fig, 'trajectory.png')
store_trajectory_plot(forward_speed_fig.fig, 'forward_speed.png')
store_trajectory_plot(throttle_fig.fig, 'throttle_output.png')
store_trajectory_plot(brake_fig.fig, 'brake_output.png')
store_trajectory_plot(steer_fig.fig, 'steer_output.png')
write_trajectory_file(x_history, y_history, speed_history,
time_history)
def _on_render(self, args):
global detected_speed
global flg_warning
global current_speed
global frame
self._timer.tick()
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))
if args.app:
new = False
detected_speed_loop = ''
if (frame % 5 ==0):
a = Image.fromarray(array, 'RGB')
a.save('Y:/Temp/carla_scene.jpg')
detected_speed_loop, new = self._traffic_sign_recogniser(new, detected_speed_loop)
if new:
detected_speed = detected_speed_loop
if flg_warning == -1:
r = 255
g = 0
b = 0
elif flg_warning == 1:
r = 0
g = 255
b = 0
else:
r = 0
g = 0
b = 255
basicfont = pygame.font.SysFont(None, 80)
text_detected = basicfont.render(detected_speed, True, (0,0,255))
textrec = text_detected.get_rect()
textrec.top = surface.get_rect().top
textrec.midtop = surface.get_rect().midtop
surface.blit(text_detected, textrec)
text_current = basicfont.render(current_speed+'km/h', True, (r,g,b))
textrec = text_current.get_rect()
textrec.top = surface.get_rect().top
textrec.bottomright = surface.get_rect().bottomright
surface.blit(text_current, textrec)
if args.app == 'Warning':
if flg_warning==-1:
basicfont = pygame.font.SysFont(None, 60)
text_warning = basicfont.render('REDUCE YOUR SPEED', True, (r,g,b))
textrec = text_warning.get_rect()
textrec.top = surface.get_rect().top
textrec.bottomleft = surface.get_rect().bottomleft
surface.blit(text_warning, textrec)
if args.app == 'Control':
if flg_warning==-1:
basicfont = pygame.font.SysFont(None, 60)
text_control = basicfont.render('SPEED REDUCED', True, (r,g,b))
textrec = text_control.get_rect()
textrec.top = surface.get_rect().top
textrec.bottomleft = surface.get_rect().bottomleft
surface.blit(text_control, textrec)
self._display.blit(surface, (0, 0))
pygame.display.flip()
def run_carla_client(host, port, far):
# Here we will run a single episode with 300 frames.
number_of_frames = 3000
frame_step = 10 # Save one image every 100 frames
output_folder = '_out'
image_size = [1280, 720]
camera_local_pos = [0.3, 0.0, 1.3] # [X, Y, Z]
camera_local_rotation = [0, 0, 0] # [pitch(Y), yaw(Z), roll(X)]
fov = 59
# Connect with the server
with make_carla_client(host, port) as client:
print('CarlaClient connected')
# Here we load the settings.
settings = CarlaSettings()
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=20,
NumberOfPedestrians=40,
WeatherId=7)
settings.randomize_seeds()
camera1 = Camera('CameraDepth', PostProcessing='Depth', FOV=fov)
camera1.set_image_size(*image_size)
camera1.set_position(*camera_local_pos)
camera1.set_rotation(*camera_local_rotation)
settings.add_sensor(camera1)
camera2 = Camera('CameraRGB', PostProcessing='SceneFinal', FOV=fov)
camera2.set_image_size(*image_size)
camera2.set_position(*camera_local_pos)
camera2.set_rotation(*camera_local_rotation)
settings.add_sensor(camera2)
client.load_settings(settings)
# Start at location index id '0'
client.start_episode(0)
# Compute the camera transform matrix
camera_to_car_transform = camera2.get_unreal_transform()
print("camera_to_car_transform", camera_to_car_transform)
# Iterate every frame in the episode except for the first one.
for frame in range(1, number_of_frames):
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
# Save one image every 'frame_step' frames
if not frame % frame_step:
# Start transformations time mesure.
# RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]]
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
world_transform = Transform(
measurements.player_measurements.transform)
# Compute the final transformation matrix.
car_to_world_transform = world_transform * camera_to_car_transform
print("{}.png".format(str(frame)))
print([
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
])
print("world_transform\n", world_transform)
#print("car_to_world_transform\n",car_to_world_transform)
print('\n')
im_bgr = cv2.cvtColor(image_RGB, cv2.COLOR_RGB2BGR)
cv2.imwrite("./{}/{}.png".format(output_folder, str(frame)),
im_bgr)
# 2d to (camera) local 3d
# We use the image_RGB to colorize each 3D point, this is optional.
# "max_depth" is used to keep only the points that are near to the
# camera, meaning 1.0 the farest points (sky)
# Save PLY to disk
# This generates the PLY string with the 3D points and the RGB colors
# for each row of the file.
client.send_control(
measurements.player_measurements.autopilot_control)
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 _on_render(self):
datapoints = []
if self._main_image is not None and self._depth_image is not None:
# Convert main image
image = image_converter.to_rgb_array(self._main_image)
# Retrieve and draw datapoints
image, datapoints = self._generate_datapoints(image)
# Draw lidar
# Camera coordinate system is left, up, forwards
if VISUALIZE_LIDAR:
# Calculation to shift bboxes relative to pitch and roll of player
rotation = self._measurements.player_measurements.transform.rotation
pitch, roll, yaw = rotation.pitch, rotation.roll, rotation.yaw
# Since measurements are in degrees, convert to radians
pitch = degrees_to_radians(pitch)
roll = degrees_to_radians(roll)
yaw = degrees_to_radians(yaw)
print('pitch: ', pitch)
print('roll: ', roll)
print('yaw: ', yaw)
# Rotation matrix for pitch
rotP = np.array([[cos(pitch), 0, sin(pitch)], [0, 1, 0],
[-sin(pitch), 0, cos(pitch)]])
# Rotation matrix for roll
rotR = np.array([[1, 0, 0], [0, cos(roll), -sin(roll)],
[0, sin(roll), cos(roll)]])
# combined rotation matrix, must be in order roll, pitch, yaw
rotRP = np.matmul(rotR, rotP)
# Take the points from the point cloud and transform to car space
point_cloud = np.array(
self._lidar_to_car_transform.transform_points(
self._lidar_measurement.data))
point_cloud[:, 2] -= LIDAR_HEIGHT_POS
point_cloud = np.matmul(rotRP, point_cloud.T).T
# Transform to camera space by the inverse of camera_to_car transform
point_cloud_cam = self._camera_to_car_transform.inverse(
).transform_points(point_cloud)
point_cloud_cam[:, 1] += LIDAR_HEIGHT_POS
image = lidar_utils.project_point_cloud(
image, point_cloud_cam, self._intrinsic, 1)
# Display image
surface = pygame.surfarray.make_surface(image.swapaxes(0, 1))
self._display.blit(surface, (0, 0))
pygame.display.flip()
if (self._timer.step + 1) % STEPS_BETWEEN_RECORDINGS == 0:
if datapoints:
# Avoid doing this twice or unnecessarily often
if not VISUALIZE_LIDAR:
# Calculation to shift bboxes relative to pitch and roll of player
rotation = self._measurements.player_measurements.transform.rotation
pitch, roll, yaw = rotation.pitch, rotation.roll, rotation.yaw
# Since measurements are in degrees, convert to radians
pitch = degrees_to_radians(pitch)
roll = degrees_to_radians(roll)
yaw = degrees_to_radians(yaw)
print('pitch: ', pitch)
print('roll: ', roll)
print('yaw: ', yaw)
# Rotation matrix for pitch
rotP = np.array([[cos(pitch), 0,
sin(pitch)], [0, 1, 0],
[-sin(pitch), 0,
cos(pitch)]])
# Rotation matrix for roll
rotR = np.array([[1, 0, 0], [0,
cos(roll), -sin(roll)],
[0, sin(roll),
cos(roll)]])
# combined rotation matrix, must be in order roll, pitch, yaw
rotRP = np.matmul(rotR, rotP)
# Take the points from the point cloud and transform to car space
point_cloud = np.array(
self._lidar_to_car_transform.transform_points(
self._lidar_measurement.data))
point_cloud[:, 2] -= LIDAR_HEIGHT_POS
point_cloud = np.matmul(rotRP, point_cloud.T).T
self._update_agent_location()
# Save screen, lidar and kitti training labels together with calibration and groundplane files
# self._save_training_files(datapoints, point_cloud)
img = get_image_data(
image_converter.to_rgb_array(self._main_image))
lid = get_lidar_data(point_cloud)
calib = get_calibration_matrices(self._intrinsic,
self._extrinsic)
mess = {
"idx": self.captured_frame_no,
"image_2": img,
"velodyne": lid,
"calib": calib
}
annos = self.evaluator.predict(mess)
# annos = self.evaluator.evaluate(self.captured_frame_no)
print(annos)
scores = annos["score"]
vertices = annos["vertices"]
bbox = annos["bbox"]
for i, score in enumerate(scores):
if score * 100 > 35:
# draw 3D
image = draw_projected_box3d(image,
vertices[i],
color=(0, 0, 255),
thickness=1)
image = cv2.putText(
image, str(round(score * 100, 2)),
(int(bbox[i][0]), int(bbox[i][1])),
cv2.FONT_HERSHEY_SIMPLEX, 0.3, (255, 0, 255),
1, cv2.LINE_AA)
# draw 2D
# bbox = annos["bbox"]
# x1, y1, x2, y2 = bbox[i]
# cv2.rectangle(image, pt1=(x1, y1), pt2=(x2, y2), color=(0, 0, 255), thickness=2)
# Display image
surface = pygame.surfarray.make_surface(
image.swapaxes(0, 1))
self._display.blit(surface, (0, 0))
pygame.display.flip()
self.captured_frame_no += 1
self._captured_frames_since_restart += 1
self._frames_since_last_capture = 0
time.sleep(0.5)
else:
logging.debug("Could save datapoint".format(
DISTANCE_SINCE_LAST_RECORDING))
else:
self._frames_since_last_capture += 1
logging.debug(
"Could not save training data - no visible agents of selected classes in scene"
)
def exec_waypoint_nav_demo(args):
""" Executes waypoint navigation demo.
"""
with make_carla_client(args.host, args.port) as client:
print('Carla client connected.')
settings, camera2 = make_carla_settings(args)
# Now we load these settings into the server. The server replies
# with a scene description containing the available start spots for
# the player. Here we can provide a CarlaSettings object or a
# CarlaSettings.ini file as string.
scene = client.load_settings(settings)
# Refer to the player start folder in the WorldOutliner to see the
# player start information
player_start = PLAYER_START_INDEX
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print('Starting new episode at %r...' % scene.map_name)
client.start_episode(player_start)
#############################################
# Load Configurations
#############################################
# Load configuration file (options.cfg) and then parses for the various
# options. Here we have two main options:
# live_plotting and live_plotting_period, which controls whether
# live plotting is enabled or how often the live plotter updates
# during the simulation run.
config = configparser.ConfigParser()
config.read(
os.path.join(os.path.dirname(os.path.realpath(__file__)),
'options.cfg'))
demo_opt = config['Demo Parameters']
# Get options
enable_live_plot = demo_opt.get('live_plotting', 'true').capitalize()
enable_live_plot = enable_live_plot == 'True'
live_plot_period = float(demo_opt.get('live_plotting_period', 0))
# Set options
live_plot_timer = Timer(live_plot_period)
#############################################
# Load Waypoints
#############################################
# Opens the waypoint file and stores it to "waypoints"
camera_to_car_transform = camera2.get_unreal_transform()
carla_utils_obj = segmentationobj.carla_utils(intrinsic)
measurement_data, sensor_data = client.read_data()
measurements = measurement_data
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
image_depth = to_rgb_array(sensor_data['CameraDepth'])
world_transform = Transform(measurements.player_measurements.transform)
im_bgr = cv2.cvtColor(image_RGB, cv2.COLOR_RGB2BGR)
pos_vector = ([
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
])
fdfd = str(world_transform)
sdsd = fdfd[1:-1].split('\n')
new = []
for i in sdsd:
dd = i[:-1].lstrip('[ ')
ff = re.sub("\s+", ",", dd.strip())
gg = ff.split(',')
new.append([float(i) for i in gg])
newworld = np.array(new)
fdfd = str(camera_to_car_transform)
sdsd = fdfd[1:-1].split('\n')
new = []
for i in sdsd:
dd = i[:-1].lstrip('[ ')
ff = re.sub("\s+", ",", dd.strip())
gg = ff.split(',')
new.append([float(i) for i in gg])
newcam = np.array(new)
extrinsic = np.matmul(newworld, newcam)
#print("dfjdfjkjfdkf",extrinsic)
get_2d_point, pointsmid, res_img = carla_utils_obj.run_seg(
im_bgr, extrinsic, pos_vector)
#print(get_2d_point)
#dis1=((get_2d_point[1]-pointsmid[0][1]),(get_2d_point[0]-pointsmid[0][0]))
#dis2=((get_2d_point[1]-pointsmid[1][1]),(get_2d_point[0]-pointsmid[1][0]))
#dis3=((get_2d_point[1]-pointsmid[2][1]),(get_2d_point[0]-pointsmid[2][0]))
#dis4=((get_2d_point[1]-pointsmid[3][1]),(get_2d_point[0]-pointsmid[3][0]))
#flagbox=darknet_proper_fps.performDetect(im_bgr,thresh,configPath, weightPath, metaPath, showImage, makeImageOnly, initOnly,pointsmid)
depth1 = image_depth[int(pointsmid[0][0]), int(pointsmid[0][1])]
depth2 = image_depth[int(pointsmid[1][0]), int(pointsmid[1][1])]
depth3 = image_depth[int(pointsmid[2][0]), int(pointsmid[2][1])]
depth4 = image_depth[int(pointsmid[3][0]), int(pointsmid[3][1])]
scale1 = depth1[0] + depth1[1] * 256 + depth1[2] * 256 * 256
scale1 = scale1 / ((256 * 256 * 256) - 1)
depth1 = scale1 * 1000
pos2d1 = np.array([(WINDOW_WIDTH - pointsmid[0][1]) * depth1,
(WINDOW_HEIGHT - pointsmid[0][0]) * depth1, depth1])
first1 = np.dot(np.linalg.inv(intrinsic), pos2d1)
first1 = np.append(first1, 1)
sec1 = np.matmul((extrinsic), first1)
scale2 = depth2[0] + depth2[1] * 256 + depth2[2] * 256 * 256
scale2 = scale2 / ((256 * 256 * 256) - 1)
depth2 = scale2 * 1000
pos2d2 = np.array([(WINDOW_WIDTH - pointsmid[1][1]) * depth2,
(WINDOW_HEIGHT - pointsmid[1][0]) * depth2, depth2])
first2 = np.dot(np.linalg.inv(intrinsic), pos2d2)
first2 = np.append(first2, 1)
sec2 = np.matmul((extrinsic), first2)
scale3 = depth3[0] + depth3[1] * 256 + depth3[2] * 256 * 256
scale3 = scale3 / ((256 * 256 * 256) - 1)
depth3 = scale3 * 1000
pos2d3 = np.array([(WINDOW_WIDTH - pointsmid[2][1]) * depth3,
(WINDOW_HEIGHT - pointsmid[2][0]) * depth3, depth3])
first3 = np.dot(np.linalg.inv(intrinsic), pos2d3)
first3 = np.append(first3, 1)
sec3 = np.matmul((extrinsic), first3)
scale4 = depth4[0] + depth4[1] * 256 + depth4[2] * 256 * 256
scale4 = scale4 / ((256 * 256 * 256) - 1)
depth4 = scale4 * 1000
pos2d4 = np.array([(WINDOW_WIDTH - pointsmid[3][1]) * depth4,
(WINDOW_HEIGHT - pointsmid[3][0]) * depth4, depth4])
first4 = np.dot(np.linalg.inv(intrinsic), pos2d4)
first4 = np.append(first4, 1)
sec4 = np.matmul((extrinsic), first4)
depth = image_depth[int(get_2d_point[0]), int(get_2d_point[1])]
scale = depth[0] + depth[1] * 256 + depth[2] * 256 * 256
scale = scale / ((256 * 256 * 256) - 1)
depth = scale * 1000
pos2d = np.array([(WINDOW_WIDTH - get_2d_point[1]) * depth,
(WINDOW_HEIGHT - get_2d_point[0]) * depth, depth])
first = np.dot(np.linalg.inv(intrinsic), pos2d)
first = np.append(first, 1)
sec = np.matmul((extrinsic), first)
print("present", pos_vector)
print('goal-', sec)
pos_vector[2] = 4.0
ini = pos_vector
goal = list(sec)
goal[2] = 1.08
aa = ini[0]
dec = abs(ini[1] - goal[1]) / abs(aa - (goal[0]))
f1 = open(WAYPOINTS_FILENAME, 'a+')
for i in range(int(goal[0]), int(aa)):
# print(int(goal[0])-i)
if abs((int(aa) - i)) < 10:
ini = [ini[0] - 1, ini[1] + dec, ini[2] - 0.03]
else:
ini = [ini[0] - 1, ini[1] + dec, ini[2]]
#if i<int(aa)-1:
f1.write(
str(ini[0]) + ',' + str(ini[1]) + ',' + str(ini[2]) + '\n')
#else:
#f1.write(str(ini[0])+','+str(ini[1])+','+str(ini[2]))
waypoints_file = WAYPOINTS_FILENAME
waypoints_np = None
with open(waypoints_file) as waypoints_file_handle:
waypoints = list(
csv.reader(waypoints_file_handle,
delimiter=',',
quoting=csv.QUOTE_NONNUMERIC))
waypoints_np = np.array(waypoints)
#print("dfjdjfhjdhfjdfhjdfdjdfdufh",waypoints_np)
print((waypoints_np))
# Because the waypoints are discrete and our controller performs better
# with a continuous path, here we will send a subset of the waypoints
# within some lookahead distance from the closest point to the vehicle.
# Interpolating between each waypoint will provide a finer resolution
# path and make it more "continuous". A simple linear interpolation
# is used as a preliminary method to address this issue, though it is
# better addressed with better interpolation methods (spline
# interpolation, for example).
# More appropriate interpolation methods will not be used here for the
# sake of demonstration on what effects discrete paths can have on
# the controller. It is made much more obvious with linear
# interpolation, because in a way part of the path will be continuous
# while the discontinuous parts (which happens at the waypoints) will
# show just what sort of effects these points have on the controller.
# Can you spot these during the simulation? If so, how can you further
# reduce these effects?
# Linear interpolation computations
# Compute a list of distances between waypoints
wp_distance = [] # distance array
for i in range(1, waypoints_np.shape[0]):
wp_distance.append(
np.sqrt((waypoints_np[i, 0] - waypoints_np[i - 1, 0])**2 +
(waypoints_np[i, 1] - waypoints_np[i - 1, 1])**2))
wp_distance.append(0) # last distance is 0 because it is the distance
# from the last waypoint to the last waypoint
# Linearly interpolate between waypoints and store in a list
wp_interp = [] # interpolated values
# (rows = waypoints, columns = [x, y, v])
wp_interp_hash = [] # hash table which indexes waypoints_np
# to the index of the waypoint in wp_interp
interp_counter = 0 # counter for current interpolated point index
for i in range(waypoints_np.shape[0] - 1):
# Add original waypoint to interpolated waypoints list (and append
# it to the hash table)
wp_interp.append(list(waypoints_np[i]))
wp_interp_hash.append(interp_counter)
interp_counter += 1
# Interpolate to the next waypoint. First compute the number of
# points to interpolate based on the desired resolution and
# incrementally add interpolated points until the next waypoint
# is about to be reached.
num_pts_to_interp = int(np.floor(wp_distance[i] /\
float(INTERP_DISTANCE_RES)) - 1)
wp_vector = waypoints_np[i + 1] - waypoints_np[i]
wp_uvector = wp_vector / np.linalg.norm(wp_vector)
for j in range(num_pts_to_interp):
next_wp_vector = INTERP_DISTANCE_RES * float(j +
1) * wp_uvector
wp_interp.append(list(waypoints_np[i] + next_wp_vector))
interp_counter += 1
# add last waypoint at the end
wp_interp.append(list(waypoints_np[-1]))
wp_interp_hash.append(interp_counter)
interp_counter += 1
#############################################
# Controller 2D Class Declaration
#############################################
# This is where we take the controller2d.py class
# and apply it to the simulator
controller = controller2d.Controller2D(waypoints)
#############################################
# Determine simulation average timestep (and total frames)
#############################################
# Ensure at least one frame is used to compute average timestep
num_iterations = ITER_FOR_SIM_TIMESTEP
if (ITER_FOR_SIM_TIMESTEP < 1):
num_iterations = 1
# Gather current data from the CARLA server. This is used to get the
# simulator starting game time. Note that we also need to
# send a command back to the CARLA server because synchronous mode
# is enabled.
sim_start_stamp = measurement_data.game_timestamp / 1000.0
# Send a control command to proceed to next iteration.
#print("dddddddddddddddddddddddddddddddddddddddddddddd",sim_start_stamp)
# This mainly applies for simulations that are in synchronous mode.
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
# Computes the average timestep based on several initial iterations
sim_duration = 0
for i in range(num_iterations):
# Gather current data
measurement_data, sensor_data = client.read_data()
# Send a control command to proceed to next iteration
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
# Last stamp
if i == num_iterations - 1:
sim_duration = measurement_data.game_timestamp / 1000.0 -\
sim_start_stamp
# Outputs average simulation timestep and computes how many frames
# will elapse before the simulation should end based on various
# parameters that we set in the beginning.
SIMULATION_TIME_STEP = sim_duration / float(num_iterations)
print("SERVER SIMULATION STEP APPROXIMATION: " + \
str(SIMULATION_TIME_STEP))
TOTAL_EPISODE_FRAMES = int((TOTAL_RUN_TIME + WAIT_TIME_BEFORE_START) /\
SIMULATION_TIME_STEP) + TOTAL_FRAME_BUFFER
#############################################
# Frame-by-Frame Iteration and Initialization
#############################################
# Store pose history starting from the start position
measurement_data, sensor_data = client.read_data()
start_x, start_y, start_yaw = get_current_pose(measurement_data)
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
x_history = [start_x]
y_history = [start_y]
yaw_history = [start_yaw]
time_history = [0]
speed_history = [0]
#############################################
# Vehicle Trajectory Live Plotting Setup
#############################################
# Uses the live plotter to generate live feedback during the simulation
# The two feedback includes the trajectory feedback and
# the controller feedback (which includes the speed tracking).
lp_traj = lv.LivePlotter(tk_title="Trajectory Trace")
lp_1d = lv.LivePlotter(tk_title="Controls Feedback")
###
# Add 2D position / trajectory plot
###
trajectory_fig = lp_traj.plot_new_dynamic_2d_figure(
title='Vehicle Trajectory',
figsize=(FIGSIZE_X_INCHES, FIGSIZE_Y_INCHES),
edgecolor="black",
rect=[PLOT_LEFT, PLOT_BOT, PLOT_WIDTH, PLOT_HEIGHT])
trajectory_fig.set_invert_x_axis() # Because UE4 uses left-handed
# coordinate system the X
# axis in the graph is flipped
trajectory_fig.set_axis_equal() # X-Y spacing should be equal in size
# Add waypoint markers
trajectory_fig.add_graph("waypoints",
window_size=waypoints_np.shape[0],
x0=waypoints_np[:, 0],
y0=waypoints_np[:, 1],
linestyle="-",
marker="",
color='g')
# Add trajectory markers
trajectory_fig.add_graph("trajectory",
window_size=TOTAL_EPISODE_FRAMES,
x0=[start_x] * TOTAL_EPISODE_FRAMES,
y0=[start_y] * TOTAL_EPISODE_FRAMES,
color=[1, 0.5, 0])
# Add lookahead path
trajectory_fig.add_graph("lookahead_path",
window_size=INTERP_MAX_POINTS_PLOT,
x0=[start_x] * INTERP_MAX_POINTS_PLOT,
y0=[start_y] * INTERP_MAX_POINTS_PLOT,
color=[0, 0.7, 0.7],
linewidth=4)
# Add starting position marker
trajectory_fig.add_graph("start_pos",
window_size=1,
x0=[start_x],
y0=[start_y],
marker=11,
color=[1, 0.5, 0],
markertext="Start",
marker_text_offset=1)
# Add end position marker
trajectory_fig.add_graph("end_pos",
window_size=1,
x0=[waypoints_np[-1, 0]],
y0=[waypoints_np[-1, 1]],
marker="D",
color='r',
markertext="End",
marker_text_offset=1)
# Add car marker
trajectory_fig.add_graph("car",
window_size=1,
marker="s",
color='b',
markertext="Car",
marker_text_offset=1)
###
# Add 1D speed profile updater
###
forward_speed_fig =\
lp_1d.plot_new_dynamic_figure(title="Forward Speed (m/s)")
forward_speed_fig.add_graph("forward_speed",
label="forward_speed",
window_size=TOTAL_EPISODE_FRAMES)
forward_speed_fig.add_graph("reference_signal",
label="reference_Signal",
window_size=TOTAL_EPISODE_FRAMES)
# Add throttle signals graph
throttle_fig = lp_1d.plot_new_dynamic_figure(title="Throttle")
throttle_fig.add_graph("throttle",
label="throttle",
window_size=TOTAL_EPISODE_FRAMES)
# Add brake signals graph
brake_fig = lp_1d.plot_new_dynamic_figure(title="Brake")
brake_fig.add_graph("brake",
label="brake",
window_size=TOTAL_EPISODE_FRAMES)
# Add steering signals graph
steer_fig = lp_1d.plot_new_dynamic_figure(title="Steer")
steer_fig.add_graph("steer",
label="steer",
window_size=TOTAL_EPISODE_FRAMES)
# live plotter is disabled, hide windows
if not enable_live_plot:
lp_traj._root.withdraw()
lp_1d._root.withdraw()
# Iterate the frames until the end of the waypoints is reached or
# the TOTAL_EPISODE_FRAMES is reached. The controller simulation then
# ouptuts the results to the controller output directory.
reached_the_end = False
skip_first_frame = True
closest_index = 0 # Index of waypoint that is currently closest to
# the car (assumed to be the first index)
closest_distance = 0 # Closest distance of closest waypoint to car
counter = 0
#print("dssssssssssssssssssssssssssssssssssssssssssssssssssssssss",TOTAL_EPISODE_FRAMES)
for frame in range(TOTAL_EPISODE_FRAMES):
# Gather current data from the CARLA server
measurement_data, sensor_data = client.read_data()
#print("lllllllllllllllllllllllllll",len(waypoints_np),waypoints_np[-1])
#update_pts=list(waypoints_np[-1])
# Update pose, timestamp
current_x, current_y, current_yaw = \
get_current_pose(measurement_data)
current_speed = measurement_data.player_measurements.forward_speed
current_timestamp = float(measurement_data.game_timestamp) / 1000.0
#print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx",current_timestamp)
# Wait for some initial time before starting the demo
if current_timestamp <= WAIT_TIME_BEFORE_START:
send_control_command(client, throttle=0.0, steer=0, brake=1.0)
counter += 1
#flagbox=darknet_proper_fps.performDetect(res_img,thresh,configPath, weightPath, metaPath, showImage, makeImageOnly, initOnly,pointsmid)
continue
else:
current_timestamp = current_timestamp - WAIT_TIME_BEFORE_START
#print("sdmskdkfjdkfjdjksf",counter)
#for i in range(1,5):
update_pts = [list(sec1), list(sec2), list(sec3), list(sec4)]
pts_2d_ls = []
for i in range(len(update_pts)):
world_coord = np.asarray(update_pts[i]).reshape(4, -1)
# print("world_coord.shape",world_coord.shape)
# world_coord = np.array([[250.0 ,129.0 ,38.0 ,1.0]]).reshape(4,-1)
world_transform = Transform(
measurement_data.player_measurements.transform)
fdfd = str(world_transform)
sdsd = fdfd[1:-1].split('\n')
new = []
for i in sdsd:
dd = i[:-1].lstrip('[ ')
ff = re.sub("\s+", ",", dd.strip())
gg = ff.split(',')
new.append([float(i) for i in gg])
newworld = np.array(new)
extrinsic = np.matmul(newworld, newcam)
cam_coord = np.linalg.inv(extrinsic) @ world_coord
img_coord = intrinsic @ cam_coord[:3]
img_coord = np.array([
img_coord[0] / img_coord[2], img_coord[1] / img_coord[2],
img_coord[2]
])
if (img_coord[2] > 0):
x_2d = WINDOW_WIDTH - img_coord[0]
y_2d = WINDOW_HEIGHT - img_coord[1]
pts_2d_ls.append([x_2d, y_2d])
#x_diff=(pts_2d_ls[0]-get_2d_point[1])
#y_diff=(pts_2d_ls[1]-get_2d_point[0])
#print("sdsdjsdsjdksjdskdjk",x_diff,y_diff,pts_2d_ls,get_2d_point)
#get_2d_point=[pts_2d_ls[1],pts_2d_ls[0]]
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
im_bgr = cv2.cvtColor(image_RGB, cv2.COLOR_RGB2BGR)
image_depth = to_rgb_array(sensor_data['CameraDepth'])
counter += 1
if counter == 0:
flagbox = False
else:
#pointsmid[0][1]=pointsmid[0][1]+x_diff
#pointsmid[0][0]=pointsmid[0][0]+y_diff
#pointsmid[1][1]=pointsmid[1][1]+x_diff
#pointsmid[1][0]=pointsmid[1][0]+y_diff
#pointsmid[2][1]=pointsmid[2][1]+x_diff
#pointsmid[2][0]=pointsmid[2][0]+y_diff
#pointsmid[3][1]=pointsmid[3][1]+x_diff
#pointsmid[3][0]=pointsmid[3][0]+y_diff
try:
pointsmid[0][1] = pts_2d_ls[0][0]
pointsmid[0][0] = pts_2d_ls[0][1]
pointsmid[1][1] = pts_2d_ls[1][0]
pointsmid[1][0] = pts_2d_ls[1][1]
pointsmid[2][1] = pts_2d_ls[2][0]
pointsmid[2][0] = pts_2d_ls[2][1]
pointsmid[3][1] = pts_2d_ls[3][0]
pointsmid[3][0] = pts_2d_ls[3][1]
disbox = False
flagbox = darknet_proper_fps.performDetect(
im_bgr, thresh, configPath, weightPath, metaPath,
showImage, makeImageOnly, initOnly, pointsmid, disbox)
except Exception as e:
disbox = True
flagbox = darknet_proper_fps.performDetect(
im_bgr, thresh, configPath, weightPath, metaPath,
showImage, makeImageOnly, initOnly, pointsmid, disbox)
if flagbox != False:
midofpts = [(pointsmid[1][1] + pointsmid[0][1]) / 2,
(pointsmid[1][0] + pointsmid[0][0]) / 2]
depthflag = image_depth[int(flagbox[1]),
int(flagbox[0])]
depthpts = image_depth[int(midofpts[1]),
int(midofpts[0])]
print(depthflag, depthpts)
#cv2.circle(im_bgr,(int(flagbox[0]),int(flagbox[1])), 5, (255,0,255), -1)
#cv2.circle(im_bgr,(int(midofpts[0]),int(midofpts[1])), 5, (0,0,255), -1)
cv2.imwrite('./seg_out/{}_zz.jpg'.format(frame),
im_bgr)
scalenew = depthflag[
0] + depthflag[1] * 256 + depthflag[2] * 256 * 256
scalenew = scalenew / ((256 * 256 * 256) - 1)
depthflag = scalenew * 1000
scalenew = depthpts[
0] + depthpts[1] * 256 + depthpts[2] * 256 * 256
scalenew = scalenew / ((256 * 256 * 256) - 1)
depthpts = scalenew * 1000
diff = abs(depthflag - depthpts)
print("entereeeeeeeeeeeeeeeeeeeeeeeeeeeeherree", diff)
if diff < 10:
flagbox = True
else:
flagbox = False
print(e)
print("fffffffffffffffffff", flagbox)
x_history.append(current_x)
y_history.append(current_y)
yaw_history.append(current_yaw)
speed_history.append(current_speed)
time_history.append(current_timestamp)
if flagbox == False:
# Store history
###
# Controller update (this uses the controller2d.py implementation)
###
# To reduce the amount of waypoints sent to the controller,
# provide a subset of waypoints that are within some
# lookahead distance from the closest point to the car. Provide
# a set of waypoints behind the car as well.
# Find closest waypoint index to car. First increment the index
# from the previous index until the new distance calculations
# are increasing. Apply the same rule decrementing the index.
# The final index should be the closest point (it is assumed that
# the car will always break out of instability points where there
# are two indices with the same minimum distance, as in the
# center of a circle)
closest_distance = np.linalg.norm(
np.array([
waypoints_np[closest_index, 0] - current_x,
waypoints_np[closest_index, 1] - current_y
]))
new_distance = closest_distance
new_index = closest_index
while new_distance <= closest_distance:
closest_distance = new_distance
closest_index = new_index
new_index += 1
if new_index >= waypoints_np.shape[0]: # End of path
break
new_distance = np.linalg.norm(
np.array([
waypoints_np[new_index, 0] - current_x,
waypoints_np[new_index, 1] - current_y
]))
new_distance = closest_distance
new_index = closest_index
while new_distance <= closest_distance:
closest_distance = new_distance
closest_index = new_index
new_index -= 1
if new_index < 0: # Beginning of path
break
new_distance = np.linalg.norm(
np.array([
waypoints_np[new_index, 0] - current_x,
waypoints_np[new_index, 1] - current_y
]))
# Once the closest index is found, return the path that has 1
# waypoint behind and X waypoints ahead, where X is the index
# that has a lookahead distance specified by
# INTERP_LOOKAHEAD_DISTANCE
waypoint_subset_first_index = closest_index - 1
if waypoint_subset_first_index < 0:
waypoint_subset_first_index = 0
waypoint_subset_last_index = closest_index
total_distance_ahead = 0
while total_distance_ahead < INTERP_LOOKAHEAD_DISTANCE:
total_distance_ahead += wp_distance[
waypoint_subset_last_index]
waypoint_subset_last_index += 1
if waypoint_subset_last_index >= waypoints_np.shape[0]:
waypoint_subset_last_index = waypoints_np.shape[0] - 1
break
# Use the first and last waypoint subset indices into the hash
# table to obtain the first and last indicies for the interpolated
# list. Update the interpolated waypoints to the controller
# for the next controller update.
new_waypoints = \
wp_interp[wp_interp_hash[waypoint_subset_first_index]:\
wp_interp_hash[waypoint_subset_last_index] + 1]
controller.update_waypoints(new_waypoints)
# Update the other controller values and controls
controller.update_values(current_x, current_y, current_yaw,
current_speed, current_timestamp,
frame)
controller.update_controls()
cmd_throttle, cmd_steer, cmd_brake = controller.get_commands()
# Skip the first frame (so the controller has proper outputs)
if skip_first_frame and frame == 0:
pass
else:
# Update live plotter with new feedback
trajectory_fig.roll("trajectory", current_x, current_y)
trajectory_fig.roll("car", current_x, current_y)
# When plotting lookahead path, only plot a number of points
# (INTERP_MAX_POINTS_PLOT amount of points). This is meant
# to decrease load when live plotting
new_waypoints_np = np.array(new_waypoints)
path_indices = np.floor(
np.linspace(0, new_waypoints_np.shape[0] - 1,
INTERP_MAX_POINTS_PLOT))
trajectory_fig.update(
"lookahead_path",
new_waypoints_np[path_indices.astype(int), 0],
new_waypoints_np[path_indices.astype(int), 1],
new_colour=[0, 0.7, 0.7])
forward_speed_fig.roll("forward_speed", current_timestamp,
current_speed)
forward_speed_fig.roll("reference_signal",
current_timestamp,
controller._desired_speed)
throttle_fig.roll("throttle", current_timestamp,
cmd_throttle)
brake_fig.roll("brake", current_timestamp, cmd_brake)
steer_fig.roll("steer", current_timestamp, cmd_steer)
# Refresh the live plot based on the refresh rate
# set by the options
if enable_live_plot and \
live_plot_timer.has_exceeded_lap_period():
lp_traj.refresh()
lp_1d.refresh()
live_plot_timer.lap()
# Output controller command to CARLA server
send_control_command(client,
throttle=cmd_throttle,
steer=cmd_steer,
brake=cmd_brake)
# Find if reached the end of waypoint. If the car is within
# DIST_THRESHOLD_TO_LAST_WAYPOINT to the last waypoint,
# the simulation will end.
dist_to_last_waypoint = np.linalg.norm(
np.array([
waypoints[-1][0] -
current_x, ###########changed waypoints[-1]
waypoints[-1][1] - current_y
]))
if dist_to_last_waypoint < DIST_THRESHOLD_TO_LAST_WAYPOINT:
reached_the_end = True
if reached_the_end:
break
else:
send_control_command(client,
throttle=0.0,
steer=0.0,
brake=1.0)
break
# End of demo - Stop vehicle and Store outputs to the controller output
# directory.
if reached_the_end:
print("Reached the end of path. Writing to controller_output...")
send_control_command(client, throttle=0.0, steer=0.0, brake=1.0)
else:
print("stop!!!!.")
# Stop the car
#send_control_command(client, throttle=0.0, steer=0.0, brake=1.0)
# Store the various outputs
store_trajectory_plot(trajectory_fig.fig, 'trajectory.png')
store_trajectory_plot(forward_speed_fig.fig, 'forward_speed.png')
store_trajectory_plot(throttle_fig.fig, 'throttle_output.png')
store_trajectory_plot(brake_fig.fig, 'brake_output.png')
store_trajectory_plot(steer_fig.fig, 'steer_output.png')
write_trajectory_file(x_history, y_history, speed_history,
time_history)
def run_carla_client(host, port, far):
# Here we will run a single episode with 300 frames.
number_of_frames = 3000
frame_step = 10 # Save one image every 100 frames
output_folder = '_out'
image_size = [1280, 720]
camera_local_pos = [0.7, 0.0, 1.3] # [X, Y, Z]
camera_local_rotation = [0, 0, 0] # [pitch(Y), yaw(Z), roll(X)]
fov = 59
WINDOW_WIDTH = 1280
WINDOW_HEIGHT = 720
MINI_WINDOW_WIDTH = 320
MINI_WINDOW_HEIGHT = 180
WINDOW_WIDTH_HALF = WINDOW_WIDTH / 2
WINDOW_HEIGHT_HALF = WINDOW_HEIGHT / 2
k = np.identity(3)
k[0, 2] = WINDOW_WIDTH_HALF
k[1, 2] = WINDOW_HEIGHT_HALF
k[0, 0] = k[1, 1] = WINDOW_WIDTH / \
(2.0 * math.tan(59.0 * math.pi / 360.0))
intrinsic = k
# Connect with the server
with make_carla_client(host, port) as client:
print('CarlaClient connected')
# Here we load the settings.
settings = CarlaSettings()
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=20,
NumberOfPedestrians=40,
WeatherId=1)
settings.randomize_seeds()
camera1 = Camera('CameraDepth', PostProcessing='Depth', FOV=fov)
camera1.set_image_size(*image_size)
camera1.set_position(*camera_local_pos)
camera1.set_rotation(*camera_local_rotation)
settings.add_sensor(camera1)
camera2 = Camera('CameraRGB', PostProcessing='SceneFinal', FOV=fov)
camera2.set_image_size(*image_size)
camera2.set_position(*camera_local_pos)
camera2.set_rotation(*camera_local_rotation)
settings.add_sensor(camera2)
#scene = client.load_settings(settings)
client.load_settings(settings)
#print("sjdsjhdjshdjshdjshgds",scene.player_start_spots[0].location.x)
# Start at location index id '0'
client.start_episode(0)
# Compute the camera transform matrix
camera_to_car_transform = camera2.get_unreal_transform()
print("camera_to_car_transform", camera_to_car_transform)
carla_utils_obj = segmentation.carla_utils(intrinsic)
# Iterate every frame in the episode except for the first one.
for frame in range(1, number_of_frames):
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
# Save one image every 'frame_step' frames
if not frame % frame_step:
# Start transformations time mesure.
# RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]]
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
image_depth = to_rgb_array(sensor_data['CameraDepth'])
#measurements.player_measurements.transform.location.x-=40
#measurements.player_measurements.transform.location.z-=2
world_transform = Transform(
measurements.player_measurements.transform)
im_bgr = cv2.cvtColor(image_RGB, cv2.COLOR_RGB2BGR)
pos_vector = ([
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
])
fdfd = str(world_transform)
sdsd = fdfd[1:-1].split('\n')
new = []
for i in sdsd:
dd = i[:-1].lstrip('[ ')
ff = re.sub("\s+", ",", dd.strip())
gg = ff.split(',')
new.append([float(i) for i in gg])
newworld = np.array(new)
fdfd = str(camera_to_car_transform)
sdsd = fdfd[1:-1].split('\n')
new = []
for i in sdsd:
dd = i[:-1].lstrip('[ ')
ff = re.sub("\s+", ",", dd.strip())
gg = ff.split(',')
new.append([float(i) for i in gg])
newcam = np.array(new)
extrinsic = np.matmul(newworld, newcam)
get_2d_point = carla_utils_obj.run_seg(im_bgr, extrinsic,
pos_vector)
print(get_2d_point)
depth = image_depth[int(get_2d_point[0]), int(get_2d_point[1])]
scale = depth[0] + depth[1] * 256 + depth[2] * 256 * 256
scale = scale / ((256 * 256 * 256) - 1)
depth = scale * 1000
pos2d = np.array([(WINDOW_WIDTH - get_2d_point[1]) * depth,
(WINDOW_HEIGHT - get_2d_point[0]) * depth,
depth])
first = np.dot(np.linalg.inv(intrinsic), pos2d)
first = np.append(first, 1)
sec = np.matmul((extrinsic), first)
print("present", pos_vector)
print('goal-', sec)
client.send_control(
measurements.player_measurements.autopilot_control)
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):
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 _on_render(self):
datapoints = []
if self._main_image is not None and self._depth_image is not None:
# Convert main image
image = image_converter.to_rgb_array(self._main_image)
# Retrieve and draw datapoints
image, datapoints = self._generate_datapoints(image)
# Draw lidar
# Camera coordinate system is left, up, forwards
if VISUALIZE_LIDAR:
# Calculation to shift bboxes relative to pitch and roll of player
rotation = self._measurements.player_measurements.transform.rotation
pitch, roll, yaw = rotation.pitch, rotation.roll, rotation.yaw
# Since measurements are in degrees, convert to radians
pitch = degrees_to_radians(pitch)
roll = degrees_to_radians(roll)
yaw = degrees_to_radians(yaw)
print('pitch: ', pitch)
print('roll: ', roll)
print('yaw: ', yaw)
# Rotation matrix for pitch
rotP = np.array([[cos(pitch), 0, sin(pitch)], [0, 1, 0],
[-sin(pitch), 0, cos(pitch)]])
# Rotation matrix for roll
rotR = np.array([[1, 0, 0], [0, cos(roll), -sin(roll)],
[0, sin(roll), cos(roll)]])
# combined rotation matrix, must be in order roll, pitch, yaw
rotRP = np.matmul(rotR, rotP)
# Take the points from the point cloud and transform to car space
point_cloud = np.array(
self._lidar_to_car_transform.transform_points(
self._lidar_measurement.data))
point_cloud[:, 2] -= LIDAR_HEIGHT_POS
point_cloud = np.matmul(rotRP, point_cloud.T).T
# print(self._lidar_to_car_transform.matrix)
# print(self._camera_to_car_transform.matrix)
# Transform to camera space by the inverse of camera_to_car transform
point_cloud_cam = self._camera_to_car_transform.inverse(
).transform_points(point_cloud)
point_cloud_cam[:, 1] += LIDAR_HEIGHT_POS
image = lidar_utils.project_point_cloud(
image, point_cloud_cam, self._intrinsic, 1)
# Display image
surface = pygame.surfarray.make_surface(image.swapaxes(0, 1))
self._display.blit(surface, (0, 0))
if self._map_view is not None:
self._display_agents(self._map_view)
pygame.display.flip()
# Determine whether to save files
distance_driven = self._distance_since_last_recording()
#print("Distance driven since last recording: {}".format(distance_driven))
has_driven_long_enough = distance_driven is None or distance_driven > DISTANCE_SINCE_LAST_RECORDING
if (self._timer.step + 1) % STEPS_BETWEEN_RECORDINGS == 0:
if has_driven_long_enough and datapoints:
# Avoid doing this twice or unnecessarily often
if not VISUALIZE_LIDAR:
# Calculation to shift bboxes relative to pitch and roll of player
rotation = self._measurements.player_measurements.transform.rotation
pitch, roll, yaw = rotation.pitch, rotation.roll, rotation.yaw
# Since measurements are in degrees, convert to radians
pitch = degrees_to_radians(pitch)
roll = degrees_to_radians(roll)
yaw = degrees_to_radians(yaw)
print('pitch: ', pitch)
print('roll: ', roll)
print('yaw: ', yaw)
# Rotation matrix for pitch
rotP = np.array([[cos(pitch), 0,
sin(pitch)], [0, 1, 0],
[-sin(pitch), 0,
cos(pitch)]])
# Rotation matrix for roll
rotR = np.array([[1, 0, 0], [0,
cos(roll), -sin(roll)],
[0, sin(roll),
cos(roll)]])
# combined rotation matrix, must be in order roll, pitch, yaw
rotRP = np.matmul(rotR, rotP)
# Take the points from the point cloud and transform to car space
point_cloud = np.array(
self._lidar_to_car_transform.transform_points(
self._lidar_measurement.data))
point_cloud[:, 2] -= LIDAR_HEIGHT_POS
point_cloud = np.matmul(rotRP, point_cloud.T).T
self._update_agent_location()
# Save screen, lidar and kitti training labels together with calibration and groundplane files
self._save_training_files(datapoints, point_cloud)
self.captured_frame_no += 1
self._captured_frames_since_restart += 1
self._frames_since_last_capture = 0
else:
logging.debug(
"Could save datapoint, but agent has not driven {} meters since last recording (Currently {} meters)"
.format(DISTANCE_SINCE_LAST_RECORDING,
distance_driven))
else:
self._frames_since_last_capture += 1
logging.debug(
"Could not save training data - no visible agents of selected classes in scene"
)
def run_carla_client(args):
# Here we will run 3 episodes with 300 frames each.
number_of_episodes = 500000
frames_per_episode = 300
# We assume the CARLA server is already waiting for a client to connect at
# host:port. To create a connection we can use the `make_carla_client`
# context manager, it creates a CARLA client object and starts the
# connection. It will throw an exception if something goes wrong. The
# context manager makes sure the connection is always cleaned up on exit.
with make_carla_client(args.host, args.port) as client:
print('CarlaClient connected')
#########################
for episode in range(0, number_of_episodes):
# Start a new episode.
if args.settings_filepath is None:
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=0,
NumberOfPedestrians=0,
# WeatherId=random.choice([1, 3, 7, 8, 14]),
WeatherId=1,
QualityLevel=args.quality_level)
settings.randomize_seeds()
# Now we want to add a couple of cameras to the player vehicle.
# We will collect the images produced by these cameras every
# frame.
# The default camera captures RGB images of the scene.
camera0 = Camera('CameraRGB')
# Set image resolution in pixels.
camera0.set_image_size(210, 160)
# Set its position relative to the car in meters.
camera0.set_position(0.30, 0, 1.30)
settings.add_sensor(camera0)
# Let's add another camera producing ground-truth depth.
camera1 = Camera('CameraDepth', PostProcessing='Depth')
camera1.set_image_size(210, 160)
camera1.set_position(0.30, 0, 1.30)
settings.add_sensor(camera1)
if args.lidar:
lidar = Lidar('Lidar32')
lidar.set_position(0, 0, 2.50)
lidar.set_rotation(0, 0, 0)
lidar.set(
Channels=32,
Range=50,
PointsPerSecond=100000,
RotationFrequency=10,
UpperFovLimit=10,
LowerFovLimit=-30)
settings.add_sensor(lidar)
else:
# Alternatively, we can load these settings from a file.
with open(args.settings_filepath, 'r') as fp:
settings = fp.read()
# Now we load these settings into the server. The server replies
# with a scene description containing the available start spots for
# the player. Here we can provide a CarlaSettings object or a
# CarlaSettings.ini file as string.
scene = client.load_settings(settings)
# Choose one player start at random.
#从哪里开始
# number_of_player_starts = len(scene.player_start_spots)
# player_start = random.randint(0, max(0, number_of_player_starts - 1))
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print('Starting new episode at %r...' % scene.map_name)
#client.start_episode(player_start)
client.start_episode(0)
#--------------------------------------------------------------------
# Iterate every frame in the episode.
# Read the data produced by the server this frame.
# 这个应该是获取到的数据
measurements, sensor_data = client.read_data()
# Print some of the measurements.
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
image_RGB_real=image_RGB.flatten()
print(image_RGB_real)
print(image_RGB_real.shape[0])
###############################################################################################
if k==1:
player_measurements = measurements.player_measurements
loc = np.array([player_measurements.transform.location.x, player_measurements.transform.location.y])
RL = DeepQNetwork(n_actions=8,
n_features=image_RGB_real.shape[0],
learning_rate=0.01, e_greedy=0.9,
replace_target_iter=100, memory_size=2000,
e_greedy_increment=0.001, )
total_steps = 0
else:
total_steps = 0
print("rl运行完毕")
k=2
for frame in range(0, frames_per_episode):
measurements, sensor_data = client.read_data()
player_measurements = measurements.player_measurements
other_lane = 100 * player_measurements.intersection_otherlane
offroad = 100 * player_measurements.intersection_offroad
# loc1=np.array([player_measurements.transform.location.x, player_measurements.transform.location.y])
# print(offroad)
# print(other_lane)
# print(loc1)
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
image_RGB_real = image_RGB.flatten()
print_measurements(measurements)
observation = image_RGB_real
ep_r = 0
done = False
# while True:
# # env.render()
action = RL.choose_action(observation)
if not args.autopilot:
brake1=0.0
steer1=0.0
if (action > 4):
brake1 = actions[action]
else:
steer1 = actions[action]
client.send_control(
# steer=random.uniform(-1.0, 1.0),
steer=steer1,
throttle=0.6,
brake=brake1,
hand_brake=False,
reverse=False)
else:
# Together with the measurements, the server has sent the
# control that the in-game autopilot would do this frame. We
# can enable autopilot by sending back this control to the
# server. We can modify it if wanted, here for instance we
# will add some noise to the steer.
control = measurements.player_measurements.autopilot_control
# control.steer += random.uniform(-0.1, 0.1)
client.send_control(control)
# loc2=np.array([player_measurements.transform.location.x, player_measurements.transform.location.y])
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
image_RGB_rea2 = image_RGB.flatten()
observation_ = image_RGB_rea2
reward = -other_lane - offroad+np.sqrt(np.square(player_measurements.transform.location.x-271.0)+np.square(player_measurements.transform.location.y-129.5))
print(offroad)
if offroad > 20 or other_lane>10:
print(111111111111111111111)
done = True
# the smaller theta and closer to center the better
# x, x_dot, theta, theta_dot = observation_
# r1 = (env.x_threshold - abs(x))/env.x_threshold - 0.8
# r2 = (env.theta_threshold_radians - abs(theta))/env.theta_threshold_radians - 0.5
# reward = r1 + r2
RL.store_transition(observation, action, reward, observation_)
ep_r += reward
if total_steps > 100:
RL.learn(do_train=1)
if done:
print('episode: ',
'ep_r: ', round(ep_r, 2),
' epsilon: ', round(RL.epsilon, 2))
# client.start_episode(0)
break
observation = observation_
total_steps += 1
# RL.plot_cost()
# Save the images to disk if requested.
if args.save_images_to_disk:
for name, measurement in sensor_data.items():
filename = args.out_filename_format.format(episode, name, frame)
measurement.save_to_disk(filename)
continue
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 run_carla_client( args):
# Here we will run 3 episodes with 300 frames each.
number_of_episodes = 150
frames_per_episode = 500
# for start_i in range(150):
# if start_i%4==0:
# output_folder = 'Packages/CARLA_0.8.2/PythonClient/new_data-viz/test_' + str(start_i)
# if not os.path.exists(output_folder):
# os.mkdir(output_folder)
# print( "make "+str(output_folder))
# ./CarlaUE4.sh -carla-server -benchmark -fps=17 -windowed
# carla-server "/usr/local/carla/Unreal/CarlaUE4/CarlaUE4.uproject" /usr/local/carla/Maps/Town03 -benchmark -fps=10 -windowed
# We assume the CARLA server is already waiting for a client to connect at
# host:port. To create a connection we can use the `make_carla_client`
# context manager, it creates a CARLA client object and starts the
# connection. It will throw an exception if something goes wrong. The
# context manager makes sure the connection is always cleaned up on exit.
with make_carla_client(args.host, args.port) as client:
print('CarlaClient connected')
global episode_nbr
print (episode_nbr)
for episode in range(0,number_of_episodes):
if episode % 1 == 0:
output_folder = 'Datasets/carla-sync/train/test_' + str(episode)
if not os.path.exists(output_folder+"/cameras.p"):
# Start a new episode.
episode_nbr=episode
frame_step = 1 # Save one image every 100 frames
pointcloud_step=50
image_size = [800, 600]
camera_local_pos = [0.3, 0.0, 1.3] # [X, Y, Z]
camera_local_rotation = [0, 0, 0] # [pitch(Y), yaw(Z), roll(X)]
fov = 70
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=50,
NumberOfPedestrians=200,
WeatherId=random.choice([1, 3, 7, 8, 14]),
QualityLevel=args.quality_level)
settings.randomize_seeds()
# Now we want to add a couple of cameras to the player vehicle.
# We will collect the images produced by these cameras every
# frame.
camera1 = Camera('CameraDepth', PostProcessing='Depth', FOV=fov)
camera1.set_image_size(*image_size)
camera1.set_position(*camera_local_pos)
camera1.set_rotation(*camera_local_rotation)
settings.add_sensor(camera1)
camera2 = Camera('CameraRGB', PostProcessing='SceneFinal', FOV=fov)
camera2.set_image_size(*image_size)
camera2.set_position(*camera_local_pos)
camera2.set_rotation(*camera_local_rotation)
settings.add_sensor(camera2)
camera3 = Camera('CameraSeg', PostProcessing='SemanticSegmentation', FOV=fov)
camera3.set_image_size(*image_size)
camera3.set_position(*camera_local_pos)
camera3.set_rotation(*camera_local_rotation)
settings.add_sensor(camera3)
# Now we load these settings into the server. The server replies
# with a scene description containing the available start spots for
# the player. Here we can provide a CarlaSettings object or a
# CarlaSettings.ini file as string.
scene = client.load_settings(settings)
# Choose one player start at random.
number_of_player_starts = len(scene.player_start_spots)
player_start = episode#random.randint(0, max(0, number_of_player_starts - 1))
output_folder = 'Datasets/carla-sync/train/test_' + str(episode)
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print('Starting new episode...')
client.start_episode(player_start)
cameras_dict = {}
pedestrians_dict = {}
cars_dict = {}
# Compute the camera transform matrix
camera_to_car_transform = camera2.get_unreal_transform()
# (Intrinsic) (3, 3) K Matrix
K = np.identity(3)
K[0, 2] = image_size[0] / 2.0
K[1, 2] = image_size[1] / 2.0
K[0, 0] = K[1, 1] = image_size[0] / (2.0 * np.tan(fov * np.pi / 360.0))
with open(output_folder + '/camera_intrinsics.p', 'w') as camfile:
pickle.dump(K, camfile)
# Iterate every frame in the episode.
for frame in range(0, frames_per_episode):
# Read the data produced by the server this frame.
measurements, sensor_data = client.read_data()
# Print some of the measurements.
#print_measurements(measurements)
if not frame % frame_step:
# Save the images to disk if requested.
for name, measurement in sensor_data.items():
filename = args.out_filename_format.format(episode, name, frame)
print (filename)
measurement.save_to_disk(filename)
# We can access the encoded data of a given image as numpy
# array using its "data" property. For instance, to get the
# depth value (normalized) at pixel X, Y
#
# depth_array = sensor_data['CameraDepth'].data
# value_at_pixel = depth_array[Y, X]
#
# Now we have to send the instructions to control the vehicle.
# If we are in synchronous mode the server will pause the
# simulation until we send this control.
# RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]]
image_RGB = to_rgb_array(sensor_data['CameraRGB'])
labels=labels_to_array(sensor_data['CameraSeg'])[:,:,np.newaxis]
image_seg = np.tile(labels, (1, 1, 3))
depth_array = sensor_data['CameraDepth'].data*1000
# 2d to (camera) local 3d
# We use the image_RGB to colorize each 3D point, this is optional.
# "max_depth" is used to keep only the points that are near to the
# camera, meaning 1.0 the farest points (sky)
if not frame % pointcloud_step:
point_cloud = depth_to_local_point_cloud(
sensor_data['CameraDepth'],
image_RGB,
max_depth=args.far
)
point_cloud_seg = depth_to_local_point_cloud(
sensor_data['CameraDepth'],
segmentation=image_seg,
max_depth=args.far
)
# (Camera) local 3d to world 3d.
# Get the transform from the player protobuf transformation.
world_transform = Transform(
measurements.player_measurements.transform
)
# Compute the final transformation matrix.
car_to_world_transform = world_transform * camera_to_car_transform
# Car to World transformation given the 3D points and the
# transformation matrix.
point_cloud.apply_transform(car_to_world_transform)
point_cloud_seg.apply_transform(car_to_world_transform)
Rt = car_to_world_transform.matrix
Rt_inv = car_to_world_transform.inverse().matrix
# R_inv=world_transform.inverse().matrix
cameras_dict[frame] = {}
cameras_dict[frame]['inverse_rotation'] = Rt_inv[:]
cameras_dict[frame]['rotation'] = Rt[:]
cameras_dict[frame]['translation'] = Rt_inv[0:3, 3]
cameras_dict[frame]['camera_to_car'] = camera_to_car_transform.matrix
# Get non-player info
vehicles = {}
pedestrians = {}
for agent in measurements.non_player_agents:
# check if the agent is a vehicle.
if agent.HasField('vehicle'):
pos = agent.vehicle.transform.location
pos_vector = np.array([[pos.x], [pos.y], [pos.z], [1.0]])
trnasformed_3d_pos = np.dot(Rt_inv, pos_vector)
pos2d = np.dot(K, trnasformed_3d_pos[:3])
# Normalize the point
norm_pos2d = np.array([
pos2d[0] / pos2d[2],
pos2d[1] / pos2d[2],
pos2d[2]])
# Now, pos2d contains the [x, y, d] values of the image in pixels (where d is the depth)
# You can use the depth to know the points that are in front of the camera (positive depth).
x_2d = image_size[0] - norm_pos2d[0]
y_2d = image_size[1] - norm_pos2d[1]
vehicles[agent.id] = {}
vehicles[agent.id]['transform'] = [agent.vehicle.transform.location.x,
agent.vehicle.transform.location.y,
agent.vehicle.transform.location.z]
vehicles[agent.id][
'bounding_box.transform'] = agent.vehicle.bounding_box.transform.location.z
vehicles[agent.id]['yaw'] = agent.vehicle.transform.rotation.yaw
vehicles[agent.id]['bounding_box'] = [agent.vehicle.bounding_box.extent.x,
agent.vehicle.bounding_box.extent.y,
agent.vehicle.bounding_box.extent.z]
vehicle_transform = Transform(agent.vehicle.bounding_box.transform)
pos = agent.vehicle.transform.location
bbox3d = agent.vehicle.bounding_box.extent
# Compute the 3D bounding boxes
# f contains the 4 points that corresponds to the bottom
f = np.array([[pos.x + bbox3d.x, pos.y - bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.bounding_box.transform.location.z],
[pos.x + bbox3d.x, pos.y + bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.bounding_box.transform.location.z],
[pos.x - bbox3d.x, pos.y + bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.bounding_box.transform.location.z],
[pos.x - bbox3d.x, pos.y - bbox3d.y,
pos.z - bbox3d.z + agent.vehicle.bounding_box.transform.location.z],
[pos.x + bbox3d.x, pos.y - bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.bounding_box.transform.location.z],
[pos.x + bbox3d.x, pos.y + bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.bounding_box.transform.location.z],
[pos.x - bbox3d.x, pos.y + bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.bounding_box.transform.location.z],
[pos.x - bbox3d.x, pos.y - bbox3d.y,
pos.z + bbox3d.z + agent.vehicle.bounding_box.transform.location.z]])
f_rotated = vehicle_transform.transform_points(f)
f_2D_rotated = []
vehicles[agent.id]['bounding_box_coord'] = f_rotated
for i in range(f.shape[0]):
point = np.array([[f_rotated[i, 0]], [f_rotated[i, 1]], [f_rotated[i, 2]], [1]])
transformed_2d_pos = np.dot(Rt_inv, point)
pos2d = np.dot(K, transformed_2d_pos[:3])
norm_pos2d = np.array([
pos2d[0] / pos2d[2],
pos2d[1] / pos2d[2],
pos2d[2]])
# print([image_size[0] - (pos2d[0] / pos2d[2]), image_size[1] - (pos2d[1] / pos2d[2])])
f_2D_rotated.append(
np.array([image_size[0] - norm_pos2d[0], image_size[1] - norm_pos2d[1]]))
vehicles[agent.id]['bounding_box_2D'] = f_2D_rotated
elif agent.HasField('pedestrian'):
pedestrians[agent.id] = {}
pedestrians[agent.id]['transform'] = [agent.pedestrian.transform.location.x,
agent.pedestrian.transform.location.y,
agent.pedestrian.transform.location.z]
pedestrians[agent.id]['yaw'] = agent.pedestrian.transform.rotation.yaw
pedestrians[agent.id]['bounding_box'] = [agent.pedestrian.bounding_box.extent.x,
agent.pedestrian.bounding_box.extent.y,
agent.pedestrian.bounding_box.extent.z]
# get the needed transformations
# remember to explicitly make it Transform() so you can use transform_points()
pedestrian_transform = Transform(agent.pedestrian.transform)
bbox_transform = Transform(agent.pedestrian.bounding_box.transform)
# get the box extent
ext = agent.pedestrian.bounding_box.extent
# 8 bounding box vertices relative to (0,0,0)
bbox = np.array([
[ ext.x, ext.y, ext.z],
[- ext.x, ext.y, ext.z],
[ ext.x, - ext.y, ext.z],
[- ext.x, - ext.y, ext.z],
[ ext.x, ext.y, - ext.z],
[- ext.x, ext.y, - ext.z],
[ ext.x, - ext.y, - ext.z],
[- ext.x, - ext.y, - ext.z]
])
# transform the vertices respect to the bounding box transform
bbox = bbox_transform.transform_points(bbox)
# the bounding box transform is respect to the pedestrian transform
# so let's transform the points relative to it's transform
bbox = pedestrian_transform.transform_points(bbox)
# pedestrian's transform is relative to the world, so now,
# bbox contains the 3D bounding box vertices relative to the world
pedestrians[agent.id]['bounding_box_coord'] =copy.deepcopy(bbox)
# Additionally, you can print these vertices to check that is working
f_2D_rotated=[]
ys=[]
xs=[]
zs=[]
for vertex in bbox:
pos_vector = np.array([
[vertex[0,0]], # [[X,
[vertex[0,1]], # Y,
[vertex[0,2]], # Z,
[1.0] # 1.0]]
])
# transform the points to camera
transformed_3d_pos =np.dot(Rt_inv, pos_vector)# np.dot(inv(self._extrinsic.matrix), pos_vector)
zs.append(transformed_3d_pos[2])
# transform the points to 2D
pos2d =np.dot(K, transformed_3d_pos[:3]) #np.dot(self._intrinsic, transformed_3d_pos[:3])
# normalize the 2D points
pos2d = np.array([
pos2d[0] / pos2d[2],
pos2d[1] / pos2d[2],
pos2d[2]
])
# print the points in the screen
if pos2d[2] > 0: # if the point is in front of the camera
x_2d = image_size[0]-pos2d[0]#WINDOW_WIDTH - pos2d[0]
y_2d = image_size[1]-pos2d[1]#WINDOW_HEIGHT - pos2d[1]
ys.append(y_2d)
xs.append(x_2d)
f_2D_rotated.append( (y_2d, x_2d))
if len(xs)>1:
bbox=[[int(min(xs)), int(max(xs))],[int(min(ys)), int(max(ys))]]
clipped_seg=labels[bbox[1][0]:bbox[1][1],bbox[0][0]:bbox[0][1]]
recounted = Counter(clipped_seg.flatten())
if 4 in recounted.keys() and recounted[4]>0.1*len(clipped_seg.flatten()):
clipped_depth=depth_array[bbox[1][0]:bbox[1][1],bbox[0][0]:bbox[0][1]]
#print (clipped_depth.shape)
people_indx=np.where(clipped_seg==4)
masked_depth=[]
for people in zip(people_indx[0],people_indx[1] ):
#print(people)
masked_depth.append(clipped_depth[people])
#masked_depth=clipped_depth[np.where(clipped_seg==4)]
#print (masked_depth)
#print ("Depth "+ str(min(zs))+" "+ str(max(zs)))
#recounted = Counter(masked_depth)
#print(recounted)
avg_depth=np.mean(masked_depth)
if avg_depth<700 and avg_depth>=min(zs)-10 and avg_depth<= max(zs)+10:
#print("Correct depth")
pedestrians[agent.id]['bounding_box_2D'] = f_2D_rotated
pedestrians[agent.id]['bounding_box_2D_size']=recounted[4]
pedestrians[agent.id]['bounding_box_2D_avg_depth']=avg_depth
pedestrians[agent.id]['bounding_box_2D_depths']=zs
#print ( pedestrians[agent.id].keys())
#else:
# print(recounted)
# print ("Depth "+ str(min(zs))+" "+ str(max(zs)))
#if sum(norm(depth_array-np.mean(zs))<1.0):
# pedestrians[agent.id] = {}
# pedestrians[agent.id]['transform'] = [agent.pedestrian.transform.location.x,
# agent.pedestrian.transform.location.y,
# agent.pedestrian.transform.location.z]
# pedestrians[agent.id]['yaw'] = agent.pedestrian.transform.rotation.yaw
# pedestrians[agent.id]['bounding_box'] = [agent.pedestrian.bounding_box.extent.x,
# agent.pedestrian.bounding_box.extent.y,
# agent.pedestrian.bounding_box.extent.z]
# vehicle_transform = Transform(agent.pedestrian.bounding_box.transform)
# pos = agent.pedestrian.transform.location
#
# bbox3d = agent.pedestrian.bounding_box.extent
#
# # Compute the 3D bounding boxes
# # f contains the 4 points that corresponds to the bottom
# f = np.array([[pos.x + bbox3d.x, pos.y - bbox3d.y, pos.z- bbox3d.z ],
# [pos.x + bbox3d.x, pos.y + bbox3d.y, pos.z- bbox3d.z ],
# [pos.x - bbox3d.x, pos.y + bbox3d.y, pos.z- bbox3d.z ],
# [pos.x - bbox3d.x, pos.y - bbox3d.y, pos.z- bbox3d.z ],
# [pos.x + bbox3d.x, pos.y - bbox3d.y, pos.z + bbox3d.z],
# [pos.x + bbox3d.x, pos.y + bbox3d.y, pos.z + bbox3d.z],
# [pos.x - bbox3d.x, pos.y + bbox3d.y, pos.z + bbox3d.z],
# [pos.x - bbox3d.x, pos.y - bbox3d.y, pos.z + bbox3d.z]])
#
# f_rotated = pedestrian_transform.transform_points(f)
# pedestrians[agent.id]['bounding_box_coord'] = f_rotated
# f_2D_rotated = []
#
# for i in range(f.shape[0]):
# point = np.array([[f_rotated[i, 0]], [f_rotated[i, 1]], [f_rotated[i, 2]], [1]])
# transformed_2d_pos = np.dot(Rt_inv, point)
# pos2d = np.dot(K, transformed_2d_pos[:3])
# norm_pos2d = np.array([
# pos2d[0] / pos2d[2],
# pos2d[1] / pos2d[2],
# pos2d[2]])
# f_2D_rotated.append([image_size[0] - norm_pos2d[0], image_size[1] - norm_pos2d[1]])
# pedestrians[agent.id]['bounding_box_2D'] = f_2D_rotated
cars_dict[frame] = vehicles
pedestrians_dict[frame] = pedestrians
#print("End of Episode")
#print(len(pedestrians_dict[frame]))
# Save PLY to disk
# This generates the PLY string with the 3D points and the RGB colors
# for each row of the file.
if not frame % pointcloud_step:
point_cloud.save_to_disk(os.path.join(
output_folder, '{:0>5}.ply'.format(frame))
)
point_cloud_seg.save_to_disk(os.path.join(
output_folder, '{:0>5}_seg.ply'.format(frame))
)
if not args.autopilot:
client.send_control(
hand_brake=True)
else:
# Together with the measurements, the server has sent the
# control that the in-game autopilot would do this frame. We
# can enable autopilot by sending back this control to the
# server. We can modify it if wanted, here for instance we
# will add some noise to the steer.
control = measurements.player_measurements.autopilot_control
control.steer += random.uniform(-0.1, 0.1)
client.send_control(control)
print ("Start pickle save")
with open(output_folder + '/cameras.p', 'w') as camerafile:
pickle.dump(cameras_dict, camerafile)
with open(output_folder + '/people.p', 'w') as peoplefile:
pickle.dump(pedestrians_dict, peoplefile)
with open(output_folder + '/cars.p', 'w') as carfile:
pickle.dump(cars_dict, carfile)
print ("Episode done")
def render(self, camera_to_render, objects_to_render):
"""
Main rendering function. Render a main camera and a map containing several objects
Args:
camera_to_render: The sensor data, images you want to render on the window
objects_to_render: A dictionary Several objects to be rendered
player_position: The current player position
waypoints: The waypoints , next positions. If you want to plot then.
agents_positions: All agent positions ( vehicles)
Returns:
None
"""
if camera_to_render is not None:
array = image_converter.to_rgb_array(camera_to_render)
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
self._display.blit(surface, (0, 0))
# only if the map view setting is set we actually plot all the positions and waypoints
if self._map_view is not None:
player_position = self._map.convert_to_pixel([
objects_to_render['player_transform'].location.x,
objects_to_render['player_transform'].location.y,
objects_to_render['player_transform'].location.z
])
player_orientation = objects_to_render[
'player_transform'].orientation
array = self._map_view
array = array[:, :, :3]
new_window_width = \
(float(self._window_height) / float(self._map_shape[0])) * \
float(self._map_shape[1])
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
# Draw other two fovs
if objects_to_render['fov_list'] is not None:
fov_1 = objects_to_render['fov_list'][0]
fov_2 = objects_to_render['fov_list'][1]
self._draw_fov(surface,
player_position,
vector_to_degrees([
player_orientation.x, player_orientation.y
]),
radius=fov_1[0] / (0.1643 * 2),
angle=fov_1[1],
color=[255, 128, 0, 255])
self._draw_fov(surface,
player_position,
vector_to_degrees([
player_orientation.x, player_orientation.y
]),
radius=fov_2[0] / (0.1643 * 2),
angle=fov_2[1],
color=[128, 64, 0, 255])
if objects_to_render['waypoints'] is not None:
self._draw_waypoints(surface, objects_to_render['waypoints'])
if objects_to_render['route'] is not None:
self._draw_route(surface, objects_to_render['route'])
self._draw_goal_position(surface)
# Draw the player positions
w_pos = int(
player_position[0] *
(float(self._window_height) / float(self._map_shape[0])))
h_pos = int(player_position[1] *
(new_window_width / float(self._map_shape[1])))
pygame.draw.circle(surface, [0, 0, 0, 255], (w_pos, h_pos), 5, 0)
for agent in objects_to_render['agents']:
if agent.HasField('pedestrian'
) and objects_to_render['draw_pedestrians']:
if agent.id in objects_to_render['active_agents_ids']:
color = [255, 0, 255, 255]
else:
color = [0, 0, 255, 255]
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(self._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, color, (w_pos, h_pos), 2, 0)
if agent.HasField('traffic_light') and objects_to_render[
'draw_traffic_lights']:
if agent.id in objects_to_render['active_agents_ids']:
color = [255, 0, 0, 255]
else:
color = [0, 255, 0, 255]
agent_position = self._map.convert_to_pixel([
agent.traffic_light.transform.location.x,
agent.traffic_light.transform.location.y,
agent.traffic_light.transform.location.z
])
w_pos = int(agent_position[0] *
(float(self._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, color, (w_pos, h_pos), 3, 0)
if agent.HasField(
'vehicle') and objects_to_render['draw_vehicles']:
if agent.id in objects_to_render['active_agents_ids']:
color = [255, 0, 255, 255]
else:
color = [0, 0, 255, 255]
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(self._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, color, (w_pos, h_pos), 3, 0)
self._display.blit(surface, (self._window_width, 0))
self._render_iter += 1
pygame.display.flip()
