def _on_new_episode(self):
self._carla_settings.randomize_seeds()
self._carla_settings.randomize_weather()
scene = self.client.load_settings(self._carla_settings)
if self._display_map:
self._city_name = scene.map_name
number_of_player_starts = len(scene.player_start_spots)
player_start = np.random.randint(number_of_player_starts)
print('Starting new episode...')
self.client.start_episode(player_start)
self._timer = Timer()
self._is_on_reverse = False
# increment episode count, reset frame count
self.episode_count += 1
self.frame_count = 0
if self.saver is None and self.save_images_to_disk:
self.saver = [
# BufferedImageSaver(f'{self.out_dir}5/ep_{self.episode_count:03d}/CameraDepth/',
# 1000, WINDOW_HEIGHT, WINDOW_WIDTH, 1),
BufferedImageSaver('{}/ep_{}/'.format(self.out_dir,self.episode_count),
1000, WINDOW_HEIGHT, WINDOW_WIDTH, 3, 'CameraRGB'),
BufferedImageSaver('{}/ep_{}/'.format(self.out_dir,self.episode_count),
1000, WINDOW_HEIGHT, WINDOW_WIDTH, 1, 'CameraSemSeg')
]
elif self.save_images_to_disk:
for i, image_saver in enumerate(self.saver):
image_saver.save()
image_saver.reset()
image_saver.filename = '{}/ep_{}/'.format(self.out_dir,self.episode_count) +\
image_saver.sensorname + '/'
def __init__(self, parent_actor, hud):
self.sensor = None
self.surface = None
self._parent = parent_actor
self.hud = hud
self.recording = False
self._camera_transforms = [
carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)),
carla.Transform(carla.Location(x=1.6, z=1.7))]
self.transform_index = 1
self.sensors = [
['sensor.camera.rgb', cc.Raw, 'Camera RGB'],
['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'],
['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'],
['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'],
['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'],
['sensor.camera.semantic_segmentation', cc.CityScapesPalette,
'Camera Semantic Segmentation (CityScapes Palette)'],
['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']]
world = self._parent.get_world()
bp_library = world.get_blueprint_library()
for item in self.sensors:
bp = bp_library.find(item[0])
if item[0].startswith('sensor.camera'):
bp.set_attribute('image_size_x', str(hud.dim[0]))
bp.set_attribute('image_size_y', str(hud.dim[1]))
elif item[0].startswith('sensor.lidar'):
bp.set_attribute('range', '5000')
## rotation freq
bp.set_attribute('rotation_frequency', '20')
item.append(bp)
self.index = None
########################################################################
## create custom sensors for collecting rgb image and Lidar data
#'''
self.sensor_rgb = None
self.sensor_lidar = None
# listen and record control signals on each image
self.control_listener = None
print("init custum rgb and Lidar sensors")
if self.sensor_rgb is not None:
self.sensor_rgb.destroy()
#self.surface = None
self.sensor_rgb = self._parent.get_world().spawn_actor(
self.sensors[0][-1], # rgb image
self._camera_transforms[self.transform_index],
attach_to=self._parent)
# We need to pass the lambda a weak reference to self to avoid
# circular reference.
weak_self = weakref.ref(self)
self.sensor_rgb.listen(lambda image: CameraManager._parse_image_rgblidar(weak_self, image, 0))
#'''
## Lidar
#'''
#print(self.sensors[6])
#['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)', <carla.libcarla.ActorBlueprint object at 0x7f1b038fd570>]
#assert(0)
if self.sensor_lidar is not None:
self.sensor_lidar.destroy()
#self.surface = None
self.sensor_lidar = self._parent.get_world().spawn_actor(
self.sensors[6][-1], # Lidar image
self._camera_transforms[self.transform_index],
attach_to=self._parent)
# We need to pass the lambda a weak reference to self to avoid
# circular reference.
#weak_self = weakref.ref(self)
self.sensor_lidar.listen(lambda image: CameraManager._parse_image_rgblidar(weak_self, image, 6))
#'''
## Control signals
'''
if self.control_listener is not None:
self.control_listener.destroy()
# sync with image input, so use sensors[0]
self.control_listener = self._parent.get_world().spawn_actor(
self.sensors[0][-1], # rgb image
self._camera_transforms[self.transform_index],
attach_to=self._parent)
self.control_listener.listen(lambda image: CameraManager._parse_image_control(weak_self, image, world))
'''
## image saver
self.out_dir = "_out_data"
self.episode_count = 0
#print('%s/ep_%d/' % (self.out_dir, self.episode_count))
#assert(0)
self.saver = [
# BufferedImageSaver(f'{self.out_dir}5/ep_{self.episode_count:03d}/CameraDepth/',
# 1000, WINDOW_HEIGHT, WINDOW_WIDTH, 1),
BufferedImageSaver('%s/ep_%d/' % (self.out_dir, self.episode_count),
100, WINDOW_HEIGHT, WINDOW_WIDTH, 3, 'CameraRGB'),
BufferedImageSaver('%s/ep_%d/' % (self.out_dir, self.episode_count),
100, 8000, 1, 3, 'Lidar')
## TODO: current limit 5000 points, might be more
]
def game_loop(args):
global current_rgb_frame
global current_lidar_frame
pygame.init()
pygame.font.init()
world = None
#########################################################
## set up nural net
## GPU
device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu")
# Assuming that we are on a CUDA machine, this should print a CUDA device:
print(device)
'''
## CNN/rgb
net = Net()
## load pre trained model
net.load_state_dict(torch.load('/home/zidong/Desktop/nn/6_5.pth.tar'))
net.to(device)
'''
## set up pointnet - Lidar
num_classes = 1
feature_transform = False
#net = PointNetCls(k=num_classes, feature_transform=False)
#net = PointNetCls(k=num_classes, feature_transform=True)
net = PointFusion(k=1, feature_transform=feature_transform)
#cls_50_6.7_4training
#weights_path = '/home/zidong/Desktop/nn/pointFusion/train_model_6.11_5/cls_model_22.pth'
#weights_path = '/home/zidong/Desktop/nn/pointFusion/t_6.11_250/cls_model_499.pth'
weights_path = '/home/zidong/Desktop/nn/pointFusion/6.12_3/cls_model_499.pth'
print('loading pretrained model from.. '+ weights_path)
#'/home/zidong/Desktop/pointnet.pytorch/utils/cls/cls_model_19.pth'
net.load_state_dict(torch.load(weights_path))
#net.to(device)
net.cuda()
##########################################################
try:
client = carla.Client(args.host, args.port)
client.set_timeout(2.0)
display = pygame.display.set_mode(
(args.width, args.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
hud = HUD(args.width, args.height)
## world settings: synchronous_mode TODO not sure..
carla_world = client.get_world()
settings = carla_world.get_settings()
settings.synchronous_mode = True
carla_world.apply_settings(settings)
frame = None
count_frame = 0
out_dir = "_out_data"
episode_count = 0
## save control signal: throttle, steer, brake, speed
saver_control = BufferedImageSaver('%s/ep_%d/' % (out_dir, episode_count),
100, 1, 1, 4, 'Control')
#####################################
world = World(client.get_world(), hud, args.filter)
controller = DualControl(world, args.autopilot)
clock = pygame.time.Clock()
while True:
clock.tick_busy_loop(60)
if controller.parse_events(world, clock):
carla_world.tick()
ts = carla_world.wait_for_tick()
return
world.tick(clock)
#################################################
if not world.manual:
## set custom control
cust_ctrl = controller._control
cust_ctrl.throttle = 0.6 # 18km/h
# get image data from sensor - current_rgb_frame
if (current_lidar_frame is not None) and (current_rgb_frame is not None):
# print(current_lidar_frame)
## Lidar pt cloud
tst_inputs = np.asarray(current_lidar_frame, dtype=np.float32)
# test if there's large points
sum_ = np.sum(np.absolute(tst_inputs), axis=1)
mask = np.logical_and(sum_ < 50*3, sum_ > 0.0001)
pts_filter = tst_inputs[mask]
if(pts_filter.shape != tst_inputs.shape):
print('pts filter : pts =', pts_filter.shape, tst_inputs.shape)
tst_inputs = torch.from_numpy(tst_inputs)
print(tst_inputs.shape)
tst_inputs = tst_inputs[0:1900,:] #
tst_inputs = tst_inputs.unsqueeze(0)
tst_inputs = tst_inputs.transpose(2, 1)
tst_inputs = tst_inputs.cuda()#.to(device)
points = tst_inputs
#print(tst_inputs)
## images
raw_img = np.frombuffer(current_rgb_frame.raw_data, dtype=np.uint8)
raw_img = raw_img.reshape(720, 1280, -1)
raw_img = raw_img[:, :, :3]
raw_img = cv2.resize(raw_img, dsize=(180, 180))
#print(raw_img)
tst_inputs = raw_img /255 *2 -1
tst_inputs = np.transpose(tst_inputs, (2, 0, 1))
tst_inputs = np.asarray(tst_inputs, dtype=np.float32)
tst_inputs = torch.from_numpy(tst_inputs)
tst_inputs = tst_inputs.unsqueeze(0)
tst_inputs = tst_inputs.cuda()
image = tst_inputs
net = net.eval()
## get Lidar input
outputs = net(image, points)
print(outputs)
outputs = outputs[0].detach().squeeze().tolist()
print(outputs)
cust_ctrl.steer = outputs
world.player.apply_control(cust_ctrl)
count_frame += 1
## control
c = world.player.get_control()
throttle = c.throttle # 0.0, 1.0
steer = c.steer #-1.0, 1.0
brake = c.brake # 0.0, 1.0
#print(throttle, steer, brake)
v = world.player.get_velocity()
speed = 3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)
#print('Speed: % 15.0f km/h' % (speed))
#print(count_frame)
control = np.array([throttle, steer, brake, speed])
#saver_control.add_image(control, "Control")
## sync mode TODO not sure..
carla_world.tick()
ts = carla_world.wait_for_tick()
#####################################
world.render(display)
pygame.display.flip()
finally:
if world is not None:
world.destroy()
pygame.quit()
class World(object):
def __init__(self, carla_world, args):
self.world = carla_world
self.actor_role_name = 'hero' #args.rolename
self.map = self.world.get_map()
self.player = None
self.camera_rgb = None
#self.camera_rgb_1 = None
#self.camera_rgb_2 = None
#self.camera_rgb_3 = None
self.camera_lidar = None
self.actor_list = []
self._actor_filter = args.filter
self.fps = args.fps
self.restart()
self.frame = None
self.delta_seconds = 1.0 / int(args.fps)
self._queues = []
self._settings = None
self.world.wait_for_tick()
def restart(self):
blueprint = self.world.get_blueprint_library().find('vehicle.tesla.model3')
blueprint.set_attribute('role_name', self.actor_role_name)
if blueprint.has_attribute('color'):
color = random.choice(blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
if blueprint.has_attribute('driver_id'):
driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values)
blueprint.set_attribute('driver_id', driver_id)
if blueprint.has_attribute('is_invincible'):
blueprint.set_attribute('is_invincible', 'true')
# Spawn the player.
if self.player is not None:
spawn_point = self.player.get_transform()
spawn_point.location.z += 2.0
spawn_point.rotation.roll = 0.0
spawn_point.rotation.pitch = 0.0
self.destroy()
print("spawn point is : " + spawn_point)
self.player = self.world.try_spawn_actor(blueprint, spawn_point)
while self.player is None:
spawn_points = self.map.get_spawn_points()
spawn_point = spawn_points[0] if spawn_points else carla.Transform()
spawn_point.location.z += 2.0
spawn_point.rotation.roll = 0.0
spawn_point.rotation.pitch = 0.0
self.player = self.world.try_spawn_actor(blueprint, spawn_point)
print('Spawning player at location = ', self.player.get_location())
self.actor_list.append(self.player)
print("Initializing custom rgb and lidar sensors")
bound_y = 0.5 + self.player.bounding_box.extent.y
sensor_location = carla.Location(x=1.6, z=1.7)
if self.camera_rgb is not None:
self.camera_rgb.destroy()
bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
bp.set_attribute('image_size_x', '1280')
bp.set_attribute('image_size_y', '720')
# self.camera_rgb = self.world.spawn_actor(
# bp,
# carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)),
# attach_to=self.player)
self.camera_rgb = self.world.spawn_actor(bp, carla.Transform(sensor_location), attach_to=self.player)
self.actor_list.append(self.camera_rgb)
'''
if self.camera_rgb_1 is not None:
self.camera_rgb_1.destroy()
bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
bp.set_attribute('image_size_x', '1280')
bp.set_attribute('image_size_y', '720')
self.camera_rgb_1 = self.world.spawn_actor(bp, carla.Transform(sensor_location, carla.Rotation(yaw=90)), attach_to=self.player)
self.actor_list.append(self.camera_rgb_1)
if self.camera_rgb_2 is not None:
self.camera_rgb_2.destroy()
bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
bp.set_attribute('image_size_x', '1280')
bp.set_attribute('image_size_y', '720')
self.camera_rgb_2 = self.world.spawn_actor(bp, carla.Transform(sensor_location, carla.Rotation(yaw=180)), attach_to=self.player)
self.actor_list.append(self.camera_rgb_2)
if self.camera_rgb_3 is not None:
self.camera_rgb_3.destroy()
bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
bp.set_attribute('image_size_x', '1280')
bp.set_attribute('image_size_y', '720')
self.camera_rgb_3 = self.world.spawn_actor(bp, carla.Transform(sensor_location, carla.Rotation(yaw=270)), attach_to=self.player)
self.actor_list.append(self.camera_rgb_3)
'''
if self.camera_lidar is not None:
self.camera_lidar.destroy()
bp = self.world.get_blueprint_library().find('sensor.lidar.ray_cast')
bp.set_attribute('range', '5000')
bp.set_attribute('rotation_frequency', self.fps)
self.camera_lidar = self.world.spawn_actor(bp, carla.Transform(sensor_location), attach_to=self.player)
self.actor_list.append(self.camera_lidar)
self.rgb_saver = BufferedImageSaver('%s/ep_%d/' % (out_dir, episode_count),
100, 1280, 720, 3, 'CameraRGB')
'''
self.rgb_saver_1 = BufferedImageSaver('%s/ep_%d/' % (out_dir, episode_count),
100, 1280, 720, 3, 'CameraRGB_1')
self.rgb_saver_2 = BufferedImageSaver('%s/ep_%d/' % (out_dir, episode_count),
100, 1280, 720, 3, 'CameraRGB_2')
self.rgb_saver_3 = BufferedImageSaver('%s/ep_%d/' % (out_dir, episode_count),
100, 1280, 720, 3, 'CameraRGB_3')
'''
self.lidar_saver = BufferedImageSaver('%s/ep_%d/' % (out_dir, episode_count),
100, 8000, 1, 3, 'Lidar')
def __enter__(self):
self._settings = self.world.get_settings()
self.frame = self.world.apply_settings(carla.WorldSettings(
no_rendering_mode=False,
synchronous_mode=True,
fixed_delta_seconds=self.delta_seconds))
def make_queue(register_event):
q = queue.Queue()
register_event(q.put)
self._queues.append(q)
make_queue(self.world.on_tick)
make_queue(self.camera_rgb.listen)
#make_queue(self.camera_rgb_1.listen)
#make_queue(self.camera_rgb_2.listen)
#make_queue(self.camera_rgb_3.listen)
make_queue(self.camera_lidar.listen)
return self
def _retrieve_data(self, sensor_queue, timeout):
while True:
data = sensor_queue.get(timeout=timeout)
if data.frame == self.frame:
return data
def tick(self, timeout):
self.frame = self.world.tick()
data = [self._retrieve_data(q, timeout) for q in self._queues]
assert all(x.frame == self.frame for x in data)
return data
def __exit__(self, *args, **kwargs):
self.world.apply_settings(self._settings)
def destroy(self):
for actor in self.actor_list:
if actor is not None:
actor.destroy()
## Customized parse image
def parse_image_custom(self, surface, image, sensor_name):
if sensor_name == 'Lidar':
points = np.frombuffer(image.raw_data, dtype=np.dtype('f4'))
points = np.reshape(points, (int(points.shape[0] / 3), 3))
lidar_data = np.array(points[:, :2])
lidar_data *= min(1280, 720) / 100.0
lidar_data += (0.5 * 1280, 0.5 * 720)
lidar_data = np.fabs(lidar_data) # pylint: disable=E1111
lidar_data = lidar_data.astype(np.int32)
lidar_data = np.reshape(lidar_data, (-1, 2))
lidar_img_size = (1280, 720, 3)
lidar_img = np.zeros((lidar_img_size), dtype = int)
lidar_img[tuple(lidar_data.T)] = (255, 255, 255)
# self.surface = pygame.surfarray.make_surface(lidar_img)
self.lidar_saver.add_image(image.raw_data, sensor_name)
elif sensor_name == 'CameraRGB':
array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8"))
array = np.reshape(array, (image.height, image.width, 4))
array = array[:, :, :3]
array = array[:, :, ::-1]
image_surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
#if sensor_name == 'CameraRGB':
surface.blit(image_surface, (0, 0))
self.rgb_saver.add_image(image.raw_data, sensor_name)
def game_loop(args):
pygame.init()
pygame.font.init()
world = None
try:
client = carla.Client(args.host, args.port)
client.set_timeout(2.0)
display = pygame.display.set_mode(
(args.width, args.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
carla_world = client.get_world()
# settings = carla_world.get_settings()
# carla_world.apply_settings(carla.WorldSettings(
# no_rendering_mode=False,
# synchronous_mode=True,
# fixed_delta_seconds=1.0 / 20))
## save control signal: throttle, steer, brake, speed
saver_control = BufferedImageSaver('%s/ep_%d/' % (out_dir, episode_count),
100, 1, 1, 4, 'Control')
## save used control signal: throttle, steer, brake, speed
saver_control_real = BufferedImageSaver('%s/ep_%d/' % (out_dir, episode_count),
100, 1, 1, 4, 'Control_real')
clock = pygame.time.Clock()
world = World(client.get_world(), args)
controller = KeyboardControl(world, args.autopilot)
## PID agent
print('current pos =', world.map.get_spawn_points()[0])
print('dest pos =', world.map.get_spawn_points()[10])
#world.player.set_transform(world.map.get_spawn_points()[0])
world.player.set_location(world.map.get_spawn_points()[0].location)
print('set: ', world.map.get_spawn_points()[0].location)
print('NNPID: current location, ', world.player.get_location())
## training road
world.player.set_transform(carla.Transform(carla.Location(x=305.0, y=129.0, z=2.0), carla.Rotation(pitch=0.0, yaw=180.0, roll=0.0)))
## testing road
world.player.set_transform(carla.Transform(carla.Location(x=310.0, y=326.0, z=2.0), carla.Rotation(pitch=0.138523, yaw=180.0, roll=0.000112504)))
agent = RoamingAgent(world.player)
with world:
while True:
# clock.tick_busy_loop(60)
# if controller.parse_events(client, world, clock):
# carla_world.tick()
# ts = carla_world.wait_for_tick()
# return
if should_quit():
return
## set custom control
pid_control = agent.run_step()
pid_control.manual_gear_shift = False
pid_control.throttle = 0.5
world.player.apply_control(pid_control)
## control
c = pid_control
throttle = c.throttle # 0.0, 1.0
steer = c.steer #-1.0, 1.0
brake = c.brake # 0.0, 1.0
#print(throttle, steer, brake)
v = world.player.get_velocity()
speed = 3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)
#print('Speed: % 15.0f km/h' % (speed))
control = np.array([throttle, steer, brake, speed])
control_used = np.array([throttle, steer, brake, speed])
# if not tst_mode:
saver_control.add_image(control, "Control")
saver_control_real.add_image(control_used, "Control")
clock.tick()
ret = world.tick(timeout=2.0)
if ret:
#snapshot, img_rgb, img_rgb_1, img_rgb_2, img_rgb_3, img_lidar = ret
snapshot, img_rgb, img_lidar = ret
world.parse_image_custom(display, img_rgb, 'CameraRGB')
#world.parse_image_custom(display, img_rgb_1, 'CameraRGB_1')
#world.parse_image_custom(display, img_rgb_2, 'CameraRGB_2')
#world.parse_image_custom(display, img_rgb_3, 'CameraRGB_3')
world.parse_image_custom(display, img_lidar, 'Lidar')
else:
print("Nothing is returned from world.tick :(")
pygame.display.flip()
finally:
print("Destroying actors...")
if world is not None:
world.destroy()
pygame.quit()
print("Done")
def restart(self):
blueprint = self.world.get_blueprint_library().find('vehicle.tesla.model3')
blueprint.set_attribute('role_name', self.actor_role_name)
if blueprint.has_attribute('color'):
color = random.choice(blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
if blueprint.has_attribute('driver_id'):
driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values)
blueprint.set_attribute('driver_id', driver_id)
if blueprint.has_attribute('is_invincible'):
blueprint.set_attribute('is_invincible', 'true')
# Spawn the player.
if self.player is not None:
spawn_point = self.player.get_transform()
spawn_point.location.z += 2.0
spawn_point.rotation.roll = 0.0
spawn_point.rotation.pitch = 0.0
self.destroy()
print("spawn point is : " + spawn_point)
self.player = self.world.try_spawn_actor(blueprint, spawn_point)
while self.player is None:
spawn_points = self.map.get_spawn_points()
spawn_point = spawn_points[0] if spawn_points else carla.Transform()
spawn_point.location.z += 2.0
spawn_point.rotation.roll = 0.0
spawn_point.rotation.pitch = 0.0
self.player = self.world.try_spawn_actor(blueprint, spawn_point)
print('Spawning player at location = ', self.player.get_location())
self.actor_list.append(self.player)
print("Initializing custom rgb and lidar sensors")
bound_y = 0.5 + self.player.bounding_box.extent.y
sensor_location = carla.Location(x=1.6, z=1.7)
if self.camera_rgb is not None:
self.camera_rgb.destroy()
bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
bp.set_attribute('image_size_x', '1280')
bp.set_attribute('image_size_y', '720')
# self.camera_rgb = self.world.spawn_actor(
# bp,
# carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)),
# attach_to=self.player)
self.camera_rgb = self.world.spawn_actor(bp, carla.Transform(sensor_location), attach_to=self.player)
self.actor_list.append(self.camera_rgb)
'''
if self.camera_rgb_1 is not None:
self.camera_rgb_1.destroy()
bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
bp.set_attribute('image_size_x', '1280')
bp.set_attribute('image_size_y', '720')
self.camera_rgb_1 = self.world.spawn_actor(bp, carla.Transform(sensor_location, carla.Rotation(yaw=90)), attach_to=self.player)
self.actor_list.append(self.camera_rgb_1)
if self.camera_rgb_2 is not None:
self.camera_rgb_2.destroy()
bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
bp.set_attribute('image_size_x', '1280')
bp.set_attribute('image_size_y', '720')
self.camera_rgb_2 = self.world.spawn_actor(bp, carla.Transform(sensor_location, carla.Rotation(yaw=180)), attach_to=self.player)
self.actor_list.append(self.camera_rgb_2)
if self.camera_rgb_3 is not None:
self.camera_rgb_3.destroy()
bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
bp.set_attribute('image_size_x', '1280')
bp.set_attribute('image_size_y', '720')
self.camera_rgb_3 = self.world.spawn_actor(bp, carla.Transform(sensor_location, carla.Rotation(yaw=270)), attach_to=self.player)
self.actor_list.append(self.camera_rgb_3)
'''
if self.camera_lidar is not None:
self.camera_lidar.destroy()
bp = self.world.get_blueprint_library().find('sensor.lidar.ray_cast')
bp.set_attribute('range', '5000')
bp.set_attribute('rotation_frequency', self.fps)
self.camera_lidar = self.world.spawn_actor(bp, carla.Transform(sensor_location), attach_to=self.player)
self.actor_list.append(self.camera_lidar)
self.rgb_saver = BufferedImageSaver('%s/ep_%d/' % (out_dir, episode_count),
100, 1280, 720, 3, 'CameraRGB')
'''
self.rgb_saver_1 = BufferedImageSaver('%s/ep_%d/' % (out_dir, episode_count),
100, 1280, 720, 3, 'CameraRGB_1')
self.rgb_saver_2 = BufferedImageSaver('%s/ep_%d/' % (out_dir, episode_count),
100, 1280, 720, 3, 'CameraRGB_2')
self.rgb_saver_3 = BufferedImageSaver('%s/ep_%d/' % (out_dir, episode_count),
100, 1280, 720, 3, 'CameraRGB_3')
'''
self.lidar_saver = BufferedImageSaver('%s/ep_%d/' % (out_dir, episode_count),
100, 8000, 1, 3, 'Lidar')
def game_loop(args):
pygame.init()
pygame.font.init()
world = None
try:
client = carla.Client(args.host, args.port)
client.set_timeout(2.0)
display = pygame.display.set_mode((args.width, args.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
hud = HUD(args.width, args.height)
## world settings: synchronous_mode TODO not sure..
carla_world = client.get_world()
settings = carla_world.get_settings()
settings.synchronous_mode = True
carla_world.apply_settings(settings)
frame = None
count_frame = 0
out_dir = "_out_data"
episode_count = 0
## save control signal: throttle, steer, brake, speed
saver_control = BufferedImageSaver(
'%s/ep_%d/' % (out_dir, episode_count), 100, 1, 1, 4, 'Control')
#####################################
world = World(client.get_world(), hud, args.filter, args.rolename)
controller = KeyboardControl(world, args.autopilot)
clock = pygame.time.Clock()
while True:
clock.tick_busy_loop(60)
if controller.parse_events(client, world, clock):
carla_world.tick()
ts = carla_world.wait_for_tick()
return
world.tick(clock)
#################################################
count_frame += 1
## control
c = world.player.get_control()
throttle = c.throttle # 0.0, 1.0
steer = c.steer #-1.0, 1.0
brake = c.brake # 0.0, 1.0
#print(throttle, steer, brake)
v = world.player.get_velocity()
speed = 3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)
#print('Speed: % 15.0f km/h' % (speed))
print(count_frame)
control = np.array([throttle, steer, brake, speed])
saver_control.add_image(control, "Control")
## sync mode TODO not sure..
carla_world.tick()
ts = carla_world.wait_for_tick()
#####################################
world.render(display)
pygame.display.flip()
finally:
if (world and world.recording_enabled):
client.stop_recorder()
if world is not None:
world.destroy()
pygame.quit()
def game_loop(args):
pygame.init()
pygame.font.init()
world = None
## Set up pointnet - Lidar
num_classes = 1
feature_transform = False
net = Net()
global episode_count
episode_count = int(args.iter)
load_pretrained = True
if load_pretrained:
weights_path = ('./result/dagger_%d.pth' % episode_count)
print('loading pretrained model from.. ' + weights_path)
net.load_state_dict(torch.load(weights_path))
net.cuda()
try:
client = carla.Client(args.host, args.port)
client.set_timeout(2.0)
display = pygame.display.set_mode((args.width, args.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
carla_world = client.get_world()
# settings = carla_world.get_settings()
# carla_world.apply_settings(carla.WorldSettings(
# no_rendering_mode=False,
# synchronous_mode=True,
# fixed_delta_seconds=1.0 / 20))
## save control signal: throttle, steer, brake, speed
episode_count += 1
saver_control = BufferedImageSaver(
'%s/ep_%d/' % (out_dir, episode_count), 100, 1, 1, 4, 'Control')
## save used control signal: throttle, steer, brake, speed
saver_control_real = BufferedImageSaver(
'%s/ep_%d/' % (out_dir, episode_count), 100, 1, 1, 4,
'Control_real')
clock = pygame.time.Clock()
world = World(client.get_world(), args)
controller = KeyboardControl(world, args.autopilot)
## PID agent
world.player.set_location(world.map.get_spawn_points()[0].location)
## training road
world.player.set_transform(
carla.Transform(carla.Location(x=305.0, y=129.0, z=2.0),
carla.Rotation(pitch=0.0, yaw=180.0, roll=0.0)))
clock.tick()
ret = world.tick(timeout=10.0)
agent = RoamingAgent(world.player)
print('NNPID: current location, ', world.player.get_location())
position = []
with world:
while True:
# clock.tick_busy_loop(60)
# if controller.parse_events(client, world, clock):
# carla_world.tick()
# ts = carla_world.wait_for_tick()
# return
if should_quit():
return
## set custom control
pid_control = agent.run_step()
waypt_buffer = agent._waypoint_buffer
while not waypt_buffer:
pid_control = agent.run_step()
global collision_glb
if collision_glb:
player_loc = world.player.get_location()
#waypt = carla_world.get_map().get_waypoint(player_loc)
waypt, _ = waypt_buffer[0]
world.player.set_transform(waypt.transform)
#world.player.set_location(waypt.transform.location)
collision_glb = False
print('hit! respawn')
pid_control = agent.run_step()
pid_control.manual_gear_shift = False
## Neural net control
cust_ctrl = controller._control
cust_ctrl.throttle = 0.5 # 18km/h
cust_ctrl.brake = 0
clock.tick()
ret = world.tick(timeout=10.0)
if ret:
snapshot = ret[0]
img_rgb = ret[1]
img_lidar = ret[2]
world.parse_image_custom(display, img_rgb, 'CameraRGB')
world.parse_image_custom(display, img_lidar, 'Lidar')
# if ret[3]:
# world.on_collision(ret[3])
'''
## Lidar
tst_inputs = np.frombuffer(img_lidar.raw_data, dtype=np.dtype('f4'))
tst_inputs = np.reshape(tst_inputs, (int(tst_inputs.shape[0] / 3), 3))
tst_inputs = np.asarray(tst_inputs, dtype=np.float32)
# test if there's large points
sum_ = np.sum(np.absolute(tst_inputs), axis=1)
mask = np.logical_and(sum_ < 50*3, sum_ > 0.0001)
pts_filter = tst_inputs[mask]
if(pts_filter.shape != tst_inputs.shape):
print('pts filter : pts =', pts_filter.shape, tst_inputs.shape)
tst_inputs = torch.from_numpy(tst_inputs)
#print(tst_inputs.shape)
tst_inputs = tst_inputs[0:1900,:] #
tst_inputs = tst_inputs.unsqueeze(0)
tst_inputs = tst_inputs.transpose(2, 1)
tst_inputs = tst_inputs.cuda()
points = tst_inputs
'''
#print(tst_inputs)
## images
raw_img = np.frombuffer(img_rgb.raw_data, dtype=np.uint8)
raw_img = raw_img.reshape(720, 1280, -1)
raw_img = raw_img[:, :, :3]
raw_img = cv2.resize(raw_img, dsize=(180, 180))
#print(raw_img)
tst_inputs = raw_img / 255
tst_inputs = np.transpose(tst_inputs, (2, 0, 1))
tst_inputs = np.asarray(tst_inputs, dtype=np.float32)
tst_inputs = torch.from_numpy(tst_inputs)
tst_inputs = tst_inputs.unsqueeze(0)
tst_inputs = tst_inputs.cuda()
image = tst_inputs
net = net.eval()
#print(image)
with torch.no_grad():
ah = net(image, [])
ah = torch.squeeze(ah)
#print(a_list[0].detach().squeeze().tolist())
#print(ah)
outputs = ah
#print(outputs)
cust_ctrl.steer = outputs.item()
#'''
world.player.apply_control(cust_ctrl)
player_loc = world.player.get_location()
position.append([player_loc.x, player_loc.y, player_loc.z])
#'''
## check the center of the lane
#waypt = carla_world.get_map().get_waypoint(player_loc)
waypt, road_option = waypt_buffer[0]
lane_center = waypt.transform.location
#print(_current_lane_info.lane_type)
#print('waypt ', lane_center)
#print('player ', player_loc)
dist = math.sqrt((lane_center.x - player_loc.x)**2 +
(lane_center.y - player_loc.y)**2)
## dif in direction
next_dir = waypt.transform.rotation.yaw % 360.0
player_dir = world.player.get_transform(
).rotation.yaw % 360.0
diff_angle = (next_dir - player_dir) % 180.0
## too far from road, use PID control
if (diff_angle > 85 and diff_angle < 105) or dist >= 15:
#print('pid_control')
#world.player.apply_control(pid_control)
#draw_waypoints(carla_world, [waypt], player_loc.z + 2.0)
player_loc = world.player.get_location()
#waypt = carla_world.get_map().get_waypoint(player_loc)
waypt, _ = waypt_buffer[0]
world.player.set_transform(waypt.transform)
#world.player.set_location(waypt.transform.location)
collision_glb = False
print('too far! respawn')
pid_control = agent.run_step()
#'''
# draw_waypoints(carla_world, [waypt], player_loc.z + 2.0)
#world.player.apply_control(pid_control)
else:
print("Nothing is returned from world.tick :(")
## Record expert (PID) control
c = pid_control
throttle = c.throttle # 0.0, 1.0
steer = c.steer #-1.0, 1.0
brake = c.brake # 0.0, 1.0
#print(throttle, steer, brake)
v = world.player.get_velocity()
speed = 3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)
#print('Speed: % 15.0f km/h' % (speed))
control = np.array([throttle, steer, brake, speed])
saver_control.add_image(control, "Control")
control_used = np.array(
[throttle, float(cust_ctrl.steer), brake, speed])
saver_control_real.add_image(control_used, "Control")
if len(position) == 2050:
break
pygame.display.flip()
finally:
print("Destroying actors...")
if world is not None:
world.destroy()
## save position
position = np.asarray(position)
save_name = './dagger_data/ep_%d/path.npy' % (episode_count)
np.save(save_name, position)
print('position saved in ', save_name)
pygame.quit()
print("Done")
def game_loop(args):
global current_rgb_frame
pygame.init()
pygame.font.init()
world = None
#########################################################
## set up nural net
## GPU
device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu")
# Assuming that we are on a CUDA machine, this should print a CUDA device:
print(device)
net = Net()
## load pre trained model
net.load_state_dict(torch.load('/home/zidong/Desktop/nn/6_5.pth.tar'))
net.to(device)
##########################################################
try:
client = carla.Client(args.host, args.port)
client.set_timeout(2.0)
display = pygame.display.set_mode((args.width, args.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
hud = HUD(args.width, args.height)
## world settings: synchronous_mode TODO not sure..
carla_world = client.get_world()
settings = carla_world.get_settings()
settings.synchronous_mode = True
carla_world.apply_settings(settings)
frame = None
count_frame = 0
out_dir = "_out_data"
episode_count = 0
## save control signal: throttle, steer, brake, speed
saver_control = BufferedImageSaver(
'%s/ep_%d/' % (out_dir, episode_count), 100, 1, 1, 4, 'Control')
#####################################
world = World(client.get_world(), hud, args.filter)
controller = DualControl(world, args.autopilot)
clock = pygame.time.Clock()
while True:
clock.tick_busy_loop(60)
if controller.parse_events(world, clock):
carla_world.tick()
ts = carla_world.wait_for_tick()
return
world.tick(clock)
#################################################
## set custom control
cust_ctrl = controller._control
cust_ctrl.throttle = 0.6 # 18km/h
# get image data from sensor - current_rgb_frame
if current_rgb_frame is not None:
# print(current_rgb_frame)
raw_img = np.frombuffer(current_rgb_frame.raw_data,
dtype=np.uint8)
raw_img = raw_img.reshape(720, 1280, -1)
raw_img = raw_img[:, :, :3]
raw_img = cv2.resize(raw_img, dsize=(180, 180))
#print(raw_img)
tst_inputs = raw_img / 255 * 2 - 1
tst_inputs = np.transpose(tst_inputs, (2, 0, 1))
tst_inputs = np.asarray(tst_inputs, dtype=np.float32)
tst_inputs = torch.from_numpy(tst_inputs)
#print(tst_inputs.shape)
tst_inputs = tst_inputs.unsqueeze(0).to(device)
outputs = net(tst_inputs) # input is 180 * 180
outputs = outputs.detach().squeeze().tolist()
print(outputs)
cust_ctrl.steer = outputs
world.player.apply_control(cust_ctrl)
count_frame += 1
## control
c = world.player.get_control()
throttle = c.throttle # 0.0, 1.0
steer = c.steer #-1.0, 1.0
brake = c.brake # 0.0, 1.0
#print(throttle, steer, brake)
v = world.player.get_velocity()
speed = 3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)
#print('Speed: % 15.0f km/h' % (speed))
#print(count_frame)
control = np.array([throttle, steer, brake, speed])
#saver_control.add_image(control, "Control")
## sync mode TODO not sure..
carla_world.tick()
ts = carla_world.wait_for_tick()
#####################################
world.render(display)
pygame.display.flip()
finally:
if world is not None:
world.destroy()
pygame.quit()