Exemplo n.º 1
0
  def __init__(self, CP, use_lanelines=True, wide_camera=False):
    self.use_lanelines = use_lanelines
    self.wide_camera = wide_camera
    self.LP = LanePlanner(wide_camera)

    self.last_cloudlog_t = 0

    self.solution_invalid_cnt = 0

    self.lane_change_enabled = Params().get_bool('LaneChangeEnabled')
    self.auto_lane_change_enabled = Params().get_bool('AutoLaneChangeEnabled')
    self.auto_lane_change_timer = 0.0
    self.prev_torque_applied = False

    self.lane_change_state = LaneChangeState.off
    self.lane_change_direction = LaneChangeDirection.none
    self.lane_change_timer = 0.0
    self.lane_change_ll_prob = 1.0
    self.keep_pulse_timer = 0.0
    self.prev_one_blinker = False
    self.desire = log.LateralPlan.Desire.none

    self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
    self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
    self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
    self.t_idxs = np.arange(TRAJECTORY_SIZE)
    self.y_pts = np.zeros(TRAJECTORY_SIZE)

    self.lat_mpc = LateralMpc()
    self.reset_mpc(np.zeros(4))
Exemplo n.º 2
0
    def __init__(self, CP, wide_camera=False):
        self.use_lanelines = True
        self.LP = LanePlanner(wide_camera)

        self.last_cloudlog_t = 0
        self.steer_rate_cost = CP.steerRateCost

        self.solution_invalid_cnt = 0
        self.lane_change_state = LaneChangeState.off
        self.lane_change_direction = LaneChangeDirection.none
        self.lane_change_timer = 0.0
        self.lane_change_ll_prob = 1.0
        self.keep_pulse_timer = 0.0
        self.prev_one_blinker = False
        self.desire = log.LateralPlan.Desire.none

        self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
        self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
        self.plan_yaw = np.zeros((TRAJECTORY_SIZE, ))
        self.t_idxs = np.arange(TRAJECTORY_SIZE)
        self.y_pts = np.zeros(TRAJECTORY_SIZE)

        self.lat_mpc = LateralMpc()
        self.reset_mpc(np.zeros(6))

        self.lateralPlan = None
Exemplo n.º 3
0
 def test_switch_convergence(self):
     lat_mpc = LateralMpc()
     sol = run_mpc(lat_mpc=lat_mpc, poly_shift=30.0, v_ref=7.0)
     right_psi_deg = np.degrees(sol[:, 2])
     sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-30.0, v_ref=7.0)
     left_psi_deg = np.degrees(sol[:, 2])
     np.testing.assert_almost_equal(right_psi_deg, -left_psi_deg, decimal=3)
Exemplo n.º 4
0
    def __init__(self, use_lanelines=True, wide_camera=False):
        self.use_lanelines = use_lanelines
        self.LP = LanePlanner(wide_camera)
        self.DH = DesireHelper()

        self.last_cloudlog_t = 0
        self.solution_invalid_cnt = 0

        self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
        self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
        self.plan_yaw = np.zeros((TRAJECTORY_SIZE, ))
        self.t_idxs = np.arange(TRAJECTORY_SIZE)
        self.y_pts = np.zeros(TRAJECTORY_SIZE)

        self.lat_mpc = LateralMpc()
        self.reset_mpc(np.zeros(4))
Exemplo n.º 5
0
    def __init__(self, CP, use_lanelines=True, wide_camera=False):
        self.use_lanelines = use_lanelines
        self.LP = LanePlanner(wide_camera)
        self.DH = DesireHelper()

        # Vehicle model parameters used to calculate lateral movement of car
        self.factor1 = CP.wheelbase - CP.centerToFront
        self.factor2 = (CP.centerToFront * CP.mass) / (CP.wheelbase *
                                                       CP.tireStiffnessRear)
        self.last_cloudlog_t = 0
        self.solution_invalid_cnt = 0

        self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
        self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
        self.plan_yaw = np.zeros((TRAJECTORY_SIZE, ))
        self.plan_curv_rate = np.zeros((TRAJECTORY_SIZE, ))
        self.t_idxs = np.arange(TRAJECTORY_SIZE)
        self.y_pts = np.zeros(TRAJECTORY_SIZE)

        self.lat_mpc = LateralMpc()
        self.reset_mpc(np.zeros(4))
    def __init__(self, CP, use_lanelines=True, wide_camera=False):
        self.use_lanelines = use_lanelines
        self.LP = LanePlanner(wide_camera)

        self.last_cloudlog_t = 0
        self.steer_rate_cost = CP.steerRateCost

        self.solution_invalid_cnt = 0
        self.lane_change_state = LaneChangeState.off
        self.lane_change_direction = LaneChangeDirection.none
        self.lane_change_timer = 0.0
        self.lane_change_ll_prob = 1.0
        self.keep_pulse_timer = 0.0
        self.prev_one_blinker = False
        self.desire = log.LateralPlan.Desire.none

        self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
        self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
        self.plan_yaw = np.zeros((TRAJECTORY_SIZE, ))
        self.t_idxs = np.arange(TRAJECTORY_SIZE)
        self.y_pts = np.zeros(TRAJECTORY_SIZE)
        self.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))

        self.lat_mpc = LateralMpc()
        self.reset_mpc(np.zeros(6))

        self.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))
        # dp
        self.dp_torque_apply_length = 1.5  # secs of torque we apply for
        self.dp_lc_auto_start = 0.  # time to start alc
        self.dp_lc_auto_start_in = 0.  # remaining time to start alc
        self.dp_lc_auto_torque_end = 0.  # time to end applying torque
        self.dp_torque_apply = False  # should we apply torque?

        self.laneless_mode = 2  # AUTO
        self.laneless_mode_status = False
        self.laneless_mode_status_buffer = False
