示例#1
0
class ScenarioManager(object):
    """
    Basic scenario manager class. This class holds all functionality
    required to start, and analyze a scenario.

    The user must not modify this class.

    To use the ScenarioManager:
    1. Create an object via manager = ScenarioManager()
    2. Load a scenario via manager.load_scenario()
    3. Trigger the execution of the scenario manager.run_scenario()
       This function is designed to explicitly control start and end of
       the scenario execution
    4. Trigger a result evaluation with manager.analyze_scenario()
    5. If needed, cleanup with manager.stop_scenario()
    """
    def __init__(self,
                 debug_mode=False,
                 sync_mode=False,
                 challenge_mode=False,
                 track=None,
                 timeout=10.0,
                 episode_max_time=10000):
        """
        Setups up the parameters, which will be filled at load_scenario()
        """
        self.scenario = None
        self.scenario_tree = None
        self.scenario_class = None
        self.ego_vehicles = None
        self.other_actors = None

        self._debug_mode = debug_mode
        self._challenge_mode = challenge_mode
        self._track = track
        self._agent = None
        self._sync_mode = sync_mode
        self._running = False
        self._timestamp_last_run = 0.0
        self._timeout = timeout
        self._watchdog = Watchdog(float(self._timeout))

        self.scenario_duration_system = 0.0
        self.scenario_duration_game = 0.0
        self.start_system_time = None
        self.end_system_time = None

        # modification: disable when running multi-process via dask
        # Register the scenario tick as callback for the CARLA world
        # Use the callback_id inside the signal handler to allow external interrupts
        # signal.signal(signal.SIGINT, self._signal_handler)

        self.episode_max_time = episode_max_time

    def _signal_handler(self, signum, frame):
        """
        Terminate scenario ticking when receiving a signal interrupt
        """
        self._running = False
        if not self.get_running_status():
            raise RuntimeError("Timeout occured during scenario execution")

    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
        GameTime.restart()

    def load_scenario(self, scenario, agent=None):
        """
        Load a new scenario
        """
        self._reset()
        self._agent = AgentWrapper(agent,
                                   self._challenge_mode) if agent else None
        if self._agent is not None:
            self._sync_mode = True
        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

        # modification registration now moves to carla_data_provider
        # CarlaDataProvider.register_actors(self.ego_vehicles)
        # CarlaDataProvider.register_actors(self.other_actors)
        # To print the scenario tree uncomment the next line
        # py_trees.display.render_dot_tree(self.scenario_tree)

        if self._agent is not None:
            self._agent.setup_sensors(self.ego_vehicles[0], self._debug_mode,
                                      self._track)

    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 _console_message(self):
        """
        Message that will be displayed via console
        """
        def get_symbol(value, desired_value, high=True):
            """
            Returns a tick or a cross depending on the values
            """
            tick = '\033[92m' + 'O' + '\033[0m'
            cross = '\033[91m' + 'X' + '\033[0m'

            multiplier = 1 if high else -1

            if multiplier * value >= desired_value:
                symbol = tick
            else:
                symbol = cross

            return symbol

        if self.scenario_tree.status == py_trees.common.Status.RUNNING:
            # If still running, all the following is None, so no point continuing
            print(
                "\n> Something happened during the simulation. Was it manually shutdown?\n"
            )
            return

        blackv = py_trees.blackboard.Blackboard()
        route_completed = blackv.get("RouteCompletion")
        collisions = blackv.get("Collision")
        outside_route_lanes = blackv.get("OutsideRouteLanes")
        stop_signs = blackv.get("RunningStop")
        red_light = blackv.get("RunningRedLight")
        in_route = blackv.get("InRoute")

        # addition: new event
        on_side_walk = blackv.get("OnSideWalk")
        off_road = blackv.get("OffRoad")
        wrong_lane = blackv.get("WrongLane")

        # If something failed, stop
        if [
                x for x in (collisions, outside_route_lanes, stop_signs,
                            red_light, in_route) if x is None
        ]:
            print("Nothing to analyze, this scenario has no criteria")
            return

        if blackv.get("RouteCompletion") >= 99:
            route_completed = 100
        else:
            route_completed = blackv.get("RouteCompletion")
        outside_route_lanes = float(outside_route_lanes)

        route_symbol = get_symbol(route_completed, 100, True)
        collision_symbol = get_symbol(collisions, 0, False)
        outside_symbol = get_symbol(outside_route_lanes, 0, False)
        red_light_symbol = get_symbol(red_light, 0, False)
        stop_symbol = get_symbol(stop_signs, 0, False)

        # addition: new event
        on_side_walk_symbol = get_symbol(on_side_walk, 0, False)
        off_road_symbol = get_symbol(off_road, 0, False)
        wrong_lane_symbol = get_symbol(wrong_lane, 0, False)

        if self.scenario_tree.status == py_trees.common.Status.FAILURE:
            if not in_route:
                message = "> FAILED: The actor deviated from the route"
            else:
                message = "> FAILED: The actor didn't finish the route"
        elif self.scenario_tree.status == py_trees.common.Status.SUCCESS:
            if route_completed == 100:
                message = "> SUCCESS: Congratulations, route finished! "
            else:
                message = "> FAILED: The actor timed out "
        else:  # This should never be triggered
            return

        if self.scenario_tree.status != py_trees.common.Status.RUNNING:
            print("\n" + message)
            print("> ")
            print("> Score: ")
            print("> - Route Completed [{}]:      {}%".format(
                route_symbol, route_completed))
            print("> - Outside route lanes [{}]:  {}%".format(
                outside_symbol, outside_route_lanes))
            print("> - Collisions [{}]:           {} times".format(
                collision_symbol, collisions))
            print("> - Red lights run [{}]:       {} times".format(
                red_light_symbol, red_light))
            print("> - Stop signs run [{}]:       {} times".format(
                stop_symbol, stop_signs))

            # addition: new event
            print("> - On side walk [{}]:         {} times".format(
                on_side_walk_symbol, on_side_walk))
            print("> - Off road [{}]:         {} times".format(
                off_road_symbol, off_road))
            print("> - Wrong lane [{}]:           {} times\n".format(
                wrong_lane_symbol, wrong_lane))

    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 get_running_status(self):
        """
        returns:
           bool: False if watchdog exception occured, True otherwise
        """
        return self._watchdog.get_status()

    def stop_scenario(self):
        """
        This function triggers a proper termination of a scenario
        """

        if self.scenario is not None:
            self.scenario.terminate()

        if self._agent is not None:
            self._agent.cleanup()
            self._agent = None

        CarlaDataProvider.cleanup()
