diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 967d418bca..e204c918b3 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -68,13 +68,6 @@ class LanePlanner: self.r_poly[3] += CAMERA_OFFSET # Find current lanewidth - self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob - self.lane_width_certainty) - current_lane_width = abs(self.l_poly[3] - self.r_poly[3]) - self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate) - speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5]) - self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \ - (1 - self.lane_width_certainty) * speed_lane_width - # This will improve behaviour when lanes suddenly widen # these numbers were tested on 2000 segments and found to work well l_prob, r_prob = self.l_prob, self.r_prob @@ -94,6 +87,13 @@ class LanePlanner: l_prob *= l_std_mod r_prob *= r_std_mod + self.lane_width_certainty += 0.05 * (l_prob * r_prob - self.lane_width_certainty) + current_lane_width = abs(self.l_poly[3] - self.r_poly[3]) + self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate) + speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5]) + self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \ + (1 - self.lane_width_certainty) * speed_lane_width + clipped_lane_width = min(4.0, self.lane_width) path_from_left_lane = self.l_poly.copy() path_from_left_lane[3] -= clipped_lane_width / 2.0 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index ec3ba0bcc9..6a073a9755 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -2d717d808d568c2a3939627430fc56747c52e5bd \ No newline at end of file +b4a8c7c439c2430082ffb49ed675ada8f3bab8e6 \ No newline at end of file