diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index f9bf15cc04..0e51d521d3 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -25,21 +25,24 @@ 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): - qp_iterations = max(0, qp_iterations) - 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) - dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego) - dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l) - dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l) - dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost - dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau - dat.liveLongitudinalMpc.qpIterations = qp_iterations - dat.liveLongitudinalMpc.mpcId = self.mpc_id - dat.liveLongitudinalMpc.calculationTime = calculation_time - pm.send('liveLongitudinalMpc', dat) + def publish(self, pm): + 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) + dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego) + dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l) + dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l) + dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost + dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau + dat.liveLongitudinalMpc.qpIterations = qp_iterations + dat.liveLongitudinalMpc.mpcId = self.mpc_id + dat.liveLongitudinalMpc.calculationTime = self.duration + pm.send('liveLongitudinalMpc', dat) def setup_mpc(self): ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id) @@ -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) - duration = int((sec_since_boot() - t) * 1e9) - - if LOG_MPC: - self.send_mpc_solution(pm, n_its, duration) + self.n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead) + self.duration = int((sec_since_boot() - t) * 1e9) # 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] diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 2583206b03..1cf819147f 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/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 diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 7b1d4fc852..92f38a30c3 100755 --- a/selfdrive/controls/lib/planner.py +++ b/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.mpc2.update(pm, sm['carState'], lead_2) + self.mpc1.update(sm['carState'], lead_1) + 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, - 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: + 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 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 - radar_can_error = car.RadarData.Error.canError in radar_errors + self.radar_fault = car.RadarData.Error.fault 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 diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index f8e584914a..d40ac97bf7 100755 --- a/selfdrive/controls/plannerd.py +++ b/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): diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 014b22e962..5968605dbe 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/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(pm, CS.carState, lead) + mpc.update(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: