Split planner and pathplanner publishing into separate 'publish' methods (#19860)

* Split planner and pathplanner publishing into separate 'publish' methods

* Updated test_following_distance.py

* Combined publish+send_mpc_solution methods
old-commit-hash: 608f00f814

selfdrive/controls/lib/long_mpc.py

@@ -25,9 +25,12 @@ class LongitudinalMpc():
     self.new_lead = False
 
     self.last_cloudlog_t = 0.0
+    self.n_its = 0
+    self.duration = 0
 
-  def send_mpc_solution(self, pm, qp_iterations, calculation_time):
+  def publish(self, pm):
-    qp_iterations = max(0, qp_iterations)
+    if LOG_MPC:
+      qp_iterations = max(0, self.n_its)
       dat = messaging.new_message('liveLongitudinalMpc')
       dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego)
       dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego)
@@ -38,7 +41,7 @@ class LongitudinalMpc():
       dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau
       dat.liveLongitudinalMpc.qpIterations = qp_iterations
       dat.liveLongitudinalMpc.mpcId = self.mpc_id
-    dat.liveLongitudinalMpc.calculationTime = calculation_time
+      dat.liveLongitudinalMpc.calculationTime = self.duration
       pm.send('liveLongitudinalMpc', dat)
 
   def setup_mpc(self):
@@ -56,7 +59,7 @@ class LongitudinalMpc():
     self.cur_state[0].v_ego = v
     self.cur_state[0].a_ego = a
 
-  def update(self, pm, CS, lead):
+  def update(self, CS, lead):
     v_ego = CS.vEgo
 
     # Setup current mpc state
@@ -91,11 +94,8 @@ class LongitudinalMpc():
 
     # Calculate mpc
     t = sec_since_boot()
-    n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
+    self.n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
-    duration = int((sec_since_boot() - t) * 1e9)
+    self.duration = int((sec_since_boot() - t) * 1e9)
-
-    if LOG_MPC:
-      self.send_mpc_solution(pm, n_its, duration)
 
     # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
     self.v_mpc = self.mpc_solution[0].v_ego[1]

selfdrive/controls/lib/pathplanner.py

@@ -57,10 +57,12 @@ class PathPlanner():
     self.lane_change_timer = 0.0
     self.lane_change_ll_prob = 1.0
     self.prev_one_blinker = False
+    self.desire = log.PathPlan.Desire.none
 
     self.path_xyz = np.zeros((TRAJECTORY_SIZE,3))
     self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
     self.t_idxs = np.arange(TRAJECTORY_SIZE)
+    self.y_pts = np.zeros(TRAJECTORY_SIZE)
 
   def setup_mpc(self):
     self.libmpc = libmpc_py.libmpc
@@ -77,7 +79,7 @@ class PathPlanner():
     self.angle_steers_des_mpc = 0.0
     self.angle_steers_des_time = 0.0
 
-  def update(self, sm, pm, CP, VM):
+  def update(self, sm, CP, VM):
     v_ego = sm['carState'].vEgo
     active = sm['controlsState'].active
     steering_wheel_angle_offset_deg = sm['liveParameters'].angleOffset
@@ -158,15 +160,16 @@ class PathPlanner():
 
     self.prev_one_blinker = one_blinker
 
-    desire = DESIRES[self.lane_change_direction][self.lane_change_state]
+    self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
 
     # Turn off lanes during lane change
-    if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft:
+    if self.desire == log.PathPlan.Desire.laneChangeRight or self.desire == log.PathPlan.Desire.laneChangeLeft:
       self.LP.lll_prob *= self.lane_change_ll_prob
       self.LP.rll_prob *= self.lane_change_ll_prob
     d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
     y_pts = np.interp(v_ego * self.t_idxs[:MPC_N+1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1])
     heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N+1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
+    self.y_pts = y_pts
 
     v_ego_mpc = max(v_ego, 5.0)  # avoid mpc roughness due to low speed
     assert len(y_pts) == MPC_N + 1
@@ -215,11 +218,13 @@ class PathPlanner():
       self.solution_invalid_cnt += 1
     else:
       self.solution_invalid_cnt = 0
+
+  def publish(self, sm, pm):
     plan_solution_valid = self.solution_invalid_cnt < 2
     plan_send = messaging.new_message('pathPlan')
     plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'modelV2'])
     plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
-    plan_send.pathPlan.dPathPoints = [float(x) for x in y_pts]
+    plan_send.pathPlan.dPathPoints = [float(x) for x in self.y_pts]
     plan_send.pathPlan.lProb = float(self.LP.lll_prob)
     plan_send.pathPlan.rProb = float(self.LP.rll_prob)
     plan_send.pathPlan.dProb = float(self.LP.d_prob)
@@ -230,7 +235,7 @@ class PathPlanner():
     plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
     plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
 
-    plan_send.pathPlan.desire = desire
+    plan_send.pathPlan.desire = self.desire
     plan_send.pathPlan.laneChangeState = self.lane_change_state
     plan_send.pathPlan.laneChangeDirection = self.lane_change_direction
 

selfdrive/controls/lib/planner.py