Exemplo n.º 7
0
def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0.,
            lane_width=3.6, poly_shift=0.):

  if lat_mpc is None:
    lat_mpc = LateralMpc()
  lat_mpc.set_weights(1., 1., 1.)

  y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
  heading_pts = np.zeros(LAT_MPC_N + 1)

  x0 = np.array([x_init, y_init, psi_init, curvature_init])
  p = np.array([v_ref, CAR_ROTATION_RADIUS])

  # converge in no more than 10 iterations
  for _ in range(10):
    lat_mpc.run(x0, p,
                y_pts, heading_pts)
  return lat_mpc.x_sol
Exemplo n.º 8
0
class LateralPlanner():
    def __init__(self, CP, use_lanelines=True, wide_camera=False):
        self.use_lanelines = use_lanelines
        self.LP = LanePlanner(wide_camera)

        self.last_cloudlog_t = 0
        self.steer_rate_cost = CP.steerRateCost

        self.solution_invalid_cnt = 0
        self.lane_change_state = LaneChangeState.off
        self.lane_change_direction = LaneChangeDirection.none
        self.lane_change_timer = 0.0
        self.lane_change_ll_prob = 1.0
        self.keep_pulse_timer = 0.0
        self.prev_one_blinker = False
        self.desire = log.LateralPlan.Desire.none

        self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
        self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
        self.plan_yaw = np.zeros((TRAJECTORY_SIZE, ))
        self.t_idxs = np.arange(TRAJECTORY_SIZE)
        self.y_pts = np.zeros(TRAJECTORY_SIZE)

        self.lat_mpc = LateralMpc()
        self.reset_mpc(np.zeros(6))

    def reset_mpc(self, x0=np.zeros(6)):
        self.x0 = x0
        self.lat_mpc.reset(x0=self.x0)
        self.desired_curvature = 0.0
        self.safe_desired_curvature = 0.0
        self.desired_curvature_rate = 0.0
        self.safe_desired_curvature_rate = 0.0

    def update(self, sm, CP):
        v_ego = sm['carState'].vEgo
        active = sm['controlsState'].active
        measured_curvature = sm['controlsState'].curvature

        md = sm['modelV2']
        self.LP.parse_model(sm['modelV2'])
        if len(md.position.x) == TRAJECTORY_SIZE and len(
                md.orientation.x) == TRAJECTORY_SIZE:
            self.path_xyz = np.column_stack(
                [md.position.x, md.position.y, md.position.z])
            self.t_idxs = np.array(md.position.t)
            self.plan_yaw = list(md.orientation.z)
        if len(md.position.xStd) == TRAJECTORY_SIZE:
            self.path_xyz_stds = np.column_stack(
                [md.position.xStd, md.position.yStd, md.position.zStd])

        # Lane change logic
        one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
        below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN

        if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX):
            self.lane_change_state = LaneChangeState.off
            self.lane_change_direction = LaneChangeDirection.none
        else:
            # LaneChangeState.off
            if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
                self.lane_change_state = LaneChangeState.preLaneChange
                self.lane_change_ll_prob = 1.0

            # LaneChangeState.preLaneChange
            elif self.lane_change_state == LaneChangeState.preLaneChange:
                # Set lane change direction
                if sm['carState'].leftBlinker:
                    self.lane_change_direction = LaneChangeDirection.left
                elif sm['carState'].rightBlinker:
                    self.lane_change_direction = LaneChangeDirection.right
                else:  # If there are no blinkers we will go back to LaneChangeState.off
                    self.lane_change_direction = LaneChangeDirection.none

                torque_applied = sm['carState'].steeringPressed and \
                                ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
                                  (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))

                blindspot_detected = (
                    (sm['carState'].leftBlindspot and
                     self.lane_change_direction == LaneChangeDirection.left) or
                    (sm['carState'].rightBlindspot and
                     self.lane_change_direction == LaneChangeDirection.right))

                if not one_blinker or below_lane_change_speed:
                    self.lane_change_state = LaneChangeState.off
                elif torque_applied and not blindspot_detected:
                    self.lane_change_state = LaneChangeState.laneChangeStarting

            # LaneChangeState.laneChangeStarting
            elif self.lane_change_state == LaneChangeState.laneChangeStarting:
                # fade out over .5s
                self.lane_change_ll_prob = max(
                    self.lane_change_ll_prob - 2 * DT_MDL, 0.0)

                # 98% certainty
                lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
                if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
                    self.lane_change_state = LaneChangeState.laneChangeFinishing

            # LaneChangeState.laneChangeFinishing
            elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
                # fade in laneline over 1s
                self.lane_change_ll_prob = min(
                    self.lane_change_ll_prob + DT_MDL, 1.0)
                if one_blinker and self.lane_change_ll_prob > 0.99:
                    self.lane_change_state = LaneChangeState.preLaneChange
                elif self.lane_change_ll_prob > 0.99:
                    self.lane_change_state = LaneChangeState.off

        if self.lane_change_state in [
                LaneChangeState.off, LaneChangeState.preLaneChange
        ]:
            self.lane_change_timer = 0.0
        else:
            self.lane_change_timer += DT_MDL

        self.prev_one_blinker = one_blinker

        self.desire = DESIRES[self.lane_change_direction][
            self.lane_change_state]

        # Send keep pulse once per second during LaneChangeStart.preLaneChange
        if self.lane_change_state in [
                LaneChangeState.off, LaneChangeState.laneChangeStarting
        ]:
            self.keep_pulse_timer = 0.0
        elif self.lane_change_state == LaneChangeState.preLaneChange:
            self.keep_pulse_timer += DT_MDL
            if self.keep_pulse_timer > 1.0:
                self.keep_pulse_timer = 0.0
            elif self.desire in [
                    log.LateralPlan.Desire.keepLeft,
                    log.LateralPlan.Desire.keepRight
            ]:
                self.desire = log.LateralPlan.Desire.none

        # Turn off lanes during lane change
        if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
            self.LP.lll_prob *= self.lane_change_ll_prob
            self.LP.rll_prob *= self.lane_change_ll_prob
        if self.use_lanelines:
            d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
            self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING,
                                     CP.steerRateCost)
        else:
            d_path_xyz = self.path_xyz
            path_cost = np.clip(
                abs(self.path_xyz[0, 1] / self.path_xyz_stds[0, 1]), 0.5,
                5.0) * MPC_COST_LAT.PATH
            # Heading cost is useful at low speed, otherwise end of plan can be off-heading
            heading_cost = interp(v_ego, [5.0, 10.0],
                                  [MPC_COST_LAT.HEADING, 0.0])
            self.lat_mpc.set_weights(path_cost, heading_cost, CP.steerRateCost)
        y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1],
                          np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
        heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1],
                                np.linalg.norm(self.path_xyz, axis=1),
                                self.plan_yaw)
        self.y_pts = y_pts

        assert len(y_pts) == LAT_MPC_N + 1
        assert len(heading_pts) == LAT_MPC_N + 1
        self.x0[4] = v_ego
        self.lat_mpc.run(self.x0, v_ego, CAR_ROTATION_RADIUS, y_pts,
                         heading_pts)
        # init state for next
        self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1],
                            self.lat_mpc.x_sol[:, 3])

        #  Check for infeasable MPC solution
        mpc_nans = any(math.isnan(x) for x in self.lat_mpc.x_sol[:, 3])
        t = sec_since_boot()
        if mpc_nans or self.lat_mpc.solution_status != 0:
            self.reset_mpc()
            self.x0[3] = measured_curvature
            if t > self.last_cloudlog_t + 5.0:
                self.last_cloudlog_t = t
                cloudlog.warning("Lateral mpc - nan: True")

        if self.lat_mpc.cost > 20000. or mpc_nans:
            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('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

        pm.send('lateralPlan', plan_send)
Exemplo n.º 9
0
class LateralPlanner:
    def __init__(self, use_lanelines=True, wide_camera=False):
        self.use_lanelines = use_lanelines
        self.LP = LanePlanner(wide_camera)
        self.DH = DesireHelper()

        self.last_cloudlog_t = 0
        self.solution_invalid_cnt = 0

        self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
        self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
        self.plan_yaw = np.zeros((TRAJECTORY_SIZE, ))
        self.t_idxs = np.arange(TRAJECTORY_SIZE)
        self.y_pts = np.zeros(TRAJECTORY_SIZE)

        self.lat_mpc = LateralMpc()
        self.reset_mpc(np.zeros(4))

    def reset_mpc(self, x0=np.zeros(4)):
        self.x0 = x0
        self.lat_mpc.reset(x0=self.x0)

    def update(self, sm):
        v_ego = sm['carState'].vEgo
        measured_curvature = sm['controlsState'].curvature

        # Parse model predictions
        md = sm['modelV2']
        self.LP.parse_model(md)
        if len(md.position.x) == TRAJECTORY_SIZE and len(
                md.orientation.x) == TRAJECTORY_SIZE:
            self.path_xyz = np.column_stack(
                [md.position.x, md.position.y, md.position.z])
            self.t_idxs = np.array(md.position.t)
            self.plan_yaw = list(md.orientation.z)
        if len(md.position.xStd) == TRAJECTORY_SIZE:
            self.path_xyz_stds = np.column_stack(
                [md.position.xStd, md.position.yStd, md.position.zStd])

        # Lane change logic
        lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
        self.DH.update(sm['carState'], sm['controlsState'].active,
                       lane_change_prob)

        # Turn off lanes during lane change
        if self.DH.desire == log.LateralPlan.Desire.laneChangeRight or self.DH.desire == log.LateralPlan.Desire.laneChangeLeft:
            self.LP.lll_prob *= self.DH.lane_change_ll_prob
            self.LP.rll_prob *= self.DH.lane_change_ll_prob

        # Calculate final driving path and set MPC costs
        if self.use_lanelines:
            d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
            self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING,
                                     MPC_COST_LAT.STEER_RATE)
        else:
            d_path_xyz = self.path_xyz
            # Heading cost is useful at low speed, otherwise end of plan can be off-heading
            heading_cost = interp(v_ego, [5.0, 10.0],
                                  [MPC_COST_LAT.HEADING, 0.15])
            self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost,
                                     MPC_COST_LAT.STEER_RATE)

        y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1],
                          np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
        heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1],
                                np.linalg.norm(self.path_xyz, axis=1),
                                self.plan_yaw)
        self.y_pts = y_pts

        assert len(y_pts) == LAT_MPC_N + 1
        assert len(heading_pts) == LAT_MPC_N + 1
        # self.x0[4] = v_ego
        p = np.array([v_ego, CAR_ROTATION_RADIUS])
        self.lat_mpc.run(self.x0, p, y_pts, heading_pts)
        # init state for next
        # mpc.u_sol is the desired curvature rate given x0 curv state.
        # with x0[3] = measured_curvature, this would be the actual desired rate.
        # instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control.
        self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1],
                            self.lat_mpc.x_sol[:, 3])

        #  Check for infeasible MPC solution
        mpc_nans = np.isnan(self.lat_mpc.x_sol[:, 3]).any()
        t = sec_since_boot()
        if mpc_nans or self.lat_mpc.solution_status != 0:
            self.reset_mpc()
            self.x0[3] = measured_curvature
            if t > self.last_cloudlog_t + 5.0:
                self.last_cloudlog_t = t
                cloudlog.warning("Lateral mpc - nan: True")

        if self.lat_mpc.cost > 20000. or mpc_nans:
            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('lateralPlan')
        plan_send.valid = sm.all_checks(
            service_list=['carState', 'controlsState', 'modelV2'])

        lateralPlan = plan_send.lateralPlan
        lateralPlan.modelMonoTime = sm.logMonoTime['modelV2']
        lateralPlan.laneWidth = float(self.LP.lane_width)
        lateralPlan.dPathPoints = self.y_pts.tolist()
        lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
        lateralPlan.curvatures = self.lat_mpc.x_sol[0:CONTROL_N, 3].tolist()
        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.solverExecutionTime = self.lat_mpc.solve_time

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

        pm.send('lateralPlan', plan_send)
