PR fixes.
* run_synchronization main method split into a synchronization_loop method and a SimulationSynchronization class. * New arguments added: --sync-vehicle-color and --sync-all * Added some SumoSimulation methods as staticmethod to avoid codacy warnings. * Other smaller PR modifications.
This commit is contained in:
Joel Moriana
Marc Garcia Puig
parent
708f3ccfe3
commit
c7b964a151
|
|
@ -53,105 +53,111 @@ from sumo_integration.constants import INVALID_ACTOR_ID # pylint: disable=wrong
|
|||
from sumo_integration.sumo_simulation import SumoSimulation # pylint: disable=wrong-import-position
|
||||
|
||||
# ==================================================================================================
|
||||
# -- simulation synchro ----------------------------------------------------------------------------
|
||||
# -- synchronization_loop --------------------------------------------------------------------------
|
||||
# ==================================================================================================
|
||||
|
||||
|
||||
def main(args):
|
||||
class SimulationSynchronization(object):
|
||||
"""
|
||||
Entry point sumo-carla co-simulation.
|
||||
SimulationSynchronization class is responsible for the synchronization of sumo and carla
|
||||
simulations.
|
||||
"""
|
||||
sumo = SumoSimulation(args)
|
||||
carla = CarlaSimulation(args)
|
||||
|
||||
def __init__(self, args):
|
||||
self.args = args
|
||||
|
||||
self.sumo = SumoSimulation(args)
|
||||
self.carla = CarlaSimulation(args)
|
||||
|
||||
# Mapped actor ids.
|
||||
sumo2carla_ids = {} # Contains only actors controlled by sumo.
|
||||
carla2sumo_ids = {} # Contains only actors controlled by carla.
|
||||
self.sumo2carla_ids = {} # Contains only actors controlled by sumo.
|
||||
self.carla2sumo_ids = {} # Contains only actors controlled by carla.
|
||||
|
||||
BridgeHelper.blueprint_library = carla.world.get_blueprint_library()
|
||||
BridgeHelper.offset = sumo.get_net_offset()
|
||||
|
||||
try:
|
||||
while True:
|
||||
start = time.time()
|
||||
BridgeHelper.blueprint_library = self.carla.world.get_blueprint_library()
|
||||
BridgeHelper.offset = self.sumo.get_net_offset()
|
||||
|
||||
def tick(self):
|
||||
"""
|
||||
Tick to simulation synchronization
|
||||
"""
|
||||
# -----------------
|
||||
# sumo-->carla sync
|
||||
# -----------------
|
||||
sumo.tick()
|
||||
self.sumo.tick()
|
||||
|
||||
# Spawning new sumo actors in carla (i.e, not controlled by carla).
|
||||
sumo_spawned_actors = sumo.spawned_actors - set(carla2sumo_ids.values())
|
||||
sumo_spawned_actors = self.sumo.spawned_actors - set(self.carla2sumo_ids.values())
|
||||
for sumo_actor_id in sumo_spawned_actors:
|
||||
SumoSimulation.subscribe(sumo_actor_id)
|
||||
sumo_actor = sumo.get_actor(sumo_actor_id)
|
||||
self.sumo.subscribe(sumo_actor_id)
|
||||
sumo_actor = self.sumo.get_actor(sumo_actor_id)
|
||||
|
||||
carla_blueprint = BridgeHelper.get_carla_blueprint(sumo_actor)
|
||||
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_actor_id = carla.spawn_actor(carla_blueprint, carla_transform)
|
||||
carla_actor_id = self.carla.spawn_actor(carla_blueprint, carla_transform)
|
||||
if carla_actor_id != INVALID_ACTOR_ID:
|
||||
sumo2carla_ids[sumo_actor_id] = carla_actor_id
|
||||
self.sumo2carla_ids[sumo_actor_id] = carla_actor_id
|
||||
else:
|
||||
SumoSimulation.unsubscribe(sumo_actor_id)
|
||||
self.sumo.unsubscribe(sumo_actor_id)
|
||||
|
||||
# Destroying sumo arrived actors in carla.
|
||||
for sumo_actor_id in sumo.destroyed_actors:
|
||||
if sumo_actor_id in sumo2carla_ids:
|
||||
carla.destroy_actor(sumo2carla_ids.pop(sumo_actor_id))
|
||||
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 sumo2carla_ids:
|
||||
carla_actor_id = sumo2carla_ids[sumo_actor_id]
|
||||
for sumo_actor_id in self.sumo2carla_ids:
|
||||
carla_actor_id = self.sumo2carla_ids[sumo_actor_id]
|
||||
|
||||
sumo_actor = sumo.get_actor(sumo_actor_id)
|
||||
carla_actor = carla.get_actor(carla_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)
|
||||
if args.sync_vehicle_lights:
|
||||
if self.args.sync_vehicle_lights:
|
||||
carla_lights = BridgeHelper.get_carla_lights_state(
|
||||
carla_actor.get_light_state(), sumo_actor.signals)
|
||||
else:
|
||||
carla_lights = None
|
||||
|
||||
carla.synchronize_vehicle(carla_actor_id, carla_transform, carla_lights)
|
||||
self.carla.synchronize_vehicle(carla_actor_id, carla_transform, carla_lights)
|
||||
|
||||
# -----------------
|
||||
# carla-->sumo sync
|
||||
# -----------------
|
||||
carla.tick()
|
||||
self.carla.tick()
|
||||
|
||||
# Spawning new carla actors (not controlled by sumo)
|
||||
carla_spawned_actors = carla.spawned_actors - set(sumo2carla_ids.values())
|
||||
carla_spawned_actors = self.carla.spawned_actors - set(self.sumo2carla_ids.values())
|
||||
for carla_actor_id in carla_spawned_actors:
|
||||
carla_actor = carla.get_actor(carla_actor_id)
|
||||
carla_actor = self.carla.get_actor(carla_actor_id)
|
||||
|
||||
type_id = BridgeHelper.get_sumo_vtype(carla_actor)
|
||||
if type_id is not None:
|
||||
sumo_actor_id = sumo.spawn_actor(type_id, carla_actor.attributes)
|
||||
sumo_actor_id = self.sumo.spawn_actor(type_id, carla_actor.attributes)
|
||||
if sumo_actor_id != INVALID_ACTOR_ID:
|
||||
carla2sumo_ids[carla_actor_id] = sumo_actor_id
|
||||
sumo.subscribe(sumo_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 carla.destroyed_actors:
|
||||
if carla_actor_id in carla2sumo_ids:
|
||||
sumo.destroy_actor(carla2sumo_ids.pop(carla_actor_id))
|
||||
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 carla2sumo_ids:
|
||||
sumo_actor_id = carla2sumo_ids[carla_actor_id]
|
||||
for carla_actor_id in self.carla2sumo_ids:
|
||||
sumo_actor_id = self.carla2sumo_ids[carla_actor_id]
|
||||
|
||||
carla_actor = carla.get_actor(carla_actor_id)
|
||||
sumo_actor = sumo.get_actor(sumo_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)
|
||||
if args.sync_vehicle_lights:
|
||||
carla_lights = carla.get_actor_light_state(carla_actor_id)
|
||||
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)
|
||||
|
|
@ -160,7 +166,39 @@ def main(args):
|
|||
else:
|
||||
sumo_lights = None
|
||||
|
||||
sumo.synchronize_vehicle(sumo_actor_id, sumo_transform, sumo_lights)
|
||||
self.sumo.synchronize_vehicle(sumo_actor_id, sumo_transform, sumo_lights)
|
||||
|
||||
def close(self):
|
||||
"""
|
||||
Cleans up 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)
|
||||
|
||||
# Destroying synchronized actors.
|
||||
for carla_actor_id in self.sumo2carla_ids.values():
|
||||
self.carla.destroy_actor(carla_actor_id)
|
||||
|
||||
for sumo_actor_id in self.carla2sumo_ids.values():
|
||||
self.sumo.destroy_actor(sumo_actor_id)
|
||||
|
||||
# Closing sumo client.
|
||||
self.sumo.close()
|
||||
|
||||
|
||||
def synchronization_loop(args):
|
||||
"""
|
||||
Entry point for sumo-carla co-simulation.
|
||||
"""
|
||||
synchronization = SimulationSynchronization(args)
|
||||
try:
|
||||
while True:
|
||||
start = time.time()
|
||||
|
||||
synchronization.tick()
|
||||
|
||||
end = time.time()
|
||||
elapsed = end - start
|
||||
|
|
@ -173,21 +211,7 @@ def main(args):
|
|||
finally:
|
||||
logging.info('Cleaning up synchronization')
|
||||
|
||||
# Configuring carla simulation in async mode.
|
||||
settings = carla.world.get_settings()
|
||||
settings.synchronous_mode = False
|
||||
settings.fixed_delta_seconds = None
|
||||
carla.world.apply_settings(settings)
|
||||
|
||||
# Destroying synchronized actors.
|
||||
for carla_actor_id in sumo2carla_ids.values():
|
||||
carla.destroy_actor(carla_actor_id)
|
||||
|
||||
for sumo_actor_id in carla2sumo_ids.values():
|
||||
sumo.destroy_actor(sumo_actor_id)
|
||||
|
||||
# Closing sumo client.
|
||||
sumo.close()
|
||||
synchronization.close()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
|
@ -222,16 +246,25 @@ if __name__ == '__main__':
|
|||
default=0.05,
|
||||
type=float,
|
||||
help='set fixed delta seconds (default: 0.05s)')
|
||||
argparser.add_argument(
|
||||
'--sync-vehicle-lights',
|
||||
argparser.add_argument('--sync-vehicle-lights',
|
||||
action='store_true',
|
||||
help='synchronize vehicle lights state between simulations (default: False)')
|
||||
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-all',
|
||||
action='store_true',
|
||||
help='synchronize all vehicle properties (default: False)')
|
||||
argparser.add_argument('--debug', action='store_true', help='enable debug messages')
|
||||
arguments = argparser.parse_args()
|
||||
|
||||
if arguments.sync_all is True:
|
||||
arguments.sync_vehicle_lights = True
|
||||
arguments.sync_vehicle_color = True
|
||||
|
||||
if arguments.debug:
|
||||
logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.DEBUG)
|
||||
else:
|
||||
logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO)
|
||||
|
||||
main(arguments)
|
||||
synchronization_loop(arguments)
|
||||
|
|
|
|||
|
|
@ -59,10 +59,12 @@ class BridgeHelper(object):
|
|||
out_location = (out_location[0] - offset[0], out_location[1] - offset[1], out_location[2])
|
||||
|
||||
# Transform to carla reference system (left-handed system).
|
||||
return carla.Transform(
|
||||
out_transform = carla.Transform(
|
||||
carla.Location(out_location[0], -out_location[1], out_location[2]),
|
||||
carla.Rotation(out_rotation[0], out_rotation[1] - 90, out_rotation[2]))
|
||||
|
||||
return out_transform
|
||||
|
||||
@staticmethod
|
||||
def get_sumo_transform(in_carla_transform, extent):
|
||||
"""
|
||||
|
|
@ -83,10 +85,12 @@ class BridgeHelper(object):
|
|||
out_location = (out_location[0] + offset[0], out_location[1] - offset[1], out_location[2])
|
||||
|
||||
# Transform to sumo reference system.
|
||||
return carla.Transform(
|
||||
out_transform = carla.Transform(
|
||||
carla.Location(out_location[0], -out_location[1], out_location[2]),
|
||||
carla.Rotation(out_rotation[0], out_rotation[1] + 90, out_rotation[2]))
|
||||
|
||||
return out_transform
|
||||
|
||||
@staticmethod
|
||||
def _get_recommended_carla_blueprint(sumo_actor):
|
||||
"""
|
||||
|
|
@ -106,7 +110,7 @@ class BridgeHelper(object):
|
|||
return random.choice(blueprints)
|
||||
|
||||
@staticmethod
|
||||
def get_carla_blueprint(sumo_actor):
|
||||
def get_carla_blueprint(sumo_actor, sync_color=False):
|
||||
"""
|
||||
Returns an appropriate blueprint based on the received sumo actor.
|
||||
"""
|
||||
|
|
@ -128,7 +132,11 @@ class BridgeHelper(object):
|
|||
return None
|
||||
|
||||
if blueprint.has_attribute('color'):
|
||||
color = "{},{},{}".format(sumo_actor.color[0], sumo_actor.color[1], sumo_actor.color[2])
|
||||
if sync_color:
|
||||
color = "{},{},{}".format(sumo_actor.color[0], sumo_actor.color[1],
|
||||
sumo_actor.color[2])
|
||||
else:
|
||||
color = random.choice(blueprint.get_attribute('color').recommended_values)
|
||||
blueprint.set_attribute('color', color)
|
||||
|
||||
if blueprint.has_attribute('driver_id'):
|
||||
|
|
|
|||
|
|
@ -56,9 +56,9 @@ class CarlaSimulation(object):
|
|||
"""
|
||||
return self.world.get_actor(actor_id)
|
||||
|
||||
# This is a workaround to fix synchronization issues when other carla
|
||||
# clients remove an actor in carla without waiting for tick (e.g.,
|
||||
# running sumo co-simulation and manual control at the same time)
|
||||
# This is a workaround to fix synchronization issues when other carla clients remove an actor in
|
||||
# carla without waiting for tick (e.g., running sumo co-simulation and manual control at the
|
||||
# same time)
|
||||
def get_actor_light_state(self, actor_id):
|
||||
"""
|
||||
Accessor for carla actor light state.
|
||||
|
|
|
|||
|
|
@ -162,20 +162,23 @@ class SumoSimulation(object):
|
|||
"""
|
||||
traci.vehicle.unsubscribe(actor_id)
|
||||
|
||||
def get_net_offset(self):
|
||||
@staticmethod
|
||||
def get_net_offset():
|
||||
"""
|
||||
Accessor for sumo net offset.
|
||||
"""
|
||||
offset = traci.simulation.convertGeo(0, 0)
|
||||
return (-offset[0], -offset[1])
|
||||
|
||||
def get_step_length(self):
|
||||
@staticmethod
|
||||
def get_step_length():
|
||||
"""
|
||||
Accessor for sumo simulation step length.
|
||||
"""
|
||||
return traci.simulation.getDeltaT()
|
||||
|
||||
def get_actor(self, actor_id):
|
||||
@staticmethod
|
||||
def get_actor(actor_id):
|
||||
"""
|
||||
Accessor for sumo actor.
|
||||
"""
|
||||
|
|
@ -220,7 +223,7 @@ class SumoSimulation(object):
|
|||
return INVALID_ACTOR_ID
|
||||
|
||||
if attrs is not None:
|
||||
if 'color' in attrs:
|
||||
if self.args.sync_vehicle_color and 'color' in attrs:
|
||||
color = attrs['color'].split(',')
|
||||
traci.vehicle.setColor(actor_id, color)
|
||||
|
||||
|
|
@ -228,7 +231,8 @@ class SumoSimulation(object):
|
|||
|
||||
return actor_id
|
||||
|
||||
def destroy_actor(self, actor_id):
|
||||
@staticmethod
|
||||
def destroy_actor(actor_id):
|
||||
"""
|
||||
Destroys the given actor.
|
||||
"""
|
||||
|
|
@ -261,7 +265,8 @@ class SumoSimulation(object):
|
|||
self.spawned_actors = set(traci.simulation.getDepartedIDList())
|
||||
self.destroyed_actors = set(traci.simulation.getArrivedIDList())
|
||||
|
||||
def close(self):
|
||||
@staticmethod
|
||||
def close():
|
||||
"""
|
||||
Closes traci client.
|
||||
"""
|
||||
|
|
|
|||
|
|
@ -121,7 +121,6 @@ def main(args):
|
|||
try:
|
||||
world = client.get_world()
|
||||
vehicle_blueprints = world.get_blueprint_library().filter('vehicle.*')
|
||||
#walker_blueprints = world.get_blueprint_library().filter('walker.pedestrian.*')
|
||||
|
||||
transform = world.get_map().get_spawn_points()[0]
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue