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)