Exemplo n.º 10
0
class LateralPlanner():
    def __init__(self, CP, use_lanelines=True, wide_camera=False):
        self.use_lanelines = use_lanelines
        self.LP = LanePlanner(wide_camera)

        self.last_cloudlog_t = 0
        self.steer_rate_cost = CP.steerRateCost

        self.solution_invalid_cnt = 0
        self.lane_change_state = LaneChangeState.off
        self.lane_change_direction = LaneChangeDirection.none
        self.lane_change_timer = 0.0
        self.lane_change_ll_prob = 1.0
        self.keep_pulse_timer = 0.0
        self.prev_one_blinker = False
        self.desire = log.LateralPlan.Desire.none

        self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
        self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
        self.plan_yaw = np.zeros((TRAJECTORY_SIZE, ))
        self.t_idxs = np.arange(TRAJECTORY_SIZE)
        self.y_pts = np.zeros(TRAJECTORY_SIZE)
        self.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))

        self.lat_mpc = LateralMpc()
        self.reset_mpc(np.zeros(6))

        self.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))
        # dp
        self.dp_torque_apply_length = 1.5  # secs of torque we apply for
        self.dp_lc_auto_start = 0.  # time to start alc
        self.dp_lc_auto_start_in = 0.  # remaining time to start alc
        self.dp_lc_auto_torque_end = 0.  # time to end applying torque
        self.dp_torque_apply = False  # should we apply torque?

        self.laneless_mode = 2  # AUTO
        self.laneless_mode_status = False
        self.laneless_mode_status_buffer = False

    def reset_mpc(self, x0=np.zeros(6)):
        self.x0 = x0
        self.lat_mpc.reset(x0=self.x0)
        self.desired_curvature = 0.0
        self.safe_desired_curvature = 0.0
        self.desired_curvature_rate = 0.0
        self.safe_desired_curvature_rate = 0.0

    def update(self, sm, CP):
        self.use_lanelines = not sm['dragonConf'].dpLaneLessModeCtrl
        self.laneless_mode = sm['dragonConf'].dpLaneLessMode
        v_ego = sm['carState'].vEgo
        active = sm['controlsState'].active
        measured_curvature = sm['controlsState'].curvature
        self.LP.update_dp_set_offsets(sm['dragonConf'].dpCameraOffset,
                                      sm['dragonConf'].dpPathOffset)

        md = sm['modelV2']
        self.LP.parse_model(sm['modelV2'])
        if len(md.position.x) == TRAJECTORY_SIZE and len(
                md.orientation.x) == TRAJECTORY_SIZE:
            self.path_xyz = np.column_stack(
                [md.position.x, md.position.y, md.position.z])
            self.t_idxs = np.array(md.position.t)
            self.plan_yaw = list(md.orientation.z)
        if len(md.position.xStd) == TRAJECTORY_SIZE:
            self.path_xyz_stds = np.column_stack(
                [md.position.xStd, md.position.yStd, md.position.zStd])

        # Lane change logic
        one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
        below_lane_change_speed = v_ego < (sm['dragonConf'].dpLcMinMph *
                                           CV.MPH_TO_MS)

        if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX):
            self.lane_change_state = LaneChangeState.off
            self.lane_change_direction = LaneChangeDirection.none
        else:
            reset = False
            if one_blinker:
                cur_time = sec_since_boot()
                # reach auto lc condition
                if not below_lane_change_speed and sm[
                        'dragonConf'].dpLateralMode == 2 and v_ego >= (
                            sm['dragonConf'].dpLcAutoMinMph * CV.MPH_TO_MS):
                    # work out alc start time and torque apply end time
                    if self.dp_lc_auto_start == 0.:
                        self.dp_lc_auto_start = cur_time + sm[
                            'dragonConf'].dpLcAutoDelay
                        self.dp_lc_auto_torque_end = self.dp_lc_auto_start + self.dp_torque_apply_length
                    else:
                        # work out how long til alc start
                        # for display only
                        self.dp_lc_auto_start_in = self.dp_lc_auto_start - cur_time
                        self.dp_torque_apply = True if self.dp_lc_auto_start < cur_time <= self.dp_lc_auto_torque_end else False
            else:
                reset = True

            # reset all vals
            if not active or reset:
                self.dp_lc_auto_start = 0.
                self.dp_lc_auto_start_in = 0.
                self.dp_lc_auto_torque_end = 0.
                self.dp_torque_apply = False

            # LaneChangeState.off
            if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
                self.lane_change_state = LaneChangeState.preLaneChange
                self.lane_change_ll_prob = 1.0

            # LaneChangeState.preLaneChange
            elif self.lane_change_state == LaneChangeState.preLaneChange:
                # Set lane change direction
                if sm['carState'].leftBlinker:
                    self.lane_change_direction = LaneChangeDirection.left
                elif sm['carState'].rightBlinker:
                    self.lane_change_direction = LaneChangeDirection.right
                else:  # If there are no blinkers we will go back to LaneChangeState.off
                    self.lane_change_direction = LaneChangeDirection.none

                torque_applied = sm['carState'].steeringPressed and \
                                ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
                                  (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))

                blindspot_detected = (
                    (sm['carState'].leftBlindspot and
                     self.lane_change_direction == LaneChangeDirection.left) or
                    (sm['carState'].rightBlindspot and
                     self.lane_change_direction == LaneChangeDirection.right))

                # if human made lane change prior alca, we should stop alca until new blinker (off -> on)
                self.dp_lc_auto_start = self.dp_lc_auto_torque_end if torque_applied else self.dp_lc_auto_start
                torque_applied = self.dp_torque_apply if self.dp_torque_apply else torque_applied
                if not one_blinker or below_lane_change_speed:
                    self.lane_change_state = LaneChangeState.off
                elif torque_applied and not blindspot_detected:
                    self.lane_change_state = LaneChangeState.laneChangeStarting

            # LaneChangeState.laneChangeStarting
            elif self.lane_change_state == LaneChangeState.laneChangeStarting:
                # fade out over .5s
                self.lane_change_ll_prob = max(
                    self.lane_change_ll_prob - 2 * DT_MDL, 0.0)

                # 98% certainty
                lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
                if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
                    self.lane_change_state = LaneChangeState.laneChangeFinishing

            # LaneChangeState.laneChangeFinishing
            elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
                # fade in laneline over 1s
                self.lane_change_ll_prob = min(
                    self.lane_change_ll_prob + DT_MDL, 1.0)
                if one_blinker and self.lane_change_ll_prob > 0.99:
                    self.lane_change_state = LaneChangeState.preLaneChange
                elif self.lane_change_ll_prob > 0.99:
                    self.lane_change_state = LaneChangeState.off

        if self.lane_change_state in [
                LaneChangeState.off, LaneChangeState.preLaneChange
        ]:
            self.lane_change_timer = 0.0
        else:
            self.lane_change_timer += DT_MDL

        self.prev_one_blinker = one_blinker

        self.desire = DESIRES[self.lane_change_direction][
            self.lane_change_state]

        # Send keep pulse once per second during LaneChangeStart.preLaneChange
        if self.lane_change_state in [
                LaneChangeState.off, LaneChangeState.laneChangeStarting
        ]:
            self.keep_pulse_timer = 0.0
        elif self.lane_change_state == LaneChangeState.preLaneChange:
            self.keep_pulse_timer += DT_MDL
            if self.keep_pulse_timer > 1.0:
                self.keep_pulse_timer = 0.0
            elif self.desire in [
                    log.LateralPlan.Desire.keepLeft,
                    log.LateralPlan.Desire.keepRight
            ]:
                self.desire = log.LateralPlan.Desire.none

        # Turn off lanes during lane change
        if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
            self.LP.lll_prob *= self.lane_change_ll_prob
            self.LP.rll_prob *= self.lane_change_ll_prob
        self.d_path_w_lines_xyz = self.LP.get_d_path(v_ego, self.t_idxs,
                                                     self.path_xyz)
        if self.use_lanelines:
            d_path_xyz = self.d_path_w_lines_xyz
            self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING,
                                     CP.steerRateCost)
            self.laneless_mode_status = False
        elif self.laneless_mode == 0:
            d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
            self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING,
                                     CP.steerRateCost)
            self.laneless_mode_status = False
        elif self.laneless_mode == 1:
            d_path_xyz = self.path_xyz
            path_cost = np.clip(
                abs(self.path_xyz[0, 1] / self.path_xyz_stds[0, 1]), 0.5,
                5.0) * MPC_COST_LAT.PATH
            # Heading cost is useful at low speed, otherwise end of plan can be off-heading
            heading_cost = interp(v_ego, [5.0, 10.0],
                                  [MPC_COST_LAT.HEADING, 0.0])
            self.lat_mpc.set_weights(path_cost, heading_cost, CP.steerRateCost)
            self.laneless_mode_status = True
        elif self.laneless_mode == 2 and (
            (self.LP.lll_prob + self.LP.rll_prob) / 2 <
                0.3) and self.lane_change_state == LaneChangeState.off:
            d_path_xyz = self.path_xyz
            path_cost = np.clip(
                abs(self.path_xyz[0, 1] / self.path_xyz_stds[0, 1]), 0.5,
                5.0) * MPC_COST_LAT.PATH
            # Heading cost is useful at low speed, otherwise end of plan can be off-heading
            heading_cost = interp(v_ego, [5.0, 10.0],
                                  [MPC_COST_LAT.HEADING, 0.0])
            self.lat_mpc.set_weights(path_cost, heading_cost, CP.steerRateCost)
            self.laneless_mode_status = True
            self.laneless_mode_status_buffer = True
        elif self.laneless_mode == 2 and ((self.LP.lll_prob + self.LP.rll_prob)/2 > 0.5) and \
         self.laneless_mode_status_buffer and self.lane_change_state == LaneChangeState.off:
            d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
            self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING,
                                     CP.steerRateCost)
            self.laneless_mode_status = False
            self.laneless_mode_status_buffer = False
        elif self.laneless_mode == 2 and self.laneless_mode_status_buffer == True and self.lane_change_state == LaneChangeState.off:
            d_path_xyz = self.path_xyz
            path_cost = np.clip(
                abs(self.path_xyz[0, 1] / self.path_xyz_stds[0, 1]), 0.5,
                5.0) * MPC_COST_LAT.PATH
            # Heading cost is useful at low speed, otherwise end of plan can be off-heading
            heading_cost = interp(v_ego, [5.0, 10.0],
                                  [MPC_COST_LAT.HEADING, 0.0])
            self.lat_mpc.set_weights(path_cost, heading_cost, CP.steerRateCost)
            self.laneless_mode_status = True
        else:
            d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
            self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING,
                                     CP.steerRateCost)
            self.laneless_mode_status = False
            self.laneless_mode_status_buffer = False

        y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1],
                          np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
        heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1],
                                np.linalg.norm(self.path_xyz, axis=1),
                                self.plan_yaw)
        self.y_pts = y_pts

        assert len(y_pts) == LAT_MPC_N + 1
        assert len(heading_pts) == LAT_MPC_N + 1
        self.x0[4] = v_ego
        self.lat_mpc.run(self.x0, v_ego, CAR_ROTATION_RADIUS, y_pts,
                         heading_pts)
        # init state for next
        self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1],
                            self.lat_mpc.x_sol[:, 3])

        #  Check for infeasable MPC solution
        mpc_nans = any(math.isnan(x) for x in self.lat_mpc.x_sol[:, 3])
        t = sec_since_boot()
        if mpc_nans or self.lat_mpc.solution_status != 0:
            self.reset_mpc()
            self.x0[3] = measured_curvature
            if t > self.last_cloudlog_t + 5.0:
                self.last_cloudlog_t = t
                cloudlog.warning("Lateral mpc - nan: True")

        if self.lat_mpc.cost > 20000. or mpc_nans:
            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('lateralPlan')
        plan_send.valid = sm.all_alive_and_valid(service_list=[
            'carState', 'controlsState', 'modelV2', 'dragonConf'
        ])
        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
        # dp
        plan_send.lateralPlan.dpLaneLessModeStatus = bool(
            self.laneless_mode_status)
        plan_send.lateralPlan.dpALCAStartIn = self.dp_lc_auto_start_in

        plan_send.lateralPlan.dPathWLinesX = [
            float(x) for x in self.d_path_w_lines_xyz[:, 0]
        ]
        plan_send.lateralPlan.dPathWLinesY = [
            float(y) for y in self.d_path_w_lines_xyz[:, 1]
        ]

        pm.send('lateralPlan', plan_send)
