class SpecificWorker(GenericWorker):
logger_signal = Signal(str, str, str)
def __init__(self, proxy_map, startup_check=False):
super(SpecificWorker, self).__init__(proxy_map)
self.width = 1280
self.height = 720
pygame.init()
pygame.font.init()
self.display = pygame.display.set_mode(
(self.width, self.height), pygame.HWSURFACE | pygame.DOUBLEBUF)
self.camera_manager = CameraManager(self.width, self.height)
self.gnss_sensor = GNSSSensor()
self.imu_sensor = IMUSensor()
self.hud = HUD(self.width, self.height, self.gnss_sensor,
self.imu_sensor)
self.controller = None
self.clock = pygame.time.Clock()
data_to_save = {
'control': [
'Time', 'Throttle', 'Steer', 'Brake', 'Gear', 'Handbrake',
'Reverse', 'Manualgear'
],
'communication': ['Time', 'CommunicationTime']
}
self.logger = Logger(self.melexlogger_proxy, 'carlaRemoteControl',
data_to_save)
self.logger_signal.connect(self.logger.publish_to_logger)
self.Period = 0
self.previousLat = None
self.previousLong = None
self.currentLat = None
self.currentLong = None
if startup_check:
self.startup_check()
else:
self.timer.timeout.connect(self.compute)
self.timer.start(self.Period)
def __del__(self):
print('SpecificWorker destructor')
def setParams(self, params):
try:
wheel_config = params["wheel"]
self.controller = DualControl(self.camera_manager, self.hud,
wheel_config,
self.carlavehiclecontrol_proxy)
except:
traceback.print_exc()
print("Error reading config params")
return True
@QtCore.Slot()
def compute(self):
if not self.previousLat and not self.previousLong:
self.previousLat = self.gnss_sensor.latitude
self.previousLong = self.gnss_sensor.longitude
self.clock.tick_busy_loop(60)
if self.controller and self.controller.parse_events(self.clock):
exit(-1)
if self.controller.car_moved():
control, elapsed_time = self.controller.publish_vehicle_control()
control_array = [
control.throttle, control.steer, control.brake, control.gear,
control.handbrake, control.reverse, control.manualgear
]
data = ';'.join(map(str, control_array))
self.logger_signal.emit('control', 'compute', data)
self.logger_signal.emit('communication', 'compute',
str(elapsed_time))
self.hud.tick(self, self.clock, control)
self.currentLat = self.gnss_sensor.latitude
self.currentLong = self.gnss_sensor.longitude
if self.currentLong != self.previousLong or self.currentLat != self.currentLat:
print("\n CURRENT POS VEHICLE -> LAT: {} , LONG: {}".format(
self.currentLat, self.currentLong))
self.previousLat = self.currentLat
self.previousLong = self.currentLong
self.camera_manager.render(self.display)
self.hud.render(self.display)
pygame.display.flip()
return True
def startup_check(self):
QTimer.singleShot(200, QApplication.instance().quit)
# =============== Methods for Component SubscribesTo ================
# ===================================================================
#
# SUBSCRIPTION to pushRGBD method from CameraRGBDSimplePub interface
#
def CarCameraRGBD_pushRGBD(self, im, dep):
self.camera_manager.images_received[im.cameraID] = im
#
# SUBSCRIPTION to updateSensorGNSS method from CarlaSensors interface
#
def CarlaSensors_updateSensorGNSS(self, gnssData):
self.gnss_sensor.update(gnssData.latitude, gnssData.longitude,
gnssData.altitude, gnssData.frame,
gnssData.timestamp)
#
# SUBSCRIPTION to updateSensorIMU method from CarlaSensors interface
#
def CarlaSensors_updateSensorIMU(self, imuData):
self.imu_sensor.update(imuData.accelerometer, imuData.gyroscope,
imuData.compass, imuData.frame,
imuData.timestamp)
class CarlaEnv():
pygame.init()
font = pygame.font.init()
def __init__(self, world):
#화면 크기
n_iters = 100
self.width = 800
self.height = 600
#센서 종류
self.actor_list = []
self.extra_list = []
self.extra_list = []
self.extra_controller_list = []
self.extra_dl_list = []
self.extra_dl_list = []
self.player = None
self.camera_rgb = None
self.camera_semseg = None
self.lane_invasion_sensor = None
self.collision_sensor = None
self.gnss_sensor = None
self.waypoint = None
self.path = []
self.save_dir = None
self.world = world
self.map = world.get_map()
self.spectator = self.world.get_spectator()
self.hud = HUD(self.width, self.height)
self.waypoints = self.map.generate_waypoints(3.0)
self.lane_change_time = time.time()
self.max_Lane_num = 4
self.ego_Lane = 2
self.agent = None
self.controller = None
self.is_first_time = True
self.decision = None
self.simul_time = time.time()
self.accumulated_reward = 0
self.end_point = 0
self.ROI_length = 1000 #(meters)
## visualize all waypoints ##
# for n, p in enumerate(self.waypoints):
# world.debug.draw_string(p.transform.location, 'o', draw_shadow=True,
# color=carla.Color(r=255, g=255, b=255), life_time=999)
settings = self.world.get_settings()
# settings.no_rendering_mode = True
# settings.synchronous_mode = True
settings.fixed_delta_seconds = 0.02
self.world.apply_settings(settings)
self.extra_num = 30
self.section = 0
self.lane_distance_between_start = None
self.lane_distance_between_end = None
self.lane_change_point = None
self.episode_start = None
self.index = 0
self.pre_max_Lane_num = self.max_Lane_num
self.restart()
self.main()
def restart(self):
self.decision = None
self.simul_time = time.time()
self.lane_change_time = time.time()
self.max_Lane_num = 4
self.ego_Lane = 2
self.controller = None
self.accumulated_reward = 0
self.section = 0
self.episode_start = time.time()
self.pre_max_Lane_num = self.max_Lane_num
self.index = 0
print('start destroying actors.')
if len(self.actor_list) != 0:
# print('destroying actors.')
# print("actor 제거 :", self.actor_list)
for actor in self.actor_list:
# print("finally 에서 actor 제거 :", self.actor_list)
if actor.is_alive:
actor.destroy()
print("finshed actor destroy")
# for x in self.actor_list:
# try:
# client.apply_batch([carla.command.DestroyActors(x.id)])
# except:
# continue
if len(self.extra_list) != 0:
# client.apply_batch([carla.command.DestoryActors(x.id) for x in self.extra_list])
# print("extra 제거 :", self.extra_list)
for x in self.extra_list:
try:
client.apply_batch([carla.command.DestroyActor(x.id)])
except:
continue
# for extra in self.extra_list:
# # print("finally 에서 actor 제거 :", self.extra_list)
# print(extra.is_alive)
# if extra.is_alive:
# extra.destroy()
print("finshed extra destroy")
# for x in self.extra_list:
# try:
# client.apply_batch([carla.command.DestroyActor(x.id)])
# except:
# continue
print('finished destroying actors.')
self.actor_list = []
self.extra_list = []
self.ROI_extra_list = []
self.extra_controller_list = []
self.extra_dl_list = []
self.ROI_extra_dl_list = []
blueprint_library = self.world.get_blueprint_library()
# start_pose = random.choice(self.map.get_spawn_points())
start_pose = self.map.get_spawn_points()[102]
##spawn points 시뮬레이션 상 출력##
# print(start_pose)
# for n, x in enumerate(self.map.get_spawn_points()):
# world.debug.draw_string(x.location, 'o', draw_shadow=True,
# color=carla.Color(r=0, g=255, b=255), life_time=30)
# self.load_traj()
self.waypoint = self.map.get_waypoint(start_pose.location,
lane_type=carla.LaneType.Driving)
self.end_point = self.waypoint.next(400)[0].transform.location
# print(self.waypoint.transform)
## Ego vehicle의 global Route 출력##
world.debug.draw_string(self.waypoint.next(400)[0].transform.location,
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=255),
life_time=100)
# print(start_pose)
# print(self.waypoint.transform)
# self.controller = MPCController.Controller
self.player = world.spawn_actor(
random.choice(blueprint_library.filter('vehicle.bmw.grandtourer')),
start_pose)
# print(self.player.bounding_box) # ego vehicle length
self.actor_list.append(self.player)
self.camera_rgb = RGBSensor(self.player, self.hud)
self.actor_list.append(self.camera_rgb.sensor)
# self.camera_depth =DepthCamera(self.player, self.hud)
# self.actor_list.append(self.camera_depth.sensor)
# self.camera_semseg = SegmentationCamera(self.player,self.hud)
# self.actor_list.append(self.camera_semseg.sensor)
self.collision_sensor = CollisionSensor(self.player,
self.hud) # 충돌 여부 판단하는 센서
self.actor_list.append(self.collision_sensor.sensor)
self.lane_invasion_sensor = LaneInvasionSensor(
self.player, self.hud) # lane 침입 여부 확인하는 센서
self.actor_list.append(self.lane_invasion_sensor.sensor)
self.gnss_sensor = GnssSensor(self.player)
self.actor_list.append(self.gnss_sensor.sensor)
# --------------
# Spawn Surrounding vehicles
# --------------
print("Generate Extra")
spawn_points = []
blueprints = self.world.get_blueprint_library().filter('vehicle.*')
blueprints = [
x for x in blueprints
if int(x.get_attribute('number_of_wheels')) == 4
]
# print(*blueprints)
for i in range(10, 10 * (self.extra_num) + 1, 10):
dl = random.choice([-1, 0, 1])
self.extra_dl_list.append(dl)
spawn_point = None
if dl == -1:
spawn_point = self.waypoint.next(
i)[0].get_left_lane().transform
elif dl == 0:
spawn_point = self.waypoint.next(i)[0].transform
elif dl == 1:
spawn_point = self.waypoint.next(
i)[0].get_right_lane().transform
else:
print("Except ")
spawn_point = carla.Transform(
(spawn_point.location + carla.Location(z=1)),
spawn_point.rotation)
spawn_points.append(spawn_point)
# print(blueprint_library.filter('vehicle.bmw.grandtourer'))
# blueprint = random.choice(blueprint_library.filter('vehicle.bmw.grandtourer'))
blueprint = random.choice(blueprints)
# print(blueprint.has_attribute('color'))
if blueprint.has_attribute('color'):
# color = random.choice(blueprint.get_attribute('color').recommended_values)
# print(blueprint.get_attribute('color').recommended_values)
color = '255,255,255'
blueprint.set_attribute('color', color)
extra = self.world.spawn_actor(blueprint, spawn_point)
self.extra_list.append(extra)
print('Extra Genration Finished')
self.spectator.set_transform(
carla.Transform(
self.player.get_transform().location + carla.Location(z=100),
carla.Rotation(pitch=-90)))
ROI = 1000
# extra_target_velocity = 10
port = 8000
traffic_manager = client.get_trafficmanager(port)
traffic_manager.set_global_distance_to_leading_vehicle(100.0)
# traffic_manager.set_synchronous_mode(True) # for std::out_of_range eeror
tm_port = traffic_manager.get_port()
for extra in self.extra_list:
extra.set_autopilot(True, tm_port)
# Pure_puresuit_controller(extra, self.waypoint, None, 50) # km/h
# target_velocity = 30 #random.randrange(10, 40) # km/h
# extra.enable_constant_velocity(extra.get_trzansform().get_right_vector() * target_velocity/3.6)
traffic_manager.auto_lane_change(extra, False)
# self.player.set_autopilot(True,tm_port)
# traffic_manager.auto_lane_change(self.player, False)
self.controller = Pure_puresuit_controller(self.player, self.waypoint,
self.extra_list, 70) # km/h
# target_velocity = 60 / 3.6
# forward_vec = self.player.get_transform().get_forward_vector()
# print(forward_vec)
# velocity_vec = target_velocity*forward_vec
# self.player.set_target_velocity(velocity_vec)
# print(velocity_vec)
# print(velocity_vec)
# client.get_trafficmanager.auto_lane_change(extra, False)
###Test####
# clock = pygame.time.Clock()
# Keyboardcontrol = KeyboardControl(self, False)
# display = pygame.display.set_mode(
# (self.width, self.height),
# pygame.HWSURFACE | pygame.DOUBLEBUF)
# while True:
# if Keyboardcontrol.parse_events(client, self, clock):
# return
# self.spectator.set_transform(
# carla.Transform(self.player.get_transform().location + carla.Location(z=50),
# carla.Rotation(pitch=-90)))
# self.camera_rgb.render(display)
# self.hud.render(display)
# pygame.display.flip()
#
# self.controller.apply_control()
# # self.world.wait_for_tick(10.0)
# clock.tick(30)
#
# self.hud.tick(self, clock)
#### Test Finished #####
#### Test2 #####
# cnt=0
# clock = pygame.time.Clock()
# while True:
# # print(self.waypoint.lane_id)
# self.spectator.set_transform(
# carla.Transform(self.player.get_transform().location + carla.Location(z=100),
# carla.Rotation(pitch=-90)))
# cnt += 1
# if cnt == 100:
# print('수해유 ㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠ')
# decision = 1
# self.controller.apply_control(decision)
# else:
# self.controller.apply_control()
# clock.tick(30)
#### Test2 Finished #####
# self.input_size = (self.extra_num)*4 + 1
self.input_size = 4 #dr dv da dl
self.output_size = 3
# for response in client.apply_batch_sync(batch):
# if response.error:
# logging.error(response.error)
# else:
# self.extra_list.append(response.actor_id)
print(5)
def step(self, decision):
SECONDS_PER_EPISODE = 1000
plc = 10
# decision = None
'''
# Simple Action (action number: 3)
action_test = action -1
self.vehicle.apply_control(carla.VehicleControl(throttle=1.0, steer=action_test*self.STEER_AMT))
'''
# Complex Action
if decision == 0: # LK
plc = 0
# elif action == -1: # left
# plc += 10
# decision = -1
# elif action == 1: # right
# plc += 10
# decision = 1
'''
if len(self.collision_hist) != 0:
done = True
reward = -200 + (time.time()-self.episode_start)*15/SECONDS_PER_EPISODE
else:
done = False
reward = 1
'''
end_length = math.sqrt(
(self.end_point.x - self.player.get_location().x)**2 +
(self.end_point.y - self.player.get_location().y)**2)
done = False
if len(self.collision_sensor.history) != 0:
done = True
reward = -100
elif end_length < 15:
done = True
reward = 1
elif self.max_Lane_num < self.ego_Lane:
done = True
reward = -100
else:
reward = 1 - (self.controller.desired_vel - self.controller.
velocity) / self.controller.desired_vel - plc
# print(reward)
self.accumulated_reward += reward
if time.time() > self.episode_start + SECONDS_PER_EPISODE:
done = True
#state length = 4 * num of extra vehicles + 1
state, x_static = self.get_next_state(decision) #get now state
# if len(state) == 29:
# print(" ")
return state, x_static, decision, reward, None, None, done
# Next State 표현 필요
def get_next_state(self, decision=None):
"""
dl : relative lane num after ching the lane
dr, dv, da : now state
"""
state = []
for x, extra in enumerate(self.extra_list):
if decision == 1:
self.extra_dl_list[x] = self.extra_dl_list[x] + 1
# self.ROI_extra_dl_list[x] =self.ROI_extra_dl_list[x]+1
elif decision == -1:
self.extra_dl_list[x] = self.extra_dl_list[x] - 1
# self.ROI_extra_dl_list[x] = self.ROI_extra_dl_list[x]-1
else:
pass
extra_pos = extra.get_transform().location
extra_vel = extra.get_velocity()
extra_acel = extra.get_acceleration()
dr = ((extra_pos.x - self.player.get_transform().location.x)**2 +
(extra_pos.y - self.player.get_transform().location.y)**2 +
((extra_pos.z - self.player.get_transform().location.z)**
2))**0.5 - abs(self.waypoint.lane_width *
(self.extra_dl_list[x]))
if isinstance(dr, complex):
print("complex")
dv = (self.player.get_velocity().x**2 +
self.player.get_velocity().y**2 +
self.player.get_velocity().z**2)**0.5 - (
extra_vel.x**2 + extra_vel.y**2 + extra_vel.z**2)**0.5
length = 2 * extra.bounding_box.extent.x / 10 # length of extra vehicle
# da = ((extra_acel.x - self.player.get_acceleration().x) ** 2 + (
# extra_acel.y - self.player.get_acceleration().y) ** 2 +
# (extra_acel.z - self.player.get_acceleration().z) ** 2) ** 0.5
state_dyn = [dr, dv, length, self.extra_dl_list[x]]
state.append(state_dyn)
# state.append(dr)
# state.append(dv)
# state.append(da)
# state.append(self.extra_dl_list[x])
if decision == 1:
self.ego_Lane += 1
elif decision == -1:
self.ego_Lane += -1
else:
pass
x_static = []
x_static.append([
self.ego_Lane, self.controller.velocity,
self.search_distance_valid() / self.ROI_length
])
# state.append(x_static)
# state.append(self.ego_Lane)
out = [state, x_static]
return out
def search_distance_valid(self):
# index= 4-self.ego_Lane
# print("index :", index)
last_lane_waypoint = self.map.get_waypoint(
self.player.get_transform().location,
lane_type=carla.LaneType.Driving) #self.controller.waypoint
search_raidus = 5
distance = 9999
# pre_distance = 0
i = 0.01
self.world.debug.draw_string(
self.controller.waypoint.transform.location,
'o',
draw_shadow=True,
color=carla.Color(r=0, g=0, b=255),
life_time=1)
try:
while last_lane_waypoint.lane_id > -4: # get waypoint of forth's lane
last_lane_waypoint = last_lane_waypoint.get_right_lane()
# print("lane id : ", last_lane_waypoint.lane_id)
except:
print("4-th lane not found")
# self.world.debug.draw_string(last_lane_waypoint.transform.location,
# '4', draw_shadow=True,
# color=carla.Color(r=0, g=255, b=255), life_time=9999)
# print(self.ego_Lane)
# if self.max_Lane_num ==3:
# print("e")
if self.max_Lane_num != self.pre_max_Lane_num:
self.index += 1
self.pre_max_Lane_num = self.max_Lane_num
# print(self.index)
if self.index % 2 == 0:
while search_raidus <= distance:
# pre_distance = distance
distance = (
(last_lane_waypoint.next(i)[0].transform.location.x -
self.lane_change_point[self.index].x)**2 +
(last_lane_waypoint.next(i)[0].transform.location.y -
self.lane_change_point[self.index].y)**2 +
(last_lane_waypoint.next(i)[0].transform.location.z -
self.lane_change_point[self.index].z)**2)**0.5
# if pre_distance <= distance:
# print("wrong")
# return distance + i
if round(distance - search_raidus) > 0:
i += round(distance - search_raidus)
# break
else:
return distance + i
else:
distance = math.hypot(
last_lane_waypoint.transform.location.x -
self.lane_change_point[self.index].x,
last_lane_waypoint.transform.location.y -
self.lane_change_point[self.index].y)
return -distance
# if self.max_Lane_num ==4:
# while search_raidus <= distance:
# pre_distance = distance
# # distance = ((last_lane_waypoint.next(i)[0].x-self.lane_start_point.x)**2+(last_lane_waypoint.next(i)[0].y-self.lane_start_point.y)**2+(last_lane_waypoint.next(i)[0].z-self.lane_start_point.z)**2)**0.5
# distance = ((last_lane_waypoint.next(i)[0].transform.location.x-self.lane_start_point[self.section].x)**2+(last_lane_waypoint.next(i)[0].transform.location.y-self.lane_start_point[self.section].y)**2+(last_lane_waypoint.next(i)[0].transform.location.z-self.lane_start_point[self.section].z)**2)**0.5
#
# # print("distance :",distance , "i :", i)
# if pre_distance <= distance:
# print("pre_distance <= distance error")
# break
# elif round(distance - search_raidus) > 0:
# i += round(distance - search_raidus)
# else:
# return min(distance + i, self.ROI_length)
# # print("i updated :", i)
# # self.max_Lane_num =3
# elif self.max_Lane_num == 3:
# while search_raidus <= distance:
# distance = ((last_lane_waypoint.next(i)[0].transform.location.x - self.lane_finished_point[
# self.section].x) ** 2 + (last_lane_waypoint.next(i)[0].transform.location.y -
# self.lane_finished_point[self.section].y) ** 2 + (
# last_lane_waypoint.next(i)[0].transform.location.z -
# self.lane_finished_point[self.section].z) ** 2) ** 0.5
# self.world.debug.draw_string(last_lane_waypoint.next(i)[0].transform.location,
# 'o', draw_shadow=True,
# color=carla.Color(r=255, g=255, b=0), life_time=9999)
# if pre_distance <= distance:
# print("pre_distance <= distance error")
# break
# elif round(distance - search_raidus) > 0:
# i += round(distance - search_raidus)
# else:
# return min(distance + i, self.ROI_length)
#
# if round(distance - search_raidus) > 0:
# i += round(distance - search_raidus)
# else:
# return max(-(distance + i), -self.ROI_length)
def safety_check(self, decision, safe_lane_change_again_time=5):
# print("method : ", self.agent.selection_method)
# if decision==-1 and self.ego_Lane ==1:
# print("here")
# elif decision ==1 and self.ego_Lane ==self.max_Lane_num:
# print("here")
if decision != 0 and decision != None:
if (time.time() -
self.lane_change_time) <= safe_lane_change_again_time:
return 0 #즉 직진
elif self.agent.selection_method == 'random' and decision == 1 and self.ego_Lane == self.max_Lane_num:
action = random.randrange(-1, 1)
return action
elif self.agent.selection_method == 'random' and decision == -1 and self.ego_Lane == 1:
action = random.randrange(0, 2)
return action
elif self.ego_Lane == self.max_Lane_num and decision == 1:
return int(self.agent.q_value[0][0:2].argmax().item()) - 1
elif self.ego_Lane == 1 and decision == -1:
return int(self.agent.q_value[0]
[1:3].argmax().item()) #not max exception
elif decision == 1 and self.ego_Lane != self.max_Lane_num:
self.lane_change_time = time.time()
return decision
elif decision == -1 and self.ego_Lane != 1:
self.lane_change_time = time.time()
return decision
else: #3차선에서 우 차도 판단, 1차선에서 좌차선 판단
if self.agent.selection_method == 'random' and decision == 1 and self.ego_Lane == self.max_Lane_num:
action = random.randrange(-1, 1)
return action
elif self.agent.selection_method == 'random' and decision == -1 and self.ego_Lane == 1:
action = random.randrange(0, 2)
return action
elif self.ego_Lane == self.max_Lane_num and decision == 1:
return int(self.agent.q_value[0][0:2].argmax().item()) - 1
elif self.ego_Lane == 1 and decision == -1:
return int(self.agent.q_value[0][1:3].argmax().item())
else:
return decision
# def safety_check_between_vehicles(self):
# if extra_lists
def main_test(self):
restart_time = time.time()
PATH = "/home/a/RL_decision/trained_info.tar"
print(torch.cuda.get_device_name())
clock = pygame.time.Clock()
Keyboardcontrol = KeyboardControl(self, False)
display = pygame.display.set_mode((self.width, self.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
self.lane_start_point = [
carla.Location(x=14.905815, y=-135.747452, z=0.000000),
carla.Location(x=172.745468, y=-364.531799, z=0.000000),
carla.Location(x=382.441040, y=-212.488907, z=0.000000)
]
self.lane_finished_point = [
carla.Location(x=14.631096, y=-205.746918, z=0.000000),
carla.Location(x=232.962860, y=-364.149139, z=0.000000),
carla.Location(x=376.542816, y=-10.352980, z=0.000000)
]
self.lane_change_point = [
carla.Location(x=14.905815, y=-135.747452, z=0.000000),
carla.Location(x=14.631096, y=-205.746918, z=0.000000),
carla.Location(x=172.745468, y=-364.531799, z=0.000000),
carla.Location(x=232.962860, y=-364.149139, z=0.000000),
carla.Location(x=382.441040, y=-212.488907, z=0.000000),
carla.Location(x=376.542816, y=-10.352980, z=0.000000)
]
self.world.debug.draw_string(carla.Location(x=14.905815,
y=-135.747452,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
self.world.debug.draw_string(carla.Location(x=14.631096,
y=-205.746918,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
# ---------------------------#
self.world.debug.draw_string(carla.Location(x=172.745468,
y=-364.531799,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
self.world.debug.draw_string(carla.Location(x=232.962860,
y=-364.149139,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
# ---------------------------#
self.world.debug.draw_string(carla.Location(x=382.441040,
y=-212.488907,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
self.world.debug.draw_string(carla.Location(x=376.542816,
y=-10.352980,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
lane_distance_between_points = []
for i in range(len(self.lane_finished_point)):
lane_distance_between_points.append(
((self.lane_start_point[i].x - self.lane_finished_point[i].x)**
2 + (self.lane_start_point[i].y -
self.lane_finished_point[i].y)**2 +
(self.lane_start_point[i].z - self.lane_finished_point[i].z)**
2)**0.5)
pre_decision = None
while True:
# if time.time()-restart_time > 10:
# print("restart")
# self.restart()
if Keyboardcontrol.parse_events(client, self, clock):
return
self.spectator.set_transform(
carla.Transform(
self.player.get_transform().location +
carla.Location(z=50), carla.Rotation(pitch=-90)))
self.camera_rgb.render(display)
self.hud.render(display)
pygame.display.flip()
## Get max lane ##
# print("start get lane")
if self.section < len(lane_distance_between_points):
self.lane_distance_between_start = (
(self.player.get_transform().location.x -
self.lane_start_point[self.section].x)**2 +
(self.player.get_transform().location.y -
self.lane_start_point[self.section].y)**2)**0.5
self.lane_distance_between_end = (
(self.player.get_transform().location.x -
self.lane_finished_point[self.section].x)**2 +
(self.player.get_transform().location.y -
self.lane_finished_point[self.section].y)**2)**0.5
# print("self.lane_distance_between_start : ",self.lane_distance_between_start,"self.lane_distance_between_end :",self.lane_distance_between_end, "lane_distance_between_points[section]",lane_distance_between_points[self.section],"section :", self.section)
if max(lane_distance_between_points[self.section], self.lane_distance_between_start, self.lane_distance_between_end) == \
lane_distance_between_points[self.section]:
self.max_Lane_num = 3
# world.debug.draw_string(self.player.get_transform().location, 'o', draw_shadow = True,
# color = carla.Color(r=255, g=255, b=0), life_time = 999)
elif max(lane_distance_between_points[self.section], self.lane_distance_between_start, self.lane_distance_between_end) == \
self.lane_distance_between_start and self.max_Lane_num ==3:
self.section += 1
self.max_Lane_num = 4
if self.section >= len(lane_distance_between_points
): # when, section_num = 3
self.section = 0
## finished get max lane ##
# print("finished get lane")
self.search_distance_valid()
# print("distance :" ,,"max_lane_num : ", self.max_Lane_num)
if self.max_Lane_num == 3:
self.world.debug.draw_string(
self.player.get_transform().location,
'o',
draw_shadow=True,
color=carla.Color(r=255, g=255, b=0),
life_time=9999)
if time.time() - self.lane_change_time > 10:
self.lane_change_time = time.time()
if pre_decision is None:
self.decision = 1
self.ego_Lane += 1
pre_decision = 1
elif pre_decision == 1:
pre_decision = -1
self.ego_Lane += -1
self.decision = -1
elif pre_decision == -1:
self.decision = 1
self.ego_Lane += 1
pre_decision = 1
else:
self.decision = 0
self.controller.apply_control(self.decision)
# self.world.wait_for_tick(10.0)
clock.tick(30)
self.hud.tick(self, clock)
def main(self):
PATH = "/home/a/RL_decision/trained_info.tar"
print(torch.cuda.get_device_name())
clock = pygame.time.Clock()
Keyboardcontrol = KeyboardControl(self, False)
display = pygame.display.set_mode((self.width, self.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
self.agent = decision_driving_Agent(self.input_size, self.output_size,
True, 1000, self.extra_num)
self.lane_start_point = [
carla.Location(x=14.905815, y=-135.747452, z=0.000000),
carla.Location(x=172.745468, y=-364.531799, z=0.000000),
carla.Location(x=382.441040, y=-212.488907, z=0.000000)
]
self.lane_finished_point = [
carla.Location(x=14.631096, y=-205.746918, z=0.000000),
carla.Location(x=232.962860, y=-364.149139, z=0.000000),
carla.Location(x=376.542816, y=-10.352980, z=0.000000)
]
self.lane_change_point = [
carla.Location(x=14.905815, y=-135.747452, z=0.000000),
carla.Location(x=14.631096, y=-205.746918, z=0.000000),
carla.Location(x=172.745468, y=-364.531799, z=0.000000),
carla.Location(x=232.962860, y=-364.149139, z=0.000000),
carla.Location(x=382.441040, y=-212.488907, z=0.000000),
carla.Location(x=376.542816, y=-10.352980, z=0.000000)
]
self.world.debug.draw_string(carla.Location(x=14.905815,
y=-135.747452,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
self.world.debug.draw_string(carla.Location(x=14.631096,
y=-205.746918,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
# ---------------------------#
self.world.debug.draw_string(carla.Location(x=172.745468,
y=-364.531799,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
self.world.debug.draw_string(carla.Location(x=232.962860,
y=-364.149139,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
# ---------------------------#
self.world.debug.draw_string(carla.Location(x=376.542816,
y=-10.352980,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
self.world.debug.draw_string(carla.Location(x=382.441040,
y=-212.488907,
z=0.000000),
'o',
draw_shadow=True,
color=carla.Color(r=0, g=255, b=0),
life_time=9999)
lane_distance_between_points = []
for i in range(len(self.lane_finished_point)):
lane_distance_between_points.append(
(self.lane_start_point[i].x -
self.lane_finished_point[i].x)**2 +
(self.lane_start_point[i].y -
self.lane_finished_point[i].y)**2 +
(self.lane_start_point[i].z -
self.lane_finished_point[i].z)**2)
epoch_init = 0
# if(os.path.exists(PATH)):
# print("저장된 가중치 불러옴")
# checkpoint = torch.load(PATH)
#
# self.agent.model.load_state_dict(checkpoint['model_state_dict'])
# device = torch.device('cuda')
# self.agent.model.to(device)
# self.agent.optimizer.load_state_dict(checkpoint['optimizer_state_dict'])
# self.agent.buffer = checkpoint['memorybuffer']
# epoch_init = checkpoint['epoch']
# self.agent.buffer.load_state_dict(self.save_dir['memorybuffer'])
# self.is_first_time = False
# controller = VehicleControl(self, True)
# self.player.apply_control(carla.Vehicle
# Control(throttle=1.0, steer=0.0, brake=0.0))
# vehicles = self.world.get_actors().filter('vehicle.*')
# for i in vehicles:
# print(i.id)
try:
n_iters = 150
is_error = 0
for epoch in range(epoch_init, n_iters):
# cnt = 0
first_time_print = True
[state, x_static] = self.get_next_state() #초기 상태 s0 초기화
while (True):
if Keyboardcontrol.parse_events(client, self, clock):
return
self.camera_rgb.render(display)
self.hud.render(display)
pygame.display.flip()
self.spectator.set_transform(
carla.Transform(
self.player.get_transform().location +
carla.Location(z=100), carla.Rotation(pitch=-90)))
## Get max lane ##
if self.section < len(lane_distance_between_points):
self.lane_distance_between_start = (
(self.player.get_transform().location.x -
self.lane_start_point[self.section].x)**2 +
(self.player.get_transform().location.y -
self.lane_start_point[self.section].y)**2)
self.lane_distance_between_end = (
(self.player.get_transform().location.x -
self.lane_finished_point[self.section].x)**2 +
(self.player.get_transform().location.y -
self.lane_finished_point[self.section].y)**2)
# print("self.lane_distance_between_start : ",self.lane_distance_between_start,"self.lane_distance_between_end :",self.lane_distance_between_end, "lane_distance_between_points[self.section]",lane_distance_between_points[self.section],"self.section :", self.section)
if max(lane_distance_between_points[self.section], self.lane_distance_between_start,
self.lane_distance_between_end) == \
lane_distance_between_points[self.section]:
self.max_Lane_num = 3
# world.debug.draw_string(self.player.get_transform().location, 'o', draw_shadow = True,
# color = carla.Color(r=255, g=255, b=0), life_time = 999)
elif max(lane_distance_between_points[self.section], self.lane_distance_between_start,
self.lane_distance_between_end) == \
self.lane_distance_between_start and self.max_Lane_num == 3:
self.section += 1
self.max_Lane_num = 4
if self.section >= len(lane_distance_between_points
): # when, section_num = 3
self.section = 0
## finished get max lane ##
##visualize when, max lane ==3 ##
if self.max_Lane_num == 3:
self.world.debug.draw_string(
self.waypoint.transform.location,
'o',
draw_shadow=True,
color=carla.Color(r=255, g=255, b=255),
life_time=9999)
# [self.extra_list, self.extra_dl_list] = self.agent.search_extra_in_ROI(self.extra_list,self.player,self.extra_dl_list)
## finished to visualize ##
if self.agent.is_training:
##dqn 과정##
# 가중치 초기화 (pytroch 내부)
# 입실론-그리디 행동 탐색 (act function)
# 메모리 버퍼에 MDP 튜플 얻기 ㅡ (step function)
# 메모리 버퍼에 MDP 튜플 저장 ㅡ
# optimal Q 추정 ㅡ (learning function)
# Loss 계산 ㅡ
# 가중치 업데이트 ㅡ
if epoch % 10 == 0:
# [w, b] = self.agent.model.parameters() # unpack parameters
self.save_dir = torch.save(
{
'epoch':
epoch,
'model_state_dict':
self.agent.model.state_dict(),
'optimizer_state_dict':
self.agent.optimizer.state_dict(),
'memorybuffer':
self.agent.buffer
}, PATH)
# print(clock.get_fps())
# if time.time() - self.simul_time >10 and clock.get_fps() < 15:
# self.restart()
# time.sleep(3)
# continue
if self.decision is not None:
[next_state, next_x_static] = self.get_next_state()
sample = [
state, x_static, self.decision, reward,
next_state, next_x_static, done
]
self.agent.buffer.append(sample)
# print("befor act")
self.decision = self.agent.act(state, x_static)
# print("after act")
# print(decision)
# if self.decision ==1 and self.max_Lane_num==self.ego_Lane:
# print( " ")
# print("befroe safte check/ 판단 :", self.decision, "최대 차선 :", self.max_Lane_num, "ego lane :",self.ego_Lane)
self.decision = self.safety_check(self.decision)
# print("판단 :", self.decision, "최대 차선 :", self.max_Lane_num, "ego lane :",self.ego_Lane)
# print("before")
is_error = self.controller.apply_control(self.decision)
# print("after")
# print("extra_controller 개수 :", len(self.extra_controller_list))
# for i in range(len(self.extra_controller_list)):
# self.extra_controller_list[i].apply_control()
# print("before step")
[state, x_static, decision, reward, __, _,
done] = self.step(self.decision)
# print("after step")
if done:
print("epsilon :", self.agent.epsilon)
print("epoch : ", epoch, "누적 보상 : ",
self.accumulated_reward)
writer.add_scalar('누적 보상 ',
self.accumulated_reward, epoch)
if len(self.agent.buffer.size()
) > self.agent.batch_size:
# print("start learning")
for i in range(300):
self.agent.learning()
# print("finished learning")
client.set_timeout(10)
self.restart()
break
else:
self.agent.act(state, x_static)
clock.tick(40)
self.hud.tick(self, clock)
finally:
print('\ndestroying %d vehicles' % len(self.extra_list))
# client.apply_batch([carla.command.DestroyActor(x) for x in self.extra_list])
print('destroying actors.')
# client.apply_batch([carla.command.DestroyActors(x.id) for x in self.actor_list])
# client.apply_batch([carla.command.DestroyActors(x.id) for x in self.extra_list])
for actor in self.actor_list:
# print("finally 에서 actor 제거 :", self.actor_list)
if actor.is_alive:
actor.destroy()
for extra in self.extra_list:
# print("finally 에서 actor 제거 :", self.extra_list)
if extra.is_alive:
extra.destroy()
# pygame.quit()
print('done.')
class CarlaEnv():
pygame.init()
font = pygame.font.init()
def __init__(self, world):
#화면 크기
self.width = 800
self.height = 600
#센서 종류
self.actor_list = []
self.extra_list = []
self.extra_controller_list = []
self.player = None
self.camera_rgb = None
self.camera_semseg = None
self.lane_invasion_sensor = None
self.collision_sensor = None
self.gnss_sensor = None
self.waypoint = None
self.path = []
self.world = world
self.map = world.get_map()
self.spectator = self.world.get_spectator()
self.hud = HUD(self.width, self.height)
self.waypoints = self.map.generate_waypoints(3.0)
## visualize all waypoints ##
# for n, p in enumerate(self.waypoints):
# if n>1000:
# break
# world.debug.draw_string(p.transform.location, 'o', draw_shadow=True,
# color=carla.Color(r=255, g=255, b=255), life_time=30)
self.extra_num = 5
self.control_mode = None
self.restart()
self.main()
def restart(self):
blueprint_library = world.get_blueprint_library()
# start_pose = random.choice(self.map.get_spawn_points())
start_pose = self.map.get_spawn_points()[102]
##spawn points 시뮬레이션 상 출력##
# print(start_pose)
# for n, x in enumerate(self.map.get_spawn_points()):
# world.debug.draw_string(x.location, 'o', draw_shadow=True,
# color=carla.Color(r=0, g=255, b=255), life_time=30)
# self.load_traj()
self.waypoint = self.map.get_waypoint(start_pose.location,
lane_type=carla.LaneType.Driving)
# print(self.waypoint.transform)
## Ego vehicle의 global Route 출력##
# world.debug.draw_string(self.waypoint.transform.location, 'o', draw_shadow=True,
# color=carla.Color(r=0, g=255, b=255), life_time=100)
# print(start_pose)
# print(self.waypoint.transform)
# self.controller = MPCController.Controller
self.spectator.set_transform(
carla.Transform(start_pose.location + carla.Location(z=50),
carla.Rotation(pitch=-90)))
self.player = world.spawn_actor(
random.choice(blueprint_library.filter('vehicle.bmw.grandtourer')),
start_pose)
self.actor_list.append(self.player)
# vehicle.set_simulate_physics(False)
self.camera_rgb = RGBSensor(self.player, self.hud)
self.actor_list.append(self.camera_rgb.sensor)
self.camera_semseg = SegmentationCamera(self.player, self.hud)
self.actor_list.append(self.camera_semseg.sensor)
self.collision_sensor = CollisionSensor(self.player,
self.hud) # 충돌 여부 판단하는 센서
self.actor_list.append(self.collision_sensor.sensor)
self.lane_invasion_sensor = LaneInvasionSensor(
self.player, self.hud) # lane 침입 여부 확인하는 센서
self.actor_list.append(self.lane_invasion_sensor.sensor)
self.gnss_sensor = GnssSensor(self.player)
self.actor_list.append(self.gnss_sensor.sensor)
# --------------
# Spawn Surrounding vehicles
# --------------
# spawn_points = carla.Transform(start_pose.location+carla.Location(0,10,0),start_pose.rotation) #world.get_map().get_spawn_points()
# spawn_points = self.map.get_waypoint(spawn_points.location,lane_type=carla.LaneType.Driving)
spawn_points = world.get_map().get_spawn_points()[85:]
number_of_spawn_points = len(
world.get_map().get_spawn_points()) #len(spawn_points)
if self.extra_num <= number_of_spawn_points:
# random.shuffle(spawn_points)
pass
elif self.extra_num > number_of_spawn_points:
msg = 'requested %d vehicles, but could only find %d spawn points'
print(msg)
# logging.warning(msg, self.extra_num, number_of_spawn_points)
self.extra_num = number_of_spawn_points
blueprints = world.get_blueprint_library().filter('vehicle.*')
blueprints = [
x for x in blueprints
if int(x.get_attribute('number_of_wheels')) == 4
]
# blueprints = [x for x in blueprints if not x.id.endswith('isetta')]
# blueprints = [x for x in blueprints if not x.id.endswith('carlacola')]
# print(number_of_spawn_points)
for n, transform in enumerate(spawn_points):
## surrounding vehicle 차량 spawn 지점 visualize
# world.debug.draw_string(transform.location, 'o', draw_shadow=True,
# color=carla.Color(r=255, g=255, b=255), life_time=30)
if n >= self.extra_num:
break
if transform == start_pose:
continue
blueprint = random.choice(blueprints)
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)
# blueprint.set_attribute('role_name', 'autopilot')
# batch.append(SpawnActor(blueprint, transform).then(SetAutopilot(FutureActor, True)))
extra = world.spawn_actor(
random.choice(
blueprint_library.filter('vehicle.bmw.grandtourer')),
transform)
extra_pose = self.map.get_waypoint(
transform.location, lane_type=carla.LaneType.Driving)
self.extra_controller_list.append(
Pure_puresuit_controller(extra, extra_pose, None, 30)) # km/h
# extra.set_autopilot(True)
self.extra_list.append(extra)
# for response in client.apply_batch_sync(batch):
# if response.error:
# logging.error(response.error)
# else:
# self.extra_list.append(response.actor_id)
def draw_image(self, surface, image, blend=False):
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 blend:
image_surface.set_alpha(100)
surface.blit(image_surface, (0, 0))
def save_traj(self, current_location):
f = open('route.txt', 'a')
f.write(str(current_location.transform.location.x) + " ")
f.write(str(current_location.transform.location.y) + " ")
f.write(str(current_location.transform.location.z))
f.write("\n")
f.close()
def load_traj(self):
f = open("route.txt", 'r')
while True:
line = f.readline()
if not line: break
_list = line.split(' ')
_list = list(map(float, _list))
point = carla.Transform()
point.location.x = _list[0]
point.location.y = _list[1]
point.location.z = _list[2]
self.path.append(point)
#trajectory visualize
# for n, x in enumerate(self.path):
# world.debug.draw_string(x.location, 'o', draw_shadow=True,
# color=carla.Color(r=255, g=255, b=255), life_time=30)
f.close()
def get_gray_segimg(self, image_semseg):
image_size = 96
array = np.frombuffer(image_semseg.raw_data, dtype=np.dtype("uint8"))
array = array.reshape((self.height, self.width, 4))
array = array[:, :, :3]
array = array[:, :, ::-1]
array = cv2.cvtColor(array, cv2.COLOR_BGR2GRAY)
cv2.imshow("camera.semantic_segmentation", array)
cv2.resizeWindow('Resized Window', 100, 100)
cv2.waitKey(1)
array = np.reshape([cv2.resize(array, (image_size, image_size))],
(1, image_size, image_size)) # (1, 96, 96)
return array
def main(self):
clock = pygame.time.Clock()
display = pygame.display.set_mode((self.width, self.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
controller = Pure_puresuit_controller(self.player, self.waypoint,
self.extra_list, 50) # km/h
Keyboardcontrol = KeyboardControl(self, False)
# controller = VehicleControl(self, True)
# self.player.apply_control(carla.Vehicle
# Control(throttle=1.0, steer=0.0, brake=0.0))
vehicles = self.world.get_actors().filter('vehicle.*')
# for i in vehicles:
# print(i.id)
cnt = 0
decision = None
try:
# Create a synchronous mode context.
with CarlaSyncMode(world,
self.camera_rgb.sensor,
self.camera_semseg.sensor,
fps=40) as sync_mode:
while True:
if Keyboardcontrol.parse_events(client, self, clock):
return
clock.tick()
cnt += 1
if cnt == 1000:
print('수해유 ㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠㅠ')
decision = 1
aa = controller.apply_control(decision)
else:
aa = controller.apply_control()
for i in range(len(self.extra_controller_list)):
self.extra_controller_list[i].apply_control()
# world.debug.draw_string(aa, 'o', draw_shadow=True,
# color=carla.Color(r=255, g=255, b=255), life_time=-1)
# print(self.waypoint.transform)
# print(self.player.get_transform())
# print(dist)
self.hud.tick(self, clock)
# print( clock.get_fps())
# Advance the simulation and wait for the data.
snapshot, image_rgb, image_semseg = sync_mode.tick(
timeout=0.1)
# Choose the next waypoint and update the car location.
# self.waypoint = random.choice(self.waypoint.next(1.5))
# self.player.set_transform(self.waypoint.transform)
# self.world.debug.draw_point(self.waypoint.transform.location,size=0.1,color=carla.Color(r=255, g=255, b=255),life_time=1)
## player trajactory history visualize ## -> project_to_road = false 시 차량의 궤적, True 시 차선센터 출력
# curent_location=self.map.get_waypoint(self.player.get_transform().location,project_to_road=True)
# world.debug.draw_string(curent_location.transform.location, 'o', draw_shadow=True,
# color=carla.Color(r=0, g=255, b=255), life_time=100)
##이동 궤적 저장
# self.save_traj(curent_location)
# self.world.debug.draw_point(ww.transform.location,size=0.1,color=carla.Color(r=255, g=255, b=255),life_time=1)
self.spectator.set_transform(
carla.Transform(
self.player.get_transform().location +
carla.Location(z=100), carla.Rotation(pitch=-90)))
image_semseg.convert(
carla.ColorConverter.CityScapesPalette)
gray_image = self.get_gray_segimg(image_semseg)
# fps = round(1.0 / snapshot.timestamp.delta_seconds)
# Draw the display.
self.draw_image(display, image_rgb)
# self.draw_image(display, image_semseg, blend=False)
# pygame.gfxdraw.filled_circle(display,int(self.waypoint.transform.location.x),int(self.waypoint.transform.location.y),5,(255,255,255))
self.hud.render(display)
pygame.display.flip()
finally:
print('\ndestroying %d vehicles' % len(self.extra_list))
# client.apply_batch([carla.command.DestroyActor(x) for x in self.extra_list])
print('destroying actors.')
for actor in self.actor_list:
actor.destroy()
for extra in self.extra_list:
extra.destroy()
pygame.quit()
print('done.')
