try to merge opencda

ucla-mobility · Mar 8, 2022 · 6ff4176 · 6ff4176
1 parent 611af5b
commit 6ff4176
Show file tree

Hide file tree

Showing 5 changed files with 188 additions and 123 deletions.
diff --git a/opencda/core/common/vehicle_manager.py b/opencda/core/common/vehicle_manager.py
@@ -19,6 +19,7 @@
     import PerceptionManager
 from opencda.core.plan.behavior_agent \
     import BehaviorAgent
+from opencda.core.map.map_manager import MapManager
 from opencda.core.common.data_dumper import DataDumper
 
 
@@ -88,6 +89,7 @@ def __init__(
 
         # retrieve the configure for different modules
         sensing_config = config_yaml['sensing']
+        map_config = config_yaml['map_manager']
         behavior_config = config_yaml['behavior']
         control_config = config_yaml['controller']
         v2x_config = config_yaml['v2x']
@@ -101,6 +103,10 @@ def __init__(
         self.perception_manager = PerceptionManager(
             vehicle, sensing_config['perception'], cav_world,
             data_dumping)
+        # map manager
+        self.map_manager = MapManager(vehicle,
+                                      carla_map,
+                                      map_config)
 
         # behavior agent
         self.agent = None
@@ -172,6 +178,9 @@ def update_info(self):
         # object detection
         objects = self.perception_manager.detect(ego_pos)
 
+        # update the ego pose for map manager
+        self.map_manager.update_information(ego_pos)
+
         # update ego position and speed to v2x manager,
         # and then v2x manager will search the nearby cavs
         self.v2x_manager.update_info(ego_pos, ego_spd)
@@ -184,6 +193,8 @@ def run_step(self, target_speed=None):
         """
         Execute one step of navigation.
         """
+        # visualize the bev map if needed
+        self.map_manager.run_step()
         target_speed, target_pos = self.agent.run_step(target_speed)
         control = self.controller.run_step(target_speed, target_pos)