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 __init__(self, world, vehicle, global_dict, args, trajectory_model):
self.world = world
self.vehicle = vehicle
self.agent = BasicAgent(self.vehicle, target_speed=30)
self.global_dict = global_dict
self.args = args
self.trajectory_model = trajectory_model
self.ctrller = CapacController(self.world, self.vehicle, 30)
# robot action space
self.low_action = np.array([-1, -1])
self.high_action = np.array([1, 1])
self.action_space = spaces.Box(low=self.low_action,
high=self.high_action,
dtype=np.float32)
# robot observation space
self.observation_space = spaces.Box(low=-np.inf,
high=np.inf,
shape=(7, ),
dtype=np.float32)
world_map = self.world.get_map()
waypoint_tuple_list = world_map.get_topology()
self.origin_map = get_map(waypoint_tuple_list)
self.spawn_points = world_map.get_spawn_points()
self.route_trace = None
# environment feedback infomation
self.state = []
self.done = False
self.reward = 0.0
self.seed()
self.reset()
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 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 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 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 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 = []
prev = None
for transform, _ in self._global_plan_world_coord:
wp = CarlaDataProvider.get_map().get_waypoint(
transform.location)
if prev:
route_segment = self._agent._trace_route(prev, wp)
plan.extend(route_segment)
prev = wp
#loc = plan[-1][0].transform.location
#self._agent.set_destination([loc.x, loc.y, loc.z])
self._agent._local_planner.set_global_plan(plan) # pylint: disable=protected-access
self._route_assigned = True
else:
control = self._agent.run_step()
return control
def game_loop(args):
pygame.init()
pygame.font.init()
world = None
try:
client = carla.Client(args.host, args.port)
client.set_timeout(4.0)
#world=client.load_world('Town02')
#print(client.get_available_maps())
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)
rgb_manager = CameraManager(world.player, hud)
frame_manager = CameraManager(world.player, hud)
if args.agent == "Roaming":
agent = RoamingAgent(world.player)
else:
agent = BasicAgent(world.player)
spawn_point = world.map.get_spawn_points()[0]
agent.set_destination(
(spawn_point.location.x, spawn_point.location.y,
spawn_point.location.z))
rgb_manager.set_sensor(0, True)
frame_manager.set_sensor_frame(0, True)
rgb_manager.toggle_camera()
frame_manager.toggle_camera_frame()
rgb_manager.toggle_recording()
frame_manager.toggle_recording_frame()
clock = pygame.time.Clock()
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)
finally:
world.destroy_sensors()
if world is not None:
world.destroy()
pygame.quit()
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 _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 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:
print("[Timestamp: {}]".format(timestamp))
control = self._agent.run_step()
return control
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)
if args.agent == "Roaming":
agent = RoamingAgent(world.player)
else:
agent = BasicAgent(world.player)
list = world.map.get_spawn_points()
if list == []:
spawn_point = carla.Transform()
else:
spawn_point = list[0]
agent.set_destination((spawn_point.location.x,
spawn_point.location.y,
spawn_point.location.z))
clock = pygame.time.Clock()
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)
finally:
if world is not None:
world.destroy()
pygame.quit()
def spawn_npcs(carla_client: carla.Client,
wpp: WaypointProvider = None,
n=10,
role_name_prefix='npc') -> List[BasicAgent]:
world: carla.World = carla_client.get_world()
vehicle_blueprints = world.get_blueprint_library().filter('vehicle.*')
if not wpp:
wpp = WaypointProvider(world)
wpp.update(free_only=True)
start_points: List[carla.Transform] = []
end_points: List[carla.Transform] = []
agent_ids: List[int] = []
agents: List[BasicAgent] = []
batch: List[carla.command.SpawnActor] = []
for i in range(n):
blueprint = random.choice(vehicle_blueprints)
blueprint.set_attribute('role_name', f'{role_name_prefix}_{i}')
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
start_points.append(wpp.get())
end_points.append(wpp.get())
batch.append(carla.command.SpawnActor(blueprint, start_points[-1]))
results = carla_client.apply_batch_sync(batch, True)
for i in range(len(results)):
if results[i].error:
logging.error(results[i].error)
else:
agent_ids.append(results[i].actor_id)
world.wait_for_tick()
agent_actors: carla.ActorList = world.get_actors(agent_ids)
for i, actor in enumerate(agent_actors):
agent: BasicAgent = BasicAgent(actor, target_speed=NPC_TARGET_SPEED)
try:
agent.set_location_destination(end_points[i].location)
agents.append(agent)
except AttributeError:
logging.error(f'Failed to spawn NPC actor with id {actor.id}.')
return agents
def main():
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town02')
weather = carla.WeatherParameters(
cloudiness=30.0,
precipitation=30.0,
sun_altitude_angle=50.0
)
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.bmw.grandtourer')
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
origin_map.resize((2000,2000)).show()
agent = BasicAgent(vehicle, target_speed=MAX_SPEED)
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, origin_map)
"""
while True:
if close2dest(vehicle, destination):
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, origin_map)
if vehicle.is_at_traffic_light():
traffic_light = vehicle.get_traffic_light()
if traffic_light.get_state() == carla.TrafficLightState.Red:
traffic_light.set_state(carla.TrafficLightState.Green)
control = agent.run_step()
control.manual_gear_shift = False
vehicle.apply_control(control)
nav = get_nav(vehicle, plan_map)
cv2.imshow('Nav', nav)
cv2.waitKey(16)
"""
cv2.destroyAllWindows()
vehicle.destroy()
def main():
global global_nav, global_vel, start_control, global_plan_map, global_vehicle, global_transform, max_steer_angle, global_a, state0, global_collision
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.yamaha.yzf')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.*.*')
global_vehicle = vehicle
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
max_steer_angle = np.deg2rad(physics_control.wheels[0].max_steer_angle)
spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
agent = BasicAgent(vehicle, target_speed=MAX_SPEED)
# prepare map
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
global_plan_map, destination = replan(agent, destination,
copy.deepcopy(origin_map),
spawn_points)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'camera:view': {
#'transform':carla.Transform(carla.Location(x=0.0, y=4.0, z=4.0), carla.Rotation(pitch=-30, yaw=-60)),
'transform':
carla.Transform(carla.Location(x=-3.0, y=0.0, z=6.0),
carla.Rotation(pitch=-45)),
#'transform':carla.Transform(carla.Location(x=0.0, y=0.0, z=6.0), carla.Rotation(pitch=-90)),
'callback':
view_image_callback,
},
'collision': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': collision_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
time.sleep(0.5)
# start to plan
plan_thread = threading.Thread(target=make_plan, args=())
# visualization_thread = threading.Thread(target = show_traj, args=())
while True:
if (global_img is not None) and (global_nav is not None):
plan_thread.start()
break
else:
time.sleep(0.001)
# wait for the first plan result
while not start_control:
time.sleep(0.001)
# visualization_thread.start()
# start to control
print('Start to control')
ctrller = CapacController(world, vehicle, 30)
while True:
# change destination
if close2dest(vehicle, destination):
#destination = get_random_destination(spawn_points)
print('get destination !', time.time())
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
# global_plan_map = replan(agent, destination, copy.deepcopy(origin_map))
global_plan_map, destination = replan(agent, destination,
copy.deepcopy(origin_map),
spawn_points)
if global_collision:
global_collision = False
start_point = random.choice(spawn_points)
vehicle.set_transform(start_point)
global_plan_map, destination = replan(agent, destination,
copy.deepcopy(origin_map),
spawn_points)
start_waypoint = agent._map.get_waypoint(
agent._vehicle.get_location())
end_waypoint = agent._map.get_waypoint(destination.location)
route_trace = agent._trace_route(start_waypoint, end_waypoint)
start_point.rotation = route_trace[0][0].transform.rotation
vehicle.set_transform(start_point)
v = global_vehicle.get_velocity()
a = global_vehicle.get_acceleration()
global_vel = np.sqrt(v.x**2 + v.y**2 + v.z**2)
global_a = np.sqrt(a.x**2 + a.y**2 + a.z**2)
#steer_angle = global_vehicle.get_control().steer*max_steer_angle
#w = global_vel*np.tan(steer_angle)/2.405
control_time = time.time()
dt = control_time - global_trajectory['time']
index = int((dt / args.max_t) // args.dt) + 2
if index > 0.99 / args.dt:
continue
"""
transform = vehicle.get_transform()
dx, dy, dyaw = get_transform(transform, global_transform)
dyaw = -dyaw
_x = global_trajectory['x'][index] - dx
_y = global_trajectory['y'][index] - dy
x = _x*np.cos(dyaw) + _y*np.sin(dyaw)
y = _y*np.cos(dyaw) - _x*np.sin(dyaw)
_vx = global_trajectory['vx'][index]
_vy = global_trajectory['vy'][index]
vx = _vx*np.cos(dyaw) + _vy*np.sin(dyaw)
vy = _vy*np.cos(dyaw) - _vx*np.sin(dyaw)
_ax = global_trajectory['ax'][index]
_ay = global_trajectory['ay'][index]
ax = _ax*np.cos(dyaw) + _ay*np.sin(dyaw)
ay = _ay*np.cos(dyaw) - _ax*np.sin(dyaw)
"""
#control = get_control(x, y, vx, vy, ax, ay)
control = ctrller.run_step(global_trajectory, index, state0)
vehicle.apply_control(control)
# x = global_trajectory['x']
# y = global_trajectory['y']
# plt.cla()
# plt.plot(x, y)
# plt.xlim(-1, 29)
# plt.ylim(-15, 15)
# plt.show()
"""
dyaw = np.deg2rad(global_transform.rotation.yaw)
x = global_trajectory['x'][index]*np.cos(dyaw) + global_trajectory['y'][index]*np.sin(dyaw)
y = global_trajectory['y'][index]*np.cos(dyaw) - global_trajectory['x'][index]*np.sin(dyaw)
"""
#localtion = carla.Location(x = global_transform.location.x+x, y=global_transform.location.y+y, z=2.0)
#world.debug.draw_point(localtion, size=0.3, color=carla.Color(255,0,0), life_time=10.0)
#print(global_vel*np.tan(control.steer)/w)
curve = None #show_traj(True)
visualize(global_view_img, global_nav, curve)
#time.sleep(1/60.)
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()
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 -= (
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
destination_transform = spawn_points[
options_dict['spawn_point_indices'][1]]
if options_dict['agent'] == 'latent':
transform = transforms.Compose(
[transforms.Resize((75, 100)),
transforms.ToTensor()])
device = torch.device(
'cuda' if torch.cuda.is_available() else 'cpu')
modelVAE = siameseCVAE(batch=1)
checkpointVAE = torch.load('./siamese_predict41616_75100.pt')
modelVAE.load_state_dict(checkpointVAE)
modelVAE.to(device)
modelVAE.eval()
modelVel = velocityNN()
checkpointVel = torch.load('./velocity_single.pt')
modelVel.load_state_dict(checkpointVel)
modelVel.to(device)
modelVel.eval()
models = {'VAE': modelVAE, 'Vel': modelVel}
agent = LatentAgent(vehicle, models=models,
device=device) # , transform=transform
agent.set_destination(options_dict['goal_image'],
transform=transform)
else:
print('Going to ', destination_transform)
agent = BasicAgent(vehicle.vehicle)
agent.set_destination((destination_transform.location.x,
destination_transform.location.y,
destination_transform.location.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=False)
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=False)
cam2 = Camera(camera_bp[0],
camera_transform[1],
vehicle,
options_dict['trajector_num'],
save_data=False)
prev_location = vehicle.vehicle.get_location()
sp = 2
print('Starting simulation.')
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()
vehicle.vehicle.apply_control(control)
# check if destination reached
current_location = vehicle.vehicle.get_location()
# kind of hacky way to test destination reached and doesn't always work - may have to manually stop with ctrl c
if current_location.distance(
prev_location) <= 0.0 and current_location.distance(
destination_transform.location) <= 10:
print(
'distance from destination: ',
current_location.distance(destination_transform.location))
# if out of destinations break else go to next destination
if len(options_dict['spawn_point_indices']) <= sp:
break
else:
destination_transform.location = spawn_points[
options_dict['spawn_point_indices'][sp]].location
print('Going to ', destination_transform.location)
# agent.set_destination((destination_transform.location.x, destination_transform.location.y, destination_transform.location.z))
agent.set_destination(vehicle_transform,
destination_transform)
sp += 1
prev_location = current_location
finally:
if world is not None:
world.destroy()
def game_loop(args):
""" Main loop for agent"""
pygame.init()
pygame.font.init()
world = None
tot_target_reached = 0
num_min_waypoints = 21
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)
controller = KeyboardControl(world)
if args.agent == "Roaming":
agent = RoamingAgent(world.player)
elif args.agent == "Basic":
agent = BasicAgent(world.player)
spawn_point = world.map.get_spawn_points()[0]
agent.set_destination(
(spawn_point.location.x, spawn_point.location.y,
spawn_point.location.z))
else:
agent = BehaviorAgent(world.player, behavior=args.behavior)
spawn_points = world.map.get_spawn_points()
random.shuffle(spawn_points)
if spawn_points[0].location != agent.vehicle.get_location():
destination = spawn_points[0].location
else:
destination = spawn_points[1].location
agent.set_destination(agent.vehicle.get_location(),
destination,
clean=True)
clock = pygame.time.Clock()
while True:
clock.tick_busy_loop(60)
if controller.parse_events():
return
# As soon as the server is ready continue!
if not world.world.wait_for_tick(10.0):
continue
if args.agent == "Roaming" or args.agent == "Basic":
if controller.parse_events():
return
# as soon as the server is ready continue!
world.world.wait_for_tick(10.0)
world.tick(clock)
world.render(display)
pygame.display.flip()
control = agent.run_step()
control.manual_gear_shift = False
world.player.apply_control(control)
else:
agent.update_information()
world.tick(clock)
world.render(display)
pygame.display.flip()
# if world.save_count<5:
# world.camera_manager.image.save_to_disk('_out/%08d' % image.frame)
# world.save_count+=1
# Set new destination when target has been reached
if len(agent.get_local_planner().waypoints_queue
) < num_min_waypoints and args.loop:
agent.reroute(spawn_points)
tot_target_reached += 1
world.hud.notification("The target has been reached " +
str(tot_target_reached) + " times.",
seconds=4.0)
elif len(agent.get_local_planner().waypoints_queue
) == 0 and not args.loop:
print("Target reached, mission accomplished...")
break
speed_limit = world.player.get_speed_limit()
agent.get_local_planner().set_speed(speed_limit)
control = agent.run_step()
world.player.apply_control(control)
finally:
if world is not None:
world.destroy1()
pygame.quit()
def main():
global global_nav, global_vel
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(
cloudiness=random.randint(0,10),
precipitation=0,
sun_altitude_angle=random.randint(50,90)
)
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
sensor_dict = {
'camera':{
'transform':carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback':image_callback,
},
'lidar':{
'transform':carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback':lidar_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
time.sleep(0.3)
spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
agent = BasicAgent(vehicle, target_speed=MAX_SPEED)
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, copy.deepcopy(origin_map))
inverse_perspective_mapping = InversePerspectiveMapping(param, sensor_master)
time.sleep(0.3)
while True:
if close2dest(vehicle, destination):
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, copy.deepcopy(origin_map))
v = vehicle.get_velocity()
global_vel = np.sqrt(v.x**2+v.y**2+v.z**2)
global_nav = get_nav(vehicle, plan_map)
result = get_cGAN_result()
img = copy.deepcopy(global_img)
mask = np.where(result > 200)
img[mask[0],mask[1]] = (255, 0, 0, 255)
ipm_image = inverse_perspective_mapping.getIPM(result)
cost_map = get_cost_map(ipm_image, global_pcd)
#plan_cost_map = draw_traj(cost_map)
plan_cost_map, trajectory = get_traj(cost_map, 0.7, show=True)
control = get_control(trajectory)
vehicle.apply_control(control)
visualize(img, plan_cost_map, global_nav)
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()
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
def game_loop(args):
""" Main loop for agent"""
pygame.init()
pygame.font.init()
world = None
tot_target_reached = 0
num_min_waypoints = 21
rand_int = np.random.randint(100)
try:
client = carla.Client(args.host, args.port)
client.set_timeout(4.0)
client.load_world('Town0%d' % args.town)
client.reload_world()
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)
# map_dir = "/home/yash/Downloads/CARLA_0.9.9/PythonAPI/examples/Recordings_%d/" % args.town
map_dir = "D:\\CARLA\\WindowsNoEditor\\PythonAPI\\examples\\Recordings_%d\\" % args.town
map_obj = ParseCarlaMap(map_dir)
points = [46, 81, 132, 10, 40, 14, 110, 71, 58, 4]
edges = list(permutations(points, 2))
edge_table = np.zeros(shape=(len(edges), 3), dtype=np.float32)
edge_table[:, 0:2] = edges
for index in range(edge_table.shape[0]):
spawn_index, dest_index = edge_table[index, 0:2].astype(np.int)
spawn_loc = carla.Location(
x=map_obj.node_dict["location"]["x"][spawn_index],
y=map_obj.node_dict["location"]["y"][spawn_index],
z=map_obj.node_dict["location"]["z"][spawn_index])
spawn_rot = carla.Rotation(
roll=map_obj.node_dict["rotation"]["roll"][spawn_index],
pitch=map_obj.node_dict["rotation"]["pitch"][spawn_index],
yaw=map_obj.node_dict["rotation"]["yaw"][spawn_index])
spawn_pt = carla.Transform(location=spawn_loc, rotation=spawn_rot)
dest_loc = carla.Location(
x=map_obj.node_dict["location"]["x"][dest_index],
y=map_obj.node_dict["location"]["y"][dest_index],
z=map_obj.node_dict["location"]["z"][dest_index])
# dest_rot = carla.Rotation(roll=map_obj.node_dict["rotation"]["roll"][dest_index],
# pitch=map_obj.node_dict["rotation"]["pitch"][dest_index],
# yaw=map_obj.node_dict["rotation"]["yaw"][dest_index])
# dest_pt = carla.Transform(location=dest_loc, rotation=dest_rot)
world.restart(args, spawn_pt=spawn_pt)
if args.agent == "Roaming":
agent = RoamingAgent(world.player)
elif args.agent == "Basic":
agent = BasicAgent(world.player)
spawn_point = world.map.get_spawn_points()[0]
agent.set_destination(
(spawn_point.location.x, spawn_point.location.y,
spawn_point.location.z))
else:
agent = BehaviorAgent(
world.player,
ignore_traffic_light=args.ignore_traffic_light,
behavior=args.behavior)
agent.set_destination(agent.vehicle.get_location(),
dest_loc,
clean=True)
clock = pygame.time.Clock()
start_time = time.time()
while True:
clock.tick_busy_loop(60)
if controller.parse_events():
return
# As soon as the server is ready continue!
if not world.world.wait_for_tick(10.0):
continue
if args.agent == "Roaming" or args.agent == "Basic":
if controller.parse_events():
return
# as soon as the server is ready continue!
world.world.wait_for_tick(10.0)
world.tick(clock)
world.render(display)
pygame.display.flip()
control = agent.run_step()
control.manual_gear_shift = False
world.player.apply_control(control)
else:
agent.update_information(world)
world.tick(clock)
world.render(display)
pygame.display.flip()
# Set new destination when target has been reached
if len(agent.get_local_planner().waypoints_queue
) < num_min_waypoints and args.loop:
agent.reroute(spawn_points)
tot_target_reached += 1
world.hud.notification("The target has been reached " +
str(tot_target_reached) +
" times.",
seconds=4.0)
elif len(agent.get_local_planner().waypoints_queue
) == 0 and not args.loop:
print("Target reached, mission accomplished...")
break
speed_limit = world.player.get_speed_limit()
agent.get_local_planner().set_speed(speed_limit)
control = agent.run_step()
world.player.apply_control(control)
if time.time() - start_time >= 280:
break
edge_table[index, 2] = time.time() - start_time
print("edge %d was completed in %3.3f seconds" %
(index, edge_table[index, 2]))
np.savetxt('Carla_0%d_%d.csv' % (args.town, rand_int),
edge_table,
fmt="%3d,%3d,%3.3f")
finally:
if world is not None:
world.destroy()
pygame.quit()
hud = HUD(args.width, args.height)
_world = client.load_world('Town01')
world_map = _world.get_map()
world = World(_world, hud)
weather = carla.WeatherParameters(cloudiness=random.randint(0, 80),
precipitation=0,
sun_altitude_angle=random.randint(
40, 90))
set_weather(_world, weather)
controller = DualControl(world)
vehicle = world.player
agent = BasicAgent(vehicle, target_speed=31)
spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, copy.deepcopy(origin_map))
#plan_map.resize((1000, 1000)).show()
clock = pygame.time.Clock()
while True:
if close2dest(vehicle, destination):
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination,
copy.deepcopy(origin_map))
def main():
global global_nav, global_control, global_pos, global_vel
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
#world = client.get_world()
world = client.load_world('Town10')
weather = carla.WeatherParameters(cloudiness=random.randint(0, 10),
precipitation=0,
sun_altitude_angle=random.randint(
50, 90))
#world.set_weather(carla.WeatherParameters.ClearNoon)
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
sensor_dict = {
'camera': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': image_callback,
},
'camera:view': {
'transform':
carla.Transform(carla.Location(x=-3.0, y=0.0, z=3.5),
carla.Rotation(pitch=-30)),
'callback':
view_image_callback,
},
'lidar': {
'transform': carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback': lidar_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
time.sleep(0.3)
spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
agent = BasicAgent(vehicle, target_speed=MAX_SPEED)
#port = 8000
#tm = client.get_trafficmanager(port)
#vehicle.set_autopilot(True,port)
#tm.ignore_lights_percentage(vehicle,100)
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, copy.deepcopy(origin_map))
joystick = JoyStick(verbose=False)
joystick.start()
for cnt in tqdm(range(args.num)):
if close2dest(vehicle, destination):
destination = get_random_destination(spawn_points)
plan_map = replan(agent, destination, copy.deepcopy(origin_map))
"""
if vehicle.is_at_traffic_light():
traffic_light = vehicle.get_traffic_light()
if traffic_light.get_state() == carla.TrafficLightState.Red:
traffic_light.set_state(carla.TrafficLightState.Green)
"""
#control = agent.run_step()
control = joystick.get()
velocity = vehicle.get_velocity()
if np.sqrt(velocity.x**2 + velocity.y**2) > 8:
control.throttle = 0
#control.manual_gear_shift = False
global_control = control
vehicle.apply_control(control)
nav = get_nav(vehicle, plan_map)
global_nav = nav
global_pos = vehicle.get_transform()
global_vel = vehicle.get_velocity()
#cv2.imshow('Nav', nav)
cv2.imshow('Vision', global_view_img)
cv2.waitKey(10)
index = str(time.time())
#save_data(index)
cmd_file.close()
pos_file.close()
vel_file.close()
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()
def game_loop(args):
""" Main loop for agent"""
pygame.init()
pygame.font.init()
world = None
original_settings = None
tot_target_reached = 0
num_min_waypoints = 21
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)
# make synchronous and disable keyboard controller BEGIN
logging.info("make synchronous and disable keyboard controller")
original_settings = world.world.get_settings()
settings = world.world.get_settings()
traffic_manager = client.get_trafficmanager(8000)
traffic_manager.set_synchronous_mode(True)
delta = 0.06
settings.fixed_delta_seconds = delta
settings.synchronous_mode = True
world.world.apply_settings(settings)
# END
controller = KeyboardControl(world)
if args.agent == "Roaming":
agent = RoamingAgent(world.player)
elif args.agent == "Basic":
agent = BasicAgent(world.player)
spawn_point = world.map.get_spawn_points()[0]
agent.set_destination(
(spawn_point.location.x, spawn_point.location.y,
spawn_point.location.z))
else:
agent = BehaviorAgent(world.player, behavior=args.behavior)
spawn_points = world.map.get_spawn_points()
random.shuffle(spawn_points)
if spawn_points[0].location != agent.vehicle.get_location():
destination = spawn_points[0].location
else:
destination = spawn_points[1].location
agent.set_destination(agent.vehicle.get_location(),
destination,
clean=True)
clock = pygame.time.Clock()
while True:
clock.tick_busy_loop(60)
world.world.tick() # synchronous
hud.on_world_tick(world.world.get_snapshot().timestamp)
if controller.parse_events():
return
# As soon as the server is ready continue!
# if not world.world.wait_for_tick(10.0):
# continue
if args.agent == "Roaming" or args.agent == "Basic":
if controller.parse_events():
return
# as soon as the server is ready continue!
# world.world.wait_for_tick(10.0)
world.tick(clock)
world.render(display)
pygame.display.flip()
control = agent.run_step()
control.manual_gear_shift = False
world.player.apply_control(control)
else:
agent.update_information()
world.tick(clock)
world.render(display)
pygame.display.flip()
# Set new destination when target has been reached
if len(agent.get_local_planner().waypoints_queue
) < num_min_waypoints and args.loop:
agent.reroute(spawn_points)
tot_target_reached += 1
world.hud.notification("The target has been reached " +
str(tot_target_reached) + " times.",
seconds=4.0)
elif len(agent.get_local_planner().waypoints_queue
) == 0 and not args.loop:
print("Target reached, mission accomplished...")
break
speed_limit = world.player.get_speed_limit()
agent.get_local_planner().set_speed(speed_limit)
control = agent.run_step()
world.player.apply_control(control)
finally:
if world is not None:
if original_settings is not None:
world.world.apply_settings(original_settings)
world.destroy()
pygame.quit()
def main():
global global_nav, global_vel, start_control, global_plan_map, global_vehicle, global_cost_map, global_transform, max_steer_angle, global_a, draw_cost_map, state0
client = carla.Client(config['host'], config['port'])
client.set_timeout(config['timeout'])
world = client.load_world('Town01')
weather = carla.WeatherParameters(
cloudiness=random.randint(0,10),
precipitation=0,
sun_altitude_angle=random.randint(50,90)
)
set_weather(world, weather)
blueprint = world.get_blueprint_library()
world_map = world.get_map()
vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.audi.a2')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.yamaha.yzf')
#vehicle = add_vehicle(world, blueprint, vehicle_type='vehicle.*.*')
global_vehicle = vehicle
# Enables or disables the simulation of physics on this actor.
vehicle.set_simulate_physics(True)
physics_control = vehicle.get_physics_control()
max_steer_angle = np.deg2rad(physics_control.wheels[0].max_steer_angle)
sensor_dict = {
'camera':{
'transform':carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback':image_callback,
},
'camera:view':{
#'transform':carla.Transform(carla.Location(x=0.0, y=4.0, z=4.0), carla.Rotation(pitch=-30, yaw=-60)),
'transform':carla.Transform(carla.Location(x=-3.0, y=0.0, z=6.0), carla.Rotation(pitch=-45)),
#'transform':carla.Transform(carla.Location(x=0.0, y=0.0, z=6.0), carla.Rotation(pitch=-90)),
'callback':view_image_callback,
},
'lidar':{
'transform':carla.Transform(carla.Location(x=0.5, y=0.0, z=2.5)),
'callback':lidar_callback,
},
}
sm = SensorManager(world, blueprint, vehicle, sensor_dict)
sm.init_all()
#spawn_points = world_map.get_spawn_points()
waypoint_tuple_list = world_map.get_topology()
origin_map = get_map(waypoint_tuple_list)
agent = BasicAgent(vehicle, target_speed=MAX_SPEED)
# prepare map
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
#destination = get_random_destination(spawn_points)
global_plan_map = replan(agent, destination, copy.deepcopy(origin_map))
# start to plan
plan_thread = threading.Thread(target = make_plan, args=())
visualization_thread = threading.Thread(target = show_traj, args=())
while True:
if (global_img is not None) and (global_nav is not None) and (global_pcd is not None):
plan_thread.start()
break
else:
time.sleep(0.001)
# wait for the first plan result
while not start_control:
time.sleep(0.001)
#visualization_thread.start()
# start to control
print('Start to control')
ctrller = CapacController(world, vehicle, 30)
while True:
# change destination
if close2dest(vehicle, destination):
#destination = get_random_destination(spawn_points)
print('get destination !', time.time())
destination = carla.Transform()
destination.location = world.get_random_location_from_navigation()
global_plan_map = replan(agent, destination, copy.deepcopy(origin_map))
v = global_vehicle.get_velocity()
a = global_vehicle.get_acceleration()
global_vel = np.sqrt(v.x**2+v.y**2+v.z**2)
global_a = np.sqrt(a.x**2+a.y**2+a.z**2)
#steer_angle = global_vehicle.get_control().steer*max_steer_angle
#w = global_vel*np.tan(steer_angle)/2.405
control_time = time.time()
dt = control_time - global_trajectory['time']
index = int((dt/args.max_t)//args.dt)
if index > 0.99/args.dt:
continue
control = ctrller.run_step(global_trajectory, index, state0)
vehicle.apply_control(control)
curve = None#show_traj(True)
visualize(global_view_img, draw_cost_map, global_nav, curve)
#time.sleep(1/60.)
cv2.destroyAllWindows()
sm.close_all()
vehicle.destroy()
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()
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(options_dict):
world = None
if options_dict['agent'] == 'cuda':
options_dict['sync'] = True
try:
# load the client and change the world
client = carla.Client('localhost', 2000)
client.set_timeout(30.0)
print('Changing world to Town 5.')
client.load_world('Town05')
# create world object
world = World(client.get_world(), options_dict['sync'])
spawn_points = world.world.get_map().get_spawn_points()
# spawn vehicle
vehicle_bp = 'model3'
vehicle_transform = spawn_points[options_dict['spawn_point_indices'][0]]
# vehicle_transform.location.x -= 6
vehicle = Car(vehicle_bp, vehicle_transform, world)
# # add obstacles and get sample nodes
# world.block_road()
# world.swerve_obstacles()
# world.random_obstacles(options_dict['num_obstacles'])
# wait for vehicle to land on ground
world_snapshot = None
ticks = 0
while ticks < 30:
world.world.tick()
world_snapshot = world.world.wait_for_tick(10.0)
if not world_snapshot:
continue
else:
ticks += 1
# get and set destination
destination_transform = spawn_points[options_dict['spawn_point_indices'][1]]
# destination_transform = carla.Transform(carla.Location(vehicle_transform.location.x -50, vehicle_transform.location.y, vehicle_transform.location.z), carla.Rotation(yaw=vehicle_transform.rotation.yaw))
print(f'Starting from {vehicle_transform}.')
print(f'Going to {destination_transform}.')
# select control agent
if options_dict['agent'] == 'cuda':
agent = CudaAgent(vehicle.vehicle)
agent.set_destination(vehicle_transform, destination_transform)
elif options_dict['agent'] == 'test':
time = options_dict['time']
agent = TestAgent(vehicle.vehicle)
agent.set_destination(vehicle_transform, destination_transform, time=time)
if time:
df = agent.time_df
df.to_csv('time_data.csv')
ax = plt.gca()
# df.plot(kind='line',x='iteration',y='elapsed', color='red', ax=ax)
df.plot(kind='line',x='iteration',y='wavefront',ax=ax)
df.plot(kind='line',x='iteration',y='wavefront_compact',ax=ax)
df.plot(kind='line',x='iteration',y='open_compact',ax=ax)
df.plot(kind='line',x='iteration',y='neighbors',ax=ax)
df.plot(kind='line',x='iteration',y='neighbors_compact',ax=ax)
df.plot(kind='line',x='iteration',y='connection',ax=ax)
plt.xlabel('iteration')
plt.ylabel('time (s)')
plt.show()
else:
agent = BasicAgent(vehicle.vehicle)
agent.set_destination((destination_transform.location.x, destination_transform.location.y, destination_transform.location.z))
# attach sensors to vehicle
# sensor_bp = ['sensor.camera.rgb', "sensor.camera.semantic_segmentation", "sensor.camera.depth"]
# sensor_transform = carla.Transform(carla.Location(x= 2.5,z=2))
# depth = Camera(sensor_bp[2], sensor_transform, vehicle, agent)
# segment= Camera(sensor_bp[1], sensor_transform, vehicle, agent)
if options_dict['record']:
sensor_transform = carla.Transform(carla.Location(x= 0.5,z=2))
rgb_camera = Camera('sensor.camera.rgb', sensor_transform, vehicle, agent,record = True)
# run the simulation
print('Starting the simulation.')
prev_location = vehicle.vehicle.get_location()
sp = 2
while True:
# wait for server to be ready
world.world.tick()
world_snapshot = world.world.wait_for_tick(10.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)
# plan, get control inputs, and apply to vehicle
control = agent.run_step(options_dict['debug'])
vehicle.vehicle.apply_control(control)
# check if destination reached
current_location = vehicle.vehicle.get_location()
# kind of hacky way to test destination reached and doesn't always work - may have to manually stop with ctrl c
if current_location.distance(prev_location) <= 0.0 and current_location.distance(destination_transform.location) <= 10:
print('distance from destination: ', current_location.distance(destination_transform.location))
# if out of destinations break else go to next destination
if len(options_dict['spawn_point_indices']) <= sp:
if options_dict['record']:
rgb_camera.video_recorder.release()
break
else:
destination_transform.location = spawn_points[options_dict['spawn_point_indices'][sp]].location
print('Going to ', destination_transform.location)
# agent.set_destination((destination_transform.location.x, destination_transform.location.y, destination_transform.location.z))
agent.set_destination(vehicle_transform, destination_transform)
sp += 1
prev_location = current_location
finally:
if world is not None:
world.destroy()
def game_loop(args):
pygame.init()
pygame.font.init()
world = None
#f = open('location_list.txt', 'r')
i = 0
location_list = [
[500, 5.25, 31.0, 0.0, 0.0],
[
475.4852470983795, 5.678740381744522, 30.0, 0.011942474433034074,
9.481388061770087
],
[
472.1113589818001, 5.715332086040473, 29.0, 0.012105306942113502,
9.012409139522038
],
[
454.0871193356091, 5.8597591238060405, 28.0, 0.012903765907572399,
8.997941150502005
],
[
436.09148605811964, 5.765087408368869, 27.0, 0.013219133555865806,
7.907664017308204
],
[
420.27927717255363, 5.450999838173674, 26.0, 0.012969219540954719,
2.6903561539015364
],
[
414.90291419050664, 5.234699064670276, 25.0, 0.012616014874265873,
5.500995187472245
],
[
403.90286894692605, 5.027825070288866, 24.0, 0.012447461468861111,
9.710390691465745
],
[
384.48235362442995, 4.926168135884138, 23.0, 0.012811767710883288,
9.985278899462706
],
[
364.51211033566875, 4.814088753707787, 22.0, 0.013206169974161696,
7.8825923991263895
],
[
348.7487507701559, 5.053978168677228, 21.0, 0.014490731237345338,
4.500000000000008
], [339.749, 4.987, 20.0, 0.014677429171226393, 10.078971599325023],
[319.592, 5.182, 19.0, 0.016213002649283742, 11.495219267156243],
[296.602, 5.324, 18.0, 0.017948052642080513, 13.122339625615528],
[270.36, 4.949, 17.0, 0.018303178498518525, 11.423603229740003],
[
247.51286058561587, 5.0043496323322945, 16.0, 0.02021578951147572,
4.500602163768276
],
[
238.5129217492751, 5.037530080270346, 15.0, 0.021117435478132073,
12.529842286551135
],
[
213.45347683222624, 4.927933762933469, 14.0, 0.02308258733630485,
4.5
],
[
204.45529975615295, 5.109067160265785, 13.0, 0.024983476288845008,
4.499999999999997
], [195.457, 5.284, 12.0, 0.027027496118305478, 8.689527849659033],
[178.078, 5.24, 11.0, 0.02941681979829673, 11.581500388550705],
[154.915, 5.246, 10.0, 0.03385079618528805, 13.327193937584907],
[128.262, 4.974, 9.0, 0.038760574456180405, 6.338593889815],
[115.585, 5.043, 8.0, 0.04360257562440571, 9.88808981805889],
[95.81, 4.827, 7.0, 0.050338400773856, 7.777515445179138],
[80.255, 4.858, 6.0, 0.060458283762458445, 7.375550860783211],
[65.506, 5.107, 5.0, 0.07780494272413684, 6.951037493288717],
[
51.60396193342049, 5.0749605275278284, 4.0, 0.09802917448443067,
4.500000000000002
],
[42.60499983592313, 4.938281458881817, 3.0, 0.11539357217913827, 4.5],
[33.605, 4.94, 2.0, 0.14595657222297126, 8.715192998436693],
[16.175, 5.056, 1.0, 0.30295879121347374, 8.088081679236431],
[0, 5.25, 0.0, 0.0, 0.0]
]
location_list.reverse()
for location in location_list:
location[0] += -315.8
location[1] += 25
print(location_list)
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)
if args.agent == "Roaming":
agent = RoamingAgent(world.player)
else:
agent = BasicAgent(world.player)
spawn_point = carla.Location(x=-315.791, y=29.9778, z=4.17103)
print(spawn_point)
waypoint = world.map.get_waypoint(world.player.get_location())
target = world.map.get_waypoint(
carla.Location(location_list[i][0], location_list[i][1], 0))
if waypoint.transform.location.distance(
carla.Location(location_list[i][0], location_list[i][1],
0)) < 0.1:
i += 1
agent.set_destination(
(location_list[i][0], location_list[i][1], 0))
print(1)
clock = pygame.time.Clock()
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)
finally:
if world is not None:
world.destroy()
pygame.quit()
