class Car: im_width = 640 im_height = 480 fov = 110 actor_list = [] front_camera = None front_camera_intrinsics = None front_camera_depth = None front_camera_depth_old = None front_camera_semantic = None front_camera_semantic_old = None imu_sensor = None def __init__(self): self.client = carla.Client("localhost", 2000) self.client.set_timeout(2.0) self.world = self.client.get_world() blueprint_library = self.world.get_blueprint_library() self.model_3 = blueprint_library.filter("model3")[0] self.model_3.set_attribute('role_name', 'hero') self.agent = None settings = self.world.get_settings() settings.synchronous_mode = False # Disables synchronous mode settings.fixed_delta_seconds = 0 self.world.apply_settings(settings) def reset(self): actors = self.world.get_actors() self.client.apply_batch([ carla.command.DestroyActor(x) for x in actors if "vehicle" in x.type_id ]) self.client.apply_batch([ carla.command.DestroyActor(x) for x in actors if "sensor" in x.type_id ]) for a in actors.filter("vehicle*"): if a.is_alive: a.destroy() for a in actors.filter("sensor*"): if a.is_alive: a.destroy() print("Scene init done!") self.actor_list = [] self.transform = self.world.get_map().get_spawn_points()[0] self.transform.location.z += 1 self.vehicle = self.world.spawn_actor(self.model_3, self.transform) # self.vehicle.set_autopilot() ##### Setup Agent ##### self.agent = BehaviorAgent(self.vehicle, behavior="normal") destination_location = self.world.get_map().get_spawn_points( )[50].location self.agent.set_destination(self.vehicle.get_location(), destination_location, clean=True) self.agent.update_information(self.world) ######################## self.actor_list.append(self.vehicle) self.other_vehicles = [] self.other_agents = [] spawn_points = self.world.get_map().get_spawn_points() for _ in range(50): print("Agent", _, "spawned!") tmp_transform = random.choice(spawn_points) tmp_transform.location.z += 1 tmp_vehicle = self.world.spawn_actor(self.model_3, tmp_transform) tmp_agent = RoamingAgent(tmp_vehicle) # tmp_destination_location = random.choice(spawn_points).location # tmp_agent.set_destination(tmp_vehicle.get_location(), tmp_destination_location, clean=True) # tmp_agent.update_information(self.world) self.other_vehicles.append(tmp_vehicle) self.other_agents.append(tmp_agent) self.actor_list.append(tmp_vehicle) self.rgb_cam = self.world.get_blueprint_library().find( "sensor.camera.rgb") self.rgb_cam.set_attribute("image_size_x", f"{self.im_width}") self.rgb_cam.set_attribute("image_size_y", f"{self.im_height}") self.rgb_cam.set_attribute("fov", "110") fx = self.im_width / (2 * np.tan(self.fov * np.pi / 360)) fy = self.im_height / (2 * np.tan(self.fov * np.pi / 360)) self.front_camera_intrinsics = np.array([[fx, 0, self.im_width / 2], [0, fy, self.im_height / 2], [0, 0, 1]]) self.depth_cam = self.world.get_blueprint_library().find( "sensor.camera.depth") self.depth_cam.set_attribute("image_size_x", f"{self.im_width}") self.depth_cam.set_attribute("image_size_y", f"{self.im_height}") self.depth_cam.set_attribute("fov", "110") self.semantic_cam = self.world.get_blueprint_library().find( "sensor.camera.depth") self.semantic_cam.set_attribute("image_size_x", f"{self.im_width}") self.semantic_cam.set_attribute("image_size_y", f"{self.im_height}") self.semantic_cam.set_attribute("fov", "110") transform = carla.Transform(carla.Location(x=2.5, z=1)) self.sensor_rgb = self.world.spawn_actor(self.rgb_cam, transform, attach_to=self.vehicle) self.actor_list.append(self.sensor_rgb) self.sensor_rgb.listen(lambda data: self.process_img(data)) self.sensor_depth = self.world.spawn_actor(self.depth_cam, transform, attach_to=self.vehicle) self.actor_list.append(self.sensor_depth) self.sensor_depth.listen(lambda data: self.process_img_depth(data)) self.sensor_semantic = self.world.spawn_actor(self.semantic_cam, transform, attach_to=self.vehicle) self.actor_list.append(self.sensor_semantic) self.sensor_semantic.listen( lambda data: self.process_img_semantic(data)) self.imu_sensor = IMUSensor(self.vehicle) while self.front_camera is None: time.sleep(0.01) self.episode_start = time.time() return self.front_camera def process_img(self, image): i = np.array(image.raw_data) i2 = i.reshape((self.im_height, self.im_width, 4)) i3 = i2[:, :, :3] self.front_camera = i3 def process_img_depth(self, image): i = np.array(image.raw_data) i2 = i.reshape((self.im_height, self.im_width, 4)) i2 = i2[:, :, :3] i3 = np.add( np.add(i2[:, :, 2], np.multiply(i2[:, :, 1], 256)), np.multiply(i2[:, :, 0], 256 * 256), ) i3 = np.divide(i3, 256**3 - 1) self.front_camera_depth_old = self.front_camera_depth self.front_camera_depth = i3 def process_img_semantic(self, image): image.convert(carla.ColorConverter.CityScapesPalette) i = np.array(image.raw_data) i2 = i.reshape((self.im_height, self.im_width, 4)) self.front_camera_semantic_old = self.front_camera_semantic self.front_camera_semantic = i2
def game_loop(args): """ Main loop for agent""" pygame.init() pygame.font.init() world = None vis = None lidar = 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.05 settings.fixed_delta_seconds = delta settings.synchronous_mode = True world.world.apply_settings(settings) # END controller = KeyboardControl(world) 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) # open3d_lidar setup BEGIN logging.info("open3d_lidar setup") blueprint_library = world.world.get_blueprint_library() lidar_bp = generate_lidar_bp(args, world.world, blueprint_library, delta) user_offset = carla.Location(args.x, args.y, args.z) lidar_transform = carla.Transform(carla.Location(x=-0.5, z=1.8) + user_offset) lidar = world.world.spawn_actor(lidar_bp, lidar_transform, attach_to=world.player) point_list = o3d.geometry.PointCloud() lidar.listen(lambda data: lidar_callback(data, point_list)) vis = o3d.visualization.Visualizer() vis.create_window( window_name='Carla Lidar', width=960, height=540, left=480, top=270) vis.get_render_option().background_color = [0.05, 0.05, 0.05] vis.get_render_option().point_size = 1 vis.get_render_option().show_coordinate_frame = True frame = 0 # open3d_lidar setup END clock = pygame.time.Clock() logging.info("entering loop") while True: # open3d_lidar render BEGIN # print("open3d_lidar render") if frame == 2: vis.add_geometry(point_list) vis.update_geometry(point_list) vis.poll_events() vis.update_renderer() # time.sleep(0.005) world.world.tick() frame += 1 # open3d_lidar render END # move self.world.on_tick(hud.on_world_tick) here BEGIN # print("HUD tick") # hud.on_world_tick( # world.world.get_snapshot().timestamp) # move self.world.on_tick(hud.on_world_tick) here END if controller.parse_events(): return # As soon as the server is ready continue! # if not world.world.wait_for_tick(10.0): # continue agent.update_information() # display.fill("red") world.tick(clock) clock.tick_busy_loop(50) 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: 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: 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 vis is not None: vis.destroy_window() if lidar is not None: lidar.destroy() if original_settings is not None \ and world is not None: world.world.apply_settings(original_settings) if world is not None: world.destroy() pygame.quit()
def game_loop(args): """ Main loop for agent""" prev_timestamp = 0 pygame.init() skip_first_frame = True pygame.font.init() world = None tot_target_reached = 0 num_min_waypoints = 21 try: start_timestamp = 0 client = carla.Client(args.host, args.port) client.set_timeout(20.0) client.load_world('Town01') client.reload_world() #measurement_data, sensor_data = client.read_data() 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) # Set Sensors #RGB camera agent = BehaviorAgent(world.player, behavior="normal") 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) waypoints_file = WAYPOINTS_FILENAME waypoints_filepath =\ os.path.join(os.path.dirname(os.path.realpath(__file__)), WAYPOINTS_FILENAME) waypoints_np = None with open(waypoints_filepath) as waypoints_file_handle: waypoints = list(csv.reader(waypoints_file_handle, delimiter=',', quoting=csv.QUOTE_NONNUMERIC)) waypoints_np = np.array(waypoints) print(waypoints_np.shape) wp_goal_index = 0 local_waypoints = None path_validity = np.zeros((NUM_PATHS, 1), dtype=bool) controller = resources.controller2d.Controller2D(waypoints) bp = resources.behavioural_planner.BehaviouralPlanner(BP_LOOKAHEAD_BASE) lp = resources.local_planner.LocalPlanner(NUM_PATHS, PATH_OFFSET, CIRCLE_OFFSETS, CIRCLE_RADII, PATH_SELECT_WEIGHT, TIME_GAP, A_MAX, SLOW_SPEED, STOP_LINE_BUFFER) clock = pygame.time.Clock() frame = 0 current_timestamp = start_timestamp reached_the_end = False while True: frame = frame +1 clock.tick_busy_loop(60) agent.update_information(world) world.tick(clock) world.render(display) if not world.world.wait_for_tick(10.0): continue pygame.display.flip() transform = world.player.get_transform() vel = world.player.get_velocity() print("Vel", vel) prev_timestamp = current_timestamp current_timestamp = hud.simulation_time control = world.player.get_control() current_x = transform.location.x current_y = transform.location.y current_yaw = np.radians(transform.rotation.yaw) current_speed = math.sqrt(vel.x**2 + vel.y**2 ) print("Variables: " , current_x,current_y,current_yaw,current_speed) open_loop_speed = lp._velocity_planner.get_open_loop_speed(current_timestamp - prev_timestamp) #open_loop_speed = lp._velocity_planner.get_open_loop_speed(current_timestamp - prev_timestamp) print("Current Timestamp", current_timestamp) print("prev_timestamp",prev_timestamp) if frame % 5 == 0: #Lane detection ego_state = [current_x, current_y, current_yaw, open_loop_speed] bp.set_lookahead(BP_LOOKAHEAD_BASE + BP_LOOKAHEAD_TIME * open_loop_speed) print("Ego_State",ego_state) bp.transition_state(waypoints,waypoints, ego_state, current_speed) goal_state_set = lp.get_goal_state_set(bp._goal_index, bp._goal_state, waypoints, ego_state) paths, path_validity = lp.plan_paths(goal_state_set) paths = resources.local_planner.transform_paths(paths, ego_state) best_index = lp._collision_checker.select_best_path_index(paths, bp._goal_state_hyp) try: if best_index == None: best_path = lp._prev_best_path else: best_path = paths[best_index] lp._prev_best_path = best_path except: continue desired_speed = bp._goal_state[2] decelerate_to_stop = False local_waypoints = lp._velocity_planner.compute_velocity_profile(best_path, desired_speed, ego_state, current_speed) if local_waypoints != None: wp_distance = [] # distance array local_waypoints_np = np.array(local_waypoints) for i in range(1, local_waypoints_np.shape[0]): wp_distance.append( np.sqrt((local_waypoints_np[i, 0] - local_waypoints_np[i-1, 0])**2 + (local_waypoints_np[i, 1] - local_waypoints_np[i-1, 1])**2)) wp_distance.append(0) wp_interp = [] for i in range(local_waypoints_np.shape[0] - 1): wp_interp.append(list(local_waypoints_np[i])) num_pts_to_interp = int(np.floor(wp_distance[i] /\ float(INTERP_DISTANCE_RES)) - 1) wp_vector = local_waypoints_np[i+1] - local_waypoints_np[i] wp_uvector = wp_vector / np.linalg.norm(wp_vector[0:2]) for j in range(num_pts_to_interp): next_wp_vector = INTERP_DISTANCE_RES * float(j+1) * wp_uvector wp_interp.append(list(local_waypoints_np[i] + next_wp_vector)) wp_interp.append(list(local_waypoints_np[-1])) controller.update_waypoints(wp_interp) pass if local_waypoints != None and local_waypoints != []: controller.update_values(current_x, current_y, current_yaw, current_speed, current_timestamp, frame) controller.update_controls() cmd_throttle, cmd_steer, cmd_brake = controller.get_commands() else: cmd_throttle = 0.0 cmd_steer = 0.0 cmd_brake = 0.0 if skip_first_frame and frame == 0: pass elif local_waypoints == None: pass else: ## if i % 5 == 0: wp_interp_np = np.array(wp_interp) path_indices = np.floor(np.linspace(0, wp_interp_np.shape[0]-1, INTERP_MAX_POINTS_PLOT)) #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) #cmd_throttle = 0.4 #cmd_steer = 0 #cmd_brake = 0 dist_to_last_waypoint = np.linalg.norm(np.array([ waypoints[-1][0] - current_x, waypoints[-1][1] - current_y])) if dist_to_last_waypoint < DIST_THRESHOLD_TO_LAST_WAYPOINT: reached_the_end = True if reached_the_end: cmd_steer = np.fmax(np.fmin(0, 1.0), -1.0) cmd_throttle = np.fmax(np.fmin(0, 1.0), 0) cmd_brake = np.fmax(np.fmin(0.1, 1.0), 0) break cmd_steer = np.fmax(np.fmin(cmd_steer, 1.0), -1.0) cmd_throttle = np.fmax(np.fmin(cmd_throttle, 1.0), 0) cmd_brake = np.fmax(np.fmin(cmd_brake, 1.0), 0) world.player.apply_control(carla.VehicleControl(throttle=cmd_throttle, steer=cmd_steer, brake=cmd_brake)) if reached_the_end: print("Reached the end of path. Writing to controller_output...") else: print("Exceeded assessment time. Writing to controller_output...") finally: if world is not None: world.destroy() pygame.quit()
def setup_scenario(world, client, synchronous_master=False, subject_behavior="normal"): # @todo cannot import these directly. SpawnActor = carla.command.SpawnActor SetAutopilot = carla.command.SetAutopilot FutureActor = carla.command.FutureActor batch = [] ego_batch = [] vehicles_list = [] blueprints = world.get_blueprint_library() blueprints = [x for x in blueprints if x.id.endswith("model3")] all_waypoints = world.get_map().generate_waypoints(3) # waypoint_list = filter_waypoints(road_id, lane_id) left_most_lane = filter_waypoints(all_waypoints, 15, -3) middle_lane = filter_waypoints(all_waypoints, 15, -5) right_lane = filter_waypoints(all_waypoints, 15, -6) sub_spawn_point = middle_lane[1].transform manual_spawn_point = left_most_lane[1].transform ego_spawn_point = right_lane[1].transform sub_spawn_point = carla.Transform( Location(x=sub_spawn_point.location.x, y=sub_spawn_point.location.y, z=0.5), Rotation(yaw=sub_spawn_point.rotation.yaw), ) ego_spawn_point = carla.Transform( Location(x=ego_spawn_point.location.x, y=ego_spawn_point.location.y, z=0.5), Rotation(yaw=ego_spawn_point.rotation.yaw), ) manual_spawn_point = carla.Transform( Location(x=manual_spawn_point.location.x, y=manual_spawn_point.location.y, z=0.5), Rotation(yaw=manual_spawn_point.rotation.yaw), ) # -------------- # Spawn vehicles # -------------- # Manual Vehicle blueprint = random.choice(blueprints) color = "0,255,0" blueprint.set_attribute("color", color) blueprint.set_attribute("role_name", "hero") batch.append( SpawnActor(blueprint, manual_spawn_point).then(SetAutopilot(FutureActor, False))) # Subject Vehicle Details blueprint = random.choice(blueprints) color = "0,0,255" blueprint.set_attribute("color", color) blueprint.set_attribute("role_name", "autopilot") batch.append( SpawnActor(blueprint, sub_spawn_point).then(SetAutopilot(FutureActor, True))) # Ego Vehicle Details color = "255,0,0" blueprint.set_attribute("color", color) blueprint.set_attribute("role_name", "ego") ego_batch.append( SpawnActor(blueprint, ego_spawn_point).then(SetAutopilot(FutureActor, True))) # Spawn ego_vehicle_id = None for response in client.apply_batch_sync(ego_batch, synchronous_master): if response.error: print("Response Error while applying ego batch!") else: # self.vehicles_list.append(response.actor_id) ego_vehicle_id = response.actor_id print("Ego vehicle id: ------------------------------", ego_vehicle_id) for response in client.apply_batch_sync(batch, synchronous_master): if response.error: print("Response Error while applying batch!") else: vehicles_list.append(response.actor_id) manual_vehicle = world.get_actors(vehicles_list)[0] subject_vehicle = world.get_actors(vehicles_list)[1] ego_vehicle = world.get_actors([ego_vehicle_id])[0] print("Warm start initiated...") warm_start_curr = 0 update_spectator(world, ego_vehicle) while warm_start_curr < 3: warm_start_curr += world.get_settings().fixed_delta_seconds if synchronous_master: world.tick() else: world.wait_for_tick() client.apply_batch_sync([SetAutopilot(ego_vehicle, False)], synchronous_master) client.apply_batch_sync([SetAutopilot(subject_vehicle, False)], synchronous_master) if subject_behavior == "manual": manual_agent = Agent(manual_vehicle) else: subject_agent = BehaviorAgent(subject_vehicle, behavior=subject_behavior) destination = carla.Location(x=240.50791931152344, y=45.247249603271484, z=0.0) subject_agent.set_destination(subject_agent.vehicle.get_location(), destination, clean=True) subject_agent.update_information(world) print("Warm start finished...") ## Get current lane waypoints ego_vehicle_location = ego_vehicle.get_location() nearest_waypoint = world.get_map().get_waypoint(ego_vehicle_location, project_to_road=True) current_lane_waypoints = nearest_waypoint.next_until_lane_end(1) if subject_behavior == "manual": return (ego_vehicle, manual_vehicle, current_lane_waypoints, manual_agent) else: return (ego_vehicle, subject_vehicle, current_lane_waypoints, subject_agent)
class World(object): """ Class representing the simulation environment. """ def __init__(self, carla_world: carla.World, traffic_manager: carla.TrafficManager, config: dict, spawn_point: carla.Transform = None): """ Constructor method. If spawn_point not given, choose random spawn point recommended by the map. """ self.carla_world = carla_world self.tm = traffic_manager self.spectator = carla_world.get_spectator() try: self.map = self.carla_world.get_map() except RuntimeError as error: print('RuntimeError: {}'.format(error)) print(' The server could not send the OpenDRIVE (.xodr) file:') print( ' Make sure it exists, has the same name of your town, and is correct.' ) sys.exit(1) self._weather_presets = find_weather_presets() self._weather_index = config['world']['weather'] self.carla_world.set_weather( self._weather_presets[self._weather_index][0]) self.ego_veh = None # Containers for managing carla sensors self.carla_sensors = {} # This dict will store references to all sensor's data container. # It is to facilitate the recording, so the recorder only needs to query this one-stop container. # When a CarlaSensor is added via add_carla_sensor(), its data container is registered automatically. # When sensor data are updated, the content in this dict is updated automatically since they are just pointers. self.all_sensor_data = {} # Ground truth extractor self.ground_truth = None # Start simuation self.restart(config, spawn_point) # Tick the world to bring the ego vehicle actor into effect self.carla_world.tick() # Placeholder for the behavior agent # See set_behavior_agent() method. self._behavior_agent = None # Placeholder for goals of behavior agent self._agent_goals = None def restart(self, config, spawn_point=None): """ Start the simulation with the configuration arguments. It spawns the actors including ego vehicle and sensors. If the ego vehicle exists already, it respawns the vehicle either at the same location or at the designated location. """ # Set up carla engine using config settings = self.carla_world.get_settings() settings.no_rendering_mode = config['world']['no_rendering'] settings.synchronous_mode = config['world']['sync_mode'] settings.fixed_delta_seconds = config['world']['delta_seconds'] self.carla_world.apply_settings(settings) # Spawn a Mustang as the ego vehicle (not stolen from John Wick, don't panic) ego_veh_bp = self.carla_world.get_blueprint_library().find( 'vehicle.mustang.mustang') if self.ego_veh: if spawn_point is None: print("Respawning ego vehicle.") spawn_point = self.ego_veh.get_transform() else: print("Respawning ego vehicle at assigned point.") # Destroy previously spawned actors self.destroy() spawn_point.location.z += 2.0 spawn_point.rotation.roll = 0.0 spawn_point.rotation.pitch = 0.0 self.ego_veh = self.carla_world.try_spawn_actor( ego_veh_bp, spawn_point) if self.ego_veh is None: print('Chosen spawn point failed.') else: if spawn_point: print("Spawning new ego vehicle at assigned point.") spawn_point.location.z += 2.0 self.ego_veh = self.carla_world.try_spawn_actor( ego_veh_bp, spawn_point) while self.ego_veh is None: if not self.map.get_spawn_points(): print('There are no spawn points available in your map/town.') sys.exit(1) print("Spawning new ego vehicle at a random point.") spawn_points = self.map.get_spawn_points() spawn_point = random.choice( spawn_points) if spawn_points else carla.Transform() self.ego_veh = self.carla_world.try_spawn_actor( ego_veh_bp, spawn_point) # Point the spectator to the ego vehicle self.see_ego_veh() # Ground truth extractor debug = False if __debug__: debug = True self.ground_truth = GroundTruthExtractor(self.ego_veh, config['gt'], debug) def add_carla_sensor(self, carla_sensor: CarlaSensor): """ Add a CarlaSensor. This sensor will be added to the carla_sensors list, and all_sensor_data will add a new key-value pair, where the key is the same as the carla_sensor's name and the value is the reference to carla_sensor's data. """ if carla_sensor.name in self.carla_sensors.keys(): raise RuntimeError( 'Trying to add a CarlaSensor with a duplicate name.') # Add the CarlaSensor self.carla_sensors[carla_sensor.name] = carla_sensor # Register the CarlaSensor's data to all_sensor_data self.all_sensor_data[carla_sensor.name] = carla_sensor.data def set_ego_autopilot(self, active, autopilot_config=None): """ Set traffic manager and register ego vehicle to it. This makes use of the traffic manager provided by Carla to control the ego vehicle. See https://carla.readthedocs.io/en/latest/adv_traffic_manager/ for more info. """ if autopilot_config: self.tm.auto_lane_change(self.ego_veh, autopilot_config['auto_lane_change']) self.tm.ignore_lights_percentage( self.ego_veh, autopilot_config['ignore_lights_percentage']) self.tm.vehicle_percentage_speed_difference( self.ego_veh, autopilot_config['vehicle_percentage_speed_difference']) self.ego_veh.set_autopilot(active, self.tm.get_port()) def force_lane_change(self, to_left): """ Force ego vehicle to change the lane regardless collision with other vehicles. It only allows lane changes in the possible direction. Carla's traffic manager doesn't seem to always respect this command. Input: to_left: boolean to indicate the direction of lane change. """ # carla uses true for right self.tm.force_lane_change(self.ego_veh, not to_left) def set_behavior_agent(self, agent_config): """ Set up a BehaviorAgent object to control the ego vehicle to follow a set of waypoints. Warning: BehaviorAgent class directly copied from Carla's repository under PythonAPI/carla. It is used in several carla example codes to control the car. However the whole agent package is not officially documented and the its use here is based on the examination of the example codes. Input: agent_config: dict storing configurations for behavior agent. """ self._behavior_agent = BehaviorAgent( self.ego_veh, agent_config['ignore_traffic_light'], agent_config['behavior']) self._agent_goals = deque() for goal in agent_config['goals']: self._agent_goals.append( carla.Location(goal['x'], goal['y'], goal['z'])) # Set the first goal self._behavior_agent.set_destination(self.ego_veh.get_location(), self._agent_goals.popleft()) def run_agent_step(self): """ Run the behavior agent at the current step. It first checks if it should add the next goal into the internal waypoints to follow, then update and apply the control signal. Output: bool to indicate if should keep running. """ # Set a new goal if about to reach the current goal if len(self._behavior_agent.get_local_planner().waypoints_queue ) < 21 and self._agent_goals: new_start = self._behavior_agent.get_local_planner( ).waypoints_queue[-1][0].transform.location new_goal = self._agent_goals.popleft() self._behavior_agent.set_destination(new_start, new_goal, clean=False) print("New target: {}".format(new_goal)) self._behavior_agent.update_information(self.ego_veh) # If debug set to True, imminent waypoints will be drawn debug = False if __debug__: debug = True control, keep_running = self._behavior_agent.run_step(debug=debug) self.ego_veh.apply_control(control) return keep_running def step_forward(self): """ Tick carla world to take simulation one step forward. Output: bool to indicate if should keep running. """ keep_running = True self.carla_world.tick() # Run behavior agent if used if self._behavior_agent: keep_running = self.run_agent_step() # Update CarlaSensors' data for carla_sensor in self.carla_sensors.values(): carla_sensor.update() self.ground_truth.update() return keep_running def see_ego_veh(self, following_dist=5, height=5, tilt_ang=-30): """ Aim the spectator down to the ego vehicle. """ spect_location = carla.Location(x=-following_dist) self.ego_veh.get_transform().transform( spect_location) # it modifies passed-in location ego_rotation = self.ego_veh.get_transform().rotation self.spectator.set_transform( carla.Transform( spect_location + carla.Location(z=height), carla.Rotation(pitch=tilt_ang, yaw=ego_rotation.yaw))) def allow_free_run(self): """ Allow carla engine to run asynchronously and freely. """ settings = self.carla_world.get_settings() settings.synchronous_mode = False settings.fixed_delta_seconds = 0.0 self.carla_world.apply_settings(settings) def destroy(self): """ Destroy spawned actors in carla world. """ if self.ego_veh: print("Destroying the ego vehicle.") self.ego_veh.destroy() self.ego_veh = None for carla_sensor in self.carla_sensors.values(): carla_sensor.destroy() self.carla_sensors.clear()
def main(): global global_nav, global_control, global_pos, global_vel, global_acceleration, global_angular_velocity client = carla.Client(config['host'], config['port']) client.set_timeout(config['timeout']) #world = client.get_world() world = client.load_world('Town01') """ weather = carla.WeatherParameters( cloudiness=random.randint(0,80), precipitation=0, sun_altitude_angle=random.randint(40,90) ) set_weather(world, weather) """ world.set_weather(carla.WeatherParameters.MidRainSunset) 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) _world = World(vehicle, world) sensor_dict = { 'camera': { 'transform': carla.Transform( carla.Location(x=0.5, y=0.0, z=2.5) ), #'transform':carla.Transform(carla.Location(x=-3.0, y=0.0, z=2.5), carla.Rotation(pitch=-20)), '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) agent = BehaviorAgent(vehicle, ignore_traffic_light=True, behavior='normal') _destination = carla.Transform() destination = world.get_random_location_from_navigation() _destination.location = destination #port = 8000 #tm = client.get_trafficmanager(port) #tm.ignore_lights_percentage(vehicle,100) #destination = get_random_destination(spawn_points) plan_map = replan2(agent, _destination, copy.deepcopy(origin_map)) #agent.set_destination(agent.vehicle.get_location(), destination, clean=True) #for cnt in tqdm(range(args.num)): for cnt in range(args.num): if close2dest(vehicle, _destination): _destination = carla.Transform() destination = world.get_random_location_from_navigation() _destination.location = destination plan_map = replan2(agent, _destination, copy.deepcopy(origin_map)) #agent.set_destination(agent.vehicle.get_location(), destination, clean=True) 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) agent.update_information(_world) speed_limit = vehicle.get_speed_limit() agent.get_local_planner().set_speed(speed_limit) control = agent.run_step() 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() global_acceleration = vehicle.get_acceleration() global_angular_velocity = vehicle.get_angular_velocity() #yaw = global_pos.rotation.yaw #_ax = global_acceleration.x #_ay = global_acceleration.y #ax = _ax*np.cos(yaw) + _ay*np.sin(yaw) #ay = _ay*np.cos(yaw) - _ax*np.sin(yaw) #cv2.imshow('Nav', nav) cv2.imshow('Vision', global_img) cv2.waitKey(10) index = str(time.time()) #save_data(index) cmd_file.close() pos_file.close() vel_file.close() acc_file.close() angular_vel_file.close() cv2.destroyAllWindows() sm.close_all() vehicle.destroy()