Exemplo n.º 11
0
class LateralPlanner:
  def __init__(self, CP, use_lanelines=True, wide_camera=False):
    self.use_lanelines = use_lanelines
    self.wide_camera = wide_camera
    self.LP = LanePlanner(wide_camera)

    self.last_cloudlog_t = 0

    self.solution_invalid_cnt = 0

    self.lane_change_enabled = Params().get_bool('LaneChangeEnabled')
    self.auto_lane_change_enabled = Params().get_bool('AutoLaneChangeEnabled')
    self.auto_lane_change_timer = 0.0
    self.prev_torque_applied = False

    self.lane_change_state = LaneChangeState.off
    self.lane_change_direction = LaneChangeDirection.none
    self.lane_change_timer = 0.0
    self.lane_change_ll_prob = 1.0
    self.keep_pulse_timer = 0.0
    self.prev_one_blinker = False
    self.desire = log.LateralPlan.Desire.none

    self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
    self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
    self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
    self.t_idxs = np.arange(TRAJECTORY_SIZE)
    self.y_pts = np.zeros(TRAJECTORY_SIZE)

    self.lat_mpc = LateralMpc()
    self.reset_mpc(np.zeros(4))

  def reset_mpc(self, x0=np.zeros(4)):
    self.x0 = x0
    self.lat_mpc.reset(x0=self.x0)

  def update(self, sm):
    v_ego = sm['carState'].vEgo
    active = sm['controlsState'].active
    measured_curvature = sm['controlsState'].curvature

    md = sm['modelV2']
    self.LP.parse_model(sm['modelV2'])
    if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE:
      self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
      self.t_idxs = np.array(md.position.t)
      self.plan_yaw = list(md.orientation.z)
    if len(md.position.xStd) == TRAJECTORY_SIZE:
      self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd])

    # Lane change logic
    one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
    below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN

    if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not one_blinker) or (not self.lane_change_enabled):
      self.lane_change_state = LaneChangeState.off
      self.lane_change_direction = LaneChangeDirection.none
    else:
      torque_applied = sm['carState'].steeringPressed and \
                       ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
                        (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) or \
                        self.auto_lane_change_enabled and \
                       (AUTO_LCA_START_TIME+0.25) > self.auto_lane_change_timer > AUTO_LCA_START_TIME

      blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
                            (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))

      lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob

      # State transitions
      # off
      if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
        if sm['carState'].leftBlinker:
          self.lane_change_direction = LaneChangeDirection.left
        elif sm['carState'].rightBlinker:
          self.lane_change_direction = LaneChangeDirection.right

        self.lane_change_state = LaneChangeState.preLaneChange
        self.lane_change_ll_prob = 1.0

      # pre
      elif self.lane_change_state == LaneChangeState.preLaneChange:
        if not one_blinker or below_lane_change_speed:
          self.lane_change_state = LaneChangeState.off
        elif torque_applied and (not blindspot_detected or self.prev_torque_applied):
          self.lane_change_state = LaneChangeState.laneChangeStarting
        elif torque_applied and blindspot_detected and self.auto_lane_change_timer != 10.0:
          self.auto_lane_change_timer = 10.0
        elif not torque_applied and self.auto_lane_change_timer == 10.0 and not self.prev_torque_applied:
          self.prev_torque_applied = True

      # LaneChangeState.laneChangeStarting
      elif self.lane_change_state == LaneChangeState.laneChangeStarting:
        # fade out over .5s
        self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0)
        # 98% certainty
        if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
          self.lane_change_state = LaneChangeState.laneChangeFinishing

      # LaneChangeState.laneChangeFinishing
      elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
        # fade in laneline over 1s
        self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
        if self.lane_change_ll_prob > 0.99:
          self.lane_change_direction = LaneChangeDirection.none
          if one_blinker:
            self.lane_change_state = LaneChangeState.preLaneChange
          else:
            self.lane_change_state = LaneChangeState.off

    if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange):
      self.lane_change_timer = 0.0
    else:
      self.lane_change_timer += DT_MDL

    if self.lane_change_state == LaneChangeState.off:
      self.auto_lane_change_timer = 0.0
      self.prev_torque_applied = False
    elif self.auto_lane_change_timer < (AUTO_LCA_START_TIME+0.25): # stop afer 3 sec resume from 10 when torque applied
      self.auto_lane_change_timer += DT_MDL

    self.prev_one_blinker = one_blinker

    self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]

    # Send keep pulse once per second during LaneChangeStart.preLaneChange
    if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting):
      self.keep_pulse_timer = 0.0
    elif self.lane_change_state == LaneChangeState.preLaneChange:
      self.keep_pulse_timer += DT_MDL
      if self.keep_pulse_timer > 1.0:
        self.keep_pulse_timer = 0.0
      elif self.desire in (log.LateralPlan.Desire.keepLeft, log.LateralPlan.Desire.keepRight):
        self.desire = log.LateralPlan.Desire.none

    # Turn off lanes during lane change
    if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
      self.LP.lll_prob *= self.lane_change_ll_prob
      self.LP.rll_prob *= self.lane_change_ll_prob
    if self.use_lanelines:
      d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
      self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, ntune_common_get('steerRateCost'))
    else:
      d_path_xyz = self.path_xyz
      path_cost = np.clip(abs(self.path_xyz[0, 1] / self.path_xyz_stds[0, 1]), 0.5, 1.5) * MPC_COST_LAT.PATH
      # Heading cost is useful at low speed, otherwise end of plan can be off-heading
      heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0])
      self.lat_mpc.set_weights(path_cost, heading_cost, ntune_common_get('steerRateCost'))

    y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1])
    heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
    self.y_pts = y_pts

    assert len(y_pts) == LAT_MPC_N + 1
    assert len(heading_pts) == LAT_MPC_N + 1
    # self.x0[4] = v_ego
    p = np.array([v_ego, CAR_ROTATION_RADIUS])
    self.lat_mpc.run(self.x0,
                     p,
                     y_pts,
                     heading_pts)
    # init state for next
    self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])

    #  Check for infeasible MPC solution
    mpc_nans = np.isnan(self.lat_mpc.x_sol[:, 3]).any()
    t = sec_since_boot()
    if mpc_nans or self.lat_mpc.solution_status != 0:
      self.reset_mpc()
      self.x0[3] = measured_curvature
      if t > self.last_cloudlog_t + 5.0:
        self.last_cloudlog_t = t
        cloudlog.warning("Lateral mpc - nan: True")

    if self.lat_mpc.cost > 20000. or mpc_nans:
      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('lateralPlan')
    plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'modelV2'])

    lateralPlan = plan_send.lateralPlan
    lateralPlan.laneWidth = float(self.LP.lane_width)
    lateralPlan.dPathPoints = self.y_pts.tolist()
    lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
    lateralPlan.curvatures = self.lat_mpc.x_sol[0:CONTROL_N, 3].tolist()
    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.solverExecutionTime = self.lat_mpc.solve_time

    lateralPlan.desire = self.desire
    lateralPlan.useLaneLines = self.use_lanelines
    lateralPlan.laneChangeState = self.lane_change_state
    lateralPlan.laneChangeDirection = self.lane_change_direction
    lateralPlan.autoLaneChangeEnabled = self.auto_lane_change_enabled
    lateralPlan.autoLaneChangeTimer = int(AUTO_LCA_START_TIME) - int(self.auto_lane_change_timer)

    pm.send('lateralPlan', plan_send)