sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

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

* Updated test_following_distance.py

* Combined publish+send_mpc_solution methods
old-commit-hash: 608f00f814
commatwo_master
Mitchell Goff 4 years ago committed by GitHub
parent b4f3a03518
commit d28831dbab
5 changed files with 76 additions and 54 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 40
      selfdrive/controls/lib/long_mpc.py
  2. 15
      selfdrive/controls/lib/pathplanner.py
  3. 63
      selfdrive/controls/lib/planner.py
  4. 6
      selfdrive/controls/plannerd.py
  5. 6
      selfdrive/controls/tests/test_following_distance.py

40
selfdrive/controls/lib/long_mpc.py
Unescape View File

@ -25,21 +25,24 @@ class LongitudinalMpc():
self.new_lead = False self.new_lead = False
self.last_cloudlog_t = 0.0 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:
dat = messaging.new_message('liveLongitudinalMpc') qp_iterations = max(0, self.n_its)
dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego) dat = messaging.new_message('liveLongitudinalMpc')
dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego) dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego)
dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego) dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego)
dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l) dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego)
dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l) dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l)
dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l)
dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost
dat.liveLongitudinalMpc.qpIterations = qp_iterations dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau
dat.liveLongitudinalMpc.mpcId = self.mpc_id dat.liveLongitudinalMpc.qpIterations = qp_iterations
dat.liveLongitudinalMpc.calculationTime = calculation_time dat.liveLongitudinalMpc.mpcId = self.mpc_id
pm.send('liveLongitudinalMpc', dat) dat.liveLongitudinalMpc.calculationTime = self.duration
pm.send('liveLongitudinalMpc', dat)
def setup_mpc(self): def setup_mpc(self):
ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id) 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].v_ego = v
self.cur_state[0].a_ego = a self.cur_state[0].a_ego = a
def update(self, pm, CS, lead): def update(self, CS, lead):
v_ego = CS.vEgo v_ego = CS.vEgo
# Setup current mpc state # Setup current mpc state
@ -91,11 +94,8 @@ class LongitudinalMpc():
# Calculate mpc # Calculate mpc
t = sec_since_boot() 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 # 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] self.v_mpc = self.mpc_solution[0].v_ego[1]

15
selfdrive/controls/lib/pathplanner.py
Unescape View File

@ -57,10 +57,12 @@ class PathPlanner():
self.lane_change_timer = 0.0 self.lane_change_timer = 0.0
self.lane_change_ll_prob = 1.0 self.lane_change_ll_prob = 1.0
self.prev_one_blinker = False self.prev_one_blinker = False
self.desire = log.PathPlan.Desire.none
self.path_xyz = np.zeros((TRAJECTORY_SIZE,3)) self.path_xyz = np.zeros((TRAJECTORY_SIZE,3))
self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
self.t_idxs = np.arange(TRAJECTORY_SIZE) self.t_idxs = np.arange(TRAJECTORY_SIZE)
self.y_pts = np.zeros(TRAJECTORY_SIZE)
def setup_mpc(self): def setup_mpc(self):
self.libmpc = libmpc_py.libmpc self.libmpc = libmpc_py.libmpc
@ -77,7 +79,7 @@ class PathPlanner():
self.angle_steers_des_mpc = 0.0 self.angle_steers_des_mpc = 0.0
self.angle_steers_des_time = 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 v_ego = sm['carState'].vEgo
active = sm['controlsState'].active active = sm['controlsState'].active
steering_wheel_angle_offset_deg = sm['liveParameters'].angleOffset steering_wheel_angle_offset_deg = sm['liveParameters'].angleOffset
@ -158,15 +160,16 @@ class PathPlanner():
self.prev_one_blinker = one_blinker 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 # 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.lll_prob *= self.lane_change_ll_prob
self.LP.rll_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) 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]) 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) 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 v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
assert len(y_pts) == MPC_N + 1 assert len(y_pts) == MPC_N + 1
@ -215,11 +218,13 @@ class PathPlanner():
self.solution_invalid_cnt += 1 self.solution_invalid_cnt += 1
else: else:
self.solution_invalid_cnt = 0 self.solution_invalid_cnt = 0
def publish(self, sm, pm):
plan_solution_valid = self.solution_invalid_cnt < 2 plan_solution_valid = self.solution_invalid_cnt < 2
plan_send = messaging.new_message('pathPlan') plan_send = messaging.new_message('pathPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'modelV2']) 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.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.lProb = float(self.LP.lll_prob)
plan_send.pathPlan.rProb = float(self.LP.rll_prob) plan_send.pathPlan.rProb = float(self.LP.rll_prob)
plan_send.pathPlan.dProb = float(self.LP.d_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.mpcSolutionValid = bool(plan_solution_valid)
plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].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.laneChangeState = self.lane_change_state
plan_send.pathPlan.laneChangeDirection = self.lane_change_direction plan_send.pathPlan.laneChangeDirection = self.lane_change_direction

63
selfdrive/controls/lib/planner.py
Unescape View File

@ -66,6 +66,8 @@ class Planner():
self.v_acc_start = 0.0 self.v_acc_start = 0.0
self.a_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 = 0.0
self.v_acc_future = 0.0 self.v_acc_future = 0.0
@ -77,6 +79,11 @@ class Planner():
self.fcw_checker = FCWChecker() self.fcw_checker = FCWChecker()
self.path_x = np.arange(192) 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.params = Params()
self.first_loop = True 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]) 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""" """Gets called when new radarState is available"""
cur_time = sec_since_boot() cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo v_ego = sm['carState'].vEgo
@ -122,6 +129,9 @@ class Planner():
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) 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 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 # Calculate speed for normal cruise control
if enabled and not self.first_loop and not sm['carState'].gasPressed: 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)] 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.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.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) self.choose_solution(v_cruise_setpoint, enabled)
@ -166,22 +176,33 @@ class Planner():
self.fcw_checker.reset_lead(cur_time) self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker 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, sm['controlsState'].active,
v_ego, sm['carState'].aEgo, v_ego, sm['carState'].aEgo,
lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.dRel, lead_1.vLead, lead_1.aLeadK,
lead_1.yRel, lead_1.vLat, lead_1.yRel, lead_1.vLat,
lead_1.fcw, blinkers) and not sm['carState'].brakePressed lead_1.fcw, blinkers) and not sm['carState'].brakePressed
if fcw: if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters) 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_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 = messaging.new_message('plan')
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState']) 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.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource 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.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'] plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState']
# Send out fcw # Send out fcw
plan_send.plan.fcw = fcw plan_send.plan.fcw = self.fcw
pm.send('plan', plan_send) 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

6
selfdrive/controls/plannerd.py
Unescape View File

@ -38,9 +38,11 @@ def plannerd_thread(sm=None, pm=None):
sm.update() sm.update()
if sm.updated['modelV2']: if sm.updated['modelV2']:
PP.update(sm, pm, CP, VM) PP.update(sm, CP, VM)
PP.publish(sm, pm)
if sm.updated['radarState']: 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): def main(sm=None, pm=None):

6
selfdrive/controls/tests/test_following_distance.py
Unescape View File

@ -61,8 +61,10 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
mpc.set_cur_state(v_ego, a_ego) mpc.set_cur_state(v_ego, a_ego)
if first: # Make sure MPC is converged on first timestep if first: # Make sure MPC is converged on first timestep
for _ in range(20): 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 # Choose slowest of two solutions
if v_cruise < mpc.v_mpc: if v_cruise < mpc.v_mpc:

Write Preview
Loading…
Cancel
Save