diff --git a/Co-Simulation/Sumo/run_synchronization.py b/Co-Simulation/Sumo/run_synchronization.py index c7e065350..e990d7b67 100644 --- a/Co-Simulation/Sumo/run_synchronization.py +++ b/Co-Simulation/Sumo/run_synchronization.py @@ -5,7 +5,6 @@ # # This work is licensed under the terms of the MIT license. # For a copy, see . - """ Script to integrate CARLA and SUMO simulations """ @@ -27,10 +26,10 @@ import os import sys try: - sys.path.append(glob.glob('../../PythonAPI/carla/dist/carla-*%d.%d-%s.egg' % ( - sys.version_info.major, - sys.version_info.minor, - 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) + sys.path.append( + glob.glob('../../PythonAPI/carla/dist/carla-*%d.%d-%s.egg' % + (sys.version_info.major, sys.version_info.minor, + 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) except IndexError: pass @@ -62,13 +61,17 @@ class SimulationSynchronization(object): SimulationSynchronization class is responsible for the synchronization of sumo and carla simulations. """ - def __init__(self, args): self.args = args self.sumo = SumoSimulation(args) self.carla = CarlaSimulation(args) + if args.tls_manager == 'carla': + self.sumo.switch_off_traffic_lights() + elif args.tls_manager == 'sumo': + self.carla.switch_off_traffic_lights() + # Mapped actor ids. self.sumo2carla_ids = {} # Contains only actors controlled by sumo. self.carla2sumo_ids = {} # Contains only actors controlled by carla. @@ -94,8 +97,8 @@ class SimulationSynchronization(object): carla_blueprint = BridgeHelper.get_carla_blueprint(sumo_actor, self.args.sync_vehicle_color) if carla_blueprint is not None: - carla_transform = BridgeHelper.get_carla_transform( - sumo_actor.transform, sumo_actor.extent) + carla_transform = BridgeHelper.get_carla_transform(sumo_actor.transform, + sumo_actor.extent) carla_actor_id = self.carla.spawn_actor(carla_blueprint, carla_transform) if carla_actor_id != INVALID_ACTOR_ID: @@ -118,13 +121,22 @@ class SimulationSynchronization(object): carla_transform = BridgeHelper.get_carla_transform(sumo_actor.transform, sumo_actor.extent) if self.args.sync_vehicle_lights: - carla_lights = BridgeHelper.get_carla_lights_state( - carla_actor.get_light_state(), sumo_actor.signals) + carla_lights = BridgeHelper.get_carla_lights_state(carla_actor.get_light_state(), + sumo_actor.signals) else: carla_lights = None self.carla.synchronize_vehicle(carla_actor_id, carla_transform, carla_lights) + # Updates traffic lights in carla based on sumo information. + if self.args.tls_manager == 'sumo': + common_landmarks = self.sumo.traffic_light_ids & self.carla.traffic_light_ids + for landmark_id in common_landmarks: + sumo_tl_state = self.sumo.get_traffic_light_state(landmark_id) + carla_tl_state = BridgeHelper.get_carla_traffic_light_state(sumo_tl_state) + + self.carla.synchronize_traffic_light(landmark_id, carla_tl_state) + # ----------------- # carla-->sumo sync # ----------------- @@ -159,8 +171,8 @@ class SimulationSynchronization(object): if self.args.sync_vehicle_lights: carla_lights = self.carla.get_actor_light_state(carla_actor_id) if carla_lights is not None: - sumo_lights = BridgeHelper.get_sumo_lights_state( - sumo_actor.signals, carla_lights) + sumo_lights = BridgeHelper.get_sumo_lights_state(sumo_actor.signals, + carla_lights) else: sumo_lights = None else: @@ -168,6 +180,16 @@ class SimulationSynchronization(object): self.sumo.synchronize_vehicle(sumo_actor_id, sumo_transform, sumo_lights) + # Updates traffic lights in sumo based on carla information. + if self.args.tls_manager == 'carla': + common_landmarks = self.sumo.traffic_light_ids & self.carla.traffic_light_ids + for landmark_id in common_landmarks: + carla_tl_state = self.carla.get_traffic_light_state(landmark_id) + sumo_tl_state = BridgeHelper.get_sumo_traffic_light_state(carla_tl_state) + + # Updates all the sumo links related to this landmark. + self.sumo.synchronize_traffic_light(landmark_id, sumo_tl_state) + def close(self): """ Cleans up synchronization. @@ -185,7 +207,8 @@ class SimulationSynchronization(object): for sumo_actor_id in self.carla2sumo_ids.values(): self.sumo.destroy_actor(sumo_actor_id) - # Closing sumo client. + # Closing sumo and carla client. + self.carla.close() self.sumo.close() @@ -252,13 +275,18 @@ if __name__ == '__main__': argparser.add_argument('--sync-vehicle-color', action='store_true', help='synchronize vehicle color (default: False)') - argparser.add_argument('--sync-all', + argparser.add_argument('--sync-vehicle-all', action='store_true', help='synchronize all vehicle properties (default: False)') + argparser.add_argument('--tls-manager', + type=str, + choices=['none', 'sumo', 'carla'], + help="select traffic light manager (default: none)", + default='none') argparser.add_argument('--debug', action='store_true', help='enable debug messages') arguments = argparser.parse_args() - if arguments.sync_all is True: + if arguments.sync_vehicle_all is True: arguments.sync_vehicle_lights = True arguments.sync_vehicle_color = True diff --git a/Co-Simulation/Sumo/sumo_integration/bridge_helper.py b/Co-Simulation/Sumo/sumo_integration/bridge_helper.py index 4485d56e3..8a2a907e5 100644 --- a/Co-Simulation/Sumo/sumo_integration/bridge_helper.py +++ b/Co-Simulation/Sumo/sumo_integration/bridge_helper.py @@ -5,7 +5,6 @@ # # This work is licensed under the terms of the MIT license. # For a copy, see . - """ This module provides a helper for the co-simulation between sumo and carla .""" # ================================================================================================== @@ -20,7 +19,7 @@ import random import carla # pylint: disable=import-error import traci # pylint: disable=import-error -from .sumo_simulation import SumoVehSignal +from .sumo_simulation import SumoSignalState, SumoVehSignal # ================================================================================================== # -- Bridge helper (SUMO <=> CARLA) ---------------------------------------------------------------- @@ -126,11 +125,11 @@ class BridgeHelper(object): blueprint = BridgeHelper._get_recommended_carla_blueprint(sumo_actor) if blueprint is not None: logging.warning( - 'sumo vtype %s not found in carla. The following blueprint will be used: %s' - , type_id, blueprint.id) + 'sumo vtype %s not found in carla. The following blueprint will be used: %s', + type_id, blueprint.id) else: - logging.error( - 'sumo vtype %s not supported. No vehicle will be spawned in carla', type_id) + logging.error('sumo vtype %s not supported. No vehicle will be spawned in carla', + type_id) return None if blueprint.has_attribute('color'): @@ -327,3 +326,41 @@ class BridgeHelper(object): current_lights ^= SumoVehSignal.BACKDRIVE return current_lights + + @staticmethod + def get_carla_traffic_light_state(sumo_tl_state): + """ + Returns carla traffic light state based on sumo traffic light state. + """ + if sumo_tl_state == SumoSignalState.RED or sumo_tl_state == SumoSignalState.RED_YELLOW: + return carla.TrafficLightState.Red + + elif sumo_tl_state == SumoSignalState.YELLOW: + return carla.TrafficLightState.Yellow + + elif sumo_tl_state == SumoSignalState.GREEN or \ + sumo_tl_state == SumoSignalState.GREEN_WITHOUT_PRIORITY: + return carla.TrafficLightState.Green + + elif sumo_tl_state == SumoSignalState.OFF: + return carla.TrafficLightState.Off + + else: # SumoSignalState.GREEN_RIGHT_TURN and SumoSignalState.OFF_BLINKING + return carla.TrafficLightState.Unknown + + @staticmethod + def get_sumo_traffic_light_state(carla_tl_state): + """ + Returns sumo traffic light state based on carla traffic light state. + """ + if carla_tl_state == carla.TrafficLightState.Red: + return SumoSignalState.RED + + elif carla_tl_state == carla.TrafficLightState.Yellow: + return SumoSignalState.YELLOW + + elif carla_tl_state == carla.TrafficLightState.Green: + return SumoSignalState.GREEN + + else: # carla.TrafficLightState.Off and carla.TrafficLightState.Unknown + return SumoSignalState.OFF diff --git a/Co-Simulation/Sumo/sumo_integration/carla_simulation.py b/Co-Simulation/Sumo/sumo_integration/carla_simulation.py index a2a91fd6f..5d4f9fa0f 100644 --- a/Co-Simulation/Sumo/sumo_integration/carla_simulation.py +++ b/Co-Simulation/Sumo/sumo_integration/carla_simulation.py @@ -5,7 +5,6 @@ # # This work is licensed under the terms of the MIT license. # For a copy, see . - """ This module is responsible for the management of the carla simulation. """ # ================================================================================================== @@ -27,7 +26,6 @@ class CarlaSimulation(object): """ CarlaSimulation is responsible for the management of the carla simulation. """ - def __init__(self, args): self.args = args host = args.carla_host @@ -50,6 +48,34 @@ class CarlaSimulation(object): self.spawned_actors = set() self.destroyed_actors = set() + # This is a temporal workaround to avoid the issue of retrieving traffic lights from + # landmarks. + # Set traffic lights. + self._tls = {} # {landmark_id: traffic_ligth_actor} + + self._location = { + '121': carla.Location(42.02934082, 101.56253906, 0.0), + '123': carla.Location(46.10708984, 92.04954102, 0.15238953), + + '130': carla.Location(101.69419922, 61.59494141, 0.0), + '129': carla.Location(92.17053711, 57.36221191, 0.0), + + '136': carla.Location(61.48416016, 50.7382666, 0.0), + '135': carla.Location(57.23968262, 59.23875977, 0.15238953) + } + for carla_actor in self.world.get_actors(): + for landmark_id, landmark_location in self._location.items(): + if carla_actor.get_location().distance(landmark_location) < 0.1: + self._tls[landmark_id] = carla_actor + + # for landmark in self.world.get_map().get_all_landmarks_of_type('1000001'): + # if landmark.id != '': + # traffic_ligth = self.world.get_traffic_light(landmark) + # if traffic_ligth is not None: + # self._tls[landmark.id] = traffic_ligth + # else: + # logging.warning('Landmark %s is not linked to any traffic light', landmark.id) + def get_actor(self, actor_id): """ Accessor for carla actor. @@ -71,6 +97,31 @@ class CarlaSimulation(object): except RuntimeError: return None + @property + def traffic_light_ids(self): + return set(self._tls.keys()) + + def get_traffic_light_state(self, landmark_id): + """ + Accessor for traffic light state. + + If the traffic ligth does not exist, returns None. + """ + if landmark_id not in self._tls: + return None + return self._tls[landmark_id].state + + def switch_off_traffic_lights(self): + """ + Switch off all traffic lights. + """ + for actor in self.world.get_actors(): + if actor.type_id == 'traffic.traffic_light': + actor.freeze(True) + # We set the traffic light to 'green' because 'off' state sets the traffic light to + # 'red'. + actor.set_state(carla.TrafficLightState.Green) + def spawn_actor(self, blueprint, transform): """ Spawns a new actor. @@ -79,13 +130,12 @@ class CarlaSimulation(object): :param transform: transform where the actor will be spawned. :return: actor id if the actor is successfully spawned. Otherwise, INVALID_ACTOR_ID. """ - transform = carla.Transform( - transform.location + carla.Location(0, 0, SPAWN_OFFSET_Z), - transform.rotation) + transform = carla.Transform(transform.location + carla.Location(0, 0, SPAWN_OFFSET_Z), + transform.rotation) batch = [ - carla.command.SpawnActor(blueprint, transform) - .then(carla.command.SetSimulatePhysics(carla.command.FutureActor, False)) + carla.command.SpawnActor(blueprint, transform).then( + carla.command.SetSimulatePhysics(carla.command.FutureActor, False)) ] response = self.client.apply_batch_sync(batch, False)[0] if response.error: @@ -121,6 +171,22 @@ class CarlaSimulation(object): vehicle.set_light_state(carla.VehicleLightState(lights)) return True + def synchronize_traffic_light(self, landmark_id, state): + """ + Updates traffic light state. + + :param landmark_id: id of the landmark to be updated. + :param state: new traffic light state. + :return: True if successfully updated. Otherwise, False. + """ + if not landmark_id in self._tls: + logging.warning('Landmark %s not found in carla', landmark_id) + return False + + traffic_light = self._tls[landmark_id] + traffic_light.set_state(state) + return True + def tick(self): """ Tick to carla simulation. @@ -128,9 +194,16 @@ class CarlaSimulation(object): self.world.tick() # Update data structures for the current frame. - current_actors = set([ - vehicle.id for vehicle in self.world.get_actors().filter('vehicle.*') - ]) + current_actors = set( + [vehicle.id for vehicle in self.world.get_actors().filter('vehicle.*')]) self.spawned_actors = current_actors.difference(self._active_actors) self.destroyed_actors = self._active_actors.difference(current_actors) self._active_actors = current_actors + + def close(self): + """ + Closes carla client. + """ + for actor in self.world.get_actors(): + if actor.type_id == 'traffic.traffic_light': + actor.freeze(False) diff --git a/Co-Simulation/Sumo/sumo_integration/constants.py b/Co-Simulation/Sumo/sumo_integration/constants.py index 2c88a3385..52112e899 100644 --- a/Co-Simulation/Sumo/sumo_integration/constants.py +++ b/Co-Simulation/Sumo/sumo_integration/constants.py @@ -5,7 +5,6 @@ # # This work is licensed under the terms of the MIT license. # For a copy, see . - """ This module defines constants used for the sumo-carla co-simulation. """ # ================================================================================================== diff --git a/Co-Simulation/Sumo/sumo_integration/sumo_simulation.py b/Co-Simulation/Sumo/sumo_integration/sumo_simulation.py index 785daa69c..093fb7e67 100644 --- a/Co-Simulation/Sumo/sumo_integration/sumo_simulation.py +++ b/Co-Simulation/Sumo/sumo_integration/sumo_simulation.py @@ -5,7 +5,6 @@ # # This work is licensed under the terms of the MIT license. # For a copy, see . - """ This module is responsible for the management of the sumo simulation. """ # ================================================================================================== @@ -27,6 +26,21 @@ from .constants import INVALID_ACTOR_ID # ================================================================================================== +# https://sumo.dlr.de/docs/Simulation/Traffic_Lights.html#signal_state_definitions +class SumoSignalState(object): + """ + SumoSignalState contains the different traffic light states. + """ + RED = 'r' + YELLOW = 'y' + GREEN = 'G' + GREEN_WITHOUT_PRIORITY = 'g' + GREEN_RIGHT_TURN = 's' + RED_YELLOW = 'u' + OFF_BLINKING = 'o' + OFF = 'O' + + # https://sumo.dlr.de/docs/TraCI/Vehicle_Signalling.html class SumoVehSignal(object): """ @@ -82,19 +96,191 @@ class SumoActorClass(enum.Enum): CUSTOM2 = "custom2" -SumoActor = collections.namedtuple( - 'SumoActor', 'type_id vclass transform signals extent color') +SumoActor = collections.namedtuple('SumoActor', 'type_id vclass transform signals extent color') # ================================================================================================== # -- sumo simulation ------------------------------------------------------------------------------- # ================================================================================================== +class SumoTLLogic(object): + """ + SumoTLLogic holds the data relative to a traffic light in sumo. + """ + def __init__(self, tlid, states, parameters): + self.tlid = tlid + self.states = states + + self._landmark2link = {} + self._link2landmark = {} + for link_index, landmark_id in parameters.items(): + # Link index information is added in the parameter as 'linkSignalID:x' + link_index = int(link_index.split(':')[1]) + + if landmark_id not in self._landmark2link: + self._landmark2link[landmark_id] = [] + self._landmark2link[landmark_id].append((tlid, link_index)) + self._link2landmark[(tlid, link_index)] = landmark_id + + def get_number_signals(self): + """ + Returns number of internal signals of the traffic light. + """ + if len(self.states) > 0: + return len(self.states[0]) + return 0 + + def get_all_signals(self): + """ + Returns all the signals of the traffic ligth. + :returns list: [(tlid, link_index), (tlid, link_index), ...] + """ + return [(self.tlid, i) for i in range(self.get_number_signals())] + + def get_all_landmarks(self): + """ + Returns all the landmarks associated with this traffic light. + """ + return self._landmark2link.keys() + + def get_associated_signals(self, landmark_id): + """ + Returns all the signals associated with the given landmark. + :returns list: [(tlid, link_index), (tlid), (link_index), ...] + """ + return self._landmark2link.get(landmark_id, []) + + +class SumoTLManager(object): + """ + SumoTLManager is responsible for the management of the sumo traffic lights (i.e., keeps control + of the current program, phase, ...) + """ + def __init__(self): + self._tls = {} # {tlid: {program_id: SumoTLLogic} + self._current_program = {} # {tlid: program_id} + self._current_phase = {} # {tlid: index_phase} + + for tlid in traci.trafficlight.getIDList(): + self.subscribe(tlid) + + self._tls[tlid] = {} + for tllogic in traci.trafficlight.getAllProgramLogics(tlid): + states = [phase.state for phase in tllogic.getPhases()] + parameters = tllogic.getParameters() + tl = SumoTLLogic(tlid, states, parameters) + self._tls[tlid][tllogic.programID] = tl + + # Get current status of the traffic lights. + self._current_program[tlid] = traci.trafficlight.getProgram(tlid) + self._current_phase[tlid] = traci.trafficlight.getPhase(tlid) + + self._off = False + + @staticmethod + def subscribe(tlid): + """ + Subscribe the given traffic ligth to the following variables: + + * Current program. + * Current phase. + """ + traci.trafficlight.subscribe(tlid, [ + traci.constants.TL_CURRENT_PROGRAM, + traci.constants.TL_CURRENT_PHASE, + ]) + + @staticmethod + def unsubscribe(tlid): + """ + Unsubscribe the given traffic ligth from receiving updated information each step. + """ + traci.trafficlight.unsubscribe(tlid) + + def get_all_signals(self): + """ + Returns all the traffic light signals. + """ + signals = set() + for tlid, program_id in self._current_program.items(): + signals.update(self._tls[tlid][program_id].get_all_signals()) + return signals + + def get_all_landmarks(self): + """ + Returns all the landmarks associated with a traffic light in the simulation. + """ + landmarks = set() + for tlid, program_id in self._current_program.items(): + landmarks.update(self._tls[tlid][program_id].get_all_landmarks()) + return landmarks + + def get_all_associated_signals(self, landmark_id): + """ + Returns all the signals associated with the given landmark. + :returns list: [(tlid, link_index), (tlid), (link_index), ...] + """ + signals = set() + for tlid, program_id in self._current_program.items(): + signals.update(self._tls[tlid][program_id].get_associated_signals(landmark_id)) + return signals + + def get_state(self, landmark_id): + """ + Returns the traffic light state of the signals associated with the given landmark. + """ + states = set() + for tlid, link_index in self.get_all_associated_signals(landmark_id): + current_program = self._current_program[tlid] + current_phase = self._current_phase[tlid] + + tl = self._tls[tlid][current_program] + states.update(tl.states[current_phase][link_index]) + + if len(states) == 1: + return states.pop() + elif len(states) > 1: + logging.warning('Landmark %s is associated with signals with different states', + landmark_id) + return SumoSignalState.RED + else: + return None + + def set_state(self, landmark_id, state): + """ + Updates the state of all the signals associated with the given landmark. + """ + for tlid, link_index in self.get_all_associated_signals(landmark_id): + traci.trafficlight.setLinkState(tlid, link_index, state) + return True + + def switch_off(self): + """ + Switch off all traffic lights. + """ + for tlid, link_index in self.get_all_signals(): + traci.trafficlight.setLinkState(tlid, link_index, SumoSignalState.OFF) + self._off = True + + def tick(self): + """ + Tick to traffic light manager + """ + if self._off is False: + for tl_id in traci.trafficlight.getIDList(): + results = traci.trafficlight.getSubscriptionResults(tl_id) + current_program = results[traci.constants.TL_CURRENT_PROGRAM] + current_phase = results[traci.constants.TL_CURRENT_PHASE] + + if current_program != 'online': + self._current_program[tl_id] = current_program + self._current_phase[tl_id] = current_phase + + class SumoSimulation(object): """ SumoSimulation is responsible for the management of the sumo simulation. """ - def __init__(self, args): self.args = args host = args.sumo_host @@ -110,9 +296,8 @@ class SumoSimulation(object): if args.sumo_gui is True: logging.info('Remember to press the play button to start the simulation') - traci.start([ - sumo_binary, - "-c", args.sumo_cfg_file, + traci.start([sumo_binary, + '--configuration-file', args.sumo_cfg_file, '--step-length', str(args.step_length), '--lateral-resolution', '0.25', '--collision.check-junctions' @@ -122,16 +307,23 @@ class SumoSimulation(object): logging.info('Connection to sumo server. Host: %s Port: %s', host, port) traci.init(host=host, port=port) - # Structures to keep track of the spawned and destroyed vehicles at each time step. - self.spawned_actors = set() - self.destroyed_actors = set() - # Creating a random route to be able to spawn carla actors. traci.route.add("carla_route", [traci.edge.getIDList()[0]]) # Variable to asign an id to new added actors. self._sequential_id = 0 + # Structures to keep track of the spawned and destroyed vehicles at each time step. + self.spawned_actors = set() + self.destroyed_actors = set() + + # Traffic light manager. + self.traffic_light_manager = SumoTLManager() + + @property + def traffic_light_ids(self): + return self.traffic_light_manager.get_all_landmarks() + @staticmethod def subscribe(actor_id): """ @@ -148,12 +340,10 @@ class SumoSimulation(object): * Signals. """ traci.vehicle.subscribe(actor_id, [ - traci.constants.VAR_TYPE, traci.constants.VAR_VEHICLECLASS, - traci.constants.VAR_COLOR, traci.constants.VAR_LENGTH, - traci.constants.VAR_WIDTH, traci.constants.VAR_HEIGHT, - traci.constants.VAR_POSITION3D, traci.constants.VAR_ANGLE, - traci.constants.VAR_SLOPE, traci.constants.VAR_SPEED, - traci.constants.VAR_SPEED_LAT, traci.constants.VAR_SIGNALS + traci.constants.VAR_TYPE, traci.constants.VAR_VEHICLECLASS, traci.constants.VAR_COLOR, + traci.constants.VAR_LENGTH, traci.constants.VAR_WIDTH, traci.constants.VAR_HEIGHT, + traci.constants.VAR_POSITION3D, traci.constants.VAR_ANGLE, traci.constants.VAR_SLOPE, + traci.constants.VAR_SPEED, traci.constants.VAR_SPEED_LAT, traci.constants.VAR_SIGNALS ]) @staticmethod @@ -194,14 +384,9 @@ class SumoSimulation(object): height = results[traci.constants.VAR_HEIGHT] location = list(results[traci.constants.VAR_POSITION3D]) - rotation = [ - results[traci.constants.VAR_SLOPE], - results[traci.constants.VAR_ANGLE], 0.0 - ] - transform = carla.Transform( - carla.Location(location[0], location[1], location[2]), - carla.Rotation(rotation[0], rotation[1], rotation[2]) - ) + rotation = [results[traci.constants.VAR_SLOPE], results[traci.constants.VAR_ANGLE], 0.0] + transform = carla.Transform(carla.Location(location[0], location[1], location[2]), + carla.Rotation(rotation[0], rotation[1], rotation[2])) signals = results[traci.constants.VAR_SIGNALS] extent = carla.Vector3D(length / 2.0, width / 2.0, height / 2.0) @@ -239,6 +424,20 @@ class SumoSimulation(object): """ traci.vehicle.remove(actor_id) + def get_traffic_light_state(self, landmark_id): + """ + Accessor for traffic light state. + + If the traffic ligth does not exist, returns None. + """ + return self.traffic_light_manager.get_state(landmark_id) + + def switch_off_traffic_lights(self): + """ + Switch off all traffic lights. + """ + self.traffic_light_manager.switch_off() + def synchronize_vehicle(self, vehicle_id, transform, signals=None): """ Updates vehicle state. @@ -256,11 +455,22 @@ class SumoSimulation(object): traci.vehicle.setSignals(vehicle_id, signals) return True + def synchronize_traffic_light(self, landmark_id, state): + """ + Updates traffic light state. + + :param tl_id: id of the traffic light to be updated (logic id, link index). + :param state: new traffic light state. + :return: True if successfully updated. Otherwise, False. + """ + self.traffic_light_manager.set_state(landmark_id, state) + def tick(self): """ Tick to sumo simulation. """ traci.simulationStep() + self.traffic_light_manager.tick() # Update data structures for the current frame. self.spawned_actors = set(traci.simulation.getDepartedIDList()) diff --git a/Co-Simulation/Sumo/data/opendrive_netconvert.typ.xml b/Co-Simulation/Sumo/util/data/opendrive_netconvert.typ.xml similarity index 100% rename from Co-Simulation/Sumo/data/opendrive_netconvert.typ.xml rename to Co-Simulation/Sumo/util/data/opendrive_netconvert.typ.xml diff --git a/Co-Simulation/Sumo/util/netconvert_carla.py b/Co-Simulation/Sumo/util/netconvert_carla.py new file mode 100644 index 000000000..85ce9d7ba --- /dev/null +++ b/Co-Simulation/Sumo/util/netconvert_carla.py @@ -0,0 +1,526 @@ +#!/usr/bin/env python + +# Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma de +# Barcelona (UAB). +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . +""" +Script to generate sumo nets based on opendrive files. Internally, it uses netconvert to generate +the net and inserts, manually, the traffic light landmarks retrieved from the opendrive. +""" + +# ================================================================================================== +# -- imports --------------------------------------------------------------------------------------- +# ================================================================================================== + +import argparse +import bisect +import collections +import datetime +import logging +import shutil +import subprocess + +import lxml.etree as ET # pylint: disable=import-error + +# ================================================================================================== +# -- find carla module ----------------------------------------------------------------------------- +# ================================================================================================== + +import glob +import os +import sys + +try: + sys.path.append( + glob.glob('../../../PythonAPI/carla/dist/carla-*%d.%d-%s.egg' % + (sys.version_info.major, sys.version_info.minor, + 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) +except IndexError: + pass + +# ================================================================================================== +# -- find sumo modules ----------------------------------------------------------------------------- +# ================================================================================================== + +if 'SUMO_HOME' in os.environ: + sys.path.append(os.path.join(os.environ['SUMO_HOME'], 'tools')) +else: + sys.exit("please declare environment variable 'SUMO_HOME'") + +# ================================================================================================== +# -- imports --------------------------------------------------------------------------------------- +# ================================================================================================== + +import carla +import sumolib + +# ================================================================================================== +# -- topology -------------------------------------------------------------------------------------- +# ================================================================================================== + + +class SumoTopology(object): + """ + This object holds the topology of a sumo net. Internally, the information is structured as + follows: + + - topology: { + (road_id, lane_id): [(successor_road_id, succesor_lane_id), ...], ...} + - paths: { + (road_id, lane_id): [ + ((in_road_id, in_lane_id), (out_road_id, out_lane_id)), ... + ], ...} + - odr2sumo_ids: { + (odr_road_id, odr_lane_id): [(sumo_edge_id, sumo_lane_id), ...], ...} + """ + def __init__(self, topology, paths, odr2sumo_ids): + # Contains only standard roads. + self._topology = topology + # Contaions only roads that belong to a junction. + self._paths = paths + # Mapped ids between sumo and opendrive. + self._odr2sumo_ids = odr2sumo_ids + + # http://sumo.sourceforge.net/userdoc/Networks/Import/OpenDRIVE.html#dealing_with_lane_sections + def get_sumo_id(self, odr_road_id, odr_lane_id, s=0): + """ + Returns the pair (sumo_edge_id, sumo_lane index) corresponding to the provided odr pair. The + argument 's' allows selecting the better sumo edge when it has been split into different + edges due to different odr lane sections. + """ + if (odr_road_id, odr_lane_id) not in self._odr2sumo_ids: + return None + + sumo_ids = list(self._odr2sumo_ids[(odr_road_id, odr_lane_id)]) + + if (len(sumo_ids)) == 1: + return sumo_ids[0] + + # The edge is split into different lane sections. We return the nearest edge based on the + # s coordinate of the provided landmark. + else: + # Ensures that all the related sumo edges belongs to the same opendrive road but to + # different lane sections. + assert set([edge.split('.', 1)[0] for edge, lane_index in sumo_ids]) == 1 + + s_coords = [float(edge.split('.', 1)[1]) for edge, lane_index in sumo_ids] + + s_coords, sumo_ids = zip(*sorted(zip(s_coords, sumo_ids))) + index = bisect.bisect_left(s_coords, s, lo=1) - 1 + return sumo_ids[index] + + def is_junction(self, odr_road_id, odr_lane_id): + """ + Checks whether the provided pair (odr_road_id, odr_lane_id) belongs to a junction. + """ + return (odr_road_id, odr_lane_id) in self._paths + + def get_successors(self, sumo_edge_id, sumo_lane_index): + """ + Returns the successors (standard roads) of the provided pair (sumo_edge_id, sumo_lane_index) + """ + if self.is_junction(sumo_edge_id, sumo_lane_index): + return [] + + return list(self._topology.get((sumo_edge_id, sumo_lane_index), set())) + + def get_incoming(self, odr_road_id, odr_lane_id): + """ + If the pair (odr_road_id, odr_lane_id) belongs to a junction, returns the incoming edges of + the path. Otherwise, return and empty list. + """ + if not self.is_junction(odr_road_id, odr_lane_id): + return [] + + result = set([(connection[0][0], connection[0][1]) + for connection in self._paths[(odr_road_id, odr_lane_id)]]) + return list(result) + + def get_outgoing(self, odr_road_id, odr_lane_id): + """ + If the pair (odr_road_id, odr_lane_id) belongs to a junction, returns the outgoing edges of + the path. Otherwise, return and empty list. + """ + if not self.is_junction(odr_road_id, odr_lane_id): + return [] + + result = set([(connection[1][0], connection[1][1]) + for connection in self._paths[(odr_road_id, odr_lane_id)]]) + return list(result) + + def get_path_connectivity(self, odr_road_id, odr_lane_id): + """ + Returns incoming and outgoing roads of the pair (odr_road_id, odr_lane_id). If the provided + pair not belongs to a junction, returns an empty list. + """ + return list(self._paths.get((odr_road_id, odr_lane_id), set())) + + +def build_topology(sumo_net): + """ + Builds sumo topology. + """ + # -------------------------- + # OpenDrive->Sumo mapped ids + # -------------------------- + # Only takes into account standard roads. + # + # odr2sumo_ids = {(odr_road_id, odr_lane_id) : [(sumo_edge_id, sumo_lane_index), ...], ...} + odr2sumo_ids = {} + for edge in sumo_net.getEdges(): + for lane in edge.getLanes(): + if lane.getParam('origId') is None: + raise RuntimeError( + 'Sumo lane {} does not have "origId" parameter. Make sure that the --output.original-names parameter is active when running netconvert.' + .format(lane.getID())) + + if len(lane.getParam('origId').split()) > 1: + logging.warning('[Building topology] Sumo net contains joined opendrive roads.') + + for odr_id in lane.getParam('origId').split(): + odr_road_id, odr_lane_id = odr_id.split('_') + if (odr_road_id, int(odr_lane_id)) not in odr2sumo_ids: + odr2sumo_ids[(odr_road_id, int(odr_lane_id))] = set() + odr2sumo_ids[(odr_road_id, int(odr_lane_id))].add((edge.getID(), lane.getIndex())) + + # ----------- + # Connections + # ----------- + # + # topology -- {(sumo_road_id, sumo_lane_index): [(sumo_road_id, sumo_lane_index), ...], ...} + # paths -- {(odr_road_id, odr_lane_id): [ + # ((sumo_edge_id, sumo_lane_index), (sumo_edge_id, sumo_lane_index)) + # ]} + topology = {} + paths = {} + + for from_edge in sumo_net.getEdges(): + for to_edge in sumo_net.getEdges(): + connections = from_edge.getConnections(to_edge) + for connection in connections: + from_ = connection.getFromLane() + to_ = connection.getToLane() + from_edge_id, from_lane_index = from_.getEdge().getID(), from_.getIndex() + to_edge_id, to_lane_index = to_.getEdge().getID(), to_.getIndex() + + if (from_edge_id, from_lane_index) not in topology: + topology[(from_edge_id, from_lane_index)] = set() + + topology[(from_edge_id, from_lane_index)].add((to_edge_id, to_lane_index)) + + # Checking if the connection is an opendrive path. + conn_odr_ids = connection.getParam('origId') + if conn_odr_ids is not None: + if len(conn_odr_ids.split()) > 1: + logging.warning( + '[Building topology] Sumo net contains joined opendrive paths.') + + for odr_id in conn_odr_ids.split(): + + odr_road_id, odr_lane_id = odr_id.split('_') + if (odr_road_id, int(odr_lane_id)) not in paths: + paths[(odr_road_id, int(odr_lane_id))] = set() + + paths[(odr_road_id, int(odr_lane_id))].add( + ((from_edge_id, from_lane_index), (to_edge_id, to_lane_index))) + + return SumoTopology(topology, paths, odr2sumo_ids) + + +# ================================================================================================== +# -- sumo definitions ------------------------------------------------------------------------------ +# ================================================================================================== + + +class SumoTrafficLight(object): + """ + SumoTrafficLight holds all the necessary data to define a traffic light in sumo: + + * connections (tlid, from_road, to_road, from_lane, to_lane, link_index). + * phases (duration, state, min_dur, max_dur, nex, name). + * parameters. + """ + DEFAULT_DURATION_GREEN_PHASE = 42 + DEFAULT_DURATION_YELLOW_PHASE = 3 + DEFAULT_DURATION_RED_PHASE = 3 + + Phase = collections.namedtuple('Phase', 'duration state min_dur max_dur next name') + Connection = collections.namedtuple('Connection', + 'tlid from_road to_road from_lane to_lane link_index') + + def __init__(self, id, program_id='0', offset=0, type='static'): + self.id = id + self.program_id = program_id + self.offset = offset + self.type = type + + self.phases = [] + self.parameters = set() + self.connections = set() + + @staticmethod + def generate_tl_id(from_edge, to_edge): + """ + Generates sumo traffic light id based on the junction connectivity. + """ + return '{}:{}'.format(from_edge, to_edge) + + @staticmethod + def generate_default_program(tl): + """ + Generates a default program for the given sumo traffic light + """ + incoming_roads = [connection.from_road for connection in tl.connections] + for road in set(incoming_roads): + phase_green = ['r'] * len(tl.connections) + phase_yellow = ['r'] * len(tl.connections) + phase_red = ['r'] * len(tl.connections) + + for connection in tl.connections: + if connection.from_road == road: + phase_green[connection.link_index] = 'g' + phase_yellow[connection.link_index] = 'y' + + tl.add_phase(SumoTrafficLight.DEFAULT_DURATION_GREEN_PHASE, ''.join(phase_green)) + tl.add_phase(SumoTrafficLight.DEFAULT_DURATION_YELLOW_PHASE, ''.join(phase_yellow)) + tl.add_phase(SumoTrafficLight.DEFAULT_DURATION_RED_PHASE, ''.join(phase_red)) + + def add_phase(self, duration, state, min_dur=-1, max_dur=-1, next=None, name=''): + """ + Adds a new phase. + """ + self.phases.append(SumoTrafficLight.Phase(duration, state, min_dur, max_dur, next, name)) + + def add_parameter(self, key, value): + """ + Adds a new parameter. + """ + self.parameters.add((key, value)) + + def add_connection(self, connection): + """ + Adds a new connection. + """ + self.connections.add(connection) + + def add_landmark(self, + landmark_id, + tlid, + from_road, + to_road, + from_lane, + to_lane, + link_index=-1): + """ + Adds a new landmark. + + Returns True if the landmark is successfully included. Otherwise, returns False. + """ + if link_index == -1: + link_index = len(self.connections) + + def is_same_connection(c1, c2): + return c1.from_road == c2.from_road and c1.to_road == c2.to_road and \ + c1.from_lane == c2.from_lane and c1.to_lane == c2.to_lane + + connection = SumoTrafficLight.Connection(tlid, from_road, to_road, from_lane, to_lane, + link_index) + if any([is_same_connection(connection, c) for c in self.connections]): + logging.warning( + 'Different landmarks controlling the same connection. Only one will be included.') + return False + + self.add_connection(connection) + self.add_parameter(link_index, landmark_id) + return True + + def to_xml(self): + info = { + 'id': self.id, + 'type': self.type, + 'programID': self.program_id, + 'offset': str(self.offset) + } + + xml_tag = ET.Element('tlLogic', info) + for phase in self.phases: + ET.SubElement(xml_tag, 'phase', {'state': phase.state, 'duration': str(phase.duration)}) + for parameter in sorted(self.parameters, key=lambda x: x[0]): + ET.SubElement(xml_tag, 'param', { + 'key': 'linkSignalID:' + str(parameter[0]), + 'value': str(parameter[1]) + }) + + return xml_tag + + +# ================================================================================================== +# -- main ------------------------------------------------------------------------------------------ +# ================================================================================================== + + +def netconvert_carla(args, tmp_path): + """ + Generates sumo net. + """ + # ---------- + # netconvert + # ---------- + tmp_file = os.path.splitext(os.path.basename(args.xodr_file))[0] + tmp_sumo_net = os.path.join(tmp_path, tmp_file + '.net.xml') + + try: + result = subprocess.call(['netconvert', + '--opendrive', args.xodr_file, + '--output-file', tmp_sumo_net, + '--geometry.min-radius.fix', + '--geometry.remove', + '--opendrive.curve-resolution', '1', + '--opendrive.import-all-lanes', + '--type-files', 'data/opendrive_netconvert.typ.xml', + # Necessary to link odr and sumo ids. + '--output.original-names', + # Discard loading traffic lights as them will be inserted manually afterwards. + '--tls.discard-loaded', 'true' + ]) + except subprocess.CalledProcessError: + raise RuntimeError('There was an error when executing netconvert.') + else: + if result != 0: + raise RuntimeError('There was an error when executing netconvert.') + + # -------- + # Sumo net + # -------- + sumo_net = sumolib.net.readNet(tmp_sumo_net) + sumo_topology = build_topology(sumo_net) + + # --------- + # Carla map + # --------- + with open(args.xodr_file, 'r') as f: + carla_map = carla.Map('netconvert', str(f.read())) + + # --------- + # Landmarks + # --------- + tls = {} # {tlsid: SumoTrafficLight} + + landmarks = carla_map.get_all_landmarks_of_type('1000001') + for landmark in landmarks: + if landmark.name == '': + # This is a workaround to avoid adding traffic lights without controllers. + logging.warning('Landmark %s has not a valid name.', landmark.name) + continue + + road_id = str(landmark.road_id) + for from_lane, to_lane in landmark.get_lane_validities(): + for lane_id in range(from_lane, to_lane + 1): + if lane_id == 0: + continue + + wp = carla_map.get_waypoint_xodr(landmark.road_id, lane_id, landmark.s) + if wp is None: + logging.warning( + 'Could not find waypoint for landmark {} (road_id: {}, lane_id: {}, s:{}' + .format(landmark.id, landmark.road_id, lane_id, landmark.s)) + continue + + # When the landmark belongs to a junction, we place te traffic light at the + # entrance of the junction. + if wp.is_junction and sumo_topology.is_junction(road_id, lane_id): + tlid = str(wp.get_junction().id) + if tlid not in tls: + tls[tlid] = SumoTrafficLight(tlid) + tl = tls[tlid] + + if args.guess_tls: + for from_edge, from_lane in sumo_topology.get_incoming(road_id, lane_id): + successors = sumo_topology.get_successors(from_edge, from_lane) + for to_edge, to_lane in successors: + tl.add_landmark(landmark.id, tl.id, from_edge, to_edge, from_lane, + to_lane) + + else: + connections = sumo_topology.get_path_connectivity(road_id, lane_id) + for from_, to_ in connections: + from_edge, from_lane = from_ + to_edge, to_lane = to_ + + tl.add_landmark(landmark.id, tl.id, from_edge, to_edge, from_lane, + to_lane) + + # When the landmarks does not belong to a junction (i.e., belongs to a std road), + # we place the traffic light between that std road and its successor. + elif not wp.is_junction and not sumo_topology.is_junction(road_id, lane_id): + from_edge, from_lane = sumo_topology.get_sumo_id(road_id, lane_id, landmark.s) + + for to_edge, to_lane in sumo_topology.get_successors(from_edge, from_lane): + tlid = SumoTrafficLight.generate_tl_id(from_edge, to_edge) + if tlid not in tls: + tls[tlid] = SumoTrafficLight(tlid) + tl = tls[tlid] + + tl.add_landmark(landmark.id, tl.id, from_edge, to_edge, from_lane, to_lane) + + else: + logging.warning('Landmark %s could not be added.', landmark.id) + + # --------------- + # Modify sumo net + # --------------- + parser = ET.XMLParser(remove_blank_text=True) + tree = ET.parse(tmp_sumo_net, parser) + root = tree.getroot() + + for tl in tls.values(): + SumoTrafficLight.generate_default_program(tl) + edges_tags = tree.xpath('//edge') + if not edges_tags: + raise RuntimeError('No edges found in sumo net.') + root.insert(root.index(edges_tags[-1]) + 1, tl.to_xml()) + + for connection in tl.connections: + tags = tree.xpath( + '//connection[@from="{}" and @to="{}" and @fromLane="{}" and @toLane="{}"]'.format( + connection.from_road, connection.to_road, connection.from_lane, + connection.to_lane)) + + if tags: + if len(tags) > 1: + logging.warning( + 'Found repeated connections from={} to={} fromLane={} toLane={}.'.format( + connection.from_road, connection.to_road, connection.from_lane, + connection.to_lane)) + + tags[0].set('tl', str(connection.tlid)) + tags[0].set('linkIndex', str(connection.link_index)) + else: + logging.warning('Not found connection from={} to={} fromLane={} toLane={}.'.format( + connection.from_road, connection.to_road, connection.from_lane, + connection.to_lane)) + + tree.write(args.output, pretty_print=True, encoding='UTF-8', xml_declaration=True) + + +if __name__ == '__main__': + argparser = argparse.ArgumentParser(description=__doc__) + argparser.add_argument('xodr_file', help='open drive file (*.xodr') + argparser.add_argument('--output', '-o', type=str, help='output file (*.net.xml)') + argparser.add_argument('--guess-tls', + action='store_true', + help='guess traffic lights at intersections (default: False)') + args = argparser.parse_args() + + try: + tmp_path = 'tmp-{date:%Y-%m-%d_%H-%M-%S-%f}'.format(date=datetime.datetime.now()) + if not os.path.exists(tmp_path): + os.mkdir(tmp_path) + + netconvert_carla(args, tmp_path) + + finally: + if os.path.exists(tmp_path): + shutil.rmtree(tmp_path) \ No newline at end of file