ALCA and radar_interface work

cereal/tesla.capnp

@@ -35,6 +35,7 @@ struct TeslaRadarPoint {
   movingState @3 :UInt8; # 0-indeterminate 1-moving 2-stopped 3-standing
   length @4 :Float32; # length in meters
   obstacleProb @5 :Float32; # probability to be an obstacle
+  timeStamp @6 :UInt64; #timestamp when the pair was received
 }
 
 struct ICCarsLR {

selfdrive/car/modules/ALCA_module.py

@@ -256,6 +256,7 @@ def __init__(self):
     self.dx = 0.
     self.poller = zmq.Poller()
     self.alcaStatus = messaging.sub_sock(service_list['alcaStatus'].port, conflate=True, poller=self.poller)
+    self.alcaState = messaging.pub_sock(service_list['alcaState'].port)
     self.alcas = None
 
 
@@ -280,18 +281,32 @@ def debug_alca(self,message):
     if ALCA_DEBUG:
       print message
 
-  def update(self, v_ego, md, r_poly, l_poly, r_prob, l_prob, lane_width):
+  def send_state(self):
+    alca_state = tesla.ALCAState.new_message()
+    #ALCA params
+    alca_state.alcaDirection = int(self.ALCA_direction)
+    alca_state.alcaError = bool(self.ALCA_error)
+    alca_state.alcaCancelling = bool(self.ALCA_cancelling)
+    alca_state.alcaEnabled = bool(self.ALCA_enabled)
+    alca_state.alcaLaneWidth = float(self.ALCA_lane_width)
+    alca_state.alcaStep = int(self.ALCA_step)
+    alca_state.alcaTotalSteps = int(self.ALCA_total_steps)
+    self.alcaState.send(alca_state.to_bytes())
+
+  def update(self, v_ego, md, r_poly, l_poly, r_prob, l_prob, lane_width, p_poly):
 
     for socket, _ in self.poller.poll(1):
       if socket is self.alcaStatus:
         self.alcas = tesla.ALCAStatus.from_bytes(socket.recv())
 
     if not self.ALCA_enabled:
-      return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width
+      self.send_state()
+      return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width, p_poly
 
     #if we don't have yet ALCA status, return same values
     if self.alcas is None:
-      return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width
+      self.send_state()
+      return np.array(r_poly),np.array(l_poly),r_prob, l_prob, lane_width, p_poly
 
     self.ALCA_direction = self.alcas.alcaDirection
     self.ALCA_enabled = self.alcas.alcaEnabled
@@ -411,18 +426,20 @@ def update(self, v_ego, md, r_poly, l_poly, r_prob, l_prob, lane_width):
 
         if (self.ALCA_direction == 1 and not self.ALCA_over_line) or (self.ALCA_direction == -1 and self.ALCA_over_line):
           r_poly = np.array(l_poly)
-          l_prob = 1
+          #l_prob = 1
           r_prob = l_prob
           r_poly[3] = l_poly[3] - self.ALCA_lane_width
         elif (self.ALCA_direction == -1 and not self.ALCA_over_line) or (self.ALCA_direction == 1 and self.ALCA_over_line):
           l_poly = np.array(r_poly)
-          r_prob = 1
+          #r_prob = 1
           l_prob = r_prob
           l_poly[3] = r_poly[3] + self.ALCA_lane_width
         l_poly[3] += self.ALCA_OFFSET_C3
         r_poly[3] += self.ALCA_OFFSET_C3
+        p_poly[3] += self.ALCA_OFFSET_C3
         l_poly[2] += self.ALCA_OFFSET_C2
         r_poly[2] += self.ALCA_OFFSET_C2
+        p_poly[2] += self.ALCA_OFFSET_C2
     else:
       self.reset_alca()
       self.ALCA_error = False
@@ -435,5 +452,6 @@ def update(self, v_ego, md, r_poly, l_poly, r_prob, l_prob, lane_width):
       else:
         lane_width = self.ALCA_lane_width
 
-    return np.array(r_poly),np.array(l_poly),r_prob, l_prob, self.ALCA_lane_width
+    self.send_state()
+    return np.array(r_poly),np.array(l_poly),r_prob, l_prob, self.ALCA_lane_width, p_poly
 

selfdrive/car/tesla/radar_interface.py

@@ -16,6 +16,8 @@
 RADAR_B_MSGS = list(range(0x311, 0x36F, 3))
 OBJECT_MIN_PROBABILITY = 20.
 CLASS_MIN_PROBABILITY = 20.
+RADAR_MESSAGE_FREQUENCY = 0.050 * 1e9 #time in ns, radar sends data at 0.06 s
+VALID_MESSAGE_COUNT_THRESHOLD = 10
 
 
 # Tesla Bosch firmware has 32 objects in all objects or a selected set of the 5 we should look at
@@ -60,6 +62,7 @@ def __init__(self,CP):
     self.TRACK_LEFT_LANE = True
     self.TRACK_RIGHT_LANE = True
     self.updated_messages = set()
+    self.canErrorCounter = 0
     if self.useTeslaRadar:
       self.pts = {}
       self.extPts = {}
@@ -69,23 +72,28 @@ def __init__(self,CP):
       self.logcan = messaging.sub_sock(service_list['can'].port)
       self.radarOffset = CarSettings().get_value("radarOffset")
       self.trackId = 1
-      self.trigger_msg = RADAR_B_MSGS[-1]
+      self.trigger_start_msg = RADAR_A_MSGS[0]
+      self.trigger_end_msg = RADAR_B_MSGS[-1]
 
 
 
   def update(self, can_strings):
-    # in Bosch radar and we are only steering for now, so sleep 0.05s to keep
     # radard at 20Hz and return no points
     if not self.useTeslaRadar:
       time.sleep(0.05)
       return car.RadarData.new_message(),self.extPts.values()
 
+
     tm = int(sec_since_boot() * 1e9)
     if can_strings != None:
       vls = self.rcp.update_strings(tm, can_strings)
       self.updated_messages.update(vls)
 
-    if self.trigger_msg not in self.updated_messages:
+
+    if self.trigger_start_msg not in self.updated_messages:
+      return None,None
+
+    if self.trigger_end_msg not in self.updated_messages:
       return None,None
 
     rr,rrext = self._update(self.updated_messages)
@@ -103,6 +111,10 @@ def _update(self, updated_messages):
           del self.extPts[message]
         continue
       cpt = self.rcp.vl[message]
+      cpt2 = self.rcp.vl[message+1]
+      # ensure the two messages are from the same frame reading
+      if cpt['Index'] != cpt2['Index2']:
+        continue
       if (cpt['LongDist'] >= BOSCH_MAX_DIST) or (cpt['LongDist']==0) or (not cpt['Tracked']):
         self.valid_cnt[message] = 0    # reset counter
         if message in self.pts:
@@ -116,15 +128,11 @@ def _update(self, updated_messages):
           del self.pts[message]
           del self.extPts[message]
 
-      #score = self.rcp.vl[ii+16]['SCORE']
-      #print ii, self.valid_cnt[ii], cpt['Valid'], cpt['LongDist'], cpt['LatDist']
-
       # radar point only valid if it's a valid measurement and score is above 50
       # bosch radar data needs to match Index and Index2 for validity
       # also for now ignore construction elements
       if (cpt['Valid'] or cpt['Tracked'])and (cpt['LongDist']>0) and (cpt['LongDist'] < BOSCH_MAX_DIST) and \
-          (cpt['Index'] == self.rcp.vl[message+1]['Index2']) and (self.valid_cnt[message] > 5) and \
-          (cpt['ProbExist'] >= OBJECT_MIN_PROBABILITY): # and (self.rcp.vl[ii+1]['Class'] < 4): # and ((self.rcp.vl[ii+1]['MovingState']<3) or (self.rcp.vl[ii+1]['Class'] > 0)):
+          (self.valid_cnt[message] > VALID_MESSAGE_COUNT_THRESHOLD) and (cpt['ProbExist'] >= OBJECT_MIN_PROBABILITY): 
         if message not in self.pts and ( cpt['Tracked']):
           self.pts[message] = car.RadarData.RadarPoint.new_message()
           self.pts[message].trackId = self.trackId 
@@ -138,14 +146,15 @@ def _update(self, updated_messages):
           self.pts[message].yRel = cpt['LatDist']  - self.radarOffset # in car frame's y axis, left is positive
           self.pts[message].vRel = cpt['LongSpeed']
           self.pts[message].aRel = cpt['LongAccel']
-          self.pts[message].yvRel = self.rcp.vl[message+1]['LatSpeed']
+          self.pts[message].yvRel = cpt2['LatSpeed']
           self.pts[message].measured = bool(cpt['Meas'])
-          self.extPts[message].dz = self.rcp.vl[message+1]['dZ']
-          self.extPts[message].movingState = self.rcp.vl[message+1]['MovingState']
-          self.extPts[message].length = self.rcp.vl[message+1]['Length']
+          self.extPts[message].dz = cpt2['dZ']
+          self.extPts[message].movingState = cpt2['MovingState']
+          self.extPts[message].length = cpt2['Length']
           self.extPts[message].obstacleProb = cpt['ProbObstacle']
+          self.extPts[message].timeStamp = int(self.rcp.ts[message+1]['Index2'])
           if self.rcp.vl[message+1]['Class'] >= CLASS_MIN_PROBABILITY:
-            self.extPts[message].objectClass = self.rcp.vl[message+1]['Class']
+            self.extPts[message].objectClass = cpt2['Class']
             # for now we will use class 0- unknown stuff to show trucks
             # we will base that on being a class 1 and length of 2 (hoping they meant width not length, but as germans could not decide)
             # 0-unknown 1-four wheel vehicle 2-two wheel vehicle 3-pedestrian 4-construction element
@@ -164,14 +173,21 @@ def _update(self, updated_messages):
     if not self.rcp.can_valid:
       errors.append("canError")
       self.tinklaClient.logCANErrorEvent(source="radar_interface", canMessage=0, additionalInformation="Invalid CAN Count")
-    ret.errors = errors
+      self.canErrorCounter += 1
+    else:
+      self.canErrorCounter = 0
+    #BB: Only trigger canError for 3 consecutive errors
+    if self.canErrorCounter > 2:
+      ret.errors = errors
+    else:
+      ret.errors = []
     return ret,self.extPts.values()
 
 # radar_interface standalone tester
 if __name__ == "__main__":
-  CP = car.CarParams()
+  CP = None
   RI = RadarInterface(CP)
   while 1:
     ret,retext = RI.update(can_strings = None)
     print(chr(27) + "[2J")
-    print(ret)
+    print(ret,retext)

selfdrive/controls/controlsd.py

@@ -262,7 +262,7 @@ def state_control(frame, rcv_frame, plan, path_plan, live_params, CS, CP, state,
   actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
                                               v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP)
   # Steering PID loop and lateral MPC
-  actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, VM, path_plan)
+  actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, VM, path_plan, live_params)
 
   # Send a "steering required alert" if saturation count has reached the limit
   if LaC.sat_flag and CP.steerLimitAlert:

selfdrive/controls/lib/lane_planner.py

@@ -24,7 +24,7 @@ def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
 
 
 class LanePlanner(object):
-  def __init__(self):
+  def __init__(self,shouldUseAlca):
     self.l_poly = [0., 0., 0., 0.]
     self.r_poly = [0., 0., 0., 0.]
     self.p_poly = [0., 0., 0., 0.]
@@ -40,7 +40,9 @@ def __init__(self):
 
     self._path_pinv = compute_path_pinv()
     self.x_points = np.arange(50)
-    self.ALCAMP = ALCAModelParser()
+    self.shouldUseAlca = shouldUseAlca
+    if shouldUseAlca:
+      self.ALCAMP = ALCAModelParser()
 
   def parse_model(self, md):
     if len(md.leftLane.poly):
@@ -70,8 +72,8 @@ def update_lane(self, v_ego, md, alca ):
                       (1 - self.lane_width_certainty) * speed_lane_width
 
     # ALCA integration
-    if alca:
-      self.r_poly,self.l_poly,self.r_prob,self.l_prob,self.lane_width = self.ALCAMP.update(v_ego, md, np.array(self.r_poly), np.array(self.l_poly), self.r_prob, self.l_prob, self.lane_width)
+    if self.shouldUseAlca and alca:
+      self.r_poly,self.l_poly,self.r_prob,self.l_prob,self.lane_width, self.p_poly = self.ALCAMP.update(v_ego, md, np.array(self.r_poly), np.array(self.l_poly), self.r_prob, self.l_prob, self.lane_width, self.p_poly)
 
     self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width)
 

selfdrive/controls/lib/latcontrol_pid.py

@@ -14,7 +14,7 @@ def __init__(self, CP):
   def reset(self):
     self.pid.reset()
 
-  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
+  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan, live_params):
     pid_log = log.ControlsState.LateralPIDState.new_message()
     pid_log.steerAngle = float(angle_steers)
     pid_log.steerRate = float(angle_steers_rate)
@@ -24,7 +24,8 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste
       pid_log.active = False
       self.pid.reset()
     else:
-      self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner
+      angle_bias = live_params.angleOffset - live_params.angleOffsetAverage
+      self.angle_steers_des = path_plan.angleSteers #+ angle_bias  # get from MPC/PathPlanner
 
       steers_max = get_steer_max(CP, v_ego)
       self.pid.pos_limit = steers_max

selfdrive/controls/lib/pathplanner.py

@@ -11,7 +11,6 @@
 from selfdrive.controls.lib.lane_planner import LanePlanner
 import selfdrive.messaging as messaging
 from selfdrive.car.tesla.readconfig import CarSettings
-from cereal import tesla
 
 LOG_MPC = os.environ.get('LOG_MPC', False)
 
@@ -24,13 +23,12 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_
 
 class PathPlanner(object):
   def __init__(self, CP):
-    self.LP = LanePlanner()
+    self.LP = LanePlanner(shouldUseAlca=True)
 
     self.last_cloudlog_t = 0
 
     self.plan = messaging.pub_sock(service_list['pathPlan'].port)
     self.livempc = messaging.pub_sock(service_list['liveMpc'].port)