@@ -66,6 +66,8 @@ class Planner():
 
     self.v_acc_start = 0.0
     self.a_acc_start = 0.0
+    self.v_acc_next = 0.0
+    self.a_acc_next = 0.0
 
     self.v_acc = 0.0
     self.v_acc_future = 0.0
@@ -77,6 +79,11 @@ class Planner():
     self.fcw_checker = FCWChecker()
     self.path_x = np.arange(192)
 
+    self.radar_dead = False
+    self.radar_fault = False
+    self.radar_can_error = False
+    self.fcw = False
+
     self.params = Params()
     self.first_loop = True
 
@@ -104,7 +111,7 @@ class Planner():
 
     self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
 
-  def update(self, sm, pm, CP, VM, PP):
+  def update(self, sm, CP, VM, PP):
     """Gets called when new radarState is available"""
     cur_time = sec_since_boot()
     v_ego = sm['carState'].vEgo
@@ -122,6 +129,9 @@ class Planner():
     enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
     following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
 
+    self.v_acc_start = self.v_acc_next
+    self.a_acc_start = self.a_acc_next
+
     # Calculate speed for normal cruise control
     if enabled and not self.first_loop and not sm['carState'].gasPressed:
       accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
@@ -156,8 +166,8 @@ class Planner():
     self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
     self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
 
-    self.mpc1.update(pm, sm['carState'], lead_1)
+    self.mpc1.update(sm['carState'], lead_1)
-    self.mpc2.update(pm, sm['carState'], lead_2)
+    self.mpc2.update(sm['carState'], lead_2)
 
     self.choose_solution(v_cruise_setpoint, enabled)
 
@@ -166,22 +176,33 @@ class Planner():
       self.fcw_checker.reset_lead(cur_time)
 
     blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
-    fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time,
+    self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time,
                                        sm['controlsState'].active,
                                        v_ego, sm['carState'].aEgo,
                                        lead_1.dRel, lead_1.vLead, lead_1.aLeadK,
                                        lead_1.yRel, lead_1.vLat,
                                        lead_1.fcw, blinkers) and not sm['carState'].brakePressed
-    if fcw:
+    if self.fcw:
       cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
 
-    radar_dead = not sm.alive['radarState']
+    self.radar_dead = not sm.alive['radarState']
 
     radar_errors = list(sm['radarState'].radarErrors)
-    radar_fault = car.RadarData.Error.fault in radar_errors
+    self.radar_fault = car.RadarData.Error.fault in radar_errors
-    radar_can_error = car.RadarData.Error.canError in radar_errors
+    self.radar_can_error = car.RadarData.Error.canError in radar_errors
+
+    # Interpolate 0.05 seconds and save as starting point for next iteration
+    a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (self.a_acc - self.a_acc_start)
+    v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0
+    self.v_acc_next = v_acc_sol
+    self.a_acc_next = a_acc_sol
+
+    self.first_loop = False
+
+  def publish(self, sm, pm):
+    self.mpc1.publish(pm)
+    self.mpc2.publish(pm)
 
-    # **** send the plan ****
     plan_send = messaging.new_message('plan')
 
     plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState'])
@@ -200,21 +221,13 @@ class Planner():
     plan_send.plan.hasLead = self.mpc1.prev_lead_status
     plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
 
-    radar_valid = not (radar_dead or radar_fault)
+    radar_valid = not (self.radar_dead or self.radar_fault)
     plan_send.plan.radarValid = bool(radar_valid)
-    plan_send.plan.radarCanError = bool(radar_can_error)
+    plan_send.plan.radarCanError = bool(self.radar_can_error)
 
     plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState']
 
     # Send out fcw
-    plan_send.plan.fcw = fcw
+    plan_send.plan.fcw = self.fcw
 
     pm.send('plan', plan_send)
-
-    # Interpolate 0.05 seconds and save as starting point for next iteration
-    a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (self.a_acc - self.a_acc_start)
-    v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0
-    self.v_acc_start = v_acc_sol
-    self.a_acc_start = a_acc_sol
-
-    self.first_loop = False

selfdrive/controls/plannerd.py

@@ -38,9 +38,11 @@ def plannerd_thread(sm=None, pm=None):
     sm.update()
 
     if sm.updated['modelV2']:
-      PP.update(sm, pm, CP, VM)
+      PP.update(sm, CP, VM)
+      PP.publish(sm, pm)
     if sm.updated['radarState']:
-      PL.update(sm, pm, CP, VM, PP)
+      PL.update(sm, CP, VM, PP)
+      PL.publish(sm, pm)
 
 
 def main(sm=None, pm=None):

selfdrive/controls/tests/test_following_distance.py

@@ -61,8 +61,10 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
     mpc.set_cur_state(v_ego, a_ego)
     if first:  # Make sure MPC is converged on first timestep
       for _ in range(20):
-        mpc.update(pm, CS.carState, lead)
+        mpc.update(CS.carState, lead)
-    mpc.update(pm, CS.carState, lead)
+        mpc.publish(pm)
+    mpc.update(CS.carState, lead)
+    mpc.publish(pm)
 
     # Choose slowest of two solutions
     if v_cruise < mpc.v_mpc: