first iteration spawn_npc_sumo
This commit is contained in:
Joel Moriana
bernat
parent
0f92f9e71c
commit
28bc7f9b47
|
|
@ -61,15 +61,23 @@ class SimulationSynchronization(object):
|
|||
SimulationSynchronization class is responsible for the synchronization of sumo and carla
|
||||
simulations.
|
||||
"""
|
||||
def __init__(self, args):
|
||||
self.args = args
|
||||
def __init__(self,
|
||||
sumo_simulation,
|
||||
carla_simulation,
|
||||
tls_manager='none',
|
||||
sync_vehicle_color=False,
|
||||
sync_vehicle_lights=False):
|
||||
|
||||
self.sumo = SumoSimulation(args)
|
||||
self.carla = CarlaSimulation(args)
|
||||
self.sumo = sumo_simulation
|
||||
self.carla = carla_simulation
|
||||
|
||||
if args.tls_manager == 'carla':
|
||||
self.tls_manager = tls_manager
|
||||
self.sync_vehicle_color = sync_vehicle_color
|
||||
self.sync_vehicle_lights = sync_vehicle_lights
|
||||
|
||||
if tls_manager == 'carla':
|
||||
self.sumo.switch_off_traffic_lights()
|
||||
elif args.tls_manager == 'sumo':
|
||||
elif tls_manager == 'sumo':
|
||||
self.carla.switch_off_traffic_lights()
|
||||
|
||||
# Mapped actor ids.
|
||||
|
|
@ -94,8 +102,7 @@ class SimulationSynchronization(object):
|
|||
self.sumo.subscribe(sumo_actor_id)
|
||||
sumo_actor = self.sumo.get_actor(sumo_actor_id)
|
||||
|
||||
carla_blueprint = BridgeHelper.get_carla_blueprint(sumo_actor,
|
||||
self.args.sync_vehicle_color)
|
||||
carla_blueprint = BridgeHelper.get_carla_blueprint(sumo_actor, self.sync_vehicle_color)
|
||||
if carla_blueprint is not None:
|
||||
carla_transform = BridgeHelper.get_carla_transform(sumo_actor.transform,
|
||||
sumo_actor.extent)
|
||||
|
|
@ -120,7 +127,7 @@ class SimulationSynchronization(object):
|
|||
|
||||
carla_transform = BridgeHelper.get_carla_transform(sumo_actor.transform,
|
||||
sumo_actor.extent)
|
||||
if self.args.sync_vehicle_lights:
|
||||
if self.sync_vehicle_lights:
|
||||
carla_lights = BridgeHelper.get_carla_lights_state(carla_actor.get_light_state(),
|
||||
sumo_actor.signals)
|
||||
else:
|
||||
|
|
@ -129,7 +136,7 @@ class SimulationSynchronization(object):
|
|||
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':
|
||||
if self.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)
|
||||
|
|
@ -148,8 +155,9 @@ class SimulationSynchronization(object):
|
|||
carla_actor = self.carla.get_actor(carla_actor_id)
|
||||
|
||||
type_id = BridgeHelper.get_sumo_vtype(carla_actor)
|
||||
color = carla_actor.attributes.get('color', None) if self.sync_vehicle_color else None
|
||||
if type_id is not None:
|
||||
sumo_actor_id = self.sumo.spawn_actor(type_id, carla_actor.attributes)
|
||||
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)
|
||||
|
|
@ -168,7 +176,7 @@ class SimulationSynchronization(object):
|
|||
|
||||
sumo_transform = BridgeHelper.get_sumo_transform(carla_actor.get_transform(),
|
||||
carla_actor.bounding_box.extent)
|
||||
if self.args.sync_vehicle_lights:
|
||||
if self.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,
|
||||
|
|
@ -181,7 +189,7 @@ 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':
|
||||
if self.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)
|
||||
|
|
@ -192,7 +200,7 @@ class SimulationSynchronization(object):
|
|||
|
||||
def close(self):
|
||||
"""
|
||||
Cleans up synchronization.
|
||||
Cleans synchronization.
|
||||
"""
|
||||
# Configuring carla simulation in async mode.
|
||||
settings = self.carla.world.get_settings()
|
||||
|
|
@ -216,8 +224,15 @@ def synchronization_loop(args):
|
|||
"""
|
||||
Entry point for sumo-carla co-simulation.
|
||||
"""
|
||||
synchronization = SimulationSynchronization(args)
|
||||
try:
|
||||
sumo_simulation = SumoSimulation(args.sumo_host, args.sumo_port, args.step_length,
|
||||
args.sumo_cfg_file, args.sumo_gui)
|
||||
carla_simulation = CarlaSimulation(args.carla_host, args.carla_port, args.step_length)
|
||||
|
||||
synchronization = SimulationSynchronization(sumo_simulation, carla_simulation,
|
||||
args.tls_manager, args.sync_vehicle_color,
|
||||
args.sync_vehicle_lights)
|
||||
|
||||
while True:
|
||||
start = time.time()
|
||||
|
||||
|
|
@ -232,7 +247,7 @@ def synchronization_loop(args):
|
|||
logging.info('Cancelled by user.')
|
||||
|
||||
finally:
|
||||
logging.info('Cleaning up synchronization')
|
||||
logging.info('Cleaning synchronization')
|
||||
|
||||
synchronization.close()
|
||||
|
||||
|
|
@ -263,8 +278,8 @@ if __name__ == '__main__':
|
|||
type=str,
|
||||
help='sumo configuration file')
|
||||
argparser.add_argument('--sumo-gui',
|
||||
default=True,
|
||||
help='run the gui version of sumo (default: True)')
|
||||
action='store_true',
|
||||
help='run the gui version of sumo')
|
||||
argparser.add_argument('--step-length',
|
||||
default=0.05,
|
||||
type=float,
|
||||
|
|
|
|||
|
|
@ -0,0 +1,277 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
# Copyright (c) 2019 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>.
|
||||
"""Spawn Sumo NPCs vehicles into the simulation"""
|
||||
|
||||
# ==================================================================================================
|
||||
# -- imports ---------------------------------------------------------------------------------------
|
||||
# ==================================================================================================
|
||||
|
||||
import argparse
|
||||
import datetime
|
||||
import json
|
||||
import logging
|
||||
import time
|
||||
import random
|
||||
import re
|
||||
import shutil
|
||||
|
||||
import lxml.etree as ET # pylint: disable=wrong-import-position
|
||||
|
||||
# ==================================================================================================
|
||||
# -- 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 traci module -----------------------------------------------------------------------------
|
||||
# ==================================================================================================
|
||||
|
||||
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'")
|
||||
|
||||
# ==================================================================================================
|
||||
# -- carla -----------------------------------------------------------------------------------------
|
||||
# ==================================================================================================
|
||||
|
||||
import carla # pylint: disable=wrong-import-position
|
||||
|
||||
import sumolib # pylint: disable=wrong-import-position
|
||||
import traci # pylint: disable=wrong-import-position
|
||||
|
||||
from sumo_integration.carla_simulation import CarlaSimulation # pylint: disable=wrong-import-position
|
||||
from sumo_integration.sumo_simulation import SumoSimulation # pylint: disable=wrong-import-position
|
||||
|
||||
from run_synchronization import SimulationSynchronization # pylint: disable=wrong-import-position
|
||||
|
||||
# ==================================================================================================
|
||||
# -- main ------------------------------------------------------------------------------------------
|
||||
# ==================================================================================================
|
||||
|
||||
|
||||
def write_sumocfg_xml(town_name, tmp_path, net_file, vtypes_file, viewsettings_file):
|
||||
"""
|
||||
Writes sumo configuration xml file.
|
||||
"""
|
||||
|
||||
root = ET.Element('configuration')
|
||||
|
||||
input_tag = ET.SubElement(root, 'input')
|
||||
ET.SubElement(input_tag, 'net-file', {'value': net_file})
|
||||
ET.SubElement(input_tag, 'route-files', {'value': vtypes_file})
|
||||
|
||||
gui_tag = ET.SubElement(root, 'gui_only')
|
||||
ET.SubElement(gui_tag, 'gui-settings-file', {'value': viewsettings_file})
|
||||
|
||||
tree = ET.ElementTree(root)
|
||||
filename = os.path.join(tmp_path, town_name + '.sumocfg')
|
||||
tree.write(filename, pretty_print=True, encoding='UTF-8', xml_declaration=True)
|
||||
|
||||
return filename
|
||||
|
||||
|
||||
def get_sumo_files(town_name, tmp_path):
|
||||
"""
|
||||
Returns sumo configuration file and sumo net.
|
||||
"""
|
||||
base_path = os.path.dirname(os.path.realpath(__file__))
|
||||
net_file = os.path.join(base_path, 'examples', 'net', town_name + '.net.xml')
|
||||
vtypes_file = os.path.join(base_path, 'examples', 'carlavtypes.rou.xml')
|
||||
viewsettings_file = os.path.join(base_path, 'examples', 'viewsettings.xml')
|
||||
|
||||
cfg_file = write_sumocfg_xml(town_name, tmp_path, net_file, vtypes_file, viewsettings_file)
|
||||
return cfg_file, net_file
|
||||
|
||||
|
||||
def main(args):
|
||||
|
||||
# Creating temporal folder to save configuration file.
|
||||
tmp_path = 'spawn_npc_sumo-{date:%Y-%m-%d_%H-%M-%S-%f}'.format(date=datetime.datetime.now())
|
||||
if not os.path.exists(tmp_path):
|
||||
os.mkdir(tmp_path)
|
||||
|
||||
try:
|
||||
# ----------------
|
||||
# carla simulation
|
||||
# ----------------
|
||||
carla_simulation = CarlaSimulation(args.host, args.port, args.step_length)
|
||||
|
||||
world = carla_simulation.client.get_world()
|
||||
current_map = world.get_map().name
|
||||
if current_map not in ('Town01', 'Town04', 'Town05'):
|
||||
raise RuntimeError('This script does not support {} yet.'.format(current_map))
|
||||
|
||||
# ---------------
|
||||
# sumo simulation
|
||||
# ---------------
|
||||
cfg_file, net_file = get_sumo_files(current_map, tmp_path)
|
||||
|
||||
sumo_net = sumolib.net.readNet(net_file)
|
||||
sumo_simulation = SumoSimulation(None, None, args.step_length, cfg_file, args.sumo_gui)
|
||||
|
||||
# ---------------
|
||||
# synchronization
|
||||
# ---------------
|
||||
synchronization = SimulationSynchronization(sumo_simulation, carla_simulation,
|
||||
args.tls_manager, args.sync_vehicle_color,
|
||||
args.sync_vehicle_lights)
|
||||
|
||||
# ----------
|
||||
# Blueprints
|
||||
# ----------
|
||||
with open('data/vtypes.json') as f:
|
||||
vtypes = json.load(f)['carla_blueprints']
|
||||
|
||||
blueprints = vtypes.keys()
|
||||
|
||||
filterv = re.compile(args.filterv)
|
||||
blueprints = list(filter(filterv.search, blueprints))
|
||||
|
||||
if args.safe:
|
||||
blueprints = [
|
||||
x for x in blueprints if vtypes[x]['vClass'] not in ('motorcycle', 'bicycle')
|
||||
]
|
||||
blueprints = [x for x in blueprints if not x.endswith('isetta')]
|
||||
blueprints = [x for x in blueprints if not x.endswith('carlacola')]
|
||||
blueprints = [x for x in blueprints if not x.endswith('cybertruck')]
|
||||
blueprints = [x for x in blueprints if not x.endswith('t2')]
|
||||
|
||||
if not blueprints:
|
||||
raise RuntimeError('No blueprints available due to user restrictions.')
|
||||
|
||||
if args.number_of_walkers > 0:
|
||||
logging.warning('Pedestrians are not supported yet. No walkers will be spawned.')
|
||||
|
||||
# --------------
|
||||
# Spawn vehicles
|
||||
# --------------
|
||||
# Spawns sumo NPC vehicles.
|
||||
sumo_edges = sumo_net.getEdges()
|
||||
|
||||
for i in range(args.number_of_vehicles):
|
||||
edge = random.choice(sumo_edges)
|
||||
type_id = random.choice(blueprints)
|
||||
|
||||
traci.route.add('route_{}'.format(i), [edge.getID()])
|
||||
result = traci.vehicle.add('sumo_{}'.format(i), 'route_{}'.format(i), typeID=type_id)
|
||||
|
||||
while True:
|
||||
start = time.time()
|
||||
|
||||
synchronization.tick()
|
||||
|
||||
# Updates vehicle routes
|
||||
for vehicle_id in traci.vehicle.getIDList():
|
||||
route = traci.vehicle.getRoute(vehicle_id)
|
||||
index = traci.vehicle.getRouteIndex(vehicle_id)
|
||||
vclass = traci.vehicle.getVehicleClass(vehicle_id)
|
||||
|
||||
if index == (len(route) - 1):
|
||||
current_edge = sumo_net.getEdge(route[index])
|
||||
next_edge = random.choice(list(current_edge.getAllowedOutgoing(vclass).keys()))
|
||||
|
||||
new_route = [current_edge.getID(), next_edge.getID()]
|
||||
traci.vehicle.setRoute(vehicle_id, new_route)
|
||||
|
||||
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:
|
||||
if os.path.exists(tmp_path):
|
||||
shutil.rmtree(tmp_path)
|
||||
|
||||
logging.info('Destroying %d sumo vehicles.', traci.vehicle.getIDCount())
|
||||
synchronization.close()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
argparser = argparse.ArgumentParser(description=__doc__)
|
||||
argparser.add_argument('--host',
|
||||
metavar='H',
|
||||
default='127.0.0.1',
|
||||
help='IP of the host server (default: 127.0.0.1)')
|
||||
argparser.add_argument('-p',
|
||||
'--port',
|
||||
metavar='P',
|
||||
default=2000,
|
||||
type=int,
|
||||
help='TCP port to listen to (default: 2000)')
|
||||
argparser.add_argument('-n',
|
||||
'--number-of-vehicles',
|
||||
metavar='N',
|
||||
default=10,
|
||||
type=int,
|
||||
help='number of vehicles (default: 10)')
|
||||
argparser.add_argument('-w',
|
||||
'--number-of-walkers',
|
||||
metavar='W',
|
||||
default=0,
|
||||
type=int,
|
||||
help='number of walkers (default: 0)')
|
||||
argparser.add_argument('--safe',
|
||||
action='store_true',
|
||||
help='avoid spawning vehicles prone to accidents')
|
||||
argparser.add_argument('--filterv',
|
||||
metavar='PATTERN',
|
||||
default='vehicle.*',
|
||||
help='vehicles filter (default: "vehicle.*")')
|
||||
argparser.add_argument('--filterw',
|
||||
metavar='PATTERN',
|
||||
default='walker.pedestrian.*',
|
||||
help='pedestrians filter (default: "walker.pedestrian.*")')
|
||||
argparser.add_argument('--sumo-gui',
|
||||
action='store_true',
|
||||
help='run the gui version of sumo')
|
||||
argparser.add_argument('--step-length',
|
||||
default=0.05,
|
||||
type=float,
|
||||
help='set fixed delta seconds (default: 0.05s)')
|
||||
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)')
|
||||
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')
|
||||
args = argparser.parse_args()
|
||||
|
||||
if args.sync_vehicle_all is True:
|
||||
args.sync_vehicle_lights = True
|
||||
args.sync_vehicle_color = True
|
||||
|
||||
if args.debug:
|
||||
logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.DEBUG)
|
||||
else:
|
||||
logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO)
|
||||
|
||||
main(args)
|
||||
|
|
@ -26,11 +26,7 @@ class CarlaSimulation(object):
|
|||
"""
|
||||
CarlaSimulation is responsible for the management of the carla simulation.
|
||||
"""
|
||||
def __init__(self, args):
|
||||
self.args = args
|
||||
host = args.carla_host
|
||||
port = args.carla_port
|
||||
|
||||
def __init__(self, host, port, step_length):
|
||||
self.client = carla.Client(host, port)
|
||||
self.client.set_timeout(2.0)
|
||||
|
||||
|
|
@ -40,7 +36,7 @@ class CarlaSimulation(object):
|
|||
# Configuring carla simulation in sync mode.
|
||||
settings = self.world.get_settings()
|
||||
settings.synchronous_mode = True
|
||||
settings.fixed_delta_seconds = args.step_length
|
||||
settings.fixed_delta_seconds = step_length
|
||||
self.world.apply_settings(settings)
|
||||
|
||||
# The following sets contain updated information for the current frame.
|
||||
|
|
@ -151,7 +147,7 @@ class CarlaSimulation(object):
|
|||
return False
|
||||
|
||||
vehicle.set_transform(transform)
|
||||
if lights is not None and self.args.sync_vehicle_lights:
|
||||
if lights is not None:
|
||||
vehicle.set_light_state(carla.VehicleLightState(lights))
|
||||
return True
|
||||
|
||||
|
|
|
|||
|
|
@ -281,24 +281,20 @@ class SumoSimulation(object):
|
|||
"""
|
||||
SumoSimulation is responsible for the management of the sumo simulation.
|
||||
"""
|
||||
def __init__(self, args):
|
||||
self.args = args
|
||||
host = args.sumo_host
|
||||
port = args.sumo_port
|
||||
|
||||
if args.sumo_gui is True:
|
||||
def __init__(self, host, port, step_length, cfg_file, sumo_gui=False):
|
||||
if sumo_gui is True:
|
||||
sumo_binary = sumolib.checkBinary('sumo-gui')
|
||||
else:
|
||||
sumo_binary = sumolib.checkBinary('sumo')
|
||||
|
||||
if args.sumo_host is None or args.sumo_port is None:
|
||||
if host is None or port is None:
|
||||
logging.info('Starting new sumo server...')
|
||||
if args.sumo_gui is True:
|
||||
if sumo_gui is True:
|
||||
logging.info('Remember to press the play button to start the simulation')
|
||||
|
||||
traci.start([sumo_binary,
|
||||
'--configuration-file', args.sumo_cfg_file,
|
||||
'--step-length', str(args.step_length),
|
||||
'--configuration-file', cfg_file,
|
||||
'--step-length', str(step_length),
|
||||
'--lateral-resolution', '0.25',
|
||||
'--collision.check-junctions'
|
||||
])
|
||||
|
|
@ -393,12 +389,12 @@ class SumoSimulation(object):
|
|||
|
||||
return SumoActor(type_id, vclass, transform, signals, extent, color)
|
||||
|
||||
def spawn_actor(self, type_id, attrs=None):
|
||||
def spawn_actor(self, type_id, color=None):
|
||||
"""
|
||||
Spawns a new actor.
|
||||
|
||||
:param type_id: vtype to be spawned.
|
||||
:param attrs: dictionary with additional attributes for this specific actor.
|
||||
:param color: color attribute for this specific actor.
|
||||
:return: actor id if the actor is successfully spawned. Otherwise, INVALID_ACTOR_ID.
|
||||
"""
|
||||
actor_id = 'carla' + str(self._sequential_id)
|
||||
|
|
@ -408,10 +404,9 @@ class SumoSimulation(object):
|
|||
logging.error('Spawn sumo actor failed: %s', error)
|
||||
return INVALID_ACTOR_ID
|
||||
|
||||
if attrs is not None:
|
||||
if self.args.sync_vehicle_color and 'color' in attrs:
|
||||
color = attrs['color'].split(',')
|
||||
traci.vehicle.setColor(actor_id, color)
|
||||
if color is not None:
|
||||
color = color.split(',')
|
||||
traci.vehicle.setColor(actor_id, color)
|
||||
|
||||
self._sequential_id += 1
|
||||
|
||||
|
|
@ -451,7 +446,7 @@ class SumoSimulation(object):
|
|||
yaw = transform.rotation.yaw
|
||||
|
||||
traci.vehicle.moveToXY(vehicle_id, "", 0, loc_x, loc_y, angle=yaw, keepRoute=2)
|
||||
if signals is not None and self.args.sync_vehicle_lights:
|
||||
if signals is not None:
|
||||
traci.vehicle.setSignals(vehicle_id, signals)
|
||||
return True
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue