Create messaging_arne from new messaging architecture

cereal/arne182.capnp

@@ -33,7 +33,6 @@ struct PhantomData {
   status @0 :Bool;
   speed @1 :Float32;
   angle @2 :Float32;
-  time @3 :Float32;
 }
 
 struct ManagerData {

panda/VERSION

@@ -1 +1 @@
-v1.6.9
+v1.5.9

selfdrive/car/car_helpers.py

@@ -134,7 +134,7 @@ def fingerprint(logcan, sendcan, has_relay):
 
     frame += 1
 
-  cloudlog.warning("fingerprinted %s", car_fingerprint)
+  cloudlog.warning("fingerprinted {}".format({car_fingerprint: finger[0]}))
   params.put("CachedFingerprint", json.dumps([car_fingerprint, {int(key): value for key, value in finger[0].items()}]))
   return car_fingerprint, finger, vin
 

selfdrive/car/toyota/carcontroller.py

@@ -8,7 +8,7 @@
                                            create_acc_cancel_command, create_fcw_command
 from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS, TSS2_CAR, SteerLimitParams
 from selfdrive.can.packer import CANPacker
-from selfdrive.phantom import Phantom
+from selfdrive.phantom.phantom import Phantom
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 

selfdrive/car/toyota/carstate.py

@@ -1,8 +1,7 @@
 import numpy as np
 import math
-from selfdrive.services import service_list
-from cereal import arne182
 import selfdrive.messaging as messaging
+import selfdrive.messaging_arne as messaging_arne
 from common.numpy_fast import interp
 from cereal import car
 from common.numpy_fast import mean
@@ -141,8 +140,7 @@ def __init__(self, CP):
     self.Angle = [0, 5, 10, 15,20,25,30,35,60,100,180,270,500]
     self.Angle_Speed = [255,160,100,80,70,60,55,50,40,33,27,17,12]
     if not travis:
-      self.traffic_data_sock = messaging.pub_sock('liveTrafficData')
-      self.arne182Status_sock = messaging.pub_sock('arne182Status')
+      self.arne_pm = messaging_arne.PubMaster(['liveTrafficData', 'arne182Status'])
     # initialize can parser
     self.car_fingerprint = CP.carFingerprint
 
@@ -218,11 +216,12 @@ def update(self, cp, cp_cam):
       self.gasbuttonstatus = 2
     if self.sport_on == 0 and self.econ_on == 0:
       self.gasbuttonstatus = 0
-    msg = arne182.Arne182Status.new_message()
-    msg.gasbuttonstatus = self.gasbuttonstatus
-    msg.readdistancelines = cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES']
+    msg = messaging_arne.new_message()
+    msg.init('arne182Status')
+    msg.arne182Status.gasbuttonstatus = self.gasbuttonstatus
+    msg.arne182Status.readdistancelines = cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES']
     if not travis:
-      self.arne182Status_sock.send(msg.to_bytes())
+      self.arne_pm.send('arne182Status', msg)
     if self.CP.carFingerprint == CAR.LEXUS_IS:
       self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON']
     else:
@@ -313,21 +312,22 @@ def update(self, cp, cp_cam):
     self.splsgn4 = cp_cam.vl["RSA2"]['SPLSGN4']
     self.noovertake = self.tsgn1 == 65 or self.tsgn2 == 65 or self.tsgn3 == 65 or self.tsgn4 == 65 or self.tsgn1 == 66 or self.tsgn2 == 66 or self.tsgn3 == 66 or self.tsgn4 == 66
     if self.spdval1 > 0 or self.spdval2 > 0:
-      dat = arne182.LiveTrafficData.new_message()
+      dat = messaging_arne.new_message()
+      dat.init('liveTrafficData')
       if self.spdval1 > 0:
-        dat.speedLimitValid = True
+        dat.liveTrafficData.speedLimitValid = True
         if self.tsgn1 == 36:
-          dat.speedLimit = self.spdval1 * 1.60934
+          dat.liveTrafficData.speedLimit = self.spdval1 * 1.60934
         elif self.tsgn1 == 1:
-          dat.speedLimit = self.spdval1
+          dat.liveTrafficData.speedLimit = self.spdval1
         else:
-          dat.speedLimit = 0
+          dat.liveTrafficData.speedLimit = 0
       else:
-        dat.speedLimitValid = False
+        dat.liveTrafficData.speedLimitValid = False
       if self.spdval2 > 0:
-        dat.speedAdvisoryValid = True
-        dat.speedAdvisory = self.spdval2
+        dat.liveTrafficData.speedAdvisoryValid = True
+        dat.liveTrafficData.speedAdvisory = self.spdval2
       else:
-        dat.speedAdvisoryValid = False
+        dat.liveTrafficData.speedAdvisoryValid = False
       if not travis:
-        self.traffic_data_sock.send(dat.to_bytes())
+        self.arne_pm.send('liveTrafficData', dat)

selfdrive/controls/controlsd.py

@@ -4,8 +4,7 @@
 import capnp
 import zmq
 import selfdrive.messaging_arne as messaging_arne
-from selfdrive.services import service_list
-from cereal import car, log, arne182
+from cereal import car, log
 from common.numpy_fast import clip
 from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL
 from common.profiler import Profiler
@@ -220,7 +219,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_
 
 
 def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
-                  AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, sm, poller, arne182Status):
+                  AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, radar_state, arne_sm):
   """Given the state, this function returns an actuators packet"""
 
   actuators = car.CarControl.Actuators.new_message()