示例#2
0
class ScenarioManager(object):

    """
    Basic scenario manager class. This class holds all functionality
    required to start, and analyze a scenario.

    The user must not modify this class.

    To use the ScenarioManager:
    1. Create an object via manager = ScenarioManager()
    2. Load a scenario via manager.load_scenario()
    3. Trigger the execution of the scenario manager.execute()
       This function is designed to explicitly control start and end of
       the scenario execution
    4. Trigger a result evaluation with manager.analyze()
    5. Cleanup with manager.stop_scenario()
    """

    def __init__(self, debug_mode=False, challenge_mode=False, track=None, timeout=20.0):
        """
        Init requires scenario as input
        """
        self.scenario = None
        self.scenario_tree = None
        self.scenario_class = None
        self.ego_vehicles = None
        self.other_actors = None

        self._debug_mode = debug_mode
        self._challenge_mode = challenge_mode
        self._track = track
        self._agent = None
        self._running = False
        self._timestamp_last_run = 0.0
        self._timeout = timeout
        self._watchdog = Watchdog(float(self._timeout))

        self.scenario_duration_system = 0.0
        self.scenario_duration_game = 0.0
        self.start_system_time = None
        self.end_system_time = None
        nvidia_smi.nvmlInit()
        self.handle = nvidia_smi.nvmlDeviceGetHandleByIndex(1)

        # Register the scenario tick as callback for the CARLA world
        # Use the callback_id inside the signal handler to allow external interrupts
        signal.signal(signal.SIGINT, self._signal_handler)

    def _signal_handler(self, signum, frame):
        """
        Terminate scenario ticking when receiving a signal interrupt
        """
        self._running = False
        if not self.get_running_status():
            raise RuntimeError("Timeout occured during scenario execution")

    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
        GameTime.restart()

    def load_scenario(self, scenario, agent=None):
        """
        Load a new scenario
        """
        self._reset()
        self._agent = AgentWrapper(agent, self._challenge_mode) if agent else None
        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
        #print(self.other_actors)
        CarlaDataProvider.register_actors(self.ego_vehicles)
        CarlaDataProvider.register_actors(self.other_actors)
        # To print the scenario tree uncomment the next line
        # py_trees.display.render_dot_tree(self.scenario_tree)

        if self._agent is not None:
            self._agent.setup_sensors(self.ego_vehicles[0], self._debug_mode, self._track)

    def run_scenario(self,path,path1):
        """
        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()
        total_risk_score = []
        self._watchdog.start()
        self._running = True

        x = 0
        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)
            x+=1
            if(x%60==1):
                res = nvidia_smi.nvmlDeviceGetUtilizationRates(self.handle)
                mem_res = nvidia_smi.nvmlDeviceGetMemoryInfo(self.handle)
                #print(f'mem: {mem_res.used / (1024**2)} (GiB)') # usage in GiB
                print(f'mem: {100 * (mem_res.used / mem_res.total):.3f}%') # percentage usage
                cpu = psutil.cpu_percent()#cpu utilization stats
                mem = psutil.virtual_memory()#virtual memory stats
                print(f'gpu: {res.gpu}%, gpu-mem: {res.memory}%')

                fields = ['GPU Utilization',
                            'GPU Memory',
                            'CPU Utilization',
                            'CPU Memory'
                        ]

                dict = [{'GPU Utilization':res.gpu, 'GPU Memory':100 * (mem_res.used / mem_res.total), 'CPU Utilization':cpu, 'CPU Memory':100 * (mem.used/mem.total)}]


                file_exists = os.path.isfile(path1)
                with open(path1, 'a') as csvfile:
                    # creating a csv dict writer object
                    writer = csv.DictWriter(csvfile, fieldnames = fields)
                    if not file_exists:
                        writer.writeheader()
                    writer.writerows(dict)


                #total_risk_score.append(risk_score)
        #print("--------------------------------------------------------------------------")
        #print("Average Risk Score:%f"%(float(sum(total_risk_score))/len(total_risk_score)))
        #print("--------------------------------------------------------------------------")

        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

        fields = ['Route Completed',
                    'Collisions'
                ]

        route_completed, collisions = self._console_message()

        dict = [{'Route Completed':route_completed, 'Collisions':collisions}]


        file_exists = os.path.isfile(path)
        with open(path, 'a') as csvfile:
            # creating a csv dict writer object
            writer = csv.DictWriter(csvfile, fieldnames = fields)
            if not file_exists:
                writer.writeheader()
            writer.writerows(dict)

    def _console_message(self):
        """
        Message that will be displayed via console
        """
        def get_symbol(value, desired_value, high=True):
            """
            Returns a tick or a cross depending on the values
            """
            tick = '\033[92m'+'O'+'\033[0m'
            cross = '\033[91m'+'X'+'\033[0m'

            multiplier = 1 if high else -1

            if multiplier*value >= desired_value:
                symbol = tick
            else:
                symbol = cross

            return symbol

        if self.scenario_tree.status == py_trees.common.Status.RUNNING:
            # If still running, all the following is None, so no point continuing
            print("\n> Something happened during the simulation. Was it manually shutdown?\n")
            return

        blackv = py_trees.blackboard.Blackboard()
        route_completed = blackv.get("RouteCompletion")
        collisions = blackv.get("Collision")
        outside_route_lanes = blackv.get("OutsideRouteLanes")
        stop_signs = blackv.get("RunningStop")
        red_light = blackv.get("RunningRedLight")
        in_route = blackv.get("InRoute")

        # If something failed, stop
        if [x for x in (collisions, outside_route_lanes, stop_signs, red_light, in_route) if x is None]:
            return

        if blackv.get("RouteCompletion") >= 99:
            route_completed = 100
        else:
            route_completed = blackv.get("RouteCompletion")
        outside_route_lanes = float(outside_route_lanes)

        route_symbol = get_symbol(route_completed, 100, True)
        collision_symbol = get_symbol(collisions, 0, False)
        outside_symbol = get_symbol(outside_route_lanes, 0, False)
        red_light_symbol = get_symbol(red_light, 0, False)
        stop_symbol = get_symbol(stop_signs, 0, False)

        if self.scenario_tree.status == py_trees.common.Status.FAILURE:
            if not in_route:
                message = "> FAILED: The actor deviated from the route"
            else:
                message = "> FAILED: The actor didn't finish the route"
        elif self.scenario_tree.status == py_trees.common.Status.SUCCESS:
            if route_completed == 100:
                message = "> SUCCESS: Congratulations, route finished! "
            else:
                message = "> FAILED: The actor timed out "
        else: # This should never be triggered
            return

        if self.scenario_tree.status != py_trees.common.Status.RUNNING:
            print("\n" + message)
            print("> ")
            print("> Score: ")
            print("> - Route Completed [{}]:      {}%".format(route_symbol, route_completed))
            #print("> - Outside route lanes [{}]:  {}%".format(outside_symbol, outside_route_lanes))
            print("> - Collisions [{}]:           {} times".format(collision_symbol, math.floor(collisions)))
            #print("> - Red lights run [{}]:       {} times".format(red_light_symbol, red_light))
            #print("> - Stop signs run [{}]:       {} times\n".format(stop_symbol, stop_signs))

        return route_completed, collisions

    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()

        #return risk_score

    def get_running_status(self):
        """
        returns:
           bool: False if watchdog exception occured, True otherwise
        """
        return self._watchdog.get_status()

    def stop_scenario(self):
        """
        This function triggers a proper termination of a scenario
        """

        if self.scenario is not None:
            self.scenario.terminate()

        if self._agent is not None:
            self._agent.cleanup()
            self._agent = None

        CarlaDataProvider.cleanup()
        CarlaActorPool.cleanup()
class ScenarioManager(object):

    """
    Basic scenario manager class. This class holds all functionality
    required to start, run and stop a scenario.

    The user must not modify this class.

    To use the ScenarioManager:
    1. Create an object via manager = ScenarioManager()
    2. Load a scenario via manager.load_scenario()
    3. Trigger the execution of the scenario manager.run_scenario()
       This function is designed to explicitly control start and end of
       the scenario execution
    4. If needed, cleanup with manager.stop_scenario()
    """


    def __init__(self, timeout, debug_mode=False, timely_commands=False, frame_rate=20):
        """
        Setups up the parameters, which will be filled at load_scenario()
        """
        self.timely_commands = timely_commands
        self.frame_rate = frame_rate
        self.scenario = None
        self.scenario_tree = None
        self.scenario_class = None
        self.ego_vehicles = None
        self.other_actors = None

        self._debug_mode = debug_mode
        self._agent = None
        self._running = False
        self._timestamp_last_run = 0.0
        self._timeout = float(timeout)

        # Used to detect if the simulation is down
        watchdog_timeout = max(5, self._timeout - 2)
        self._watchdog = Watchdog(watchdog_timeout)

        # Avoid the agent from freezing the simulation
        agent_timeout = watchdog_timeout - 1
        self._agent_watchdog = Watchdog(agent_timeout)

        self.scenario_duration_system = 0.0
        self.scenario_duration_game = 0.0
        self.start_system_time = None
        self.end_system_time = None
        self.end_game_time = None

        # Register the scenario tick as callback for the CARLA world
        # Use the callback_id inside the signal handler to allow external interrupts
        signal.signal(signal.SIGINT, self.signal_handler)

        self._delayed_cmds = []
        brake_control = carla.VehicleControl()
        brake_control.throttle = 0
        brake_control.brake = 1
        brake_control.steer = 0
        brake_control.reverse = False
        brake_control.hand_brake = False
        brake_control.manual_gear_shift = False
        self._last_cmd = (0, 0, brake_control)

    def signal_handler(self, signum, frame):
        """
        Terminate scenario ticking when receiving a signal interrupt
        """
        self._running = False

    def cleanup(self):
        """
        Reset all parameters
        """
        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
        self.end_game_time = None

    def load_scenario(self, scenario, agent, rep_number):
        """
        Load a new scenario
        """

        GameTime.restart()
        agent.frame_delta_ms = int(1000 / self.frame_rate)
        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 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()

        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)

    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()

            game_time_ms = int(GameTime.get_time() * 1000)
            try:
                res = self._agent()
                if self.timely_commands:
                    assert isinstance(res, tuple), 'Agent did not return the runtime'
                    ego_action, runtime = res
                else:
                    if isinstance(res, tuple):
                        ego_action = res[0]
                    else:
                        ego_action = res
                    runtime = 0
                heapq.heappush(self._delayed_cmds,
                               (game_time_ms + runtime, game_time_ms, ego_action))

            # 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)

            while len(self._delayed_cmds) > 0 and game_time_ms >= self._delayed_cmds[0][0]:
                self._last_cmd = self._delayed_cmds[0]
                heapq.heappop(self._delayed_cmds)

            print("Command at {} using {}".format(self._last_cmd[0], self._last_cmd[1]))
            self.ego_vehicles[0].apply_control(self._last_cmd[2])

            # 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 get_running_status(self):
        """
        returns:
           bool: False if watchdog exception occured, True otherwise
        """
        return self._watchdog.get_status()

    def stop_scenario(self):
        """
        This function triggers a proper termination of a scenario
        """
        self._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 analyze_scenario(self):
        """
        Analyzes and prints the results of the route
        """
        global_result = '\033[92m'+'SUCCESS'+'\033[0m'

        for criterion in self.scenario.get_criteria():
            if criterion.test_status != "SUCCESS":
                global_result = '\033[91m'+'FAILURE'+'\033[0m'

        if self.scenario.timeout_node.timeout:
            global_result = '\033[91m'+'FAILURE'+'\033[0m'

        ResultOutputProvider(self, global_result)
示例#4
0
class ScenarioManager(object):
    """
    Basic scenario manager class. This class holds all functionality
    required to start, run and stop a scenario.

    The user must not modify this class.

    To use the ScenarioManager:
    1. Create an object via manager = ScenarioManager()
    2. Load a scenario via manager.load_scenario()
    3. Trigger the execution of the scenario manager.run_scenario()
       This function is designed to explicitly control start and end of
       the scenario execution
    4. If needed, cleanup with manager.stop_scenario()
    """
    def __init__(self, timeout, debug_mode=False):
        """
        Setups up the parameters, which will be filled at load_scenario()
        """
        self.scenario = None
        self.scenario_tree = None
        self.scenario_class = None
        self.ego_vehicles = None
        self.other_actors = None

        self._debug_mode = debug_mode
        self._agent = None
        self._running = False
        self._timestamp_last_run = 0.0
        self._timeout = float(timeout)

        self._watchdog = Watchdog(
            self._timeout)  # Detects if the simulation is down
        self._agent_watchdog = Watchdog(
            self._timeout)  # Stop the agent from freezing the simulation

        self.scenario_duration_system = 0.0
        self.scenario_duration_game = 0.0
        self.start_system_time = None
        self.end_system_time = None
        self.end_game_time = None

        # Register the scenario tick as callback for the CARLA world
        # Use the callback_id inside the signal handler to allow external interrupts
        signal.signal(signal.SIGINT, self.signal_handler)

    def signal_handler(self, signum, frame):
        """
        Terminate scenario ticking when receiving a signal interrupt
        """
        if self._agent_watchdog and not self._agent_watchdog.get_status():
            raise RuntimeError(
                "Agent took longer than {}s to send its command".format(
                    self._timeout))
        elif self._watchdog and not self._watchdog.get_status():
            raise RuntimeError(
                "The simulation took longer than {}s to update".format(
                    self._timeout))
        self._running = False

    def cleanup(self):
        """
        Reset all parameters
        """
        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
        self.end_game_time = None

        self._spectator = None

    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

        self._spectator = CarlaDataProvider.get_world().get_spectator()

        # 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 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()

        self._watchdog.start()
        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 _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

            ego_trans = self.ego_vehicles[0].get_transform()
            self._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 get_running_status(self):
        """
        returns:
           bool: False if watchdog exception occured, True otherwise
        """
        return self._watchdog.get_status()

    def stop_scenario(self):
        """
        This function triggers a proper termination of a scenario
        """
        self._watchdog.stop()
        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 analyze_scenario(self):
        """
        Analyzes and prints the results of the route
        """
        global_result = '\033[92m' + 'SUCCESS' + '\033[0m'

        for criterion in self.scenario.get_criteria():
            if criterion.test_status != "SUCCESS":
                global_result = '\033[91m' + 'FAILURE' + '\033[0m'

        if self.scenario.timeout_node.timeout:
            global_result = '\033[91m' + 'FAILURE' + '\033[0m'

        ResultOutputProvider(self, global_result)