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 stop_scenario(self): """ This function triggers a proper termination of a scenario """ if self._watchdog: self._watchdog.stop() if self._agent_watchdog: self._agent_watchdog.stop() self.end_system_time = time.time() self.end_game_time = GameTime.get_time() self.scenario_duration_system = self.end_system_time - self.start_system_time self.scenario_duration_game = self.end_game_time - self.start_game_time if self.get_running_status(): if self.scenario is not None: self.scenario.terminate() if self._agent is not None: self._agent.cleanup() self._agent = None self.analyze_scenario()
def terminate(self, new_status): """ Terminate and mark Blackboard entry with END """ # Report whether we ended with End or Cancel # If we were ended or cancelled, our state will be INVALID and # We will have an ancestor (a parallel SUCCESS_ON_ALL) with a successful child/children # It's possible we ENDed AND CANCELled if both condition groups were true simultaneously # NOTE 'py_trees.common.Status.INVALID' is the status of a behaviur which was terminated by a parent rules = [] if new_status == py_trees.common.Status.INVALID: # We were terminated from above unnaturally # Figure out if were ended or cancelled terminating_ancestor = self.parent while terminating_ancestor.status == py_trees.common.Status.INVALID: terminating_ancestor = terminating_ancestor.parent # We have found an ancestory which was not terminated by a parent # Check what caused it to terminate its children if terminating_ancestor.status == py_trees.common.Status.SUCCESS: successful_children = [ child.name for child in terminating_ancestor.children if child.status == py_trees.common.Status.SUCCESS ] if "StopTrigger" in successful_children: rules.append("END") # END is the default status unless we have a more detailed one rules = rules or ["END"] for rule in rules: self.blackboard.set(name="({}){}-{}".format( self.story_element_type.upper(), self.element_name, rule), value=GameTime.get_time(), overwrite=True)
def tick_scenarios_control(self, controls): """ Here we tick the scenarios and also change the control based on the scenario properties """ GameTime.on_carla_tick(self.world.get_snapshot().timestamp) CarlaDataProvider.on_carla_tick() for scenario in self._list_scenarios: # scenario.scenario.scenario_tree.tick_once() controls = scenario.change_control(controls) if self._save_data: self._environment_data['ego_controls'] = controls return controls
def update(self): """ Check collision count """ new_status = py_trees.common.Status.RUNNING if self._terminate_on_failure and (self.test_status == "FAILURE"): new_status = py_trees.common.Status.FAILURE actor_location = CarlaDataProvider.get_location(self.actor) new_registered_collisions = [] # Loops through all the previous registered collisions for collision_location in self.registered_collisions: # Get the distance to the collision point distance_vector = actor_location - collision_location distance = math.sqrt( math.pow(distance_vector.x, 2) + math.pow(distance_vector.y, 2)) # If far away from a previous collision, forget it if distance <= self.MAX_AREA_OF_COLLISION: new_registered_collisions.append(collision_location) self.registered_collisions = new_registered_collisions if self.last_id and GameTime.get_time( ) - self.collision_time > self.MAX_ID_TIME: self.last_id = None self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status
def run(self): first_time = True latest_time = GameTime.get_time() while self._run_ps: if self._callback is not None: current_time = GameTime.get_time() # Second part forces the sensors to send data at the first tick, regardless of frequency if current_time - latest_time > (1 / self._reading_frequency) \ or (first_time and GameTime.get_frame() != 0): self._callback(GenericMeasurement(self.__call__(), GameTime.get_frame())) latest_time = GameTime.get_time() first_time = False else: time.sleep(0.001)
def __init__(self, args, statistics_manager): """ Setup CARLA client and world Setup ScenarioManager """ self.statistics_manager = statistics_manager self.sensors = None self.sensor_icons = [] self._vehicle_lights = carla.VehicleLightState.Position | carla.VehicleLightState.LowBeam # First of all, we need to create the client that will send the requests # to the simulator. Here we'll assume the simulator is accepting # requests in the localhost at port 2000. self.client = carla.Client(args.host, int(args.port)) if args.timeout: self.client_timeout = float(args.timeout) self.client.set_timeout(self.client_timeout) dist = pkg_resources.get_distribution("carla") if LooseVersion(dist.version) < LooseVersion('0.9.9'): raise ImportError( "CARLA version 0.9.9 or newer required. CARLA version found: {}" .format(dist)) # Load agent module_name = os.path.basename(args.agent).split('.')[0] sys.path.insert(0, os.path.dirname(args.agent)) self.module_agent = importlib.import_module(module_name) # Create the ScenarioManager self.manager = ScenarioManager(args.debug > 1, args.challenge_mode) # Time control for summary purposes self._start_time = GameTime.get_time() self._end_time = None
def update(self): """ As long as the stop condition (duration or distance) is not violated, set a new vehicle control For vehicles: set throttle to throttle_value, as long as velocity is < target_velocity For walkers: simply apply the given self._control """ new_status = py_trees.common.Status.RUNNING if self._type == 'vehicle': if CarlaDataProvider.get_velocity( self._actor) < self._target_velocity: self._control.throttle = 1.0 else: self._control.throttle = 0.0 self._actor.apply_control(self._control) new_location = CarlaDataProvider.get_location(self._actor) self._distance += calculate_distance(self._location, new_location) self._location = new_location if self._distance > self._target_distance: new_status = py_trees.common.Status.SUCCESS if GameTime.get_time() - self._start_time > self._duration: new_status = py_trees.common.Status.SUCCESS self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status
def __call__(self): """ Execute the agent call, e.g. agent() Returns the next vehicle controls """ input_data = self.sensor_interface.get_data() timestamp = GameTime.get_time() wallclock = GameTime.get_wallclocktime() print('======[Agent] Wallclock_time = {} / Sim_time = {}'.format( wallclock, timestamp)) control = self.run_step(input_data, timestamp) control.manual_gear_shift = False return control
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:# This is the entry point to the agent 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 run_route(self, trajectory, no_master=False): while no_master or self.route_is_running(): # update all scenarios GameTime.on_carla_tick(self.timestamp) CarlaDataProvider.on_carla_tick() # update all scenarios ego_action = self.agent_instance() for scenario in self.list_scenarios: scenario.scenario.scenario_tree.tick_once() if self.debug > 1: for actor in self.world.get_actors(): if 'vehicle' in actor.type_id or 'walker' in actor.type_id: print(actor.get_transform()) # ego vehicle acts self.ego_vehicle.apply_control(ego_action) if self.spectator: spectator = self.world.get_spectator() ego_trans = self.ego_vehicle.get_transform() spectator.set_transform( carla.Transform(ego_trans.location + carla.Location(z=50), carla.Rotation(pitch=-90))) if self.route_visible: turn_positions_and_labels = clean_route(trajectory) self.draw_waypoints(trajectory, turn_positions_and_labels, vertical_shift=1.0, persistency=50000.0) self.route_visible = False # time continues attempts = 0 while attempts < self.MAX_CONNECTION_ATTEMPTS: try: self.world.tick() self.timestamp = self.world.wait_for_tick( self.wait_for_world) break except Exception: attempts += 1 print( '======[WARNING] The server is frozen [{}/{} attempts]!!' .format(attempts, self.MAX_CONNECTION_ATTEMPTS)) time.sleep(2.0) continue
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 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._challenge_mode: ChallengeStatisticsManager.compute_current_statistics() 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.perform_carla_tick(self._timeout)
def __init__(self, args, statistics_manager, customized_data=None): """ Setup CARLA client and world Setup ScenarioManager """ self.statistics_manager = statistics_manager self.sensors = [] self._vehicle_lights = carla.VehicleLightState.Position | carla.VehicleLightState.LowBeam # First of all, we need to create the client that will send the requests # to the simulator. Here we'll assume the simulator is accepting # requests in the localhost at port 2000. self.client = carla.Client(args.host, int(args.port)) if args.timeout: self.client_timeout = float(args.timeout) self.client.set_timeout(self.client_timeout) dist = pkg_resources.get_distribution("carla") if LooseVersion(dist.version) < LooseVersion('0.9.9'): raise ImportError( "CARLA version 0.9.9 or newer required. CARLA version found: {}" .format(dist)) # Load agent module_name = os.path.basename(args.agent).split('.')[0] sys.path.insert(0, os.path.dirname(args.agent)) self.module_agent = importlib.import_module(module_name) # Create the ScenarioManager self.manager = ScenarioManager(args.debug, args.sync, args.challenge_mode, args.track, self.client_timeout) # Time control for summary purposes self._start_time = GameTime.get_time() self._end_time = None # addition parent_folder = args.save_folder print('-' * 100, args.agent, os.environ['TEAM_AGENT'], '-' * 100) if not os.path.exists(parent_folder): os.mkdir(parent_folder) string = pathlib.Path(os.environ['ROUTES']).stem current_record_folder = pathlib.Path(parent_folder) / string if os.path.exists(str(current_record_folder)): import shutil shutil.rmtree(current_record_folder) current_record_folder.mkdir(exist_ok=False) (current_record_folder / 'rgb').mkdir() (current_record_folder / 'rgb_left').mkdir() (current_record_folder / 'rgb_right').mkdir() (current_record_folder / 'topdown').mkdir() (current_record_folder / 'rgb_with_car').mkdir() # if args.agent == 'leaderboard/team_code/auto_pilot.py': # (current_record_folder / 'topdown').mkdir() self.save_path = str(current_record_folder / 'events.txt')
def __call__(self): input_data = self.sensor_interface.get_data() timestamp = GameTime.get_time() control = self.run_step(input_data, timestamp) control.manual_gear_shift = False return control
def update(self): """ Check if the _actor stands still (v=0) """ new_status = py_trees.common.Status.RUNNING velocity = CarlaDataProvider.get_velocity(self._actor) if velocity > EPSILON: self._start_time = GameTime.get_time() if GameTime.get_time() - self._start_time > self._duration: new_status = py_trees.common.Status.SUCCESS self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status
def initialise(self): """ Record the elements's start time on the blackboard """ self.blackboard.set(name="({}){}-{}".format( self.story_element_type.upper(), self.element_name, "START"), value=GameTime.get_time(), overwrite=True)
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() try: ego_action = self._agent() # 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.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 load_scenario(self, scenario, agent, rep_number): """ Load a new scenario """ GameTime.restart() self._agent = AgentWrapper(agent) self.scenario_class = scenario self.scenario = scenario.scenario self.scenario_tree = self.scenario.scenario_tree self.ego_vehicles = scenario.ego_vehicles self.other_actors = scenario.other_actors self.repetition_number = rep_number # To print the scenario tree uncomment the next line #py_trees.display.render_dot_tree(self.scenario_tree) self._agent.setup_sensors(self.ego_vehicles[0], self._debug_mode)
def __init__(self, args, statistics_manager): """ Setup CARLA client and world Setup ScenarioManager """ self.statistics_manager = statistics_manager self.sensors = None self.sensor_icons = [] self._vehicle_lights = carla.VehicleLightState.Position | carla.VehicleLightState.LowBeam # First of all, we need to create the client that will send the requests # to the simulator. Here we'll assume the simulator is accepting # requests in the localhost at port 2000. self.client = carla.Client(args.host, int(args.port)) if args.timeout: self.client_timeout = float(args.timeout) self.client.set_timeout(self.client_timeout) self.traffic_manager = self.client.get_trafficmanager( int(args.trafficManagerPort)) dist = pkg_resources.get_distribution("carla") if dist.version != 'leaderboard': if LooseVersion(dist.version) < LooseVersion('0.9.10'): raise ImportError( "CARLA version 0.9.10.1 or newer required. CARLA version found: {}" .format(dist)) # Load agent module_name = os.path.basename(args.agent).split('.')[0] sys.path.insert(0, os.path.dirname(args.agent)) self.module_agent = importlib.import_module(module_name) # Create the ScenarioManager self.manager = ScenarioManager(args.timeout, args.debug > 1) # Time control for summary purposes self._start_time = GameTime.get_time() self._end_time = None # Create the agent timer self._agent_watchdog = Watchdog(int(float(args.timeout))) signal.signal(signal.SIGINT, self._signal_handler) # Set scenario class self._scenario_class = { 'route_scenario': RouteScenario, 'train_scenario': TrainScenario, 'nocrash_train_scenario': NoCrashTrainScenario, 'nocrash_eval_scenario': NoCrashEvalScenario, }.get(args.scenario_class) if args.scenario_class == 'train_scenario': self.town = args.town else: self.town = None
def update(self): """ Keep running until termination condition is satisfied """ new_status = py_trees.common.Status.RUNNING if GameTime.get_time() - self._start_time > self._duration: new_status = py_trees.common.Status.SUCCESS return new_status
def initialise(self): self._location = CarlaDataProvider.get_location(self._actor) self._start_time = GameTime.get_time() # In case of walkers, we have to extract the current heading if self._type == 'walker': self._control.speed = self._target_velocity self._control.direction = CarlaDataProvider.get_transform(self._actor).get_forward_vector() super(KeepVelocity, self).initialise()
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 __call__(self): """ Execute the agent call, e.g. agent() Returns the next vehicle controls """ input_data = self.sensor_interface.get_data() timestamp = GameTime.get_time() if not self.wallclock_t0: self.wallclock_t0 = GameTime.get_wallclocktime() wallclock = GameTime.get_wallclocktime() wallclock_diff = (wallclock - self.wallclock_t0).total_seconds() # print('======[Agent] Wallclock_time = {} / {} / Sim_time = {} / {}x'.format(wallclock, wallclock_diff, timestamp, timestamp/(wallclock_diff+0.001))) control = self.run_step(input_data, timestamp) control.manual_gear_shift = False return control
def __init__(self, element_type, element_name, rule): """ Setup element details """ super(AfterTerminationCondition, self).__init__("AfterTerminationCondition") self.logger.debug("%s.__init__()" % (self.__class__.__name__)) self._element_type = element_type.upper() self._element_name = element_name self._rule = rule.upper() self._start_time = GameTime.get_time() self._blackboard = py_trees.blackboard.Blackboard()
def update(self): new_time = GameTime.get_time() delta_time = new_time - self._current_time # Switch to a different weather every 5s if delta_time > 10: self._current_time = new_time weather = random.choice(self.WEATHERS) CarlaDataProvider.get_world().set_weather(weather) return py_trees.common.Status.RUNNING
def _tick_scenario(self, timestamp): """ Run next tick of scenario This function is a callback for world.on_tick() Important: - It hast 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() # Tick scenario self.scenario_tree.tick_once() if self.agent: # Invoke agent action = self.agent() action = self.scenario_class.change_control(action) self.ego_vehicle.apply_control(action) 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
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._running = True while self._running: time.sleep(0.5) 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 _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 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