carla/Co-Simulation/Sumo/run_synchronization.py

320 lines
14 KiB
Python
Raw Permalink Normal View History

2020-03-05 01:55:33 +08:00
#!/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 integrate CARLA and SUMO simulations
"""
2020-03-05 23:49:16 +08:00
# ==================================================================================================
# -- imports ---------------------------------------------------------------------------------------
# ==================================================================================================
import argparse
import logging
import time
# ==================================================================================================
# -- find carla module -----------------------------------------------------------------------------
# ==================================================================================================
2020-03-05 01:55:33 +08:00
import glob
import os
import sys
try:
2020-04-08 21:06:57 +08:00
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])
2020-03-05 01:55:33 +08:00
except IndexError:
pass
2020-03-05 23:49:16 +08:00
# ==================================================================================================
# -- find traci module -----------------------------------------------------------------------------
# ==================================================================================================
2020-03-05 01:55:33 +08:00
if 'SUMO_HOME' in os.environ:
2020-03-05 23:49:16 +08:00
sys.path.append(os.path.join(os.environ['SUMO_HOME'], 'tools'))
2020-03-05 01:55:33 +08:00
else:
sys.exit("please declare environment variable 'SUMO_HOME'")
2020-03-05 23:49:16 +08:00
# ==================================================================================================
2020-03-30 18:35:10 +08:00
# -- sumo integration imports ----------------------------------------------------------------------
2020-03-05 23:49:16 +08:00
# ==================================================================================================
2020-03-05 01:55:33 +08:00
2020-03-05 23:49:16 +08:00
from sumo_integration.bridge_helper import BridgeHelper # pylint: disable=wrong-import-position
from sumo_integration.carla_simulation import CarlaSimulation # pylint: disable=wrong-import-position
from sumo_integration.constants import INVALID_ACTOR_ID # pylint: disable=wrong-import-position
from sumo_integration.sumo_simulation import SumoSimulation # pylint: disable=wrong-import-position
2020-03-05 01:55:33 +08:00
2020-03-05 23:49:16 +08:00
# ==================================================================================================
# -- synchronization_loop --------------------------------------------------------------------------
2020-03-05 23:49:16 +08:00
# ==================================================================================================
2020-03-05 01:55:33 +08:00
class SimulationSynchronization(object):
2020-03-05 23:49:16 +08:00
"""
SimulationSynchronization class is responsible for the synchronization of sumo and carla
simulations.
2020-03-05 23:49:16 +08:00
"""
2020-04-10 00:45:34 +08:00
def __init__(self,
sumo_simulation,
carla_simulation,
tls_manager='none',
sync_vehicle_color=False,
sync_vehicle_lights=False):
2020-04-10 00:45:34 +08:00
self.sumo = sumo_simulation
self.carla = carla_simulation
2020-04-10 00:45:34 +08:00
self.tls_manager = tls_manager
self.sync_vehicle_color = sync_vehicle_color
self.sync_vehicle_lights = sync_vehicle_lights
if tls_manager == 'carla':
2020-04-08 21:06:57 +08:00
self.sumo.switch_off_traffic_lights()
2020-04-10 00:45:34 +08:00
elif tls_manager == 'sumo':
2020-04-08 21:06:57 +08:00
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.
BridgeHelper.blueprint_library = self.carla.world.get_blueprint_library()
BridgeHelper.offset = self.sumo.get_net_offset()
2020-04-10 23:50:33 +08:00
# Configuring carla simulation in sync mode.
settings = self.carla.world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = self.carla.step_length
self.carla.world.apply_settings(settings)
2020-12-18 00:40:33 +08:00
traffic_manager = self.carla.client.get_trafficmanager()
traffic_manager.set_synchronous_mode(True)
def tick(self):
"""
Tick to simulation synchronization
"""
# -----------------
# sumo-->carla sync
# -----------------
self.sumo.tick()
# Spawning new sumo actors in carla (i.e, not controlled by carla).
sumo_spawned_actors = self.sumo.spawned_actors - set(self.carla2sumo_ids.values())
for sumo_actor_id in sumo_spawned_actors:
self.sumo.subscribe(sumo_actor_id)
sumo_actor = self.sumo.get_actor(sumo_actor_id)
2020-04-10 00:45:34 +08:00
carla_blueprint = BridgeHelper.get_carla_blueprint(sumo_actor, self.sync_vehicle_color)
if carla_blueprint is not None:
2020-04-08 21:06:57 +08:00
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:
self.sumo2carla_ids[sumo_actor_id] = carla_actor_id
else:
self.sumo.unsubscribe(sumo_actor_id)
# Destroying sumo arrived actors in carla.
for sumo_actor_id in self.sumo.destroyed_actors:
if sumo_actor_id in self.sumo2carla_ids:
self.carla.destroy_actor(self.sumo2carla_ids.pop(sumo_actor_id))
# Updating sumo actors in carla.
for sumo_actor_id in self.sumo2carla_ids:
carla_actor_id = self.sumo2carla_ids[sumo_actor_id]
sumo_actor = self.sumo.get_actor(sumo_actor_id)
carla_actor = self.carla.get_actor(carla_actor_id)
carla_transform = BridgeHelper.get_carla_transform(sumo_actor.transform,
sumo_actor.extent)
2020-04-10 00:45:34 +08:00
if self.sync_vehicle_lights:
2020-04-08 21:06:57 +08:00
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)
2020-04-08 21:06:57 +08:00
# Updates traffic lights in carla based on sumo information.
2020-04-10 00:45:34 +08:00
if self.tls_manager == 'sumo':
2020-04-08 21:06:57 +08:00
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
# -----------------
self.carla.tick()
# Spawning new carla actors (not controlled by sumo)
carla_spawned_actors = self.carla.spawned_actors - set(self.sumo2carla_ids.values())
for carla_actor_id in carla_spawned_actors:
carla_actor = self.carla.get_actor(carla_actor_id)
type_id = BridgeHelper.get_sumo_vtype(carla_actor)
2020-04-10 00:45:34 +08:00
color = carla_actor.attributes.get('color', None) if self.sync_vehicle_color else None
if type_id is not None:
2020-04-10 00:45:34 +08:00
sumo_actor_id = self.sumo.spawn_actor(type_id, color)
if sumo_actor_id != INVALID_ACTOR_ID:
self.carla2sumo_ids[carla_actor_id] = sumo_actor_id
self.sumo.subscribe(sumo_actor_id)
# Destroying required carla actors in sumo.
for carla_actor_id in self.carla.destroyed_actors:
if carla_actor_id in self.carla2sumo_ids:
self.sumo.destroy_actor(self.carla2sumo_ids.pop(carla_actor_id))
# Updating carla actors in sumo.
for carla_actor_id in self.carla2sumo_ids:
sumo_actor_id = self.carla2sumo_ids[carla_actor_id]
carla_actor = self.carla.get_actor(carla_actor_id)
sumo_actor = self.sumo.get_actor(sumo_actor_id)
sumo_transform = BridgeHelper.get_sumo_transform(carla_actor.get_transform(),
carla_actor.bounding_box.extent)
2020-04-10 00:45:34 +08:00
if self.sync_vehicle_lights:
carla_lights = self.carla.get_actor_light_state(carla_actor_id)
if carla_lights is not None:
2020-04-08 21:06:57 +08:00
sumo_lights = BridgeHelper.get_sumo_lights_state(sumo_actor.signals,
carla_lights)
else:
sumo_lights = None
else:
sumo_lights = None
2020-03-05 01:55:33 +08:00
self.sumo.synchronize_vehicle(sumo_actor_id, sumo_transform, sumo_lights)
2020-03-05 01:55:33 +08:00
2020-04-08 21:06:57 +08:00
# Updates traffic lights in sumo based on carla information.
2020-04-10 00:45:34 +08:00
if self.tls_manager == 'carla':
2020-04-08 21:06:57 +08:00
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):
"""
2020-04-10 00:45:34 +08:00
Cleans synchronization.
"""
# Configuring carla simulation in async mode.
settings = self.carla.world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
self.carla.world.apply_settings(settings)
2020-03-05 01:55:33 +08:00
# Destroying synchronized actors.
for carla_actor_id in self.sumo2carla_ids.values():
self.carla.destroy_actor(carla_actor_id)
2020-03-05 01:55:33 +08:00
for sumo_actor_id in self.carla2sumo_ids.values():
self.sumo.destroy_actor(sumo_actor_id)
2020-03-05 01:55:33 +08:00
2020-04-08 21:06:57 +08:00
# Closing sumo and carla client.
self.carla.close()
self.sumo.close()
2020-03-05 01:55:33 +08:00
def synchronization_loop(args):
"""
Entry point for sumo-carla co-simulation.
"""
2020-04-14 18:37:54 +08:00
sumo_simulation = SumoSimulation(args.sumo_cfg_file, args.step_length, args.sumo_host,
args.sumo_port, args.sumo_gui, args.client_order)
2020-04-14 18:37:54 +08:00
carla_simulation = CarlaSimulation(args.carla_host, args.carla_port, args.step_length)
2020-04-10 00:45:34 +08:00
2020-04-14 18:37:54 +08:00
synchronization = SimulationSynchronization(sumo_simulation, carla_simulation, args.tls_manager,
args.sync_vehicle_color, args.sync_vehicle_lights)
try:
while True:
start = time.time()
2020-03-05 01:55:33 +08:00
synchronization.tick()
2020-03-05 01:55:33 +08:00
end = time.time()
elapsed = end - start
if elapsed < args.step_length:
time.sleep(args.step_length - elapsed)
except KeyboardInterrupt:
logging.info('Cancelled by user.')
finally:
2020-04-10 00:45:34 +08:00
logging.info('Cleaning synchronization')
2020-03-05 01:55:33 +08:00
synchronization.close()
2020-03-05 01:55:33 +08:00
2020-03-05 23:49:16 +08:00
2020-03-05 01:55:33 +08:00
if __name__ == '__main__':
2020-03-05 23:49:16 +08:00
argparser = argparse.ArgumentParser(description=__doc__)
2020-04-10 23:50:33 +08:00
argparser.add_argument('sumo_cfg_file', type=str, help='sumo configuration file')
2020-03-05 23:49:16 +08:00
argparser.add_argument('--carla-host',
metavar='H',
default='127.0.0.1',
help='IP of the carla host server (default: 127.0.0.1)')
argparser.add_argument('--carla-port',
metavar='P',
default=2000,
type=int,
help='TCP port to listen to (default: 2000)')
argparser.add_argument('--sumo-host',
metavar='H',
default=None,
help='IP of the sumo host server (default: 127.0.0.1)')
argparser.add_argument('--sumo-port',
metavar='P',
default=None,
type=int,
help='TCP port to listen to (default: 8813)')
2020-04-10 23:50:33 +08:00
argparser.add_argument('--sumo-gui', action='store_true', help='run the gui version of sumo')
2020-03-05 23:49:16 +08:00
argparser.add_argument('--step-length',
default=0.05,
type=float,
help='set fixed delta seconds (default: 0.05s)')
argparser.add_argument('--client-order',
metavar='TRACI_CLIENT_ORDER',
default=1,
type=int,
help='client order number for the co-simulation TraCI connection (default: 1)')
argparser.add_argument('--sync-vehicle-lights',
action='store_true',
help='synchronize vehicle lights state (default: False)')
argparser.add_argument('--sync-vehicle-color',
action='store_true',
help='synchronize vehicle color (default: False)')
2020-04-08 21:06:57 +08:00
argparser.add_argument('--sync-vehicle-all',
action='store_true',
help='synchronize all vehicle properties (default: False)')
2020-04-08 21:06:57 +08:00
argparser.add_argument('--tls-manager',
type=str,
choices=['none', 'sumo', 'carla'],
help="select traffic light manager (default: none)",
default='none')
2020-03-05 23:49:16 +08:00
argparser.add_argument('--debug', action='store_true', help='enable debug messages')
arguments = argparser.parse_args()
2020-03-05 01:55:33 +08:00
2020-04-08 21:06:57 +08:00
if arguments.sync_vehicle_all is True:
arguments.sync_vehicle_lights = True
arguments.sync_vehicle_color = True
2020-03-05 23:49:16 +08:00
if arguments.debug:
2020-03-05 01:55:33 +08:00
logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.DEBUG)
else:
logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO)
synchronization_loop(arguments)