Reverted 0 p_poly (commaai#202)

* Reverted 0 p_poly

* better path offset

* revert poly offset

selfdrive/controls/lib/model_parser.py

@@ -29,7 +29,6 @@ def update(self, v_ego, md):
       #p_poly = np.array(md.path.poly)
       p_poly = np.array(self.d_poly)
       p_poly[3] = 0.0
-      p_poly[2] = 0.0
     else:
       l_poly = model_polyfit(md.leftLane.points, self._path_pinv)  # left line
       r_poly = model_polyfit(md.rightLane.points, self._path_pinv)  # right line
@@ -53,6 +52,15 @@ def update(self, v_ego, md):
     speed_lane_width = interp(v_ego, [0., 31.], [3.0, 3.8])
     self.lane_width = self.lane_width_certainty * self.lane_width_estimate + (1.0 - self.lane_width_certainty) * speed_lane_width
 
+    '''half_lane_width = self.lane_width / 2.0
+    if l_poly[3] > -r_poly[3]:
+      if -r_poly[3] < half_lane_width:
+        p_poly[3] = r_prob * (r_poly[3] + half_lane_width)
+    else:
+      if l_poly[3] < half_lane_width:
+        p_poly[3] = l_prob * (l_poly[3] - half_lane_width)
+    '''
+
     self.lead_dist = md.lead.dist
     self.lead_prob = md.lead.prob
     self.lead_var = md.lead.std**2