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

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},
@@ -310,8 +310,10 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"MaxAngleFrames", PERSISTENT},
     {"SoftHoldMode", PERSISTENT},
     {"CarrotLatControl", PERSISTENT},
+    {"CarrotLatFilter", PERSISTENT},
     {"LatMpcPathCost", PERSISTENT },
     {"LatMpcMotionCost", PERSISTENT },
+    {"LatMpcMotionCost2", PERSISTENT },
     {"LatMpcAccelCost", PERSISTENT},
     {"LatMpcJerkCost", PERSISTENT},
     {"LatMpcSteeringRateCost", PERSISTENT},

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",

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)
 

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]);
+      float bearing = RAD2DEG(orientation.getValue()[2]);
-      lat = pos.getValue()[0];
+      float lat = pos.getValue()[0];
-      lon = pos.getValue()[1];
+      float lon = pos.getValue()[1];
-      gps_updated = true;
+    
-    }
+      if (active_carrot_man) {
-  }
-  if (active_carrot_man && sm->updated("carrotMan")) {
         bearing = carrotMan.getXPosAngle();
         lat = carrotMan.getXPosLat();
         lon = carrotMan.getXPosLon();
-    gps_updated = true;
       }
         
-  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,6 +163,8 @@ void MapRenderer::msgUpdate() {
       // fallback to sending a blank frame
       if (!rendered()) {
         publish(0, false);
+        printf("blank frame\n");
+      }
     }
   }
 
@@ -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);

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;

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));

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),
 };
 

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"),