synchronization traffic lights

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 <https://opensource.org/licenses/MIT>.
-
 """
 Script to integrate CARLA and SUMO simulations
 """
@@ -27,9 +26,9 @@ import os
 import sys
 
 try:
-    sys.path.append(glob.glob('../../PythonAPI/carla/dist/carla-*%d.%d-%s.egg' % (
+    sys.path.append(
-        sys.version_info.major,
+        glob.glob('../../PythonAPI/carla/dist/carla-*%d.%d-%s.egg' %
-        sys.version_info.minor,
+                  (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(
+                carla_transform = BridgeHelper.get_carla_transform(sumo_actor.transform,
-                    sumo_actor.transform, sumo_actor.extent)
+                                                                   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_lights = BridgeHelper.get_carla_lights_state(carla_actor.get_light_state(),
-                    carla_actor.get_light_state(), sumo_actor.signals)
+                                                                   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_lights = BridgeHelper.get_sumo_lights_state(sumo_actor.signals,
-                        sumo_actor.signals, carla_lights)
+                                                                     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
 

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 <https://opensource.org/licenses/MIT>.
-
 """ 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'
+                    'sumo vtype %s not found in carla. The following blueprint will be used: %s',
-                    , type_id, blueprint.id)
+                    type_id, blueprint.id)
             else:
-                logging.error(
+                logging.error('sumo vtype %s not supported. No vehicle will be spawned in carla',
-                    'sumo vtype %s not supported. No vehicle will be spawned in carla', type_id)
+                              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

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 <https://opensource.org/licenses/MIT>.
-
 """ 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 = carla.Transform(transform.location + carla.Location(0, 0, SPAWN_OFFSET_Z),
-            transform.location + carla.Location(0, 0, SPAWN_OFFSET_Z),
                                     transform.rotation)
 
         batch = [
-            carla.command.SpawnActor(blueprint, transform)
+            carla.command.SpawnActor(blueprint, transform).then(
-            .then(carla.command.SetSimulatePhysics(carla.command.FutureActor, False))
+                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([
+        current_actors = set(
-            vehicle.id for vehicle in self.world.get_actors().filter('vehicle.*')
+            [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)

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 <https://opensource.org/licenses/MIT>.
-
 """ This module defines constants used for the sumo-carla co-simulation. """
 
 # ==================================================================================================

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 <https://opensource.org/licenses/MIT>.
-
 """ 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 = collections.namedtuple('SumoActor', 'type_id vclass transform signals extent color')
-    '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([
+            traci.start([sumo_binary,
-                sumo_binary,
+                '--configuration-file', args.sumo_cfg_file,
-                "-c", 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_TYPE, traci.constants.VAR_VEHICLECLASS, traci.constants.VAR_COLOR,
-            traci.constants.VAR_COLOR, traci.constants.VAR_LENGTH,
+            traci.constants.VAR_LENGTH, traci.constants.VAR_WIDTH, traci.constants.VAR_HEIGHT,
-            traci.constants.VAR_WIDTH, traci.constants.VAR_HEIGHT,
+            traci.constants.VAR_POSITION3D, traci.constants.VAR_ANGLE, traci.constants.VAR_SLOPE,
-            traci.constants.VAR_POSITION3D, traci.constants.VAR_ANGLE,
+            traci.constants.VAR_SPEED, traci.constants.VAR_SPEED_LAT, traci.constants.VAR_SIGNALS
-            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 = [
+        rotation = [results[traci.constants.VAR_SLOPE], results[traci.constants.VAR_ANGLE], 0.0]
-            results[traci.constants.VAR_SLOPE],
+        transform = carla.Transform(carla.Location(location[0], location[1], location[2]),
-            results[traci.constants.VAR_ANGLE], 0.0
+                                    carla.Rotation(rotation[0], rotation[1], rotation[2]))
-        ]
-        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())

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 <https://opensource.org/licenses/MIT>.
+"""
+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)