-    self.alca = messaging.pub_sock(service_list['alcaState'].port)
 
     self.setup_mpc(CP.steerRateCost)
     self.solution_invalid_cnt = 0
@@ -135,18 +133,6 @@ def update(self, sm, CP, VM):
 
     self.plan.send(plan_send.to_bytes())
 
-    alca_state = tesla.ALCAState.new_message()
-    #ALCA params
-    alca_state.alcaDirection = int(self.LP.ALCAMP.ALCA_direction)
-    alca_state.alcaError = bool(self.LP.ALCAMP.ALCA_error)
-    alca_state.alcaCancelling = bool(self.LP.ALCAMP.ALCA_cancelling)
-    alca_state.alcaEnabled = bool(self.LP.ALCAMP.ALCA_enabled)
-    alca_state.alcaLaneWidth = float(self.LP.ALCAMP.ALCA_lane_width)
-    alca_state.alcaStep = int(self.LP.ALCAMP.ALCA_step)
-    alca_state.alcaTotalSteps = int(self.LP.ALCAMP.ALCA_total_steps)
-
-    self.alca.send(alca_state.to_bytes())
-
     if LOG_MPC:
       dat = messaging.new_message()
       dat.init('liveMpc')

selfdrive/controls/radard.py

@@ -349,12 +349,6 @@ def update(self, frame, delay, sm, rr, has_radar,MP,rrext):
     datext.lead2trackId = l2x['trackId']
     datext.lead2oClass = l2x['oClass']
     datext.lead2length = l2x['length']
-    #datext.lead1trackId = l1x.trackId
-    #datext.lead1oClass = l1x.oClass
-    #datext.lead1length = l1x.length
-    #datext.lead2trackId = l2x.trackId
-    #datext.lead2oClass = l2x.oClass
-    #datext.lead2length = l2x.length
     return dat, datext
 
 
@@ -385,7 +379,7 @@ def radard_thread(gctx=None):
 
   rk = Ratekeeper(rate, print_delay_threshold=None)
   RD = RadarD(mocked, RI)
-  MP = LanePlanner()
+  MP = LanePlanner(shouldUseAlca=False)
 
   has_radar = not CP.radarOffCan or mocked
   last_md_ts = 0.