def manual_connect_request_handler(self, channel, data):
print("Received message on channel ", channel)
msg = manual_connect_request.decode(data)
transform = BridgeHelper.transform_LCM_to_SUMO_Waypoint(msg.init_pos)
min_dist = -1
min_id = None
for id in self.carla2sumo_ids.values():
print("sumo id: ", id)
try:
sumo_transform = BridgeHelper.transform_LCM_to_SUMO_Waypoint(
msg.init_pos)
(x1, y1) = (sumo_transform.location.x,
sumo_transform.location.y)
(x2, y2) = self.sumo.getPosition(id)
print(x1, y1, x2, y2)
dist = BridgeHelper.calc_point_square_distance(x1, y1, x2, y2)
print("dist: ", dist)
if min_dist == -1 or min_dist > dist:
min_dist = dist
min_id = id
except Exception:
print("error.")
# print("dist: ", dist)
print("final id for manual client: ", min_id)
self.manual_client_ids.append(min_id)
response = manual_connect_response()
response.vehicle_id = min_id
self.lc.publish(manual_connect_response_keyword, response.encode())
def synchronize_vehicles(self):
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)
def tick(self):
"""
Tick to simulation synchronization
"""
# -----------------
# sumo-->carla sync
# -----------------
self.sumo.tick()
# # Destroy all sensors if the number of sensors exceeds a certain amount
# if self.carla.num_of_sensors() > 80:
# self.carla.destroy_all_sensors()
# self.sumo.sensor_to_spawn = self.sumo.perception_actors
# self.sumo.sensor_to_stop = set()
# Return if Simulation finished
if self.sumo.ego_vehicle_state == 2:
return
# 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.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 sumo_actor_id == self.sumo.ego_vehicle:
self.carla.ego_vehicle_id = carla_actor_id
ego_vehicle = self.carla.world.get_actor(carla_actor_id)
extent = ego_vehicle.bounding_box.extent
filename = self.sensor_cfg['root_path'] + "/" + str(
carla_actor_id) + ".ego"
with open(filename, 'w') as f:
f.write('{:.3f}, {:.3f}, {:.3f}'.format(
extent.x * 2,
extent.y * 2,
extent.y * 2,
))
print('\nEgo id:', carla_actor_id)
if carla_actor_id != INVALID_ACTOR_ID:
self.sumo2carla_ids[sumo_actor_id] = carla_actor_id
else:
self.sumo.unsubscribe(sumo_actor_id)
# if self.sumo.ego_vehicle_state==1:
# self.carla.synchronize_sensors()
# Spawn sensors for perception nodes in carla
for sumo_actor_id in self.sumo.sensor_to_spawn:
if int(sumo_actor_id) < 50:
carla_actor_id = self.sumo2carla_ids[sumo_actor_id]
self.carla.spawn_sensors_for(carla_actor_id)
# Stop sensors for perception nodes in carla
for sumo_actor_id in self.sumo.sensor_to_stop:
if int(sumo_actor_id) < 50:
carla_actor_id = self.sumo2carla_ids[sumo_actor_id]
self.carla.stop_sensors_for(carla_actor_id)
# Destroying sumo arrived actors in carla.
if len(self.sumo.destroyed_actors) > 0:
print('Distroyed actors in carla:', self.sumo.destroyed_actors)
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.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)
# Updates traffic lights in carla based on sumo information.
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)
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)
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, 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)
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, carla_lights)
else:
sumo_lights = None
else:
sumo_lights = None
self.sumo.synchronize_vehicle(sumo_actor_id, sumo_transform,
sumo_lights)
# Updates traffic lights in sumo based on carla information.
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)
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 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.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.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)
# Updates traffic lights in carla based on sumo information.
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)
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)
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, 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)
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,
carla_lights)
else:
sumo_lights = None
else:
sumo_lights = None
self.sumo.synchronize_vehicle(sumo_actor_id, sumo_transform, sumo_lights)
# Updates traffic lights in sumo based on carla information.
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)
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 tick(self):
"""
Tick to simulation synchronization
"""
# -----------------
# sumo-->carla sync
# -----------------
try:
del_id = self.del_id.get(timeout=0.001)
print("del id is ", del_id)
# self.sumo_id_lock.acquire()
# del_carla_id = self.sumo2carla_ids[del_id]
# self.sumo_id_lock.release()
# print("del carla id is ", del_carla_id)
# self.sumo.unsubscribe(del_id)
# self.sumo.destroy_actor(del_id)
except queue.Empty:
pass
self.sumo.tick()
# Spawning new sumo actors in carla (i.e, not controlled by carla).
# 客户端车辆不在此处直接生成，而是通过connect_response发送报文从客户端生成
sumo_spawned_actors = self.sumo.spawned_actors - set(
self.carla2sumo_ids.values()) - set(self.new_clients)
for sumo_actor_id in self.new_clients:
self.sumo.subscribe(sumo_actor_id)
sumo_actor = self.sumo.get_actor(sumo_actor_id)
waypoint = BridgeHelper.transform_SUMO_to_LCM_Waypoint(
sumo_actor.transform)
connect_res = connect_response()
connect_res.init_pos = waypoint
connect_res.vehicle_id = sumo_actor_id
self.lc.publish(connect_response_keyword, connect_res.encode())
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.sumo_id_lock.acquire()
self.sumo2carla_ids[sumo_actor_id] = carla_actor_id
self.sumo_id_lock.release()
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.
self.sumo_id_lock.acquire()
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)
self.sumo_id_lock.release()
# -----------------
# carla-->sumo sync
# -----------------
self.carla.tick()
# Spawning new carla actors (not controlled by sumo)
self.sumo_id_lock.acquire()
sumo2carla_ids = self.sumo2carla_ids
self.sumo_id_lock.release()
carla_spawned_actors = self.carla.spawned_actors - set(
sumo2carla_ids.values()) - set(self.client_carla_ids)
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 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)