diff --git a/Co-Simulation/Sumo/run_synchronization.py b/Co-Simulation/Sumo/run_synchronization.py index 583e3cb3e..b49d802e2 100644 --- a/Co-Simulation/Sumo/run_synchronization.py +++ b/Co-Simulation/Sumo/run_synchronization.py @@ -53,114 +53,152 @@ 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) - # Mapped actor ids. - sumo2carla_ids = {} # Contains only actors controlled by sumo. - carla2sumo_ids = {} # Contains only actors controlled by carla. + def __init__(self, args): + self.args = args - BridgeHelper.blueprint_library = carla.world.get_blueprint_library() - BridgeHelper.offset = sumo.get_net_offset() + self.sumo = SumoSimulation(args) + self.carla = CarlaSimulation(args) + # 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() + + 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) + + 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 = 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) + 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 + + self.carla.synchronize_vehicle(carla_actor_id, carla_transform, carla_lights) + + # ----------------- + # 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) + if type_id is not None: + sumo_actor_id = self.sumo.spawn_actor(type_id, carla_actor.attributes) + 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) + 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) + else: + sumo_lights = None + else: + sumo_lights = None + + 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() - # ----------------- - # sumo-->carla sync - # ----------------- - sumo.tick() - - # Spawning new sumo actors in carla (i.e, not controlled by carla). - sumo_spawned_actors = sumo.spawned_actors - set(carla2sumo_ids.values()) - for sumo_actor_id in sumo_spawned_actors: - SumoSimulation.subscribe(sumo_actor_id) - sumo_actor = sumo.get_actor(sumo_actor_id) - - carla_blueprint = BridgeHelper.get_carla_blueprint(sumo_actor) - 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) - if carla_actor_id != INVALID_ACTOR_ID: - sumo2carla_ids[sumo_actor_id] = carla_actor_id - else: - SumoSimulation.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)) - - # Updating sumo actors in carla. - for sumo_actor_id in sumo2carla_ids: - carla_actor_id = sumo2carla_ids[sumo_actor_id] - - sumo_actor = sumo.get_actor(sumo_actor_id) - carla_actor = carla.get_actor(carla_actor_id) - - carla_transform = BridgeHelper.get_carla_transform(sumo_actor.transform, - sumo_actor.extent) - if 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) - - # ----------------- - # carla-->sumo sync - # ----------------- - carla.tick() - - # Spawning new carla actors (not controlled by sumo) - carla_spawned_actors = carla.spawned_actors - set(sumo2carla_ids.values()) - for carla_actor_id in carla_spawned_actors: - carla_actor = 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) - if sumo_actor_id != INVALID_ACTOR_ID: - carla2sumo_ids[carla_actor_id] = sumo_actor_id - 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)) - - # Updating carla actors in sumo. - for carla_actor_id in carla2sumo_ids: - sumo_actor_id = carla2sumo_ids[carla_actor_id] - - carla_actor = carla.get_actor(carla_actor_id) - sumo_actor = 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 carla_lights is not None: - sumo_lights = BridgeHelper.get_sumo_lights_state( - sumo_actor.signals, carla_lights) - else: - sumo_lights = None - else: - sumo_lights = None - - sumo.synchronize_vehicle(sumo_actor_id, sumo_transform, sumo_lights) + 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', - action='store_true', - help='synchronize vehicle lights state between simulations (default: False)') + 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-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) diff --git a/Co-Simulation/Sumo/sumo_integration/bridge_helper.py b/Co-Simulation/Sumo/sumo_integration/bridge_helper.py index 836f491cd..14ad12602 100644 --- a/Co-Simulation/Sumo/sumo_integration/bridge_helper.py +++ b/Co-Simulation/Sumo/sumo_integration/bridge_helper.py @@ -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'): diff --git a/Co-Simulation/Sumo/sumo_integration/carla_simulation.py b/Co-Simulation/Sumo/sumo_integration/carla_simulation.py index 0dfb9900f..c22e33bed 100644 --- a/Co-Simulation/Sumo/sumo_integration/carla_simulation.py +++ b/Co-Simulation/Sumo/sumo_integration/carla_simulation.py @@ -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. diff --git a/Co-Simulation/Sumo/sumo_integration/sumo_simulation.py b/Co-Simulation/Sumo/sumo_integration/sumo_simulation.py index 1e7f775ef..3b212b55b 100644 --- a/Co-Simulation/Sumo/sumo_integration/sumo_simulation.py +++ b/Co-Simulation/Sumo/sumo_integration/sumo_simulation.py @@ -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. """ diff --git a/Co-Simulation/Sumo/util/create_sumo_vtypes.py b/Co-Simulation/Sumo/util/create_sumo_vtypes.py index 87e304325..6c3e17a57 100644 --- a/Co-Simulation/Sumo/util/create_sumo_vtypes.py +++ b/Co-Simulation/Sumo/util/create_sumo_vtypes.py @@ -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]