def game_loop():
client = carla.Client('127.0.0.1', 2000)
client.set_timeout(4.0)
world = client.get_world()
controller = carla.VehicleControl()
blueprint = random.choice(world.get_blueprint_library().filter('vehicle.*'))
start_pose = carla.Transform(carla.Location(107, 133, 0.7))
player = world.spawn_actor(blueprint, start_pose)
agent = BasicAgent(player)
agent.set_destination((326, 133, 0.7))
# locate the view
spectator = world.get_spectator()
spectator.set_transform(get_transform(player.get_location(), 180))
while True:
try:
world.wait_for_tick(10.0)
world.tick()
control = agent.run_step()
player.apply_control(control)
except Exception as exception:
print("Exception")
def main():
#pygame.init()
client = carla.Client('localhost', 2000)
client.set_timeout(10)
world = client.get_world()
vehicle2 = world.get_actor(86)
frame = None
#custom_controller = VehiclePIDController(vehicle2, args_lateral = {'K_P': 1, 'K_D': 0.0, 'K_I': 0}, args_longitudinal = {'K_P': 1, 'K_D': 0.0, 'K_I': 0.0})
#print('Enabling synchronous mode')
#settings = world.get_settings()
#settings.synchronous_mode = True
#world.apply_settings(settings)
vehicle2.set_simulate_physics(True)
amap = world.get_map()
sampling_resolution = 2
dao = GlobalRoutePlannerDAO(amap, sampling_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
spawn_points = world.get_map().get_spawn_points()
driving_car = BasicAgent(vehicle2, target_speed=15)
destiny = spawn_points[100].location
driving_car.set_destination((destiny.x, destiny.y, destiny.z))
vehicle2.set_autopilot(True)
#clock = pygame.time.Clock()
while True:
#clock.tick()
world.tick()
ts = world.wait_for_tick()
# Get control commands
control_hero = driving_car.run_step()
vehicle2.apply_control(control_hero)
if frame is not None:
if ts.frame_count != frame + 1:
logging.warning('frame skip!')
print("frame skip!")
frame = ts.frame_count
def game_loop():
global world
pygame.init()
pygame.font.init()
try:
client = carla.Client(HOST, PORT)
client.set_timeout(TIMEOUT)
display = pygame.display.set_mode((IMG_LENGTH, IMG_WIDTH),
pygame.HWSURFACE | pygame.DOUBLEBUF)
hud = HUD(IMG_LENGTH, IMG_WIDTH)
world = World(client.get_world(), hud, 'vehicle.bmw.grandtourer')
controller = KeyboardControl(world, False)
# get random starting point and destination
spawn_points = world.map.get_spawn_points()
#start_transform = world.player.get_transform()
start_transform = spawn_points[20]
world.player.set_transform(start_transform)
#destination = spawn_points[random.randint(0,len(spawn_points)-1)]
destination = spawn_points[15]
agent = BasicAgent(world.player, target_speed=MAX_SPEED)
agent.set_destination((destination.location.x, destination.location.y,
destination.location.z))
blueprint_library = world.world.get_blueprint_library()
vehicle = world.player
rgb_camera = add_rgb_camera(blueprint_library, vehicle)
semantic_camera = add_semantic_camera(blueprint_library, vehicle)
rgb_camera.listen(lambda image: get_rgb_img(image))
semantic_camera.listen(lambda image: get_semantic_img(image))
clock = pygame.time.Clock()
weather = carla.WeatherParameters(
cloudyness=0, #0-100
precipitation=0, #random.randint(0,20),#0-100
precipitation_deposits=0, #random.randint(0,20),#0-100
wind_intensity=0, #0-100
sun_azimuth_angle=90, #0-360
sun_altitude_angle=90) #-90~90
world.world.set_weather(weather)
# put vehicle on starting point
world.player.set_transform(start_transform)
frames = 0
while frames < MAX_FRAMES:
frames += 1
if controller.parse_events(client, world, clock):
return
# as soon as the server is ready continue!
if not world.world.wait_for_tick(TIMEOUT):
continue
world.tick(clock)
world.render(display)
pygame.display.flip()
control = agent.run_step()
control.manual_gear_shift = False
world.player.apply_control(control)
my_location = world.player.get_transform().location
me2destination = my_location.distance(destination.location)
if me2destination < 50:
destination = spawn_points[random.randint(
0,
len(spawn_points) - 1)]
agent.set_destination(
(destination.location.x, destination.location.y,
destination.location.z))
print("destination change !!!")
finally:
rgb_camera.stop()
semantic_camera.stop()
if world is not None:
world.destroy()
pygame.quit()
def main():
global actor_list, flag, frame
client = carla.Client(host, port)
client.set_timeout(5.0)
try:
world = client.get_world()
except:
print("ERROR: Cannot get world !")
import sys
sys.exit()
set_weather(world)
#carla.TrafficLight.set_green_time(float(999))
#carla.TrafficLight.set_red_time(0)
try:
blueprint_library = world.get_blueprint_library()
# add vehicle
vehicle = add_vehicle_component(world, blueprint_library)
# put the vehicle to drive around.
#vehicle.set_autopilot(True)
map = world.get_map()
spawn_points = map.get_spawn_points()
destination = spawn_points[random.randint(0,
len(spawn_points) -
1)].location
agent = BasicAgent(vehicle, target_speed=20)
agent.set_destination((destination.x, destination.y, destination.z))
dao = GlobalRoutePlannerDAO(map)
planner = GlobalRoutePlanner(dao)
planner.setup()
vehicle.set_simulate_physics(False)
my_location = vehicle.get_location()
trace_list = planner.trace_route(my_location, destination)
waypoints = []
for (waypoint, road_option) in trace_list:
waypoints.append(waypoint)
next_point = waypoints[0].transform
camera = add_camera_component(world, blueprint_library, vehicle)
camera.stop()
while True:
vehicle.set_transform(next_point)
my_location = next_point.location
#my_location = vehicle.get_location()
me2destination = my_location.distance(destination)
if me2destination < 50:
destination = spawn_points[random.randint(
0,
len(spawn_points) - 1)].location
#agent.set_destination((destination.x,destination.y,destination.z))
print("destination change !!!")
trace_list = planner.trace_route(my_location, destination)
waypoints = []
for (waypoint, road_option) in trace_list:
waypoints.append(waypoint)
print(len(waypoints))
if len(waypoints) < 5:
print('my_location:', my_location.x, my_location.y,
my_location.z)
print('destination:', destination.x, destination.y,
destination.z)
print('me2destination:', me2destination)
next_point = waypoints[random.randint(0,
min(len(waypoints) - 1,
50))].transform
camera.listen(lambda image: deal_image(image))
while not flag:
sleep(0.01)
camera.stop()
flag = False
get_instruct(waypoints)
for waypoint in waypoints[0:min(len(waypoints) - 1, 30)]:
box_point = carla.Location(waypoint.transform.location.x,
waypoint.transform.location.y,
waypoint.transform.location.z - 0.4)
box = carla.BoundingBox(box_point,
carla.Vector3D(x=2, y=0.1, z=0.5))
rotation = carla.Rotation(
pitch=waypoint.transform.rotation.pitch,
yaw=waypoint.transform.rotation.yaw,
roll=waypoint.transform.rotation.roll)
world.debug.draw_box(box=box,
rotation=rotation,
thickness=1.2,
life_time=0)
sleep(0.3)
camera.listen(lambda image: deal_image(image))
while not flag:
sleep(0.01)
camera.stop()
flag = False
sleep(1.0)
finally:
print('destroying actors')
for actor in actor_list:
actor.destroy()
actor_list = []
print('done.')
def game_loop(args):
"""
Main loop of the simulation. It handles updating all the HUD information,
ticking the agent and, if needed, the world.
"""
pygame.init()
pygame.font.init()
world = None
try:
if args.seed:
random.seed(args.seed)
client = carla.Client(args.host, args.port)
client.set_timeout(4.0)
traffic_manager = client.get_trafficmanager()
sim_world = client.get_world()
if args.sync:
settings = sim_world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = 0.05
sim_world.apply_settings(settings)
traffic_manager.set_synchronous_mode(True)
display = pygame.display.set_mode((args.width, args.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
hud = HUD(args.width, args.height)
world = World(client.get_world(), hud, args)
controller = KeyboardControl(world)
if args.agent == "Basic":
agent = BasicAgent(world.player)
else:
agent = BehaviorAgent(world.player, behavior=args.behavior)
# Set the agent destination
spawn_points = world.map.get_spawn_points()
destination = random.choice(spawn_points).location
agent.set_destination(destination)
clock = pygame.time.Clock()
while True:
clock.tick()
if args.sync:
world.world.tick()
else:
world.world.wait_for_tick()
if controller.parse_events():
return
world.tick(clock)
world.render(display)
pygame.display.flip()
if agent.done():
if args.loop:
agent.set_destination(random.choice(spawn_points).location)
world.hud.notification(
"The target has been reached, searching for another target",
seconds=4.0)
print(
"The target has been reached, searching for another target"
)
else:
print(
"The target has been reached, stopping the simulation")
break
control = agent.run_step()
control.manual_gear_shift = False
world.player.apply_control(control)
finally:
if world is not None:
settings = world.world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
world.world.apply_settings(settings)
traffic_manager.set_synchronous_mode(True)
world.destroy()
pygame.quit()
class Game_Loop:
def __init__(self, args):
self.init_waypoint = None
self.veh_id = None
self.init = False
self.should_publish = True
self.finish_current_action = False
self.message_waypoints = 3
self.action_pack_count = 0
self.agent = None
self.world = None
self.args = args
self.lc = lcm.LCM()
self.msg_queue = Queue()
self.action_result_count = 0
self.waypoints_buffer = deque(maxlen=600)
self.self_drive = False
self.init_controller()
# 进一步对各类成员进行初始化工作
def init_controller(self):
self.lc.subscribe(connect_response_keyword,
self.connect_response_handler)
self.lc.subscribe(action_package_keyword, self.action_package_handler)
self.lc.subscribe(end_connection_keyword, self.end_connection_dealer)
self.message_listener = threading.Thread(
target=self.message_listen_process, name='MessageListenThread')
# 将子线程设置为守护线程，在主线程退出后自动退出
self.message_listener.setDaemon(True)
self.message_listener.start()
self.suspend_simulation_dealer = threading.Thread(
target=self.suspend_simulation_control_process,
name='SuspendSimulationThread')
self.suspend_simulation_dealer.setDaemon(True)
self.suspend_simulation_dealer.start()
# 监听线程需要执行的过程，仅需要监听LCM消息并将消息放入消息队列等待主线程处理即可
def message_listen_process(self):
while True:
self.lc.handle()
# 通过监听一段时间内发送action_result的个数来判断是否需要发送suspend_simulation
def suspend_simulation_control_process(self):
local_result_count = 0
listening_interval = 1
while True:
# 每隔若干秒查看一次
time.sleep(listening_interval)
print("action result count: ", self.action_result_count)
if local_result_count == self.action_result_count and local_result_count != 0:
print("no action result sent in ", listening_interval,
" seconds, sending suspend simulation package.")
suspend = suspend_simulation()
suspend.vehicle_id = self.veh_id
suspend.current_pos = self.transform_to_lcm_waypoint(
self.world.vehicle.get_transform())
if self.should_publish is True:
self.lc.publish(suspend_simulation_keyword,
suspend.encode())
elif local_result_count == 0:
pack = reset_simulation()
pack.vehicle_id = self.veh_id
self.lc.publish(reset_simulation_keyword, pack.encode())
else:
local_result_count = self.action_result_count
# from carla transform to lcm waypoint
def transform_to_lcm_waypoint(self, transform):
lcm_waypoint = Waypoint()
lcm_waypoint.Location = [
transform.location.x, -1 * transform.location.y,
transform.location.z
]
lcm_waypoint.Rotation = [
transform.rotation.pitch, transform.rotation.yaw,
transform.rotation.roll
]
print("lcm waypoint location: ", lcm_waypoint.Location)
return lcm_waypoint
def transform_waypoint(self, lcm_waypoint):
"""
transfrom from LCM waypoint structure to local_planner waypoint structure.
"""
new_waypoint = carla.libcarla.Transform()
new_waypoint.location.x = lcm_waypoint.Location[0]
new_waypoint.location.y = -1 * lcm_waypoint.Location[1]
new_waypoint.location.z = lcm_waypoint.Location[2]
new_waypoint.rotation.pitch = lcm_waypoint.Rotation[0]
new_waypoint.rotation.yaw = lcm_waypoint.Rotation[1] - 90.0
new_waypoint.rotation.roll = lcm_waypoint.Rotation[2]
return new_waypoint
def action_package_handler(self, channel, data):
msg = action_package.decode(data)
if msg.vehicle_id != self.veh_id:
return
print('receive message on channel ', channel)
# print('type of this message: ', type(msg))
que_element = [action_package_keyword, msg]
self.msg_queue.put(que_element)
# concrete dealer function of action package.
def action_package_dealer(self, msg):
if msg.vehicle_id != self.veh_id:
print("invalid vehicle id from message! self id: ", self.veh_id)
return
# print("len of msg waypoints: ", len(msg.waypoints))
for i in range(self.message_waypoints):
new_point = self.transform_waypoint(msg.waypoints[i])
# print("new point for vehicle ", msg.vehicle_id, ": ", new_point)
self.waypoints_buffer.append(new_point)
def connect_response_handler(self, channel, data):
msg = connect_response.decode(data)
if self.init:
return
print('receive message on channel ', channel)
# print('type of this message: ', type(msg))
que_element = [connect_response_keyword, msg]
self.msg_queue.put(que_element)
def connect_response_dealer(self, msg):
self.veh_id = msg.vehicle_id
print("veh id: ", self.veh_id)
self.init_waypoint = msg.init_pos
self.init = True
def end_connection_dealer(self, channel, data):
msg = end_connection.decode(data)
print('receive message on channel ', channel)
que_element = [end_connection_keyword, msg]
self.msg_queue.put(que_element)
def game_loop(self):
pygame.init()
pygame.font.init()
# world = None
try:
client = carla.Client(self.args.host, self.args.port)
client.set_timeout(4.0)
display = pygame.display.set_mode(
(self.args.width, self.args.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
connect_request_msg = connect_request()
self.lc.publish(connect_request_keyword,
connect_request_msg.encode())
print(
"connect request message publish done, waiting for connecting response..."
)
# 不断监听初始化回应的消息
while True:
try:
[keyword, msg] = self.msg_queue.get(timeout=0.01)
if keyword == connect_response_keyword:
self.connect_response_dealer(msg)
break
except queue.Empty:
continue
hud = HUD(self.args.width, self.args.height)
self.world = World(client.get_world(), hud, self.args.filter)
controller = KeyboardControl(self.world, False)
spawn_point = self.transform_waypoint(self.init_waypoint)
print("spawn_point: ", spawn_point)
self.world.player.set_transform(spawn_point)
# world.vehicle.set_location(spawn_point.location)
clock = pygame.time.Clock()
# time.sleep(10)
# print("location: ", world.vehicle.get_location())
if self.args.agent == "Roaming":
# print("Roaming!")
self.agent = RoamingAgent(self.world.player)
else:
self.agent = BasicAgent(self.world.player,
target_speed=self.args.speed)
spawn_point = self.world.map.get_spawn_points()[0]
print(spawn_point)
self.agent.set_destination(
(spawn_point.location.x, spawn_point.location.y,
spawn_point.location.z))
self.agent.drop_waypoint_buffer()
# 在这里发送车辆初始位置给服务器
# print("location: ", world.vehicle.get_transform())
# init_lcm_waypoint = self.transform_to_lcm_waypoint(world.vehicle.get_transform())
# connect_request_msg.init_pos = init_lcm_waypoint
# clock = pygame.time.Clock()
# print(len(client.get_world().get_map().get_spawn_points()))
# pre_loc = [0.0, 0.0, 0.0]
# 在这里进行后续的循环接收消息
'''
main loop of the client end.
'''
i_var = 0
settings = self.world.world.get_settings()
settings.fixed_delta_seconds = None
self.world.world.apply_settings(settings)
while True:
if controller.parse_events(client, self.world, clock):
return
# as soon as the server is ready continue!
if not self.world.world.wait_for_tick(10.0):
continue
self.world.tick(clock)
self.world.render(display)
pygame.display.flip()
# 是否需要向SUMO服务器发送action result消息
should_publish_result_msg = False
try:
[keyword, msg] = self.msg_queue.get(timeout=0.01)
# print("keyword of message is ", keyword)
# Receive an action package
if keyword == action_package_keyword:
print("Receive an action package!")
self.agent.drop_waypoint_buffer()
# 在收到新的路点消息后丢弃当前缓冲中剩余的路点
self.action_package_dealer(msg)
# print("waypoint length: ", len(self.waypoints_buffer))
for i in range(self.message_waypoints):
temp_waypoint = self.waypoints_buffer.popleft()
# print("waypoint in main loop is ", temp_waypoint)
self.agent.add_waypoint(temp_waypoint)
self.waypoints_buffer.clear()
elif keyword == connect_response_keyword:
self.connect_response_dealer(msg)
elif keyword == end_connection_keyword:
if msg.vehicle_id != self.veh_id:
print(
"invalid vehicle id from end connection package"
)
else:
print("connection to SUMO ended.")
# self.agent.set_target_speed(20)
self.agent.set_sumo_drive(False)
# self.agent.compute_next_waypoints(3)
# print("simulation of this vehicle ended. Destroying ego.")
# self.should_publish = False
# if self.world is not None:
# self.world.destroy()
# return
else:
pass
except queue.Empty:
pass
control = self.agent.run_step()
if control:
self.world.player.apply_control(control)
if self.agent.get_finished_waypoints(
) >= self.message_waypoints:
should_publish_result_msg = True
# 获取当前位置和速度信息并发送到SUMO服务器
if should_publish_result_msg:
current_speed = self.world.player.get_velocity()
current_transform = self.world.player.get_transform()
action_res_pack = action_result()
action_res_pack.current_pos = self.transform_to_lcm_waypoint(
current_transform)
action_res_pack.vehicle_id = self.veh_id
# print("current speed: ", current_speed)
action_res_pack.current_speed = [
current_speed.x, current_speed.y, current_speed.z
]
self.lc.publish(action_result_keyword,
action_res_pack.encode())
self.action_result_count += 1
should_publish_result_msg = False
finally:
if self.world is not None:
self.world.destroy()
pygame.quit()
def main():
#pygame.init()
client = carla.Client('localhost', 2000)
client.set_timeout(10.0)
world = client.get_world()
frame = None
#Create WaypointMap
amap = world.get_map()
sampling_resolution = 2
dao = GlobalRoutePlannerDAO(amap, sampling_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
spawn_points = world.get_map().get_spawn_points()
#Spawn Cars on Waypoint
model1 = random.choice(
world.get_blueprint_library().filter('vehicle.bmw.*'))
model2 = random.choice(
world.get_blueprint_library().filter('vehicle.bmw.*'))
spawn_points = world.get_map().get_spawn_points()
spawn_point_numeric_value1 = randrange(300)
print("Spawpoint 1: ")
print(spawn_point_numeric_value1)
spawn_point1 = spawn_points[spawn_point_numeric_value1]
vehicle1 = world.try_spawn_actor(model1, spawn_point1)
time.sleep(5)
location1 = vehicle1.get_location()
print(location1)
print(vehicle1.id)
spawn_point_numeric_value2 = randrange(300)
print("Spawpoint 2: ")
print(spawn_point_numeric_value2)
spawn_point2 = spawn_points[spawn_point_numeric_value2]
vehicle2 = world.try_spawn_actor(model2, spawn_point2)
time.sleep(5)
location2 = vehicle1.get_location()
print(location2)
print(vehicle2.id)
#print path of vehicle 2
a = carla.Location(spawn_point1.location)
b = carla.Location(spawn_point2.location)
w1 = grp.trace_route(a, b)
i = 0
for w in w1:
if i % 10 == 0:
world.debug.draw_string(w[0].transform.location,
'O',
draw_shadow=False,
color=carla.Color(r=255, g=0, b=0),
life_time=120.0,
persistent_lines=True)
else:
world.debug.draw_string(w[0].transform.location,
'O',
draw_shadow=False,
color=carla.Color(r=0, g=0, b=255),
life_time=1000.0,
persistent_lines=True)
i += 1
#Start Car
vehicle1.set_simulate_physics(True)
driving_car = BasicAgent(vehicle1, target_speed=50)
destiny = spawn_point2.location
driving_car.set_destination((destiny.x, destiny.y, destiny.z))
#vehicle1.set_autopilot(True)
while True:
world.tick()
ts = world.wait_for_tick()
# Get control commands
control_hero = driving_car.run_step()
vehicle1.apply_control(control_hero)
if frame is not None:
if ts.frame_count != frame + 1:
logging.warning('frame skip!')
print("frame skip!")
frame = ts.frame_count
def game_loop(args):
pygame.init()
pygame.font.init()
world = None
try:
client = carla.Client(args.host, args.port)
client.set_timeout(4.0)
display = pygame.display.set_mode((args.width, args.height),
pygame.HWSURFACE | pygame.DOUBLEBUF)
hud = HUD(args.width, args.height)
world = World(client.get_world(), hud, args.filter)
controller = KeyboardControl(world, False)
agent = BasicAgent(world.player, 30)
spawn_points = world.map.get_spawn_points()
destination = spawn_points[random.randint(0,
len(spawn_points) -
1)].location
agent.set_destination((destination.x, destination.y, destination.z))
clock = pygame.time.Clock()
dao = GlobalRoutePlannerDAO(world.map)
planner = GlobalRoutePlanner(dao)
planner.setup()
while True:
if controller.parse_events(client, world, clock):
return
# as soon as the server is ready continue!
if not world.world.wait_for_tick(10.0):
continue
world.tick(clock)
world.render(display)
pygame.display.flip()
control = agent.run_step()
control.manual_gear_shift = False
world.player.apply_control(control)
my_location = world.player.get_location()
me2destination = my_location.distance(destination)
if me2destination < 10:
destination = spawn_points[random.randint(
0,
len(spawn_points) - 1)].location
agent.set_destination(
(destination.x, destination.y, destination.z))
print("destination change !!!")
trace_list = planner.trace_route(my_location, destination)
for (waypoint, road_option) in trace_list[0:20]:
world.world.debug.draw_string(waypoint.transform.location,
'O',
draw_shadow=False,
color=carla.Color(r=255,
g=0,
b=0),
life_time=1.0,
persistent_lines=True)
#origin = world.player.get_location()
#turn_list = planner.abstract_route_plan(origin, spawn_point.location)
#print('next cmd:',turn_list[0])
finally:
if world is not None:
world.destroy()
pygame.quit()
def game_loop(args):
global X, X_init
actor_list = []
X = readX() # X: coords_motionPlanner.txt
X_init = X
try:
client = carla.Client(args.host, args.port) # get client
client.set_timeout(4.0)
world = client.get_world() # get world
settings = world.get_settings()
print(settings)
# -----------------------------------------
# create our ego car：make; color; name; initial state
# -----------------------------------------
init_state = [X[0][3], X[0][4], X[0][5]]
car_ego_make = 'vehicle.audi.tt'
car_ego_color = '0,0,255'
car_ego_name = 'car_ego'
car_ego = create_car_ego(world, car_ego_make, car_ego_color,
car_ego_name, init_state)
actor_list.append(car_ego)
log.write('ego car is created and its id: %d!\n' % car_ego.id)
log.flush()
print('ego car is created and its id:', car_ego.id)
# -----------------------------------------
# use .log to record running process (video)
# -----------------------------------------
if not os.path.exists(address1 + 'saved/'):
os.mkdir(address1 + 'saved/')
recording = address1 + 'saved/' + 'recording_' + str(round(
time.time())) + '.log'
log.write('recording video filename: %s\n' % recording)
log.flush()
client.start_recorder(recording)
print('recording starts')
# -----------------------------------------
# implement agents: basic_agent.py
# -----------------------------------------
agent_ego = BasicAgent(car_ego)
# -----------------------------------------
# global view: get all information of the world
# -----------------------------------------
world = client.get_world()
vehicles = world.get_actors().filter('vehicle.*')
# -----------------------------------------
# our ego car can move only if other cars exist and their speed > 0
# -----------------------------------------
while len(vehicles) <= 1: # other cars exist
world = client.get_world()
vehicles = world.get_actors().filter('vehicle.*')
log.write('%d cars in the world' % len(vehicles))
log.flush()
for car in vehicles: # other cars' speed > 0
check_car = car
if check_car.attributes.get(
'role_name') != 'car_ego': # find other car
speed_car = math.sqrt(check_car.get_velocity().x**2 +
check_car.get_velocity().y**2)
break
else:
continue
time.sleep(2.)
# -----------------------------------------
# execute task and motion planning
# -----------------------------------------
i = 0 # step
behavior = X[i][1] # 1: merge lane; 0: not merge lane
while i < len(X) - 1:
if behavior == 0: # if not merging lane, skip risk model
# -----------------------------------------
# set the current step
# -----------------------------------------
curr_lane = X[i][2]
next_lane = X[i + 1][2]
curr_step = [X[i + 1][3], X[i + 1][4]]
agent_ego.set_destination((curr_step[0], curr_step[1], 1.2))
log.write('current lane is %d\n' % curr_lane)
log.write('our ego car does not merge lane in current step\n')
log.write(
'our ego car performs current step to get next lane %d\n' %
next_lane)
log.flush()
print('current lane is %d' % curr_lane)
print('our ego car does not merge lane in current step')
print(
'our ego car performs current step to get next lane %d\n' %
next_lane)
# -----------------------------------------
# execute the current step
# -----------------------------------------
ego_loc = car_ego.get_location(
) # current location of our ego car
mini_dis = 15 # a minimal distance to check if our ego car achieves the destination
while math.sqrt((ego_loc.x - curr_step[0])**2 +
(ego_loc.y - curr_step[1])**2) > mini_dis:
if not world.wait_for_tick(
): # as soon as the server is ready continue!
continue
control = agent_ego.run_step()
car_ego.apply_control(control)
control.manual_gear_shift = False
ego_loc = car_ego.get_location()
log.write('our ego car reaches the target lane %d\n' %
X[i + 1][2])
log.flush()
# -----------------------------------------
# ready for the next step
# -----------------------------------------
i = i + 1
behavior = X[i][1]
else: # if our ego car merge lane, apply risk model and compute "p_risk"
# -----------------------------------------
# compute risk value "p_risk"
# -----------------------------------------
curr_lane = X[i][2]
p_risk = fuc_risk(i, car_ego, world, curr_lane)
log.write('current lane is %d\n' % curr_lane)
log.write('p_risk is %3.3f if our ego car merges lane\n' %
p_risk)
log.flush()
print('current lane is %d' % curr_lane)
print('p_risk is %3.3f if our ego car merges lane' % p_risk)
# -----------------------------------------
# if "p_risk" is smaller than "thre_risk", our ego car still merges lane
# if "thre_risk" < 0, it is our TMPUD
# if "thre_risk" > 0, it is one baseline
# -----------------------------------------
thre_risk = 0.0
if p_risk < thre_risk:
# -----------------------------------------
# set the current step
# -----------------------------------------
curr_step = [X[i + 1][3], X[i + 1][4]]
next_lane = X[i + 1][2]
agent_ego.set_destination(
(curr_step[0], curr_step[1], 1.2))
log.write('current lane is %d\n' % curr_lane)
log.write(
'our ego car does not merge lane in current step\n')
log.write(
'our ego car performs current step to get next lane %d\n'
% next_lane)
log.flush()
print('current lane is %d' % curr_lane)
print('our ego car does not merge lane in current step')
print(
'our ego car performs current step to get next lane %d\n'
% next_lane)
# -----------------------------------------
# execute the current step
# -----------------------------------------
ego_loc = car_ego.get_location(
) # current location of our ego car
while math.sqrt((ego_loc.x - curr_step[0])**2 +
(ego_loc.y - curr_step[1])**2) > mini_dis:
if not world.wait_for_tick(
): # as soon as the server is ready continue!
continue
control = agent_ego.run_step()
car_ego.apply_control(control)
control.manual_gear_shift = False
ego_loc = car_ego.get_location()
log.write('our ego car reaches the target lane %d\n' %
X[i + 1][2])
log.flush()
# -----------------------------------------
# ready for the next step
# -----------------------------------------
i = i + 1
behavior = X[i][1]
else:
# -----------------------------------------
# if "p_risk" is bigger than "thre_risk", it does not mean our ego car cannot merge lane definitely
# we need to do task planning again with the updated information
# but if no new plan is generated, our ego car are forced to merge lane
# -----------------------------------------
# log.write('Risk! cannot merge lane, please do task planning again!\n')
# print('Risk! cannot merge lane, please do task planning again!')
# -----------------------------------------
# update the risk value of target lane in lanes_risk.txt
# -----------------------------------------
with open(address1 + 'interaction/lanes_risk.txt',
'r') as f: # read original lane risk
lanes_risk = [line.rstrip() for line in f]
f.close()
init_risk = int(lanes_risk[int(X[i][2] - 1) * 12 + 6 - 1])
updated_risk = int(p_risk * 100)
lanes_risk[int(X[i][2] - 1) * 12 + 6 - 1] = updated_risk
f = open(address1 + 'interaction/lanes_risk.txt',
'w') # write updated lane risk
for item in lanes_risk:
f.write('%s\n' % str(item))
f.flush()
f.close()
log.write('update risk value of lane %d\n' %
int(curr_lane))
log.write('original value is %d, now it is %d!\n' %
(init_risk, updated_risk))
log.flush()
print('update risk value of lane %d\n' % int(curr_lane))
print('original value is %d, now it is %d!\n' %
(init_risk, updated_risk))
# -----------------------------------------
# do task planning again and generate the new X (coords_motionPlanner.txt)
# -----------------------------------------
source = str(round(curr_lane))
dest = str(round(X[-1][2]))
command1 = 'python' + ' ' + address1 + 'task-level' + '/' + 'get_optimal_task_plan.py' + ' ' + source + ' ' + dest
os.system(command1) # get optimal task plan
command2 = 'python' + ' ' + address1 + 'task-level/ground_task_plan.py'
os.system(command2) # ground task plan
X = readX() # read X again
# -----------------------------------------
# if X changes, it means our ego car finds a new task plan
# -----------------------------------------
if not (X == X_init):
i = 0
behavior = X[i][1]
X_init = X
else:
# -----------------------------------------
# if X not change
# -----------------------------------------
behavior = 0 # temporaily do not change
log.write(
'do task task planning again, start and dest lanes are %s and %s, respectively\n'
% (source, dest))
log.write(
'Here to update coordinate for motion planner (X)\n')
log.flush()
log.write('task and motion planning is done!\n\n\n\n')
log.flush()
finally:
for actor in actor_list: # delete our ego car
actor.destroy()
client.stop_recorder()
print("ALL cleaned up!")
def main():
global actor_list
client = carla.Client(host, port)
client.set_timeout(5.0)
try:
world = client.get_world()
except:
print("ERROR: Cannot get world !")
import sys
sys.exit()
set_weather(world)
#carla.TrafficLight.set_green_time(float(999))
#carla.TrafficLight.set_red_time(0)
try:
blueprint_library = world.get_blueprint_library()
# add vehicle
vehicle = add_vehicle_component(world, blueprint_library)
# put the vehicle to drive around.
#vehicle.set_autopilot(True)
map = world.get_map()
spawn_points = map.get_spawn_points()
destination = spawn_points[random.randint(0,
len(spawn_points) -
1)].location
agent = BasicAgent(vehicle, target_speed=20)
agent.set_destination((destination.x, destination.y, destination.z))
dao = GlobalRoutePlannerDAO(map)
planner = GlobalRoutePlanner(dao)
planner.setup()
vehicle.set_simulate_physics(False)
my_location = vehicle.get_location()
trace_list = planner.trace_route(my_location, destination)
waypoints = []
for (waypoint, road_option) in trace_list:
waypoints.append(waypoint)
next_point = waypoints[0].transform
#add_camera_component(world, blueprint_library, vehicle)
while True:
vehicle.set_transform(next_point)
my_location = next_point.location
#my_location = vehicle.get_location()
me2destination = my_location.distance(destination)
if me2destination < 50:
destination = spawn_points[random.randint(
0,
len(spawn_points) - 1)].location
#agent.set_destination((destination.x,destination.y,destination.z))
print("destination change !!!")
trace_list = planner.trace_route(my_location, destination)
waypoints = []
for (waypoint, road_option) in trace_list:
waypoints.append(waypoint)
print(len(waypoints))
if len(waypoints) < 5:
print('my_location:', my_location.x, my_location.y,
my_location.z)
print('destination:', destination.x, destination.y,
destination.z)
print('me2destination:', me2destination)
next_point = waypoints[random.randint(0,
min(len(waypoints) - 1,
50))].transform
x = []
y = []
#theta = waypoints[0].transform.rotation.roll
theta = math.atan2((waypoints[3].transform.location.y -
waypoints[0].transform.location.y),
(waypoints[3].transform.location.x -
waypoints[0].transform.location.x))
for i in range(min(len(waypoints) - 1, 50)):
_x = waypoints[i].transform.location.x - waypoints[
0].transform.location.x
_y = waypoints[i].transform.location.y - waypoints[
0].transform.location.y
new_theta = math.pi / 2 - theta
x_ = _x * math.cos(new_theta) - _y * math.sin(new_theta)
y_ = _y * math.cos(new_theta) + _x * math.sin(new_theta)
x.append(x_)
y.append(y_)
scale = 10
plt.figure(figsize=(8, 4))
plt.xlim(-scale, scale)
plt.ylim(0, scale)
plt.plot(x, y, "r-", linewidth=50)
plt.show()
sleep(0.3)
finally:
print('destroying actors')
for actor in actor_list:
actor.destroy()
actor_list = []
print('done.')
def game_loop(options_dict):
world = None
try:
client = carla.Client('localhost', 2000)
client.set_timeout(60.0)
print('Changing world to Town 5')
client.load_world('Town05')
world = World(client.get_world(), options_dict['sync'])
spawn_points = world.world.get_map().get_spawn_points()
vehicle_bp = 'model3'
vehicle_transform = spawn_points[options_dict['spawn_point_indices'][0]]
# vehicle_transform.location.x -= 60
vehicle_transform.location.x -= (20 + np.random.normal(0.0, 20, size=1).item()) # start 80 std 20 # start 40 std 15 # start 10 std 10
vehicle = Car(vehicle_bp, vehicle_transform, world)
ticks = 0
while ticks < 60:
world.world.tick()
world_snapshot = world.world.wait_for_tick(10.0)
if not world_snapshot:
continue
else:
ticks += 1
agent = BasicAgent(vehicle.vehicle)
destination_point = spawn_points[options_dict['spawn_point_indices'][1]].location
print('Going to ', destination_point)
agent.set_destination((destination_point.x, destination_point.y, destination_point.z))
camera_bp = ['sensor.camera.rgb', 'sensor.camera.depth', 'sensor.lidar.ray_cast']
if options_dict['num_cam'] == 1:
camera_transform = carla.Transform(carla.Location(x=1.5, z=2.4), carla.Rotation(pitch=-15, yaw=0))
cam1 = Camera(camera_bp[0], camera_transform, vehicle, options_dict['trajector_num'], save_data=True)
elif options_dict['num_cam'] == 2:
camera_transform = [carla.Transform(carla.Location(x=1.5, z=2.4), carla.Rotation(pitch=-15, yaw=40)), carla.Transform(carla.Location(x=1.5, z=2.4), carla.Rotation(pitch=-15, yaw=-40)), carla.Transform(carla.Location(x=1.5, z=2.4))]
cam1 = Camera(camera_bp[0], camera_transform[0], vehicle, options_dict['trajector_num'], save_data=True)
cam2 = Camera(camera_bp[0], camera_transform[1], vehicle, options_dict['trajector_num'], save_data=True)
# depth1 = Camera(camera_bp[1], camera_transform[0], vehicle)
# depth2 = Camera(camera_bp[1], camera_transform[1], vehicle)
# lidar = Lidar(camera_bp[2], camera_transform[2], vehicle)
prev_location = vehicle.vehicle.get_location()
sp = 2
file = open("control_input.txt", "a")
while True:
world.world.tick()
world_snapshot = world.world.wait_for_tick(60.0)
if not world_snapshot:
continue
# wait for sensors to sync
# while world_snapshot.frame_count!=depth.frame_n or world_snapshot.frame_count!=segment.frame_n:
# time.sleep(0.05)
control = agent.run_step()
control.steer = np.clip(control.steer+np.random.normal(0.0, 0.5, size=1), -1.0, 1.0).item()
control.throttle = np.clip(control.throttle+np.random.normal(0.5, 0.25, size=1), 0.0, 1.0).item()
vehicle.vehicle.apply_control(control)
w = str(options_dict['trajector_num']) + ',' + str(world_snapshot.frame_count) + ',' + str(control.steer) + ',' + str(get_speed(vehicle.vehicle)) + '\n'
file.write(w)
current_location = vehicle.vehicle.get_location()
if current_location.distance(prev_location) <= 0.0 and current_location.distance(destination_point) <= 10:
print('distance from destination: ', current_location.distance(destination_point))
if len(options_dict['spawn_point_indices']) <= sp:
break
else:
destination_point = spawn_points[options_dict['spawn_point_indices'][sp]].location
print('Going to ', destination_point)
agent.set_destination((destination_point.x, destination_point.y, destination_point.z))
sp += 1
prev_location = current_location
finally:
if world is not None:
world.destroy()
class MyController():
_isSteeringWheelConnected = False
_positionFeedback = 0
_feedbackAngle = 0
_log_message = None
world = None
###################### Init #######################
def __init__(self, args):
pygame.init()
pygame.font.init()
self._controller = DriveController("/dev/ttyUSB0", 1, False)
##### Register Callbacks #####
self._controller.logCallback = self._logCallback
self._controller.errorCallback = self._errorCallback
self._controller.readingCallback = self._readingCallback
self._controller.connectedCallback = self._connectedCallback
try:
# initialize carla client
client = carla.Client(args.host, args.port)
client.set_timeout(4.0)
self._display = pygame.display.set_mode(
(args.width, args.height), pygame.HWSURFACE | pygame.DOUBLEBUF)
self._hud = HUD(args.width, args.height)
self._world = World(client.get_world(), self._hud, args.filter)
self._keyboardController = KeyboardControl(self._world)
if args.agent == "Roaming":
self._agent = RoamingAgent(self._world.player)
else:
self._agent = BasicAgent(self._world.player)
spawn_point = self._world.map.get_spawn_points()[0]
self._agent.set_destination(
(spawn_point.location.x, spawn_point.location.y,
spawn_point.location.z))
self._clock = pygame.time.Clock()
# Steering Wheel is connected
self._setupSteeringWheel(args)
# run loop logic
while not self._keyboardController.parse_events():
self._runLoop()
finally:
self.__del__()
def _setupSteeringWheel(self, args):
self._clock.tick(10) # run at 10 fps
font = pygame.font.Font(pygame.font.get_default_font(), 34)
# Few variables
self.steeringWheelSetupStage = 0
isSettingUp = True
while isSettingUp:
self._display.fill((0, 0, 0))
# Setup stages
# stage - to connect
if self.steeringWheelSetupStage == 0:
# Connect and start
self._controller.connect()
self.steeringWheelSetupStage += 1
# stage - connecting
elif self.steeringWheelSetupStage == 1:
infoText = font.render(
'Connecting to Steering Wheel %s' %
('.' * (int(pygame.time.get_ticks() / 1000) % 4)), True,
(255, 255, 255))
infoTextRect = infoText.get_rect()
infoTextRect.center = (args.width // 2, args.height // 4)
self._display.blit(infoText, infoTextRect)
if self._isSteeringWheelConnected:
print("Connected !")
self.steeringWheelSetupStage += 1
# stage - set steering to zero
elif self.steeringWheelSetupStage == 2:
self._display.blit(infoText, infoTextRect)
infoText = font.render(
'Move the Steering wheel to zero position and press \'s\' key',
True, (255, 255, 255))
infoTextRect = infoText.get_rect()
infoTextRect.center = (args.width // 2, args.height // 4)
self._display.blit(infoText, infoTextRect)
text = font.render(
'Steering angle : %lf' % self._feedbackAngle, True,
(255, 0, 0))
textRect = text.get_rect()
textRect.center = (args.width // 2, args.height // 2)
self._display.blit(text, textRect)
#
# finished stages logic
if self._log_message is not None:
if self._log_message == "Opening SM bus failed":
self._display.fill((0, 0, 0))
logText = font.render(
"Cannot connect to Steering wheel, please check and try again",
True, (255, 100, 100))
logTextRect = logText.get_rect()
logTextRect.center = (args.width // 2, args.height // 2)
self._display.blit(logText, logTextRect)
else: # show any log message
logText = pygame.font.Font(pygame.font.get_default_font(),
24).render(
self._log_message, True,
(250, 255, 250))
self._display.blit(logText, (20, args.height - 40))
pygame.display.flip()
for event in pygame.event.get():
if event.type == pygame.QUIT:
sys.exit(0)
elif event.type == pygame.KEYUP:
# exit application
if (event.key == pygame.K_ESCAPE) or (
event.key == pygame.K_q
and pygame.key.get_mods() & pygame.KMOD_CTRL):
sys.exit(0)
# If s key is pressed
if event.key == pygame.K_s:
self._controller.setZero()
time.sleep(1)
isSettingUp = False
# Setup other settings for the steering wheel
self._controller.setAddedConstantTorque(150)
# self._controller.disableSetpointTracking(500, 0.001)
def _runLoop(self):
# as soon as the server is ready continue!
self._world.world.wait_for_tick(10.0)
self._world.tick(self._clock)
self._world.render(self._display)
pygame.display.flip()
# skip traffic lights
if self._world.player.is_at_traffic_light():
traffic_light = self._world.player.get_traffic_light()
if traffic_light.get_state() == carla.TrafficLightState.Red:
self._hud.notification("Traffic light changed!")
traffic_light.set_state(carla.TrafficLightState.Green)
control = self._agent.run_step()
# control.brake = False
# control.throttle = 1
# control.hand_brake = False
control.manual_gear_shift = False
# Move steering wheel
steer = int(control.steer * 90)
self._goToAngle(steer, 1000, 3, 0.05)
# print(steer - self._feedbackAngle)
control.steer = self._feedbackSteer * 2
self._world.player.apply_control(control)
def _goToAngle(self, pos, max_torque=2000, Kp=1.6, Kd=0.1):
self._controller.setAbsoluteSetpoint(int((pos / 360) * 10000),
max_torque, Kp, Kd)
################### Destructor ###################
def __del__(self):
######################
# nullptr ALL CALLBACKS before calling desctructor
# Very very important to mitigate deadlock while exiting
######################
self._controller.logCallback = None
self._controller.errorCallback = None
self._controller.readingCallback = None
self._controller.connectedCallback = None
try:
if self._world is not None:
self._world.destroy()
finally:
pass
pygame.quit()
print("Quitting....")
################# Event Callbacks #################
def _logCallback(self, obj):
# logType
# message
print("LOG>", obj.message)
self._hud.notification(obj.message)
self._log_message = obj.message
def _connectedCallback(self, obj):
# isConnected
print("CON>", obj.isConnected)
self._isSteeringWheelConnected = obj.isConnected
def _errorCallback(self, obj):
# bool trackingError,
# bool driveFault
print("Error>", obj.trackingError, obj.driveFault)
def _readingCallback(self, obj):
# int posSetpoint
# int posFeedback
# int torqueSetpoint
# print("Reading >", obj.posSetpoint,
# obj.posFeedback, obj.torqueSetpoint)
self._positionFeedback = obj.posFeedback
self._feedbackAngle = (obj.posFeedback / 10000) * 360
self._feedbackSteer = (self._feedbackAngle / 360)
