Simulator: Speed up process by removing second camera (#24679)

* simulator: support running wide camera only

* proper frame id

* use param name that makes more sense

* do some cleanup

* Update tools/sim/bridge.py

Co-authored-by: Willem Melching <willem.melching@gmail.com>
old-commit-hash: b5aed2bf6781cd4d74d28f6f44d24737ec08f0db

common/params.cc

@@ -104,7 +104,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"DoReboot", CLEAR_ON_MANAGER_START},
     {"DoShutdown", CLEAR_ON_MANAGER_START},
     {"DoUninstall", CLEAR_ON_MANAGER_START},
-    {"EnableWideCamera", CLEAR_ON_MANAGER_START},
     {"EndToEndToggle", PERSISTENT},
     {"ForcePowerDown", CLEAR_ON_MANAGER_START},
     {"GitBranch", PERSISTENT},
@@ -159,6 +158,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"UpdateFailedCount", CLEAR_ON_MANAGER_START},
     {"Version", PERSISTENT},
     {"VisionRadarToggle", PERSISTENT},
+    {"WideCameraOnly", PERSISTENT},
     {"ApiCache_Device", PERSISTENT},
     {"ApiCache_DriveStats", PERSISTENT},
     {"ApiCache_NavDestinations", PERSISTENT},

selfdrive/controls/plannerd.py

@@ -18,7 +18,7 @@ def plannerd_thread(sm=None, pm=None):
   cloudlog.info("plannerd got CarParams: %s", CP.carName)
 
   use_lanelines = not params.get_bool('EndToEndToggle')
-  wide_camera = params.get_bool('EnableWideCamera') if TICI else False
+  wide_camera = params.get_bool('WideCameraOnly')
 
   cloudlog.event("e2e mode", on=use_lanelines)
 

selfdrive/locationd/calibrationd.py

@@ -20,7 +20,6 @@ from common.realtime import set_realtime_priority
 from common.transformations.model import model_height
 from common.transformations.camera import get_view_frame_from_road_frame
 from common.transformations.orientation import rot_from_euler, euler_from_rot
-from selfdrive.hardware import TICI
 from selfdrive.swaglog import cloudlog
 
 MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS
@@ -68,7 +67,7 @@ class Calibrator:
     # Read saved calibration
     params = Params()
     calibration_params = params.get("CalibrationParams")
-    self.wide_camera = TICI and params.get_bool('EnableWideCamera')
+    self.wide_camera = params.get_bool('WideCameraOnly')
     rpy_init = RPY_INIT
     valid_blocks = 0
 

selfdrive/modeld/modeld.cc

@@ -171,7 +171,7 @@ int main(int argc, char **argv) {
     assert(ret == 0);
   }
 
-  bool main_wide_camera = Params().getBool("EnableWideCamera");
+  bool main_wide_camera = Params().getBool("WideCameraOnly");
   bool use_extra_client = !main_wide_camera;  // set for single camera mode
 
   // cl init

selfdrive/test/process_replay/process_replay.py

@@ -350,7 +350,7 @@ def setup_env(simulation=False):
   params.put_bool("OpenpilotEnabledToggle", True)
   params.put_bool("Passive", False)
   params.put_bool("DisengageOnAccelerator", True)
-  params.put_bool("EnableWideCamera", False)
+  params.put_bool("WideCameraOnly", False)
   params.put_bool("DisableLogging", False)
 
   os.environ["NO_RADAR_SLEEP"] = "1"

selfdrive/ui/qt/onroad.cc

@@ -91,7 +91,7 @@ void OnroadWindow::offroadTransition(bool offroad) {
   alerts->updateAlert({}, bg);
 
   // update stream type
-  bool wide_cam = Hardware::TICI() && Params().getBool("EnableWideCamera");
+  bool wide_cam = Params().getBool("WideCameraOnly");
   nvg->setStreamType(wide_cam ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD);
 }
 

selfdrive/ui/ui.cc

