Skip to content

Commit

Permalink
lateral_planner.py/publish: follow capnp best practices (#23359)
Browse files Browse the repository at this point in the history
  • Loading branch information
deanlee authored Jan 2, 2022
1 parent 1e309a5 commit 2ca0925
Showing 1 changed file with 17 additions and 15 deletions.
32 changes: 17 additions & 15 deletions selfdrive/controls/lib/lateral_planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -204,20 +204,22 @@ def publish(self, sm, pm):
plan_solution_valid = self.solution_invalid_cnt < 2
plan_send = messaging.new_message('lateralPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'modelV2'])
plan_send.lateralPlan.laneWidth = float(self.LP.lane_width)
plan_send.lateralPlan.dPathPoints = [float(x) for x in self.y_pts]
plan_send.lateralPlan.psis = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 2]]
plan_send.lateralPlan.curvatures = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 3]]
plan_send.lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
plan_send.lateralPlan.lProb = float(self.LP.lll_prob)
plan_send.lateralPlan.rProb = float(self.LP.rll_prob)
plan_send.lateralPlan.dProb = float(self.LP.d_prob)

plan_send.lateralPlan.mpcSolutionValid = bool(plan_solution_valid)

plan_send.lateralPlan.desire = self.desire
plan_send.lateralPlan.useLaneLines = self.use_lanelines
plan_send.lateralPlan.laneChangeState = self.lane_change_state
plan_send.lateralPlan.laneChangeDirection = self.lane_change_direction

lateralPlan = plan_send.lateralPlan
lateralPlan.laneWidth = float(self.LP.lane_width)
lateralPlan.dPathPoints = [float(x) for x in self.y_pts]
lateralPlan.psis = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 2]]
lateralPlan.curvatures = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 3]]
lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
lateralPlan.lProb = float(self.LP.lll_prob)
lateralPlan.rProb = float(self.LP.rll_prob)
lateralPlan.dProb = float(self.LP.d_prob)

lateralPlan.mpcSolutionValid = bool(plan_solution_valid)

lateralPlan.desire = self.desire
lateralPlan.useLaneLines = self.use_lanelines
lateralPlan.laneChangeState = self.lane_change_state
lateralPlan.laneChangeDirection = self.lane_change_direction

pm.send('lateralPlan', plan_send)

0 comments on commit 2ca0925

Please sign in to comment.