Merge pull request ucla-mobility#133 from ucla-mobility/feature/infra…

…_manager

Intelligent roadside units added.

.gitignore

@@ -148,4 +148,4 @@ evaluation_outputs/
 docs/tmp.py
 data_dumping
 video_dumping
-
+opv2v_data_dumping

opencda/core/common/cav_world.py

@@ -29,6 +29,9 @@ class CavWorld(object):
  _platooning_dict : dict
  A dictionary that stores platooning managers.
 
+ _rsu_manager_dict : dict
+ A dictionary that stores RSU managers.
+
  ml_manager : opencda object.
  The machine learning manager class.
  """
@@ -38,6 +41,7 @@ def __init__(self, apply_ml=False):
  self.vehicle_id_set = set()
  self._vehicle_manager_dict = {}
  self._platooning_dict = {}
+ self._rsu_manager_dict = {}
  self.ml_manager = None
 
  if apply_ml:
@@ -76,6 +80,17 @@ def update_platooning(self, platooning_manger):
  self._platooning_dict.update(
  {platooning_manger.pmid: platooning_manger})
 
+ def update_rsu_manager(self, rsu_manager):
+ """
+ Add rsu manager.
+
+ Parameters
+ ----------
+ rsu_manager : opencda object
+ The RSU manager class.
+ """
+ self._rsu_manager_dict.update({rsu_manager.rid: rsu_manager})
+
  def update_sumo_vehicles(self, sumo2carla_ids):
  """
  Update the sumo carla mapping dict. This is only called

opencda/core/common/data_dumper.py