@@ -222,7 +222,7 @@ void UIState::updateStatus() {
       status = STATUS_DISENGAGED;
       scene.started_frame = sm->frame;
       scene.end_to_end = Params().getBool("EndToEndToggle");
-      wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false;
+      wide_camera = Params().getBool("WideCameraOnly");
     }
     started_prev = scene.started;
     emit offroadTransition(!scene.started);
@@ -237,7 +237,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
   });
 
   Params params;
-  wide_camera = Hardware::TICI() ? params.getBool("EnableWideCamera") : false;
+  wide_camera = params.getBool("WideCameraOnly");
   prime_type = std::atoi(params.get("PrimeType").c_str());
 
   // update timer

tools/sim/bridge.py

@@ -36,6 +36,7 @@ def parse_args(add_args=None):
   parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.')
   parser.add_argument('--joystick', action='store_true')
   parser.add_argument('--high_quality', action='store_true')
+  parser.add_argument('--dual_camera', action='store_true')
   parser.add_argument('--town', type=str, default='Town04_Opt')
   parser.add_argument('--spawn_point', dest='num_selected_spawn_point', type=int, default=16)
 
@@ -112,7 +113,7 @@ class Camerad:
 
     dat = messaging.new_message(pub_type)
     msg = {
-      "frameId": image.frame,
+      "frameId": frame_id,
       "transform": [1.0, 0.0, 0.0,
                     0.0, 1.0, 0.0,
                     0.0, 0.0, 1.0]
@@ -239,6 +240,7 @@ class CarlaBridge:
     msg.liveCalibration.validBlocks = 20
     msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0]
     Params().put("CalibrationParams", msg.to_bytes())
+    Params().put_bool("WideCameraOnly", not arguments.dual_camera)
 
     self._args = arguments
     self._carla_objects = []
@@ -333,10 +335,13 @@ class CarlaBridge:
       return camera
 
     self._camerad = Camerad()
-    road_camera = create_camera(fov=40, callback=self._camerad.cam_callback_road)
-    road_wide_camera = create_camera(fov=120, callback=self._camerad.cam_callback_wide_road)  # fov bigger than 120 shows unwanted artifacts
 
-    self._carla_objects.extend([road_camera, road_wide_camera])
+    if self._args.dual_camera:
+      road_camera = create_camera(fov=40, callback=self._camerad.cam_callback_road)
+      self._carla_objects.append(road_camera)
+
+    road_wide_camera = create_camera(fov=120, callback=self._camerad.cam_callback_wide_road)  # fov bigger than 120 shows unwanted artifacts
+    self._carla_objects.append(road_wide_camera)
 
     vehicle_state = VehicleState()
 
@@ -504,6 +509,7 @@ class CarlaBridge:
   def close(self):
     self.started = False
     self._exit_event.set()
+
     for s in self._carla_objects:
       try:
         s.destroy()
@@ -522,17 +528,23 @@ if __name__ == "__main__":
   q: Any = Queue()
   args = parse_args()
 
-  carla_bridge = CarlaBridge(args)
-  p = carla_bridge.run(q)
+  try:
+    carla_bridge = CarlaBridge(args)
+    p = carla_bridge.run(q)
 
-  if args.joystick:
-    # start input poll for joystick
-    from tools.sim.lib.manual_ctrl import wheel_poll_thread
+    if args.joystick:
+      # start input poll for joystick
+      from tools.sim.lib.manual_ctrl import wheel_poll_thread
 
-    wheel_poll_thread(q)
-  else:
-    # start input poll for keyboard
-    from tools.sim.lib.keyboard_ctrl import keyboard_poll_thread
+      wheel_poll_thread(q)
+    else:
+      # start input poll for keyboard
+      from tools.sim.lib.keyboard_ctrl import keyboard_poll_thread
 
-    keyboard_poll_thread(q)
-  p.join()
+      keyboard_poll_thread(q)
+    p.join()
+
+  finally:
+    # Try cleaning up the wide camera param
+    # in case users want to use replay after
+    Params().delete("WideCameraOnly")