synchronization traffic lights
This commit is contained in:
Joel Moriana
bernat
parent
8ebb8a12a1
commit
6fde83fbc9
|
|
@ -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,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
|
||||
|
||||
|
|
|
|||
|
|
@ -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'
|
||||
, 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
|
||||
|
|
|
|||
|
|
@ -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.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)
|
||||
|
|
|
|||
|
|
@ -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. """
|
||||
|
||||
# ==================================================================================================
|
||||
|
|
|
|||
|
|
@ -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', '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())
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
Loading…
Reference in New Issue