move-fast: v-tsc fix (commaai#40)

carleeno · Feb 20, 2023 · adf844e · adf844e
1 parent c97b3b5
commit adf844e
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 5 deletions.
diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py
@@ -41,6 +41,7 @@ def __init__(self, CP, use_lanelines=True, wide_camera=False):
     self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,))
     self.t_idxs = np.arange(TRAJECTORY_SIZE)
     self.y_pts = np.zeros(TRAJECTORY_SIZE)
+    self.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))
 
     self.lat_mpc = LateralMpc()
     self.reset_mpc(np.zeros(4))
@@ -181,6 +182,9 @@ def publish(self, sm, pm):
     lateralPlan.dynamicLaneProfile = int(self.dynamic_lane_profile)
     lateralPlan.dynamicLaneProfileStatus = bool(self.dynamic_lane_profile_status)
 
+    lateralPlan.dPathWLinesX = [float(x) for x in self.d_path_w_lines_xyz[:, 0]]
+    lateralPlan.dPathWLinesY = [float(y) for y in self.d_path_w_lines_xyz[:, 1]]
+
     if self.standstill:
       self.standstill_elapsed += DT_MDL
     else:

diff --git a/selfdrive/controls/lib/vision_turn_controller.py b/selfdrive/controls/lib/vision_turn_controller.py
@@ -6,7 +6,7 @@
 from common.realtime import sec_since_boot
 from common.conversions import Conversions as CV
 from selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE
-from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
+from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
 
 
 _MIN_V = 5.6  # Do not operate under 20km/h
@@ -186,10 +186,11 @@ def _update_calculations(self, sm):
         c_y = width_pts / 2 + lll_y
         path_poly = np.polyfit(ll_x, c_y, 3)
 
-    # 2. If not polynomial derived from lanes, then derive it from driving path as provided by `lateralPlanner`.
-    if path_poly is None and lat_planner_data is not None and len(lat_planner_data.psis) == CONTROL_N \
-       and lat_planner_data.dPathPoints[0] > 0:
-      path_poly = np.polyfit(lat_planner_data.psis, lat_planner_data.dPathPoints, 3)
+    # 2. If not polynomial derived from lanes, then derive it from compensated driving path with lanes as
+    # provided by `lateralPlanner`.
+    if path_poly is None and lat_planner_data is not None and len(lat_planner_data.dPathWLinesX) > 0 \
+       and lat_planner_data.dPathWLinesX[0] > 0:
+      path_poly = np.polyfit(lat_planner_data.dPathWLinesX, lat_planner_data.dPathWLinesY, 3)
 
     # 3. If no polynomial derived from lanes or driving path, then provide a straight line poly.
     if path_poly is None: