Record roadcam, test dynamic motion cost & map_renderer, add carrot path filter count...

2025-02-06 12:14:17 +09:00 · 2025-02-06 12:14:17 +09:00 · 1bb8fcdb4f
commit 1bb8fcdb4f
parent ff47fce0ba
8 changed files with 91 additions and 33 deletions
--- a/common/params.cc
+++ b/common/params.cc
@ -217,8 +217,6 @@ std::unordered_map<std::string, uint32_t> keys = {
    {"GMapKey", PERSISTENT},
    {"SearchInput", PERSISTENT},

-
-
    {"CarSelected3", PERSISTENT},
    {"SupportedCars", PERSISTENT},
    {"SupportedCars_gm", PERSISTENT},
@ -236,6 +234,8 @@ std::unordered_map<std::string, uint32_t> keys = {
    {"ShowPathModeLane", PERSISTENT},
    {"ShowPathColorLane", PERSISTENT},
    {"ShowPlotMode", PERSISTENT},
+    {"RecordRoadCam", PERSISTENT },
+
    {"AutoCruiseControl", PERSISTENT},
    {"CruiseEcoControl", PERSISTENT},
    {"AutoGasTokSpeed", PERSISTENT},
@ -309,9 +309,11 @@ std::unordered_map<std::string, uint32_t> keys = {
    {"LaneChangeNeedTorque", PERSISTENT},
    {"MaxAngleFrames", PERSISTENT},
    {"SoftHoldMode", PERSISTENT},
-    {"CarrotLatControl", PERSISTENT },
+    {"CarrotLatControl", PERSISTENT},
+    {"CarrotLatFilter", PERSISTENT},
    {"LatMpcPathCost", PERSISTENT },
    {"LatMpcMotionCost", PERSISTENT },
+    {"LatMpcMotionCost2", PERSISTENT },
    {"LatMpcAccelCost", PERSISTENT},
    {"LatMpcJerkCost", PERSISTENT},
    {"LatMpcSteeringRateCost", PERSISTENT},
--- a/selfdrive/carrot_settings.json
+++ b/selfdrive/carrot_settings.json
@ -66,6 +66,19 @@
      "default": 1,
      "unit": 1
    },
+    {
+      "group": "조향튜닝",
+      "name": "CarrotLatFilter",
+      "title": "*당근조향제어필터(5)",
+      "descr": "필터링개수",
+      "egroup": "LAT",
+      "etitle": "*Carrot lateral filter(5)",
+      "edescr": "",
+      "min": 2,
+      "max": 30,
+      "default": 5,
+      "unit": 1
+    },
    {
      "group": "조향튜닝",
      "name": "LatMpcPathCost",
@ -92,6 +105,19 @@
      "default": 11,
      "unit": 1
    },
+    {
+      "group": "조향튜닝",
+      "name": "LatMpcMotionCost2",
+      "title": "*LATMPC.MotionCost2(11)",
+      "descr": "헤딩가중치",
+      "egroup": "LAT",
+      "etitle": "*LATMPC.MotionCost2(11)",
+      "edescr": "",
+      "min": 0,
+      "max": 500,
+      "default": 11,
+      "unit": 1
+    },
    {
      "group": "조향튜닝",
      "name": "LatMpcAccelCost",
@ -625,6 +651,19 @@
      "default": 0,
      "unit": 1
    },
+    {
+      "group": "시작",
+      "name": "RecordRoadCam",
+      "title": "RoadCam녹화",
+      "descr": "1:RoadCam, 2:WideRoadCam",
+      "egroup": "START",
+      "etitle": "Record RoadCam",
+      "edescr": "1:RoadCam, 2:WideRoadCam",
+      "min": 0,
+      "max": 2,
+      "default": 0,
+      "unit": 1
+    },
    {
      "group": "시작",
      "name": "MapboxStyle",
--- a/selfdrive/controls/lib/lateral_planner.py
+++ b/selfdrive/controls/lib/lateral_planner.py
@ -74,8 +74,12 @@ class LateralPlanner:
    self.curve_speed = 0
    
    self.prev_path_xyz = None
-    self.path_history = deque(maxlen=5)
    self.carrot_lat_control = 0
+    self.carrot_lat_filter = 5
+    self.path_history = deque(maxlen=self.carrot_lat_filter)
+
+    self.lateralMotionCost = LATERAL_MOTION_COST
+    self.lateralMotionCost2 = LATERAL_MOTION_COST

  def reset_mpc(self, x0=None):
    if x0 is None:
@ -84,18 +88,23 @@ class LateralPlanner:
    self.lat_mpc.reset(x0=self.x0)

  def update(self, sm):
-    global PATH_COST, LATERAL_ACCEL_COST, LATERAL_JERK_COST, STEERING_RATE_COST, LATERAL_MOTION_COST
+    global PATH_COST, LATERAL_ACCEL_COST, LATERAL_JERK_COST, STEERING_RATE_COST
    self.readParams -= 1
    if self.readParams <= 0:
      self.readParams = 100
      self.useLaneLineSpeedApply = self.params.get_int("UseLaneLineSpeedApply")
      self.pathOffset = 0.0 #float(self.params.get_int("PathOffset")) * 0.01
      PATH_COST = self.params.get_float("LatMpcPathCost") * 0.01
-      LATERAL_MOTION_COST = self.params.get_float("LatMpcMotionCost") * 0.01
+      self.lateralMotionCost = self.params.get_float("LatMpcMotionCost") * 0.01
+      self.lateralMotionCost2 = self.params.get_float("LatMpcMotionCost2") * 0.01
      LATERAL_ACCEL_COST = self.params.get_float("LatMpcAccelCost") * 0.01
      LATERAL_JERK_COST = self.params.get_float("LatMpcJerkCost") * 0.01
      STEERING_RATE_COST = self.params.get_float("LatMpcSteeringRateCost")
      self.carrot_lat_control = self.params.get_int("CarrotLatControl")
+      carrot_lat_filter = self.params.get_int("CarrotLatFilter")
+      if carrot_lat_filter != self.carrot_lat_filter:
+        self.carrot_lat_filter = carrot_lat_filter
+        self.path_history = deque(maxlen=self.carrot_lat_filter)

    # clip speed , lateral planning is not possible at 0 speed
    measured_curvature = sm['controlsState'].curvature
@ -140,6 +149,14 @@ class LateralPlanner:
    else:
      self.LP.lane_change_multiplier = 1.0

+    lateral_motion_cost = self.lateralMotionCost
+    atc_type = sm['carrotMan'].atcType
+    if atc_activate:
+      if atc_type == "turn left" and md.meta.desireState[1] > 0.1:
+        lateral_motion_cost = self.lateralMotionCost2
+      elif atc_type == "turn right" and md.meta.desireState[2] > 0.1:
+        lateral_motion_cost = self.lateralMotionCost2
+
    # lanelines calculation?
    self.LP.lanefull_mode = self.useLaneLineMode
    self.LP.lane_width_left = md.meta.laneWidthLeft
@ -161,15 +178,14 @@ class LateralPlanner:
    self.path_xyz = self.alpha * self.path_xyz + (1 - self.alpha) * self.prev_path_xyz
    self.prev_path_xyz = self.path_xyz
    """
-
    if self.carrot_lat_control in [1,2]:
-      if self.plan_a[0] < -1.0:
+      if False: #self.plan_a[0] < -1.0:
        self.path_history.clear()
      
      self.path_history.append(self.path_xyz)
      self.path_xyz = np.mean(np.array(self.path_history), axis=0)

-    self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST,
+    self.lat_mpc.set_weights(PATH_COST, lateral_motion_cost,
                             LATERAL_ACCEL_COST, LATERAL_JERK_COST,
                             STEERING_RATE_COST)

--- a/selfdrive/navd/map_renderer.cc
+++ b/selfdrive/navd/map_renderer.cc
@ -128,33 +128,25 @@ MapRenderer::MapRenderer(const QMapLibre::Settings &settings, bool online) : m_s
 void MapRenderer::msgUpdate() {
  sm->update(1000);

-  float lat = 0.0;
-  float lon = 0.0;
-  float bearing = 0.0;
-  bool gps_updated = false;
  auto carrotMan = (*sm)["carrotMan"].getCarrotMan();
  bool active_carrot_man = carrotMan.getActiveCarrot() > 1;

-  if (!active_carrot_man && sm->updated("liveLocationKalman")) {
+  if (sm->updated("liveLocationKalman")) {
    auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman();
    auto pos = location.getPositionGeodetic();
    auto orientation = location.getCalibratedOrientationNED();

    if ((sm->rcv_frame("liveLocationKalman") % LLK_DECIMATION) == 0) {
-      bearing = RAD2DEG(orientation.getValue()[2]);
-      lat = pos.getValue()[0];
-      lon = pos.getValue()[1];
-      gps_updated = true;
-    }
-  }
-  if (active_carrot_man && sm->updated("carrotMan")) {
-    bearing = carrotMan.getXPosAngle();
-    lat = carrotMan.getXPosLat();
-    lon = carrotMan.getXPosLon();
-    gps_updated = true;
-  }
+      float bearing = RAD2DEG(orientation.getValue()[2]);
+      float lat = pos.getValue()[0];
+      float lon = pos.getValue()[1];
+    
+      if (active_carrot_man) {
+        bearing = carrotMan.getXPosAngle();
+        lat = carrotMan.getXPosLat();
+        lon = carrotMan.getXPosLon();
+      }
        
-  if(gps_updated) {
      updatePosition(get_point_along_line(lat, lon, bearing, MAP_OFFSET), bearing);
      // TODO: use the static rendering mode instead
      // retry render a few times
@ -171,7 +163,9 @@ void MapRenderer::msgUpdate() {
      // fallback to sending a blank frame
      if (!rendered()) {
        publish(0, false);
+        printf("blank frame\n");
      }
+    }
  }

  if (sm->updated("navRoute")) {
@ -234,6 +228,9 @@ void MapRenderer::publish(const double render_time, const bool loaded) {
  auto carrotMan = (*sm)["carrotMan"].getCarrotMan();
  auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman();
  bool valid = loaded && ((carrotMan.getActiveCarrot() > 1) || ((location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && location.getPositionGeodetic().getValid()));
+  if (!valid) {
+    printf("loaded %d, active_carrot %d, status %d, valid %d\n", loaded, carrotMan.getActiveCarrot(), (int)location.getStatus(), location.getPositionGeodetic().getValid());
+  }
  ever_loaded = ever_loaded || loaded;
  uint64_t ts = nanos_since_boot();
  VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP);
--- a/selfdrive/navd/map_renderer.h
+++ b/selfdrive/navd/map_renderer.h
@ -46,7 +46,7 @@ private:
  uint32_t frame_id = 0;
  uint64_t last_llk_rendered = 0;
  bool rendered() {
-    return last_llk_rendered == (*sm)["carrotMan"].getLogMonoTime();
+    return last_llk_rendered == (*sm)["liveLocationKalman"].getLogMonoTime();
  }

  QTimer* timer;
--- a/selfdrive/ui/qt/offroad/settings.cc
+++ b/selfdrive/ui/qt/offroad/settings.cc
@ -776,6 +776,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
  startToggles->addItem(new ParamControl("DisableDM", "Disable DM", "", "../assets/img_driver_face_static_x.png", this));
  //startToggles->addItem(new CValueControl("CarrotCountDownSpeed", "NaviCountDown Speed(10)", "", "../assets/offroad/icon_shell.png", 0, 200, 5));
  startToggles->addItem(new CValueControl("MapboxStyle", "Mapbox Style(0)", "", "../assets/offroad/icon_shell.png", 0, 2, 1));
+  startToggles->addItem(new CValueControl("RecordRoadCam", "Record Road camera(0)", "1:RoadCam, 2:RoadCam+WideRoadCam", "../assets/offroad/icon_shell.png", 0, 2, 1));
  startToggles->addItem(new ParamControl("HotspotOnBoot", "Hotspot enabled on boot", "", "../assets/offroad/icon_shell.png", this));
  //startToggles->addItem(new ParamControl("NoLogging", "Disable Logger", "", "../assets/offroad/icon_shell.png", this));
  //startToggles->addItem(new ParamControl("LaneChangeNeedTorque", "LaneChange: Need Torque", "", "../assets/offroad/icon_shell.png", this));
--- a/system/loggerd/loggerd.h
+++ b/system/loggerd/loggerd.h
@ -57,21 +57,21 @@ public:
 const EncoderInfo main_road_encoder_info = {
  .publish_name = "roadEncodeData",
  .filename = "fcamera.hevc",
-  .record = false,
+  .record = Params().getInt("RecordRoadCam") > 0,
  INIT_ENCODE_FUNCTIONS(RoadEncode),
 };

 const EncoderInfo main_wide_road_encoder_info = {
  .publish_name = "wideRoadEncodeData",
  .filename = "ecamera.hevc",
-  .record = false,
+  .record = Params().getInt("RecordRoadCam") > 1,
  INIT_ENCODE_FUNCTIONS(WideRoadEncode),
 };

 const EncoderInfo main_driver_encoder_info = {
  .publish_name = "driverEncodeData",
  .filename = "dcamera.hevc",
-  .record = false, //Params().getBool("RecordFront"),
+  .record = Params().getBool("RecordFront"),
  INIT_ENCODE_FUNCTIONS(DriverEncode),
 };

--- a/system/manager/manager.py
+++ b/system/manager/manager.py
@ -124,6 +124,7 @@ def get_default_params():
    ("LaneChangeNeedTorque", "0"),
    ("MaxAngleFrames", "89"),
    ("CarrotLatControl", "0"),
+    ("CarrotLatFilter", "5"),
    ("DampingFactor", "0"),
    ("LateralTorqueCustom", "0"),
    ("LateralTorqueAccelFactor", "2500"),
@ -131,6 +132,7 @@ def get_default_params():
    ("LateralTorqueKd", "0"),
    ("LatMpcPathCost", "100"),
    ("LatMpcMotionCost", "11"),
+    ("LatMpcMotionCost2", "11"),
    ("LatMpcAccelCost", "0"),
    ("LatMpcJerkCost", "4"),
    ("LatMpcSteeringRateCost", "700"),
@ -142,6 +144,7 @@ def get_default_params():
    ("ModelActuatorDelay", "20"),
    ("MaxTimeOffroadMin", "60"),
    ("DisableDM", "0"),
+    ("RecordRoadCam", "0"),
    ("CruiseOnDist", "400"),
    ("HotspotOnBoot", "0"),
    ("CustomSR", "0"),