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)