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 game_loop(args):
pygame.init()
pygame.font.init()
world = None
try:
client = carla.Client(args.host, args.port)
client.set_timeout(4.0)
client.load_world("Town01")
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)
#if args.agent == "Roaming":
# agent = RoamingAgent(world.player)
# else:
agent = BasicAgent(world.player, 50)
spawn_point = world.map.get_spawn_points()[1]
agent.\
set_destination((spawn_point.location.x,
spawn_point.location.y,
spawn_point.location.z))
clock = pygame.time.Clock()
# Enable synchronous mode
sim_settings = client.get_world().get_settings()
sim_settings.fixed_delta_seconds = 0.1
sim_settings.synchronous_mode = True
client.get_world().apply_settings(sim_settings)
while True:
if controller.parse_events():
return
# as soon as the server is ready continue!
world.world.tick()
world.tick(clock)
world.render(display)
pygame.display.flip()
control = agent.run_step()
control.manual_gear_shift = False
world.player.apply_control(control)
finally:
if world is not None:
world.destroy()
pygame.quit()
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(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 Carla_Interface():
def __init__(self, args, town='Town01'):
self.args = args
self.verbose = args.verbose
self.client = carla.Client('127.0.0.1', 2000)
self.client.set_timeout(10.0)
# Load Desired Map
if (self.client.get_world().get_map().name != town):
self.client.load_world(town)
time.sleep(10.0)
self.delta_secs = 1.0/self.args.fps
# Get world and map from carla
self.world = self.client.get_world()
self.world_map = self.world.get_map()
self.debug = self.world.debug
settings = self.world.get_settings()
settings.fixed_delta_seconds = self.delta_secs
settings.synchronous_mode = True
self.world.apply_settings(settings)
# Find valid points for spawning the vehicle
self.spawn_points = self.world_map.get_spawn_points()
self.sensors = {}
self.vehicles_list = []
self.plan_pid = []
self.plan_ilqr = []
self.navigation_agent = None
self.spawn_ego_vehicle()
if self.args.add_npc_agents:
self.spawn_npc()
self.create_global_plan()
self.setup_rendering()
def setup_rendering(self):
pygame.init()
w = self.args.camera_width
h = self.args.camera_height
self.display = pygame.display.set_mode((2*w, h), pygame.HWSURFACE | pygame.DOUBLEBUF)
self.camera_1 = pygame.Rect(0,0,w,h)
self.camera_2 = pygame.Rect(w,0,2*w,h)
self.surface = {}
# Setup Camera 2
cam_transform = carla.Transform(carla.Location(x=1, y=0, z=30.0), carla.Rotation(pitch=-90, roll=0 ,yaw=0))
self.add_sensor('rgb_top', w, h, cam_transform, self.image_callback)
# Setup Camera 1
cam_transform = carla.Transform(carla.Location(x=1, y=0, z=2.0), carla.Rotation(pitch=0, roll=0 ,yaw=0))
self.add_sensor('rgb_front', w, h, cam_transform, self.image_callback)
def spawn_ego_vehicle(self):
# Get a random vehicle from world
blueprint = random.choice(self.world.get_blueprint_library().filter('grandtourer'))
blueprint.set_attribute('role_name', 'hero')
# Set spawn point(start) and goal point according to use case
self.spawn_point = random.choice(self.spawn_points)
# Temp to spawn vehicle at same point
self.spawn_point.location = carla.Location(x=383.179871, y=-2.199161, z=1.322136)
self.spawn_point.rotation = carla.Rotation(roll=0.0, pitch=0.0, yaw=180)
print("\nSpawned vehicle at position: {}".format(self.spawn_point.location))
self.ego_vehicle = self.world.try_spawn_actor(blueprint, self.spawn_point)
physics = self.ego_vehicle.get_physics_control()
physics.gear_switch_time = 0.01
self.ego_vehicle.apply_physics_control(physics)
self.world.tick()
def add_sensor(self, sensor_tag, image_width, image_height, transform, callback):
bp = self.world.get_blueprint_library().find('sensor.camera.rgb')
bp.set_attribute('image_size_x', str(image_width))
bp.set_attribute('image_size_y', str(image_height))
bp.set_attribute('fov', str(100))
bp.set_attribute('sensor_tick', str(self.delta_secs))
self.sensors[sensor_tag] = self.world.spawn_actor(bp, transform, self.ego_vehicle)
self.sensors[sensor_tag].listen(lambda image: callback(sensor_tag, image))
# self.sensors[sensor_tag].listen(lambda image: image.save_to_disk('output/%06d.png' % image.frame))
def image_callback(self, sensor_tag, image):
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]
self.surface[sensor_tag] = pygame.surfarray.make_surface(array.swapaxes(0, 1))
if(sensor_tag == 'rgb_front'):
self.display.blit(self.surface[sensor_tag], self.camera_1)
else:
self.display.blit(self.surface[sensor_tag], self.camera_2)
def create_global_plan(self, end_point=None):
if end_point == None:
end_point = random.choice(self.spawn_points)
print("\nGoal position: {}".format(end_point.location))
end_point.location = carla.Location(x=383.179871, y=100.199161, z=1.322136)
# Give set of intermediate points spaced far away from start to goal
rough_route = [self.spawn_point.location, end_point.location]
# Interpolate the route to waypoints which are spaced 1m apart
_, route = interpolate_trajectory(self.world, rough_route)
# Make a Plan list which stores points as desired by BasicAgent Class
for transform, road_option in route:
wp = self.world_map.get_waypoint(transform.location)
self.plan_pid.append((wp, road_option))
self.plan_ilqr.append(np.array([wp.transform.location.x, wp.transform.location.y]))
def get_ego_states(self):
vehicle_transform = self.ego_vehicle.get_transform()
vehicle_velocity = self.ego_vehicle.get_velocity()
vehicle_angular_velocity = self.ego_vehicle.get_angular_velocity()
vehicle_acceleration = self.ego_vehicle.get_acceleration()
rotated_velocity = transforms.carla_velocity_to_numpy_local_velocity(vehicle_velocity, vehicle_transform.rotation)
roll, pitch, yaw = transforms.carla_rotation_to_RPY(vehicle_transform.rotation)
angular_velocity = transforms.carla_angular_velocity_to_numpy_vector(vehicle_angular_velocity)
acceleration = transforms.carla_acceleration_to_numpy_local_velocity(vehicle_acceleration, vehicle_transform.rotation)
ego_states = np.array([[vehicle_transform.location.x, vehicle_transform.location.y, vehicle_transform.location.z],
[ rotated_velocity[0], rotated_velocity[1], rotated_velocity[2]],
[ roll, pitch, yaw],
[ angular_velocity[0], angular_velocity[1], angular_velocity[2]],
[ acceleration[0], acceleration[1], acceleration[2]]])
return ego_states
def create_pid_agent(self, target_speed=20):
# Carla Funtion for Creating a Basic Navigation Agent
self.navigation_agent = BasicAgent(self.ego_vehicle, target_speed)
self.navigation_agent._local_planner.set_global_plan(self.plan_pid)
from ilqr.local_planner import LocalPlanner
self.local_planner = LocalPlanner(args)
self.local_planner.set_global_planner(self.plan_ilqr)
def run_step_pid(self):
# Loop for controls
assert self.navigation_agent != None, "Navigation Agent not initialized"
while True:
control = self.navigation_agent.run_step(debug=self.verbose)
self.ego_vehicle.apply_control(control)
self.local_planner.set_ego_state(self.get_ego_states())
ref_traj, poly_coeff = self.local_planner.get_local_plan()
drawer_utils.draw_path(self.debug, ref_traj, drawer_utils.red, lt=0.05, thickness=0.08)
# render scene
pygame.display.flip()
self.world.tick()
if self.verbose:
print(self.ego_vehicle.get_location())
def create_ilqr_agent(self):
self.navigation_agent = iLQR(self.args, self.get_npc_bounding_box())
self.navigation_agent.set_global_plan(self.plan_ilqr)
self.low_level_controller = LowLevelController(self.ego_vehicle.get_physics_control(), verbose=False, plot=True)
def run_step_ilqr(self):
assert self.navigation_agent != None, "Navigation Agent not initialized"
while True:
counter = 0
desired_path, local_plan, control = self.navigation_agent.run_step(self.get_ego_states(), self.get_npc_state())
while counter < self.args.mpc_horizon:
drawer_utils.draw_path(self.debug, local_plan, drawer_utils.red, lt=0.05, thickness=0.08)
drawer_utils.draw_path(self.debug, desired_path, drawer_utils.green, lt=0.05, thickness=0.05)
print("High Level Controller: Acc {} Steer {}".format(control[0, counter], control[1, counter]))
low_control = self.low_level_controller.get_control(self.get_ego_states(), control[0, counter], control[1, counter])
self.ego_vehicle.apply_control(low_control)
self.world.tick()
# render scene
pygame.display.flip()
counter += 1
if not self.args.use_mpc:
break
def spawn_npc(self):
blueprints = self.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')]
SpawnActor = carla.command.SpawnActor
SetAutopilot = carla.command.SetAutopilot
FutureActor = carla.command.FutureActor
batch = []
for n, transform in enumerate(self.spawn_points):
if n >= self.args.number_of_npc:
break
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)))
id_list = []
for response in self.client.apply_batch_sync(batch):
if response.error:
logging.error(response.error)
else:
id_list.append(response.actor_id)
self.vehicles_list = self.world.get_actors(id_list)
def get_npc_state(self):
vehicle_states = []
for n in range(len(self.vehicles_list)):
vehicle_transform = self.vehicles_list[n].get_transform()
vehicle_velocity = self.vehicles_list[n].get_velocity()
vehicle_angular_velocity = self.vehicles_list[n].get_angular_velocity()
vehicle_states.append(np.array([vehicle_transform.location.x,
vehicle_transform.location.y,
vehicle_velocity.x,,
vehicle_transform.rotation.yaw]))
return vehicle_states
def get_npc_bounding_box(self):
"""
Gives the x and y dimension of the bounding box
"""
bbs = []
for n in range(len(self.vehicles_list)):
bbs.append(np.array([2*self.vehicles_list[n].bounding_box.extent.x,
2*self.vehicles_list[n].bounding_box.extent.y]))
return bbs
def destroy(self):
settings = self.world.get_settings()
settings.synchronous_mode = False
self.world.apply_settings(settings)
print('\ndestorying ego vehicle')
self.ego_vehicle.destroy()
print('\ndestroying %d vehicles' % len(self.vehicles_list))
self.client.apply_batch([carla.command.DestroyActor(x.id) for x in self.vehicles_list])
print('\ndestroying %d sensors' % len(self.sensors))
for sensor in self.sensors.values():
sensor.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()
class NPCAgent(object):
def __init__(self, agent_params):
self._agent_params = agent_params
self._route_assigned = False
self._agent = None
def _setup(self, exp):
"""
Setup the agent for the current ongoing experiment. Basically if it is
the first time it will setup the agent.
:param exp:
:return:
"""
if not self._agent:
self._agent = BasicAgent(exp._ego_actor)
if not self._route_assigned:
plan = []
for transform, road_option in exp._route:
wp = exp._ego_actor.get_world().get_map().get_waypoint(
transform.location)
plan.append((wp, road_option))
self._agent._local_planner.set_global_plan(plan)
self._route_assigned = True
def get_sensors_dict(self):
"""
The agent sets the sensors that it is going to use. That has to be
set into the environment for it to produce this data.
"""
sensors_dict = [{
'type': 'sensor.camera.rgb',
'x': 2.0,
'y': 0.0,
'z': 1.40,
'roll': 0.0,
'pitch': -15.0,
'yaw': 0.0,
'width': 800,
'height': 600,
'fov': 100,
'id': 'rgb_central'
}]
return sensors_dict
def get_state(self, exp_list):
"""
The state function that need to be defined to be used by cexp to return
the state at every iteration.
:param exp_list: the list of experiments to be processed on this batch.
:return:
"""
# The first time this function is call we initialize the agent.
self._setup(exp_list[0])
exp = exp_list[0]
return exp.get_sensor_data()
def step(self, state):
"""
The step function, receives the output from the state function ( get_sensors)
:param state:
:return:
"""
# The sensors however are not needed since this basically run an step for the
# NPC default agent at CARLA:
control = self._agent.run_step()
logging.debug("Output %f %f %f " %
(control.steer, control.throttle, control.brake))
return control
def reset(self):
self._route_assigned = False
self._agent = None
class NpcAgent(AutonomousAgent):
"""
NPC autonomous agent to control the ego vehicle
"""
_agent = None
_route_assigned = False
def setup(self, path_to_conf_file):
"""
Setup the agent parameters
"""
self._agent = None
def sensors(self):
"""
Define the sensor suite required by the agent
:return: a list containing the required sensors in the following format:
[
{'type': 'sensor.camera.rgb', 'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0,
'width': 300, 'height': 200, 'fov': 100, 'id': 'Left'},
{'type': 'sensor.camera.rgb', 'x': 0.7, 'y': 0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0,
'width': 300, 'height': 200, 'fov': 100, 'id': 'Right'},
{'type': 'sensor.lidar.ray_cast', 'x': 0.7, 'y': 0.0, 'z': 1.60, 'yaw': 0.0, 'pitch': 0.0, 'roll': 0.0,
'id': 'LIDAR'}
]
"""
sensors = [
{
'type': 'sensor.camera.rgb',
'x': 0.7,
'y': -0.4,
'z': 1.60,
'roll': 0.0,
'pitch': 0.0,
'yaw': 0.0,
'width': 300,
'height': 200,
'fov': 100,
'id': 'Left'
},
]
return sensors
def run_step(self, input_data, timestamp):
"""
Execute one step of navigation.
"""
if not self._agent:
# Search for the ego actor
hero_actor = None
for actor in CarlaDataProvider.get_world().get_actors():
if 'role_name' in actor.attributes and actor.attributes[
'role_name'] == 'hero':
hero_actor = actor
break
if not hero_actor:
return carla.VehicleControl()
# Add an agent that follows the route to the ego
self._agent = BasicAgent(hero_actor, 30)
plan = []
prev_wp = None
for transform, _ in self._global_plan_world_coord:
wp = CarlaDataProvider.get_map().get_waypoint(
transform.location)
if prev_wp:
plan.extend(self._agent.trace_route(prev_wp, wp))
prev_wp = wp
self._agent.set_global_plan(plan)
return carla.VehicleControl()
else:
return self._agent.run_step()
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 ConvoyStrategy(EgoStrategy):
def __init__(self,
id: int,
waypoint_provider: WaypointProvider = None,
config: int = 0,
wait_for_egos: int = 0,
**kwargs):
super().__init__()
self.id: int = id
self.config: int = config
self.ready: bool = False
self.point_start: carla.Transform = None
self.point_end: carla.Transform = None
self.agent: BasicAgent = None
self.wait_for: int = wait_for_egos
self.wpp: WaypointProvider = waypoint_provider
self.kwargs = kwargs
self._player: carla.Vehicle = None
self._step_count: int = 0
self._start_time: float = 0
def init(self, ego):
super().init(ego)
if not self.wpp:
self._init_missing_waypoint_provider(ego)
n_egos_present: int = simulation.count_present_vehicles(
SCENE2_EGO_PREFIX, self.ego.world)
self.point_start = self.wpp.get_by_index(n_egos_present)
self.point_end = self.wpp.get_by_index(-1)
self._player = self._create_player() # Create player
self.agent = BasicAgent(self.player,
target_speed=EGO_TARGET_SPEED,
ignore_traffic_lights=True)
self.agent.set_location_destination(self.point_end.location)
def step(self, clock=None) -> bool:
if self.ego is None or not self.ready and simulation.count_present_vehicles(
SCENE2_EGO_PREFIX, self.ego.world) < self.wait_for:
return False
if self._start_time == 0:
self._start_time = time.monotonic()
self.ready = True
# Wait some time before starting
if time.monotonic() - self._start_time < 3.0:
return False
control: carla.VehicleControl = self.agent.run_step(debug=False)
self._step_count += 1
if self._step_count % 10 == 0 and self.agent.done():
logging.info(f'{self.ego.name} has reached its destination.')
return True
return self.player.apply_control(control)
@property
def player(self) -> carla.Vehicle:
return self._player
def _create_player(self) -> carla.Vehicle:
blueprint = random.choice(
self.ego.world.get_blueprint_library().filter(
'vehicle.tesla.model3'))
blueprint.set_attribute('role_name', self.ego.name)
if blueprint.has_attribute('color'):
colors = blueprint.get_attribute('color').recommended_values
blueprint.set_attribute('color',
colors[self.id % (len(colors) - 1)])
return self.ego.world.spawn_actor(blueprint, self.point_start)
def _init_missing_waypoint_provider(self, ego: 'ego.Ego'):
seed: int = self.kwargs['seed'] if 'seed' in self.kwargs else 0
logging.info(f'Using {seed} as a random seed.')
self.wpp = WaypointProvider(ego.world, seed=seed)
if self.config == 0:
self.wpp.set_waypoints([
carla.Transform(location=carla.Location(296, 55, .1),
rotation=carla.Rotation(0, 180, 0)),
carla.Transform(location=carla.Location(302, 55, .1),
rotation=carla.Rotation(0, 180, 0)),
carla.Transform(location=carla.Location(308, 55, .1),
rotation=carla.Rotation(0, 180, 0)),
carla.Transform(location=carla.Location(314, 55, .1),
rotation=carla.Rotation(0, 180, 0)),
carla.Transform(location=carla.Location(320, 55, .1),
rotation=carla.Rotation(0, 180, 0)),
carla.Transform(location=carla.Location(326, 55, .1),
rotation=carla.Rotation(0, 180, 0)),
carla.Transform(location=carla.Location(92, 36, .1),
rotation=carla.Rotation(0, -90, 0))
])
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
surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
camera_holder.surface = surface
camera_sensor.listen(lambda image: parse_image(image))
# Project camera image on pygame
display = pygame.display.set_mode(
(1920/2, 1080/2),
pygame.HWSURFACE | pygame.DOUBLEBUF)
# Game loop
clock = pygame.time.Clock()
waypoint_index = 0
while True:
control = agent.run_step()
vehicle.apply_control(control)
for event in pygame.event.get():
if event.type == pygame.QUIT:
sys.exit()
clock.tick_busy_loop(60)
while camera_holder.surface is None:
# print 'waiting for surface'
pass
display.blit(camera_holder.surface, (0, 0))
xv, yv = vehicle.get_location().x, vehicle.get_location().y
xv, yv = np.round([xv, yv], 2)
# HUD
current_location_surface = myfont.render("Now: X "+str(xv)+" Y "+str(yv), True, (0, 0, 0))
next_location = route[waypoint_index]
next_location = carla.Location(
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!")
class NpcAgent(AutonomousAgent):
"""
NPC autonomous agent to control the ego vehicle
"""
_agent = None
_route_assigned = False
def setup(self, path_to_conf_file):
"""
Setup the agent parameters
"""
self._route_assigned = False
self._agent = None
def sensors(self):
"""
Define the sensor suite required by the agent
:return: a list containing the required sensors in the following format:
[
{'type': 'sensor.camera.rgb', 'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0,
'width': 300, 'height': 200, 'fov': 100, 'id': 'Left'},
{'type': 'sensor.camera.rgb', 'x': 0.7, 'y': 0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0,
'width': 300, 'height': 200, 'fov': 100, 'id': 'Right'},
{'type': 'sensor.lidar.ray_cast', 'x': 0.7, 'y': 0.0, 'z': 1.60, 'yaw': 0.0, 'pitch': 0.0, 'roll': 0.0,
'id': 'LIDAR'}
"""
sensors = [
{
'type': 'sensor.camera.rgb',
'x': 0.7,
'y': -0.4,
'z': 1.60,
'roll': 0.0,
'pitch': 0.0,
'yaw': 0.0,
'width': 300,
'height': 200,
'fov': 100,
'id': 'Left'
},
]
return sensors
def run_step(self, input_data, timestamp):
"""
Execute one step of navigation.
"""
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 0.0
control.hand_brake = False
if not self._agent:
hero_actor = None
for actor in CarlaDataProvider.get_world().get_actors():
if 'role_name' in actor.attributes and actor.attributes[
'role_name'] == 'hero':
hero_actor = actor
break
if hero_actor:
self._agent = BasicAgent(hero_actor)
return control
if not self._route_assigned:
if self._global_plan:
plan = []
for transform, road_option in self._global_plan_world_coord:
wp = CarlaDataProvider.get_map().get_waypoint(
transform.location)
plan.append((wp, road_option))
self._agent._local_planner.set_global_plan(plan) # pylint: disable=protected-access
self._route_assigned = True
else:
control = self._agent.run_step()
return control
class NPCAgent(object):
def __init__(self):
self._route_assigned = False
self._agent = None
def _setup(self, exp):
"""
Setup the agent for the current ongoing experiment. Basically if it is
the first time it will setup the agent.
:param exp:
:return:
"""
if not self._agent:
self._agent = BasicAgent(exp._ego_actor)
if not self._route_assigned:
plan = []
for transform, road_option in exp._route:
wp = exp._ego_actor.get_world().get_map().get_waypoint(
transform.location)
plan.append((wp, road_option))
self._agent._local_planner.set_global_plan(plan)
self._route_assigned = True
def get_sensors_dict(self):
"""
The agent sets the sensors that it is going to use. That has to be
set into the environment for it to produce this data.
"""
sensors_dict = [
{
'type': 'sensor.other.gnss',
'x': 0.7,
'y': -0.4,
'z': 1.60,
'id': 'GPS'
},
]
return sensors_dict
def get_state(self, exp_list):
"""
The state function that need to be defined to be used by cexp to return
the state at every iteration.
:param exp_list: the list of experiments to be processed on this batch.
:return:
"""
# The first time this function is call we initialize the agent.
self._setup(exp_list[0])
return exp_list[0].get_sensor_data()
def step(self, state):
"""
The step function, receives the output from the state function ( get_sensors)
:param state:
:return:
"""
# We print downs the sensors that are being received.
# The agent received the following sensor data.
print("=====================>")
for key, val in state.items():
if hasattr(val[1], 'shape'):
shape = val[1].shape
print("[{} -- {:06d}] with shape {}".format(
key, val[0], shape))
else:
print("[{} -- {:06d}] ".format(key, val[0]))
print("<=====================")
# The sensors however are not needed since this basically run an step for the
# NPC default agent at CARLA:
control = self._agent.run_step()
logging.debug("Output %f %f %f " %
(control.steer, control.throttle, control.brake))
return control
def reset(self):
print(" Correctly reseted the agent")
self._route_assigned = False
self._agent = None
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()
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)
