def __init__(self, actor, args=None): super(SimpleVehicleControl, self).__init__(actor) self._generated_waypoint_list = [] self._last_update = None self._consider_traffic_lights = False self._consider_obstacles = False self._proximity_threshold = float('inf') self._max_deceleration = None self._max_acceleration = None self._obstacle_sensor = None self._obstacle_distance = float('inf') self._obstacle_actor = None self._visualizer = None if args and 'consider_obstacles' in args and strtobool( args['consider_obstacles']): self._consider_obstacles = strtobool(args['consider_obstacles']) bp = CarlaDataProvider.get_world().get_blueprint_library().find( 'sensor.other.obstacle') bp.set_attribute('distance', '250') if args and 'proximity_threshold' in args: self._proximity_threshold = float(args['proximity_threshold']) bp.set_attribute( 'distance', str(max(float(args['proximity_threshold']), 250))) bp.set_attribute('hit_radius', '1') bp.set_attribute('only_dynamics', 'True') self._obstacle_sensor = CarlaDataProvider.get_world().spawn_actor( bp, carla.Transform( carla.Location(x=self._actor.bounding_box.extent.x, z=1.0)), attach_to=self._actor) self._obstacle_sensor.listen( lambda event: self._on_obstacle(event)) # pylint: disable=unnecessary-lambda if args and 'consider_trafficlights' in args and strtobool( args['consider_trafficlights']): self._consider_traffic_lights = strtobool( args['consider_trafficlights']) if args and 'max_deceleration' in args: self._max_deceleration = float(args['max_deceleration']) if args and 'max_acceleration' in args: self._max_acceleration = float(args['max_acceleration']) if args and 'attach_camera' in args and strtobool( args['attach_camera']): self._visualizer = Visualizer(self._actor)
def _reset(self): """ Reset all parameters """ self._running = False self._timestamp_last_run = 0.0 self.scenario_duration_system = 0.0 self.scenario_duration_game = 0.0 self.start_system_time = None self.end_system_time = None if self._callback_id: CarlaDataProvider.get_world().remove_on_tick(self._callback_id) self._callback_id = None GameTime.restart()
def _tick_scenario(self, timestamp): """ Run next tick of scenario and the agent and tick the world. """ if self._timestamp_last_run < timestamp.elapsed_seconds and self._running: self._timestamp_last_run = timestamp.elapsed_seconds self._watchdog.update() # Update game time and actor information GameTime.on_carla_tick(timestamp) CarlaDataProvider.on_carla_tick() self._watchdog.pause() try: self._agent_watchdog.resume() self._agent_watchdog.update() ego_action = self._agent() self._agent_watchdog.pause() # Special exception inside the agent that isn't caused by the agent except SensorReceivedNoData as e: raise RuntimeError(e) except Exception as e: raise AgentError(e) self._watchdog.resume() self.ego_vehicles[0].apply_control(ego_action) # Tick scenario self.scenario_tree.tick_once() if self._debug_mode: print("\n") py_trees.display.print_ascii_tree(self.scenario_tree, show_status=True) sys.stdout.flush() if self.scenario_tree.status != py_trees.common.Status.RUNNING: self._running = False spectator = CarlaDataProvider.get_world().get_spectator() ego_trans = self.ego_vehicles[0].get_transform() spectator.set_transform( carla.Transform(ego_trans.location + carla.Location(z=50), carla.Rotation(pitch=-90))) if self._running and self.get_running_status(): CarlaDataProvider.get_world().tick(self._timeout)
def _tick_scenario(self, timestamp): """ Run next tick of scenario This function is a callback for world.on_tick() Important: - It has to be ensured that the scenario has not yet completed/failed and that the time moved forward. - A thread lock should be used to avoid that the scenario tick is performed multiple times in parallel. """ if self._timestamp_last_run < timestamp.elapsed_seconds: self._timestamp_last_run = timestamp.elapsed_seconds self._watchdog.update() # Update game time and actor information GameTime.on_carla_tick(timestamp) CarlaDataProvider.on_carla_tick() if self._agent is not None: ego_action = self._agent() # Tick scenario self.scenario_tree.tick_once() if self._debug_mode > 1: print("\n") py_trees.display.print_ascii_tree(self.scenario_tree, show_status=True) sys.stdout.flush() if self.scenario_tree.status != py_trees.common.Status.RUNNING: self._running = False if self._challenge_mode: spectator = CarlaDataProvider.get_world().get_spectator() ego_trans = self.ego_vehicles[0].get_transform() spectator.set_transform( carla.Transform(ego_trans.location + carla.Location(z=50), carla.Rotation(pitch=-90))) if self._agent is not None: self.ego_vehicles[0].apply_control(ego_action) if self._agent and self._running and self._watchdog.get_status(): CarlaDataProvider.get_world().tick()
def update(self): """ Check if new weather settings are available on the blackboard, and if yes fetch these into the _weather attribute. Apply the weather settings from _weather to CARLA. Note: To minimize CARLA server interactions, the weather is only updated, when the blackboard is updated, or if the weather animation flag is true. In the latter case, the update frequency is 1 Hz. returns: py_trees.common.Status.RUNNING """ weather = None try: check_weather = operator.attrgetter("CarlaWeather") weather = check_weather(py_trees.blackboard.Blackboard()) except AttributeError: pass if weather: self._weather = weather delattr(py_trees.blackboard.Blackboard(), "CarlaWeather") CarlaDataProvider.get_world().set_weather( self._weather.carla_weather) py_trees.blackboard.Blackboard().set("Datetime", self._weather.datetime, overwrite=True) if self._weather and self._weather.animation: new_time = GameTime.get_time() delta_time = new_time - self._current_time if delta_time > 1: self._weather.update(delta_time) self._current_time = new_time CarlaDataProvider.get_world().set_weather( self._weather.carla_weather) py_trees.blackboard.Blackboard().set("Datetime", self._weather.datetime, overwrite=True) return py_trees.common.Status.RUNNING
def _set_route(self, hop_resolution=1.0): world = CarlaDataProvider.get_world() dao = GlobalRoutePlannerDAO(world.get_map(), hop_resolution) grp = GlobalRoutePlanner(dao) grp.setup() spawn_points = CarlaDataProvider._spawn_points # start, target = random.choice(spawn_points, size=2) start_idx, target_idx = random.choice(len(spawn_points), size=2) # DEBUG # start_idx, target_idx = 57, 87 # start_idx, target_idx = 2, 18 # print (start_idx, target_idx) start = spawn_points[start_idx] target = spawn_points[target_idx] route = grp.trace_route(start.location, target.location) self.route = [(w.transform, c) for w, c in route] CarlaDataProvider.set_ego_vehicle_route([(w.transform.location, c) for w, c in route]) gps_route = location_route_to_gps(self.route, *_get_latlon_ref(world)) self.config.agent.set_global_plan(gps_route, self.route) self.timeout = self._estimate_route_timeout()
def run_scenario(self): """ Trigger the start of the scenario and wait for it to finish/fail """ print("ScenarioManager: Running scenario {}".format( self.scenario_tree.name)) self.start_system_time = time.time() start_game_time = GameTime.get_time() self._watchdog.start() self._running = True while self._running: timestamp = None world = CarlaDataProvider.get_world() if world: snapshot = world.get_snapshot() if snapshot: timestamp = snapshot.timestamp if timestamp: self._tick_scenario(timestamp) self._watchdog.stop() self.cleanup() self.end_system_time = time.time() end_game_time = GameTime.get_time() self.scenario_duration_system = self.end_system_time - \ self.start_system_time self.scenario_duration_game = end_game_time - start_game_time if self.scenario_tree.status == py_trees.common.Status.FAILURE: print("ScenarioManager: Terminated due to failure")
def __init__(self, reference_actor, actor, distance, comparison_operator=operator.lt, distance_type="cartesianDistance", freespace=False, name="TriggerDistanceToVehicle"): """ Setup trigger distance """ super(InTriggerDistanceToVehicle, self).__init__(name) self.logger.debug("%s.__init__()" % (self.__class__.__name__)) self._reference_actor = reference_actor self._actor = actor self._distance = distance self._distance_type = distance_type self._freespace = freespace self._comparison_operator = comparison_operator if distance_type == "longitudinal": self._global_rp = GlobalRoutePlanner( CarlaDataProvider.get_world().get_map(), 1.0) else: self._global_rp = None
def __init__(self, traffic_light_id, state, name="TrafficLightStateSetter"): """ Init """ super(TrafficLightStateSetter, self).__init__(name) self._actor = None actor_list = CarlaDataProvider.get_world().get_actors() for actor in actor_list: if actor.id == int(traffic_light_id): self._actor = actor break new_state = carla.TrafficLightState.Unknown if state.upper() == "GREEN": new_state = carla.TrafficLightState.Green elif state.upper() == "RED": new_state = carla.TrafficLightState.Red elif state.upper() == "YELLOW": new_state = carla.TrafficLightState.Yellow elif state.upper() == "OFF": new_state = carla.TrafficLightState.Off self._new_traffic_light_state = new_state self.logger.debug("%s.__init__()" % (self.__class__.__name__))
def __init__(self, debug=False, name="TrafficJamChecker"): super(TrafficJamChecker, self).__init__(name) self.debug = debug self.blackboard = Blackboard() self.world = CarlaDataProvider.get_world() self.map = CarlaDataProvider.get_map() self.list_intersection_waypoints = [] # remove initial collisions during setup list_actors = list(CarlaActorPool.get_actors()) for _, actor in list_actors: if actor.attributes['role_name'] == 'autopilot': if detect_lane_obstacle(actor, margin=0.2): CarlaActorPool.remove_actor_by_id(actor.id) # prepare a table to check for stalled vehicles during the execution of the scenario self.table_blocked_actors = {} current_game_time = GameTime.get_time() for actor_id, actor in CarlaActorPool.get_actors(): if actor.attributes['role_name'] == 'autopilot': actor.set_autopilot(True) self.table_blocked_actors[actor_id] = { 'location': actor.get_location(), 'time': current_game_time } self.logger.debug("%s.__init__()" % (self.__class__.__name__))
def run_scenario(self): """ Trigger the start of the scenario and wait for it to finish/fail """ self.start_system_time = time.time() self.start_game_time = GameTime.get_time() # Detects if the simulation is down self._watchdog = Watchdog(self._timeout) self._watchdog.start() # Stop the agent from freezing the simulation self._agent_watchdog = Watchdog(self._timeout) self._agent_watchdog.start() self._running = True while self._running: timestamp = None world = CarlaDataProvider.get_world() if world: snapshot = world.get_snapshot() if snapshot: timestamp = snapshot.timestamp if timestamp: self._tick_scenario(timestamp)
def __init__(self, actor, other_actor, time, side_lane, comparison_operator=operator.lt, name="InTimeToArrivalToVehicleSideLane"): """ Setup parameters """ self._other_actor = other_actor self._side_lane = side_lane self._world = CarlaDataProvider.get_world() self._map = CarlaDataProvider.get_map(self._world) other_location = other_actor.get_transform().location other_waypoint = self._map.get_waypoint(other_location) if self._side_lane == 'right': other_side_waypoint = other_waypoint.get_left_lane() elif self._side_lane == 'left': other_side_waypoint = other_waypoint.get_right_lane() else: raise Exception("cut_in_lane must be either 'left' or 'right'") other_side_location = other_side_waypoint.transform.location super(InTimeToArrivalToVehicleSideLane, self).__init__(actor, time, other_side_location, comparison_operator, name) self.logger.debug("%s.__init__()" % (self.__class__.__name__))
def get_geometric_linear_intersection(ego_location, other_location): """ Obtain a intersection point between two actor's location by using their waypoints (wp) @return point of intersection of the two vehicles """ wp_ego_1 = CarlaDataProvider.get_map().get_waypoint(ego_location) wp_ego_2 = wp_ego_1.next(1)[0] ego_1_loc = wp_ego_1.transform.location ego_2_loc = wp_ego_2.transform.location wp_other_1 = CarlaDataProvider.get_world().get_map().get_waypoint( other_location) wp_other_2 = wp_other_1.next(1)[0] other_1_loc = wp_other_1.transform.location other_2_loc = wp_other_2.transform.location s = np.vstack([(ego_1_loc.x, ego_1_loc.y), (ego_2_loc.x, ego_2_loc.y), (other_1_loc.x, other_1_loc.y), (other_2_loc.x, other_2_loc.y)]) h = np.hstack((s, np.ones((4, 1)))) line1 = np.cross(h[0], h[1]) line2 = np.cross(h[2], h[3]) x, y, z = np.cross(line1, line2) if z == 0: return None return carla.Location(x=x / z, y=y / z, z=0)
def run_scenario(self): """ Trigger the start of the scenario and wait for it to finish/fail """ print("ScenarioManager: Running scenario {}".format( self.scenario_tree.name)) self.start_system_time = time.time() start_game_time = GameTime.get_time() self._watchdog.start() self._running = True while self._running: timestamp = None world = CarlaDataProvider.get_world() if world: snapshot = world.get_snapshot() if snapshot: timestamp = snapshot.timestamp if timestamp: self._tick_scenario(timestamp) self._watchdog.stop() self.end_system_time = time.time() end_game_time = GameTime.get_time() self.scenario_duration_system = self.end_system_time - \ self.start_system_time self.scenario_duration_game = end_game_time - start_game_time self._console_message()
def get_geometric_linear_intersection(ego_actor, other_actor): """ Obtain a intersection point between two actor's location by using their waypoints (wp) @return point of intersection of the two vehicles """ wp_ego_1 = CarlaDataProvider.get_map().get_waypoint( ego_actor.get_location()) wp_ego_2 = wp_ego_1.next(1)[0] x_ego_1 = wp_ego_1.transform.location.x y_ego_1 = wp_ego_1.transform.location.y x_ego_2 = wp_ego_2.transform.location.x y_ego_2 = wp_ego_2.transform.location.y wp_other_1 = CarlaDataProvider.get_world().get_map().get_waypoint( other_actor.get_location()) wp_other_2 = wp_other_1.next(1)[0] x_other_1 = wp_other_1.transform.location.x y_other_1 = wp_other_1.transform.location.y x_other_2 = wp_other_2.transform.location.x y_other_2 = wp_other_2.transform.location.y s = np.vstack([(x_ego_1, y_ego_1), (x_ego_2, y_ego_2), (x_other_1, y_other_1), (x_other_2, y_other_2)]) h = np.hstack((s, np.ones((4, 1)))) line1 = np.cross(h[0], h[1]) line2 = np.cross(h[2], h[3]) x, y, z = np.cross(line1, line2) if z == 0: return (float('inf'), float('inf')) intersection = carla.Location(x=x / z, y=y / z, z=0) return intersection
def __init__(self, actor_type_list, transform, threshold, blackboard_queue_name, actor_limit=999, name="ActorSource", # ======== additional args ======== init_speed=0., # initial speed of spawned vehicle randomize=False, ): """ Setup class members """ super(ActorSource, self).__init__(name) self._world = CarlaDataProvider.get_world() self._actor_types = actor_type_list self._spawn_point = transform self._threshold = threshold + 5. # todo fix this with accurate vehicle length self._queue = Blackboard().get(blackboard_queue_name) self._actor_limit = actor_limit self._last_blocking_actor = None # ========== additional attributes ========== self.init_speed = init_speed self.randomize = randomize self.distance_gap = None
def run_scenario(self): """ Trigger the start of the scenario and wait for it to finish/fail """ print("ScenarioManager: Running scenario {}".format( self.scenario_tree.name)) self.start_system_time = time.time() start_game_time = GameTime.get_time() self._watchdog.start() self._running = True parent_folder = os.environ['SAVE_FOLDER'] string = pathlib.Path(os.environ['ROUTES']).stem save_path = pathlib.Path(parent_folder) / string step = 0 # hack: control the time of running # debug: s = time.time() with (save_path / 'measurements_loc.csv').open("a") as f_out: while self._running: timestamp = None world = CarlaDataProvider.get_world() if world: snapshot = world.get_snapshot() if snapshot: timestamp = snapshot.timestamp if timestamp: self._tick_scenario(timestamp) # addition if step == 0: f_out.write('x,y,yaw\n') # if step % 10 == 0: loc = self.ego_vehicles[0].get_location() ori = self.ego_vehicles[0].get_transform().rotation f_out.write( str(loc.x) + ',' + str(loc.y) + ',' + str(ori.yaw) + '\n') step += 1 # hack: control the time of running # debug: if time.time() - s > self.episode_max_time: print('maximum time reaches, end running') self._running = False break self._watchdog.stop() self.end_system_time = time.time() end_game_time = GameTime.get_time() self.scenario_duration_system = self.end_system_time - \ self.start_system_time self.scenario_duration_game = end_game_time - start_game_time self._console_message()
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 setup_sensors(self, vehicle, debug_mode=False): """ Create the sensors defined by the user and attach them to the ego-vehicle :param vehicle: ego vehicle :return: """ bp_library = CarlaDataProvider.get_world().get_blueprint_library() for sensor_spec in self._agent.sensors(): # These are the sensors spawned on the carla world bp = bp_library.find(str(sensor_spec['type'])) if sensor_spec['type'].startswith('sensor.camera'): bp.set_attribute('image_size_x', str(sensor_spec['width'])) bp.set_attribute('image_size_y', str(sensor_spec['height'])) bp.set_attribute('fov', str(sensor_spec['fov'])) sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'], z=sensor_spec['z']) sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'], roll=sensor_spec['roll'], yaw=sensor_spec['yaw']) elif sensor_spec['type'].startswith('sensor.lidar'): bp.set_attribute('range', str(sensor_spec['range'])) bp.set_attribute('rotation_frequency', str(sensor_spec['rotation_frequency'])) bp.set_attribute('channels', str(sensor_spec['channels'])) bp.set_attribute('upper_fov', str(sensor_spec['upper_fov'])) bp.set_attribute('lower_fov', str(sensor_spec['lower_fov'])) bp.set_attribute('points_per_second', str(sensor_spec['points_per_second'])) sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'], z=sensor_spec['z']) sensor_rotation = carla.Rotation(pitch=sensor_spec['pitch'], roll=sensor_spec['roll'], yaw=sensor_spec['yaw']) elif sensor_spec['type'].startswith('sensor.other.gnss'): sensor_location = carla.Location(x=sensor_spec['x'], y=sensor_spec['y'], z=sensor_spec['z']) sensor_rotation = carla.Rotation() # create sensor sensor_transform = carla.Transform(sensor_location, sensor_rotation) sensor = CarlaDataProvider.get_world().spawn_actor(bp, sensor_transform, vehicle) # setup callback sensor.listen(CallBack(sensor_spec['id'], sensor, self._agent.sensor_interface)) self._sensors_list.append(sensor) while not self._agent.all_sensors_ready(): if debug_mode: print(" waiting for one data reading from sensors...") CarlaDataProvider.get_world().tick()
def stop_scenario(self): """ This function triggers a proper termination of a scenario """ with self._my_lock: if self._callback_id: CarlaDataProvider.get_world().remove_on_tick(self._callback_id) self._callback_id = None if self.scenario is not None: self.scenario.terminate() if self._agent is not None: self._agent.cleanup() self._agent = None CarlaDataProvider.cleanup()
def _tick_scenario(self, timestamp): """ Run next tick of scenario This function is a callback for world.on_tick() Important: - It has to be ensured that the scenario has not yet completed/failed and that the time moved forward. - A thread lock should be used to avoid that the scenario tick is performed multiple times in parallel. """ with self._my_lock: if self._running and self._timestamp_last_run < timestamp.elapsed_seconds: self._timestamp_last_run = timestamp.elapsed_seconds if self._debug_mode: print("\n--------- Tick ---------\n") # Update game time and actor information GameTime.on_carla_tick(timestamp) CarlaDataProvider.on_carla_tick() if self._agent is not None: ego_action = self._agent() # Tick scenario self.scenario_tree.tick_once() if self._debug_mode: print("\n") py_trees.display.print_ascii_tree( self.scenario_tree, show_status=True) sys.stdout.flush() if self.scenario_tree.status != py_trees.common.Status.RUNNING: self._running = False if self._challenge_mode: ChallengeStatisticsManager.compute_current_statistics() if self._agent is not None: self.ego_vehicles[0].apply_control(ego_action) if self._agent: CarlaDataProvider.get_world().tick()
def __init__(self, vehicle, reading_frequency=1.0): super().__init__(vehicle, reading_frequency) client = CarlaDataProvider.get_client() world = CarlaDataProvider.get_world() town_map = CarlaDataProvider.get_map() # Initialize map map_utils.init(client, world, town_map, vehicle)
def __init__(self, bp_library, vehicle, reading_frequency=1.0): self._collided = False bp = bp_library.find('sensor.other.collision') self.sensor = CarlaDataProvider.get_world().spawn_actor(bp, carla.Transform(), vehicle) self.sensor.listen(lambda event: self.__class__.on_collision(weakref.ref(self), event)) self._reading_frequency = reading_frequency self._callback = None self._run_ps = True self.run()
def __init__(self, actor, args=None): super(SimpleVehicleControl, self).__init__(actor) self._generated_waypoint_list = [] self._last_update = None self._consider_obstacles = False self._proximity_threshold = float('inf') self._cv_image = None self._camera = None self._obstacle_sensor = None self._obstacle_distance = float('inf') self._obstacle_actor = None if args and 'consider_obstacles' in args and strtobool( args['consider_obstacles']): self._consider_obstacles = strtobool(args['consider_obstacles']) bp = CarlaDataProvider.get_world().get_blueprint_library().find( 'sensor.other.obstacle') bp.set_attribute('distance', '250') if args and 'proximity_threshold' in args: bp.set_attribute('distance', args['proximity_threshold']) bp.set_attribute('hit_radius', '1') bp.set_attribute('only_dynamics', 'True') self._obstacle_sensor = CarlaDataProvider.get_world().spawn_actor( bp, carla.Transform( carla.Location(x=self._actor.bounding_box.extent.x, z=1.0)), attach_to=self._actor) self._obstacle_sensor.listen( lambda event: self._on_obstacle(event)) # pylint: disable=unnecessary-lambda if args and 'attach_camera' in args and strtobool( args['attach_camera']): bp = CarlaDataProvider.get_world().get_blueprint_library().find( 'sensor.camera.rgb') self._camera = CarlaDataProvider.get_world().spawn_actor( bp, carla.Transform(carla.Location(x=0.0, z=30.0), carla.Rotation(pitch=-60)), attach_to=self._actor) self._camera.listen(lambda image: self._on_camera_update(image)) # pylint: disable=unnecessary-lambda
def _tick_scenario(self, timestamp): """ Run next tick of scenario and the agent. If running synchornously, it also handles the ticking of the world. """ if self._timestamp_last_run < timestamp.elapsed_seconds: self._timestamp_last_run = timestamp.elapsed_seconds self._watchdog.update() # Update game time and actor information GameTime.on_carla_tick(timestamp) CarlaDataProvider.on_carla_tick() if self._agent is not None: ego_action = self._agent() # Tick scenario self.scenario_tree.tick_once() if self._debug_mode > 1: print("\n") py_trees.display.print_ascii_tree(self.scenario_tree, show_status=True) sys.stdout.flush() if self.scenario_tree.status != py_trees.common.Status.RUNNING: self._running = False if self._challenge_mode: spectator = CarlaDataProvider.get_world().get_spectator() ego_trans = self.ego_vehicles[0].get_transform() spectator.set_transform( carla.Transform(ego_trans.location + carla.Location(z=50), carla.Rotation(pitch=-90))) if self._agent is not None: self.ego_vehicles[0].apply_control(ego_action) if self._sync_mode and self._running and self._watchdog.get_status(): CarlaDataProvider.get_world().tick()
def __init__(self, actor_type_list, transform, threshold, blackboard_queue_name, actor_limit=7, name="ActorSource"): """ Setup class members """ super(ActorSource, self).__init__(name) self._world = CarlaDataProvider.get_world() self._actor_types = actor_type_list self._spawn_point = transform self._threshold = threshold self._queue = Blackboard().get(blackboard_queue_name) self._actor_limit = actor_limit self._last_blocking_actor = None
def prepare_ego_vehicles(self, config, wait_for_ego_vehicles=False): """ Spawn or update the ego vehicle according to its parameters provided in config As the world is re-loaded for every scenario, no ego exists so far """ if not wait_for_ego_vehicles: for vehicle in config.ego_vehicles: self.ego_vehicles.append( CarlaActorPool.setup_actor(vehicle.model, vehicle.transform, vehicle.rolename, True)) else: ego_vehicle_missing = True while ego_vehicle_missing: self.ego_vehicles = [] ego_vehicle_missing = False for ego_vehicle in config.ego_vehicles: ego_vehicle_found = False carla_vehicles = CarlaDataProvider.get_world().get_actors( ).filter('vehicle.*') for carla_vehicle in carla_vehicles: if carla_vehicle.attributes[ 'role_name'] == ego_vehicle.rolename: ego_vehicle_found = True self.ego_vehicles.append(carla_vehicle) break if not ego_vehicle_found: ego_vehicle_missing = True break for i, _ in enumerate(self.ego_vehicles): self.ego_vehicles[i].set_transform( config.ego_vehicles[i].transform) # sync state CarlaDataProvider.get_world().tick()
def _prepare_ego_vehicles(self, ego_vehicles, wait_for_ego_vehicles=False): """ Spawn or update the ego vehicles """ if not wait_for_ego_vehicles: for vehicle in ego_vehicles: self.ego_vehicles.append( CarlaActorPool.setup_actor( vehicle.model, vehicle.transform, vehicle.rolename, True, color=vehicle.color, vehicle_category=vehicle.category)) else: ego_vehicle_missing = True while ego_vehicle_missing: self.ego_vehicles = [] ego_vehicle_missing = False for ego_vehicle in ego_vehicles: ego_vehicle_found = False carla_vehicles = CarlaDataProvider.get_world().get_actors( ).filter('vehicle.*') for carla_vehicle in carla_vehicles: if carla_vehicle.attributes[ 'role_name'] == ego_vehicle.rolename: ego_vehicle_found = True self.ego_vehicles.append(carla_vehicle) break if not ego_vehicle_found: ego_vehicle_missing = True break for i, _ in enumerate(self.ego_vehicles): self.ego_vehicles[i].set_transform(ego_vehicles[i].transform) # sync state CarlaDataProvider.get_world().tick()
def _tick_scenario(self, timestamp): """ Run next tick of scenario and the agent. If running synchornously, it also handles the ticking of the world. """ if self._timestamp_last_run < timestamp.elapsed_seconds and self._running: self._timestamp_last_run = timestamp.elapsed_seconds self._watchdog.update() if self._debug_mode: print("\n--------- Tick ---------\n") # Update game time and actor information GameTime.on_carla_tick(timestamp) CarlaDataProvider.on_carla_tick() if self._agent is not None: ego_action = self._agent() # Tick scenario self.scenario_tree.tick_once() if self._debug_mode: print("\n") py_trees.display.print_ascii_tree(self.scenario_tree, show_status=True) sys.stdout.flush() if self.scenario_tree.status != py_trees.common.Status.RUNNING: self._running = False if self._agent is not None: self.ego_vehicles[0].apply_control(ego_action) if self._agent and self._running and self._watchdog.get_status(): CarlaDataProvider.get_world().tick()
def run_scenario(self): """ Trigger the start of the scenario and wait for it to finish/fail """ print("ScenarioManager: Running scenario {}".format(self.scenario_tree.name)) self.start_system_time = time.time() start_game_time = GameTime.get_time() self._watchdog.start() self._running = True # self.fields = ['location', # 'velocity', # 'ego_location', # 'ego_velocity' # ] while self._running: timestamp = None world = CarlaDataProvider.get_world() if world: snapshot = world.get_snapshot() if snapshot: timestamp = snapshot.timestamp if timestamp: self._tick_scenario(timestamp) # dict = [{'location':location, 'velocity':vel, 'ego_location':ego_location, 'ego_velocity':ego_vel}] # # file_exists = os.path.isfile(self.distance_path) # with open(self.distance_path, 'a') as csvfile: # # creating a csv dict writer object # writer = csv.DictWriter(csvfile, fieldnames = self.fields) # if not file_exists: # writer.writeheader() # writer.writerows(dict) self._watchdog.stop() self.cleanup() self.end_system_time = time.time() end_game_time = GameTime.get_time() self.scenario_duration_system = self.end_system_time - \ self.start_system_time self.scenario_duration_game = end_game_time - start_game_time if self.scenario_tree.status == py_trees.common.Status.FAILURE: print("ScenarioManager: Terminated due to failure")