@@ -263,27 +262,16 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
   a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget - plan.aStart)
   v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
 
-  try:
-    gasinterceptor = CP.enableGasInterceptor
-  except AttributeError:
-    gasinterceptor = False
+
 
   # Gas/Brake PID loop
-  try:
-    leadOne = sm['radarState'].leadOne
-  except KeyError:
-    leadOne = None
-  gasstatus = None
-  for socket, _ in poller.poll(0):
-    if socket is arne182Status:
-      gasstatus = arne182.Arne182Status.from_bytes(socket.recv())
-  if gasstatus is None:
-    gasbuttonstatus = 0
-  else:
-    gasbuttonstatus = gasstatus.gasbuttonstatus
-  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, gasinterceptor, gasbuttonstatus, plan.decelForTurn, plan.longitudinalPlanSource,
-                                              leadOne, CS.gasPressed, plan.fcw)
+  arne_sm.update(0)
+  gas_button_status = arne_sm['arne182Status'].gasbuttonstatus
+  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,
+                                              gas_button_status, plan.decelForTurn, plan.longitudinalPlanSource,
+                                              radar_state.leadOne, CS.gasPressed, plan.fcw)
   # 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, path_plan)
 
@@ -460,8 +448,8 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
     sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \
                               'gpsLocation', 'radarState'], ignore_alive=['gpsLocation'])
 
-  poller = zmq.Poller()
-  arne182Status = messaging_arne.sub_sock(service_list['arne182Status'].port, poller, conflate=True)  # todo: can we use messaging_arne here?
+  arne_sm = messaging_arne.SubMaster('arne182Status')
+
   if can_sock is None:
     can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
     can_sock = messaging.sub_sock('can', timeout=can_timeout)
@@ -576,7 +564,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
     # Compute actuators (runs PID loops and lateral MPC)
     actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \
       state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
-                    driver_status, LaC, LoC, read_only, is_metric, cal_perc, sm, poller, arne182Status)
+                    driver_status, LaC, LoC, read_only, is_metric, cal_perc, sm['radarState'], arne_sm)
 
     prof.checkpoint("State Control")
 

selfdrive/controls/lib/long_mpc.py

@@ -8,8 +8,7 @@
 from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
 from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
 from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
-from selfdrive.phantom import Phantom
-import time
+from selfdrive.phantom.phantom import Phantom
 
 LOG_MPC = os.environ.get('LOG_MPC', False)
 
@@ -32,10 +31,10 @@ def __init__(self, mpc_id):
     self.car_data = {"lead_vels": [], "traffic_vels": []}
     self.mpc_frame = 0  # idea thanks to kegman
     self.last_time = None
-    self.v_lead = None
-    self.x_lead = None
+    self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None}
     self.df_frame = 0
-    self.phantom = Phantom(timeout=1.)
+    self.rate = 20
+    self.phantom = Phantom()
 
     self.last_cloudlog_t = 0.0
 
@@ -72,23 +71,23 @@ def set_cur_state(self, v, a):
 
   def get_acceleration(self):  # calculate acceleration to generate more accurate following distances
     a = 0.0
-    if len(self.car_data["lead_vels"]) > self.calc_rate(2):
+    if len(self.car_data["lead_vels"]) > self.rate * 2:
       num = (self.car_data["lead_vels"][-1] - self.car_data["lead_vels"][0])
-      den = len(self.car_data["lead_vels"]) / self.calc_rate()
+      den = len(self.car_data["lead_vels"]) / self.rate
       if den > 0:
         a = num / float(den)
     return a
 
   def save_car_data(self):
