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 methodsalbatross
Mitchell Goff
GitHub
parent
39e3672e82
commit
608f00f814
|
|
@ -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]
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
Loading…
Reference in New Issue