metadrive: fix dual_camera mode (#30743)

* Use wide cam in metadrive

* 120 fov
old-commit-hash: 5fe9f1459c

tools/sim/bridge/metadrive/metadrive_bridge.py

@@ -36,7 +36,7 @@ class RGBCameraWide(CopyRamRGBCamera):
     cam = self.get_cam()
     cam.setPos(C3_POSITION)
     lens = self.get_lens()
-    lens.setFov(160)
+    lens.setFov(120)
 
 class RGBCameraRoad(CopyRamRGBCamera):
   def __init__(self, *args, **kwargs):
@@ -116,4 +116,4 @@ class MetaDriveBridge(SimulatorBridge):
       preload_models=False
     )
 
-    return MetaDriveWorld(config)
+    return MetaDriveWorld(config, self.dual_camera)

tools/sim/bridge/metadrive/metadrive_process.py

@@ -39,10 +39,13 @@ def apply_metadrive_patches():
 
   MetaDriveEnv._is_arrive_destination = arrive_destination_patch
 
-def metadrive_process(dual_camera: bool, config: dict, camera_array, controls_recv: Connection, state_send: Connection, exit_event):
+def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera_array, controls_recv: Connection, state_send: Connection, exit_event):
   apply_metadrive_patches()
 
   road_image = np.frombuffer(camera_array.get_obj(), dtype=np.uint8).reshape((H, W, 3))
+  if dual_camera:
+    assert wide_camera_array is not None
+    wide_road_image = np.frombuffer(wide_camera_array.get_obj(), dtype=np.uint8).reshape((H, W, 3))
 
   env = MetaDriveEnv(config)
 
@@ -92,8 +95,8 @@ def metadrive_process(dual_camera: bool, config: dict, camera_array, controls_re
       if terminated:
         reset()
 
-      #if dual_camera:
-      #  wide_road_image = get_cam_as_rgb("rgb_wide")
+      if dual_camera:
+        wide_road_image[...] = get_cam_as_rgb("rgb_wide")
       road_image[...] = get_cam_as_rgb("rgb_road")
 
     rk.keep_time()

tools/sim/bridge/metadrive/metadrive_world.py

@@ -15,6 +15,10 @@ class MetaDriveWorld(World):
     super().__init__(dual_camera)
     self.camera_array = Array(ctypes.c_uint8, W*H*3)
     self.road_image = np.frombuffer(self.camera_array.get_obj(), dtype=np.uint8).reshape((H, W, 3))
+    self.wide_camera_array = None
+    if dual_camera:
+      self.wide_camera_array = Array(ctypes.c_uint8, W*H*3)
+      self.wide_road_image = np.frombuffer(self.wide_camera_array.get_obj(), dtype=np.uint8).reshape((H, W, 3))
 
     self.controls_send, self.controls_recv = Pipe()
     self.state_send, self.state_recv = Pipe()
@@ -23,7 +27,7 @@ class MetaDriveWorld(World):
 
     self.metadrive_process = multiprocessing.Process(name="metadrive process", target=
                               functools.partial(metadrive_process, dual_camera, config,
-                                                self.camera_array, self.controls_recv, self.state_send, self.exit_event))
+                                                self.camera_array, self.wide_camera_array, self.controls_recv, self.state_send, self.exit_event))
     self.metadrive_process.start()
 
     print("----------------------------------------------------------")