-    if self.v_lead is not None:
-      while len(self.car_data["lead_vels"]) > self.calc_rate(3):  # 3 seconds
+    if self.lead_data['v_lead'] is not None:
+      while len(self.car_data["lead_vels"]) > self.rate * 3:  # 3 seconds
         del self.car_data["lead_vels"][0]
-      self.car_data["lead_vels"].append(self.v_lead)
+      self.car_data["lead_vels"].append(self.lead_data['v_lead'])
 
-      if self.mpc_frame >= self.calc_rate():  # add to traffic list every second so we're not working with a huge list
+      if self.mpc_frame >= self.rate:  # add to traffic list every second so we're not working with a huge list
         while len(self.car_data["traffic_vels"]) > 180:  # 3 minutes of traffic logging
           del self.car_data["traffic_vels"][0]
-        self.car_data["traffic_vels"].append(self.v_lead)
+        self.car_data["traffic_vels"].append(self.lead_data['v_lead'])
         self.mpc_frame = 0  # reset every second
       self.mpc_frame += 1  # increment every frame
 
@@ -119,14 +118,15 @@ def smooth_follow(self):  # in m/s
       y = [1.8, interp(x[1], x_vel, y_mod)]
       TR = interp(self.v_ego, x, y)
 
-    if self.v_lead is not None:  # since the new mpc now handles braking nicely, simplify mods
-      x = [2.68, 2.1, 1.26, 0.61, 0, 0.61, 1.26, 2.1, 2.68]  # relative velocity values todo: add back negative values to get farther away from lead when it's slower
+    if self.lead_data['v_lead'] is not None:  # since the new mpc now handles braking nicely, simplify mods
+      x = [-2.68, -2.1, -1.26, -0.61, 0, 0.61, 1.26, 2.1, 2.68]  # relative velocity values
       y = [0.272, 0.154, 0.053, 0.017, 0, -0.017, -0.053, -0.154, -0.272]  # modification values
-      TR_mod = interp(self.v_lead - self.v_ego, x, y)  # quicker acceleration/don't brake when lead is overtaking
+      TR_mod = interp(self.lead_data['v_lead'] - self.v_ego, x, y)  # quicker acceleration/don't brake when lead is overtaking
 
-      # x = [-1.49, -1.1, -0.67, 0.0, 0.67, 1.1, 1.49]  # todo: work on adding this back
-      # y = [0.056, 0.032, 0.016, 0.0, -0.016, -0.032, -0.056]
-      # TR_mod += interp(self.get_acceleration(), x, y)  # when lead car has been braking over the past 3 seconds, slightly increase TR
+      x = [-1.49, -1.1, -0.67, 0.0, 0.67, 1.1, 1.49]
+      y = [0.056, 0.032, 0.016, 0.0, -0.016, -0.032, -0.056]
+      TR_mod += interp(self.lead_data['v_lead'], x, y)
+      # TR_mod += interp(self.get_acceleration(), x, y)  # todo: when lead car has been braking over the past 3 seconds, slightly increase TR
 
       TR += TR_mod
       #TR *= self.get_traffic_level()  # modify TR based on last minute of traffic data  # todo: look at getting this to work, a model could be used
@@ -138,12 +138,12 @@ def smooth_follow(self):  # in m/s
   def get_cost(self, TR):
     x = [.9, 1.8, 2.7]
     y = [4.5, 2.8, 1.3]
-    if self.x_lead is not None and self.v_ego is not None and self.v_ego != 0:
-      real_TR = self.x_lead / float(self.v_ego)  # switched to cost generation using actual distance from lead car; should be safer
+    if self.lead_data['x_lead'] is not None and self.v_ego is not None and self.v_ego != 0:
+      real_TR = self.lead_data['x_lead'] / float(self.v_ego)  # switched to cost generation using actual distance from lead car; should be safer
       if abs(real_TR - TR) >= .25:  # use real TR if diff is greater than x safety threshold
         TR = real_TR
-    if self.v_lead is not None and self.v_ego > 5:
-      factor = min(1,max(2,(self.v_lead - self.v_ego)/2 + 1.5))
+    if self.lead_data['v_lead'] is not None and self.v_ego > 5:
+      factor = min(1,max(2,(self.lead_data['v_lead'] - self.v_ego)/2 + 1.5))
       return min(round(float(interp(TR, x, y)), 3)/factor, 0.1)
     else:
       return round(float(interp(TR, x, y)), 3)
@@ -176,19 +176,6 @@ def get_TR(self):
         self.last_cost = 0.05
       return 2.7  # 30m at 40km/hr
 