@@ -201,9 +201,13 @@ def save_yaml_file(self,
 
  dump_yml.update({'vehicles': vehicle_dict})
 
- # dump ego pose and speed
+ # dump ego pose and speed, if vehicle does not exist, then it is
+ # a rsu(road side unit).
  predicted_ego_pos = localization_manager.get_ego_pos()
- true_ego_pos = localization_manager.vehicle.get_transform()
+ true_ego_pos = localization_manager.vehicle.get_transform() \
+ if hasattr(localization_manager, 'vehicle') \
+ else localization_manager.true_ego_pos
+
  dump_yml.update({'predicted_ego_pos': [
  predicted_ego_pos.location.x,
  predicted_ego_pos.location.y,
@@ -261,19 +265,23 @@ def save_yaml_file(self,
  camera_param.update({'extrinsic': lidar2camera})
  dump_yml.update({'camera%d' % i: camera_param})
 
- # dump the planned trajectory
- trajectory_deque = behavior_agent.get_local_planner().get_trajectory()
- trajectory_list = []
+ dump_yml.update({'RSU': True})
+ # dump the planned trajectory if it exisit.
+ if behavior_agent is not None:
+ trajectory_deque = \
+ behavior_agent.get_local_planner().get_trajectory()
+ trajectory_list = []
 
- for i in range(len(trajectory_deque)):
- tmp_buffer = trajectory_deque.popleft()
- x = tmp_buffer[0].location.x
- y = tmp_buffer[0].location.y
- spd = tmp_buffer[1]
+  for i in range(len(trajectory_deque)):
+  tmp_buffer = trajectory_deque.popleft()
+  x = tmp_buffer[0].location.x
+  y = tmp_buffer[0].location.y
+  spd = tmp_buffer[1]
 
- trajectory_list.append([x, y, spd])
+  trajectory_list.append([x, y, spd])
 
- dump_yml.update({'plan_trajectory': trajectory_list})
+ dump_yml.update({'plan_trajectory': trajectory_list})
+ dump_yml.update({'RSU': False})
 
  yml_name = '%06d' % frame + '.yaml'
  save_path = os.path.join(self.save_parent_folder,

opencda/core/common/rsu_manager.py

@@ -0,0 +1,127 @@
+# -*- coding: utf-8 -*-
+"""
+Basic class for RSU(Roadside Unit) management.
+"""
+# Author: Runsheng Xu <[email protected]>
+# License: TDG-Attribution-NonCommercial-NoDistrib
+
+from opencda.core.common.data_dumper import DataDumper
+from opencda.core.sensing.perception.perception_manager import \
+ PerceptionManager
+from opencda.core.sensing.localization.rsu_localization_manager import \
+ LocalizationManager
+
+
+class RSUManager(object):
+ """
+ A class manager for RSU. Currently a RSU only has perception and
+ localization modules to dump sensing information.
+ TODO: add V2X module to it to enable sharing sensing information online.
+
+ Parameters
+ ----------
+ carla_world : carla.World
+ CARLA simulation world, we need this for blueprint creation.
+
+ config_yaml : dict
+ The configuration dictionary of the RSU.
+
+ carla_map : carla.Map
+ The CARLA simulation map.
+
+ cav_world : opencda object
+ CAV World for simulation V2X communication.
+
+ current_time : str
+ Timestamp of the simulation beginning, this is used for data dump.
+
+ data_dumping : bool
+ Indicates whether to dump sensor data during simulation.
+
+ Attributes
+ ----------
+ localizer : opencda object
+ The current localization manager.
+
+ perception_manager : opencda object
+ The current V2X perception manager.
+
+ data_dumper : opencda object
+ Used for dumping sensor data.
+ """
+ def __init__(
+ self,
+ carla_world,
+ config_yaml,
+ carla_map,
+ cav_world,
+ current_time='',
+ data_dumping=False):
+
+ self.rid = config_yaml['id']
+ # The id of rsu is always a negative int
+ if self.rid > 0:
+ self.rid = -self.rid
+
+ # read map from the world everytime is time-consuming, so we need
+ # explicitly extract here
+ self.carla_map = carla_map
+
+ # retrieve the configure for different modules
+ # todo: add v2x module to rsu later
+ sensing_config = config_yaml['sensing']
+ sensing_config['localization']['global_position'] = \
+ config_yaml['spawn_position']
+ sensing_config['perception']['global_position'] = \
+ config_yaml['spawn_position']
+
+ # localization module
+ self.localizer = LocalizationManager(carla_world,
+ sensing_config['localization'],
+ self.carla_map)
+ # perception module
+ self.perception_manager = PerceptionManager(vehicle=None,
+ config_yaml=sensing_config['perception'],
+ cav_world=cav_world,
+ carla_world=carla_world,
+ data_dump=data_dumping,
+ infra_id=self.rid)
+ if data_dumping:
+ self.data_dumper = DataDumper(self.perception_manager,
+ self.rid,
+ save_time=current_time)
+ else:
+ self.data_dumper = None
+
+ cav_world.update_rsu_manager(self)
+
+ def update_info(self):
+ """
+ Call perception and localization module to
+ retrieve surrounding info an ego position.
+ """
+ # localization
+ self.localizer.localize()
+
+ ego_pos = self.localizer.get_ego_pos()
+ ego_spd = self.localizer.get_ego_spd()
+
+ # object detection todo: pass it to other CAVs for V2X percetion
+ objects = self.perception_manager.detect(ego_pos)
+
+ def run_step(self):
+ """
+ Currently only used for dumping data.
+ """
+ # dump data
+ if self.data_dumper:
+ self.data_dumper.run_step(self.perception_manager,
+ self.localizer,
+ None)
+
+ def destroy(self):
+ """
+ Destroy the actor vehicle
+ """
+ self.perception_manager.destroy()
+ self.localizer.destroy()

opencda/core/common/v2x_manager.py

@@ -79,6 +79,7 @@ def __init__(self, cav_world, config_yaml, vid):
  self.speed_noise = 0.0
  self.lag = 0
 
+ # Add noise to V2X communication if needed.
  if 'loc_noise' in config_yaml:
  self.loc_noise = config_yaml['loc_noise']
  if 'yaw_noise' in config_yaml:

opencda/core/common/vehicle_manager.py

@@ -32,7 +32,7 @@ class VehicleManager(object):
  The carla.Vehicle. We need this class to spawn our gnss and imu sensor.
 
  config_yaml : dict
- The configuration dictionary of the localization module.
+ The configuration dictionary of this CAV.
 
  application : list
  The application category, currently support:['single','platoon'].

opencda/core/sensing/localization/rsu_localization_manager.py

@@ -0,0 +1,161 @@
+# -*- coding: utf-8 -*-
+"""
+Localization module for RSU.
+"""
+# Author: Runsheng Xu <[email protected]>
+# License: TDG-Attribution-NonCommercial-NoDistrib
+
+import weakref
+from collections import deque
+
+import carla
+
+from opencda.core.common.misc import get_speed
+from opencda.core.sensing.localization.coordinate_transform \
+ import geo_to_transform
+
+
+class GnssSensor(object):
+ """
+ The default GNSS sensor module for rsu.
+
+ Parameters
+ world : carla.world
+ Carla world.
+
+ config : dict
+ The configuration dictionary of the localization module.
+
+ global_position : list
+ The global position of the rsu.
+
+ Attributes
+
+ blueprint : carla.blueprint
+ The current blueprint of the sensor actor.
+
+ weak_self : opencda Object
+ A weak reference point to avoid circular reference.
+
+ sensor : CARLA actor
+ The current sensor actors that will be attach to the vehicles.
+ """
+
+ def __init__(self, world, config, global_position):
+ blueprint = world.get_blueprint_library().find('sensor.other.gnss')
+
+ # set the noise for gps
+ blueprint.set_attribute(
+ 'noise_alt_stddev', str(
+ config['noise_alt_stddev']))
+ blueprint.set_attribute(
+ 'noise_lat_stddev', str(
+ config['noise_lat_stddev']))
+ blueprint.set_attribute(
+ 'noise_lon_stddev', str(
+ config['noise_lon_stddev']))
+ # spawn the sensor
+ self.sensor = world.spawn_actor(
+ blueprint,
+ carla.Transform(
+ carla.Location(
+ x=global_position[0],
+ y=global_position[1],
+ z=global_position[2])))
+
+ # latitude and longitude at current timestamp
+ self.lat, self.lon, self.alt, self.timestamp = 0.0, 0.0, 0.0, 0.0
+ # create weak reference to avoid circular reference
+ weak_self = weakref.ref(self)
+ self.sensor.listen(
+ lambda event: GnssSensor._on_gnss_event(
+ weak_self, event))
+
+ @staticmethod
+ def _on_gnss_event(weak_self, event):
+ """GNSS method that returns the current geo location."""
+ self = weak_self()
+ if not self:
+ return
+ self.lat = event.latitude
+ self.lon = event.longitude
+ self.alt = event.altitude
+ self.timestamp = event.timestamp
+
+
+class LocalizationManager(object):
+ """
+ Default localization module for infrastructure.
+
+ Parameters
+ world : carla.world
+ CARLA world.
+ config_yaml : dict
+ The configuration dictionary of the localization module.
+ carla_map : carla.Map
+ The carla HDMap. We need this to find the map origin to
+ convert wg84 to enu coordinate system.
+
+ Attributes
+ gnss : opencda object
+ GNSS sensor manager for spawning gnss sensor and listen to the data
+ transmission.
+ """
+
+ def __init__(self, world, config_yaml, carla_map):
+
+ self.activate = config_yaml['activate']
+ self.map = carla_map
+ self.geo_ref = self.map.transform_to_geolocation(
+ carla.Location(x=0, y=0, z=0))
+
+ # speed and transform and current timestamp
+ self._ego_pos = None
+ self._speed = 0
+
+ # history track
+ self._ego_pos_history = deque(maxlen=100)
+ self._timestamp_history = deque(maxlen=100)
+
+ self.gnss = GnssSensor(world,
+ config_yaml['gnss'],
+ config_yaml['global_position'])
+ self.true_ego_pos = carla.Transform(
+ carla.Location(x=config_yaml['global_position'][0],
+ y=config_yaml['global_position'][1],
+ z=config_yaml['global_position'][2]))
+ self._speed = 0
+
+ def localize(self):
+ """
+ Currently implemented in a naive way.
+ """
+
+ if not self.activate:
+ self._ego_pos = self.true_ego_pos
+ else:
+ x, y, z = geo_to_transform(self.gnss.lat,
+ self.gnss.lon,
+ self.gnss.alt,
+ self.geo_ref.latitude,
+ self.geo_ref.longitude, 0.0)
+ self._ego_pos = carla.Transform(
+ carla.Location(x=x, y=y, z=z))
+
+ def get_ego_pos(self):
+ """
+ Retrieve ego vehicle position
+ """
+ return self._ego_pos
+
+ def get_ego_spd(self):
+ """
+ Retrieve ego vehicle speed
+ """
+ return self._speed
+
+ def destroy(self):
+ """
+ Destroy the sensors
+ """
+ self.gnss.sensor.destroy()