-  def calc_rate(self, seconds=1.0, new_frame=False):  # return current rate of long_mpc in fps/hertz
-    current_time = time.time()
-    if self.last_time is None or (current_time - self.last_time) <= 0:
-      rate = int(round(40.42 * seconds))  # average of tests on long_mpc
-    else:
-      rate = (1.0 / (current_time - self.last_time)) * seconds
-
-    min_return = 10
-    max_return = seconds * 100
-    if new_frame:
-      self.last_time = current_time
-    return int(round(max(min(rate, max_return), min_return)))  # ensure we return a value between range, in hertz
-
   def process_phantom(self, lead):
     if lead is not None and lead.status:
       v_lead = max(0.0, lead.vLead)
@@ -197,10 +184,10 @@ def process_phantom(self, lead):
       # if radar lead is available, ensure we use that as the real lead rather than ignoring it and running into it
       # todo: this is buggy and probably needs to be looked at
       x_lead = min(9.144, lead.dRel)
-      v_lead = min(self.phantom.data["speed"], v_lead)
+      v_lead = min(self.phantom["speed"], v_lead)
     else:
       x_lead = 9.144
-      v_lead = self.phantom.data["speed"]
+      v_lead = self.phantom["speed"]
     return x_lead, v_lead
 
   def update(self, pm, CS, lead, v_cruise_setpoint):
@@ -212,17 +199,23 @@ def update(self, pm, CS, lead, v_cruise_setpoint):
     # Setup current mpc state
     self.cur_state[0].x_ego = 0.0
 
-    if self.phantom.data["status"]:
-      if self.phantom.data["speed"] != 0.0:
+    if self.phantom["status"]:
+      a_lead = 0.0
+      if self.phantom["speed"] != 0.0:
         x_lead, v_lead = self.process_phantom(lead)
-      elif 'lost_connection' in self.phantom.data:
-        x_lead = 1.5
-        v_lead = 0.  # stop as quickly as possible
+      elif self.phantom.lost_connection:
+        x = [0, 14.3053]  # 32 mph
+        y = [0.6096, 6.096]  # 2, 20 feet
+        x_lead = interp(v_ego, x, y)
+        v_lead = max(v_ego - 4.4704, 0)  # stop at a quick pace
+        x = [0, 14.3053]
+        y = [0, -2.2352]
+        a_lead = interp(v_ego, x, y)
       else:  # else, smooth deceleration
         x_lead = 3.75
-        v_lead = max(self.v_ego - (.7 / max(max(self.v_ego, 0) ** .4, .01)), 0.0)  # smoothly decelerate to 0 todo: tune this!
+        v_lead = max(v_ego - 1.34112, 0)  # smoothly decelerate to 0
+        a_lead = -0.44704
 
-      a_lead = 0.0
       self.a_lead_tau = lead.aLeadTau
       self.new_lead = False
       if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
@@ -233,7 +226,7 @@ def update(self, pm, CS, lead, v_cruise_setpoint):
       self.prev_lead_x = x_lead
       self.cur_state[0].x_l = x_lead
       self.cur_state[0].v_l = v_lead
-    elif lead is not None and lead.status:
+    elif lead is not None and lead.status:  # not phantom, is lead
       x_lead = lead.dRel
       v_lead = max(0.0, lead.vLead)
       a_lead = lead.aLeadK
@@ -242,8 +235,7 @@ def update(self, pm, CS, lead, v_cruise_setpoint):
         v_lead = 0.0
         a_lead = 0.0
 
-      self.v_lead = v_lead
-      self.x_lead = x_lead
+      self.lead_data['v_lead'], self.lead_data['x_lead'], self.lead_data['a_lead'] = v_lead, x_lead, a_lead
 
       self.a_lead_tau = lead.aLeadTau
       self.new_lead = False
@@ -255,14 +247,13 @@ def update(self, pm, CS, lead, v_cruise_setpoint):
       self.prev_lead_x = x_lead
       self.cur_state[0].x_l = x_lead
       self.cur_state[0].v_l = v_lead
-    else:
+    else:  # no lead
       self.prev_lead_status = False
       # Fake a fast lead car, so mpc keeps running
       self.cur_state[0].x_l = 50.0
       self.cur_state[0].v_l = v_ego + 10.0
       a_lead = 0.0
-      self.v_lead = None
-      self.x_lead = None
+      self.lead_data['v_lead'], self.lead_data['x_lead'], self.lead_data['a_lead'] = (None,) * 3
       self.a_lead_tau = _LEAD_ACCEL_TAU
 
     # Calculate mpc