Control

def __init__(self, vehicle: Vehicle):
    """

    Args:
        vehicle: Vehicle instance

    """
    self.vehicle = vehicle
    self.logger = logging.getLogger(__name__)

@abstractmethod
def run_step(self, vehicle: Vehicle, next_waypoint: Transform, **kwargs) \
        -> VehicleControl:
    """
    Abstract function for run step

    Args:
        vehicle: new vehicle state
        next_waypoint: next waypoint
        **kwargs:

    Returns:
        VehicleControl
    """
    self.sync_data(vehicle=vehicle)
    return VehicleControl()

def sync_data(self, vehicle: Vehicle) -> None:
    """
    default sync data function
    Args:
        vehicle: new vehicle state

    Returns:
        None

    """
    self.vehicle = vehicle

def __init__(self,
             vehicle: Vehicle,
             route_file_path: Path,  # read in route
             target_speed=float("inf"),
             steps_ahead=10,
             max_throttle=1,
             max_steering=1,
             dt=0.1):
    super().__init__(vehicle)
    self.logger = logging.getLogger(__name__)
    # Read in route file
    self.track_DF = pd.read_csv(route_file_path, header=None)
    # Fit the route to a curve
    spline_points = 10000
    self.pts_2D = self.track_DF.loc[:, [0, 1]].values
    tck, u = splprep(self.pts_2D.T, u=None, s=2.0, per=1, k=3)
    u_new = np.linspace(u.min(), u.max(), spline_points)
    x_new, y_new = splev(u_new, tck, der=0)
    self.pts_2D = np.c_[x_new, y_new]

    # Modified parm
    self.prev_cte = 0

    self.target_speed = target_speed
    self.state_vars = ('x', 'y', 'v', 'ψ', 'cte', 'eψ')

    self.steps_ahead = steps_ahead
    self.dt = dt

    # Cost function coefficients
    self.cte_coeff = 100  # 100
    self.epsi_coeff = 100  # 100
    self.speed_coeff = 0.4  # 0.2
    self.acc_coeff = 1  # 1
    self.steer_coeff = 0.1  # 0.1
    self.consec_acc_coeff = 50
    self.consec_steer_coeff = 50

    # Front wheel L
    self.Lf = 2.5

    # How the polynomial fitting the desired curve is fitted
    self.steps_poly = 30  # modify to 3 when using 3D data
    self.poly_degree = 3

    # Bounds for the optimizer
    self.bounds = (
            6 * self.steps_ahead * [(None, None)]
            + self.steps_ahead * [(0, max_throttle)]  # throttle bounds
            + self.steps_ahead * [(-max_steering, max_steering)]
    # steer bounds
    )

    # State 0 placeholder
    num_vars = (len(
        self.state_vars) + 2)  # State variables and two actuators
    self.state0 = np.zeros(self.steps_ahead * num_vars)

    # Lambdify and minimize stuff
    self.evaluator = 'numpy'
    self.tolerance = 1
    self.cost_func, self.cost_grad_func, self.constr_funcs = \
        self.get_func_constraints_and_bounds()

    # To keep the previous state
    self.steer = 0
    self.throttle = 0

    self.logger.debug("MPC Controller initiated")

@staticmethod
def clip_throttle(throttle, curr_speed, target_speed):
    return np.clip(
        throttle - 0.01 * (curr_speed - target_speed),
        0.4,
        0.9
    )

@staticmethod
def create_array_of_symbols(str_symbol, N):
    return sym.symbols('{symbol}0:{N}'.format(symbol=str_symbol, N=N))

def generate_fun(self, symb_fun, vars_, init, poly):
    """
    Generates a function of the form `fun(x, *args)`
    """
    args = init + poly
    return sym.lambdify((vars_, *args), symb_fun, self.evaluator)

def generate_grad(self, symb_fun, vars_, init, poly):
    """
    TODO: add comments
    """
    args = init + poly
    return sym.lambdify(
        (vars_, *args),
        derive_by_array(symb_fun, vars_ + args)[:len(vars_)],
        self.evaluator
    )

def get_closest_waypoint_index_2D(self, car_location, waypoint_location):
    """Get the index of the closest waypoint in self.pts_2D
        Note: it may give wrong index when the route is overlapped
    """
    location_arr = np.array([
        car_location.x,
        car_location.y
    ])
    dists = np.linalg.norm(self.pts_2D - location_arr, axis=1)
    return np.argmin(dists)

def get_closest_waypoint_index_3D(self, car_location, waypoint_location):
    """Get the index of the closest waypoint in self.track_DF
        car_location: current car location
        waypoint_location: next_waypoint
    """
    index = self.track_DF.loc[(self.track_DF[0] == waypoint_location.x)
                              & (self.track_DF[
                                     1] == waypoint_location.y)].index
    if len(index) > 0:
        return index[0]
    else:
        location_arr = np.array([
            car_location.x,
            car_location.y,
            car_location.z,
        ])
        dists = np.linalg.norm(self.track_DF - location_arr, axis=1)
        return np.argmin(dists)

def get_func_constraints_and_bounds(self):
    """
    Defines MPC's cost function and constraints.
    """
    # Polynomial coefficients will also be symbolic variables
    poly = self.create_array_of_symbols('poly', self.poly_degree + 1)

    # Initialize the initial state
    x_init = sym.symbols('x_init')
    y_init = sym.symbols('y_init')
    ψ_init = sym.symbols('ψ_init')
    v_init = sym.symbols('v_init')
    cte_init = sym.symbols('cte_init')
    eψ_init = sym.symbols('eψ_init')

    init = (x_init, y_init, ψ_init, v_init, cte_init, eψ_init)

    # State variables
    x = self.create_array_of_symbols('x', self.steps_ahead)
    y = self.create_array_of_symbols('y', self.steps_ahead)
    ψ = self.create_array_of_symbols('ψ', self.steps_ahead)
    v = self.create_array_of_symbols('v', self.steps_ahead)
    cte = self.create_array_of_symbols('cte', self.steps_ahead)
    eψ = self.create_array_of_symbols('eψ', self.steps_ahead)

    # Actuators
    a = self.create_array_of_symbols('a', self.steps_ahead)
    δ = self.create_array_of_symbols('δ', self.steps_ahead)

    vars_ = (
        # Symbolic arrays (but NOT actuators)
        *x, *y, *ψ, *v, *cte, *eψ,

        # Symbolic arrays (actuators)
        *a, *δ,
    )

    cost = 0
    for t in range(self.steps_ahead):
        cost += (
            # Reference state penalties
                self.cte_coeff * cte[t] ** 2
                + self.epsi_coeff * eψ[t] ** 2 +
                + self.speed_coeff * (v[t] - self.target_speed) ** 2

                # Actuator penalties
                + self.acc_coeff * a[t] ** 2
                + self.steer_coeff * δ[t] ** 2
        )

    # Penalty for differences in consecutive actuators
    for t in range(self.steps_ahead - 1):
        cost += (
                self.consec_acc_coeff * (a[t + 1] - a[t]) ** 2
                + self.consec_steer_coeff * (δ[t + 1] - δ[t]) ** 2
        )

    # Initialize constraints
    eq_constr = _EqualityConstraints(self.steps_ahead, self.state_vars)
    eq_constr['x'][0] = x[0] - x_init
    eq_constr['y'][0] = y[0] - y_init
    eq_constr['ψ'][0] = ψ[0] - ψ_init
    eq_constr['v'][0] = v[0] - v_init
    eq_constr['cte'][0] = cte[0] - cte_init
    eq_constr['eψ'][0] = eψ[0] - eψ_init

    for t in range(1, self.steps_ahead):
        curve = sum(
            poly[-(i + 1)] * x[t - 1] ** i for i in range(len(poly)))
        # The desired ψ is equal to the derivative of the polynomial
        # curve at
        #  point x[t-1]
        ψdes = sum(poly[-(i + 1)] * i * x[t - 1] ** (i - 1) for i in
                   range(1, len(poly)))

        eq_constr['x'][t] = x[t] - (
                    x[t - 1] + v[t - 1] * sym.cos(ψ[t - 1]) * self.dt)
        eq_constr['y'][t] = y[t] - (
                    y[t - 1] + v[t - 1] * sym.sin(ψ[t - 1]) * self.dt)
        eq_constr['ψ'][t] = ψ[t] - (
                    ψ[t - 1] - v[t - 1] * δ[t - 1] / self.Lf * self.dt)
        eq_constr['v'][t] = v[t] - (v[t - 1] + a[t - 1] * self.dt)
        eq_constr['cte'][t] = cte[t] - (
                    curve - y[t - 1] + v[t - 1] * sym.sin(
                eψ[t - 1]) * self.dt)
        eq_constr['eψ'][t] = eψ[t] - (ψ[t - 1] - ψdes - v[t - 1] * δ[
            t - 1] / self.Lf * self.dt)

    # Generate actual functions from
    cost_func = self.generate_fun(cost, vars_, init, poly)
    cost_grad_func = self.generate_grad(cost, vars_, init, poly)

    constr_funcs = []
    for symbol in self.state_vars:
        for t in range(self.steps_ahead):
            func = self.generate_fun(eq_constr[symbol][t], vars_, init,
                                     poly)
            grad_func = self.generate_grad(eq_constr[symbol][t], vars_,
                                           init, poly)
            constr_funcs.append(
                {'type': 'eq', 'fun': func, 'jac': grad_func,
                 'args': None},
            )

    return cost_func, cost_grad_func, constr_funcs

def get_state0(self, v, cte, epsi, a, delta, poly):
    a = a or 0
    delta = delta or 0

    x = np.linspace(0, 1, self.steps_ahead)
    y = np.polyval(poly, x)
    psi = 0

    self.state0[:self.steps_ahead] = x
    self.state0[self.steps_ahead:2 * self.steps_ahead] = y
    self.state0[2 * self.steps_ahead:3 * self.steps_ahead] = psi
    self.state0[3 * self.steps_ahead:4 * self.steps_ahead] = v
    self.state0[4 * self.steps_ahead:5 * self.steps_ahead] = cte
    self.state0[5 * self.steps_ahead:6 * self.steps_ahead] = epsi
    self.state0[6 * self.steps_ahead:7 * self.steps_ahead] = a
    self.state0[7 * self.steps_ahead:8 * self.steps_ahead] = delta
    return self.state0

def minimize_cost(self, bounds, x0, init):
    for constr_func in self.constr_funcs:
        constr_func['args'] = init

    return minimize(
        fun=self.cost_func,
        x0=x0,
        args=init,
        jac=self.cost_grad_func,
        bounds=bounds,
        constraints=self.constr_funcs,
        method='SLSQP',
        tol=self.tolerance,
    )

def run_step(self, vehicle: Vehicle, next_waypoint: Transform,
             **kwargs) -> VehicleControl:
    super(VehicleMPCController, self).run_step(vehicle, next_waypoint)
    # get vehicle location (x, y)
    # location = self.vehicle.transform.location
    location = vehicle.transform.location
    x, y = location.x, location.y
    # get vehicle rotation
    # rotation = self.vehicle.transform.rotation
    rotation = vehicle.transform.rotation
    ψ = rotation.yaw / 180 * np.pi  # transform into radient
    cos_ψ = np.cos(ψ)
    sin_ψ = np.sin(ψ)
    # get vehicle speed
    # v = Vehicle.get_speed(self.vehicle)
    v = Vehicle.get_speed(vehicle)
    # get next waypoint location
    wx, wy = next_waypoint.location.x, next_waypoint.location.y
    # debug logging
    # self.logger.debug(f"car location:  ({x}, {y})")
    # self.logger.debug(f"car ψ: {ψ}")
    # self.logger.debug(f"car speed: {v}")
    # self.logger.debug(f"next waypoint: ({wx}, {wy})")

    ### 3D ###
    # get the index of next waypoint
    # waypoint_index = self.get_closest_waypoint_index_3D(location,
    # next_waypoint.location)
    # # find more waypoints index to fit a polynomial
    # waypoint_index_shifted = waypoint_index - 2
    # indeces = waypoint_index_shifted + self.steps_poly * np.arange(
    # self.poly_degree + 1)
    # indeces = indeces % self.track_DF.shape[0]
    # # get waypoints for polynomial fitting
    # pts = np.array([[self.track_DF.iloc[i][0], self.track_DF.iloc[i][
    # 1]] for i in indeces])

    ### 2D ###
    index_2D = self.get_closest_waypoint_index_2D(location,
                                                  next_waypoint.location)
    index_2D_shifted = index_2D - 5
    indeces_2D = index_2D_shifted + self.steps_poly * np.arange(
        self.poly_degree + 1)
    indeces_2D = indeces_2D % self.pts_2D.shape[0]
    pts = self.pts_2D[indeces_2D]

    # self.logger.debug(f'\nwaypoint index:\n  {index_2D}')
    # self.logger.debug(f'\nindeces:\n  {indeces_2D}')

    # transform waypoints from world to car coorinate
    pts_car = VehicleMPCController.transform_into_cars_coordinate_system(
        pts,
        x,
        y,
        cos_ψ,
        sin_ψ
    )
    # fit the polynomial
    poly = np.polyfit(pts_car[:, 0], pts_car[:, 1], self.poly_degree)

    # Debug
    # self.logger.debug(f'\nwaypoint index:\n  {waypoint_index}')
    # self.logger.debug(f'\nindeces:\n  {indeces}')
    # self.logger.debug(f'\npts for poly_fit:\n  {pts}')
    # self.logger.debug(f'\npts_car:\n  {pts_car}')

    ###########

    cte = poly[-1]
    eψ = -np.arctan(poly[-2])

    init = (0, 0, 0, v, cte, eψ, *poly)
    self.state0 = self.get_state0(v, cte, eψ, self.steer, self.throttle,
                                  poly)
    result = self.minimize_cost(self.bounds, self.state0, init)

    # self.steer = -0.6 * cte - 5.5 * (cte - self.prev_cte)
    # self.prev_cte = cte
    # self.throttle = VehicleMPCController.clip_throttle(self.throttle,
    # v, self.target_speed)

    control = VehicleControl()
    if 'success' in result.message:
        self.steer = result.x[-self.steps_ahead]
        self.throttle = result.x[-2 * self.steps_ahead]
    else:
        self.logger.debug('Unsuccessful optimization')

    control.steering = self.steer
    control.throttle = self.throttle

    return control

def sync_data(self, vehicle) -> None:
    super(VehicleMPCController, self).sync_data(vehicle=vehicle)

@staticmethod
def transform_into_cars_coordinate_system(pts, x, y, cos_ψ, sin_ψ):
    diff = (pts - [x, y])
    pts_car = np.zeros_like(diff)
    pts_car[:, 0] = cos_ψ * diff[:, 0] + sin_ψ * diff[:, 1]
    pts_car[:, 1] = sin_ψ * diff[:, 0] - cos_ψ * diff[:, 1]
    return pts_car

def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=0.0, dt=0.03):
    """
    Constructor method.
        :param vehicle: actor to apply to local planner logic onto
        :param K_P: Proportional term
        :param K_D: Differential term
        :param K_I: Integral term
        :param dt: time differential in seconds
    """
    self.vehicle: Vehicle = vehicle
    self.k_p = K_P
    self.k_d = K_D
    self.k_i = K_I
    self.dt = dt
    self._e_buffer = deque(maxlen=10)

def run_step(self, target_waypoint: Transform) -> float:
    """
    Execute one step of lateral control to steer
    the vehicle towards a certain waypoin.
        :param target_waypoint:
        :return: steering control in the range [-1, 1] where:
        -1 maximum steering to left
        +1 maximum steering to right
    """
    return self._pid_control(
        target_waypoint=target_waypoint, vehicle_transform=self.vehicle.transform
    )

def __init__(self, vehicle: Vehicle, K_P=1.0, K_D=0.0, K_I=0.0, dt=0.03):
    """
    Constructor method.
        :param vehicle: actor to apply to local planner logic onto
        :param K_P: Proportional term
        :param K_D: Differential term
        :param K_I: Integral term
        :param dt: time differential in seconds
    """
    self.vehicle = vehicle
    self._k_p = K_P
    self._k_d = K_D
    self._k_i = K_I
    self._dt = dt
    self._error_buffer = deque(maxlen=10)

def run_step(self, target_speed):
    """
    Execute one step of longitudinal control to reach a given target speed.
        :param target_speed: target speed in Km/h
        :return: throttle control
    """
    current_speed = Vehicle.get_speed(self.vehicle)
    return self._pid_control(target_speed, current_speed)

@staticmethod
def default_lateral_param():
    return PIDParam(K_P=1.95, K_D=0.2, K_I=0.07, dt=1.0 / 20.0)

@staticmethod
def default_longitudinal_param():
    return PIDParam(K_P=1, K_D=0, K_I=0.05, dt=1.0 / 20.0)

def __init__(
    self,
    vehicle: Vehicle,
    args_lateral: PIDParam,
    args_longitudinal: PIDParam,
    target_speed=float("inf"),
    max_throttle=1,
    max_steering=1,
):
    """

    Args:
        vehicle: actor to apply to local planner logic onto
        args_lateral:  dictionary of arguments to set the lateral PID control
        args_longitudinal: dictionary of arguments to set the longitudinal
        target_speed: target speedd in km/h
        max_throttle: maximum throttle from, will be capped at 1
        max_steering: absolute maximum steering ranging from -1 - 1
    """

    super().__init__(vehicle)
    self.logger = logging.getLogger(__name__)
    self.max_throttle = max_throttle
    self.max_steer = max_steering

    self.target_speed = target_speed

    self.past_steering = self.vehicle.control.steering
    self._lon_controller = PIDLongitudinalController(
        self.vehicle,
        K_P=args_longitudinal.K_P,
        K_D=args_longitudinal.K_D,
        K_I=args_longitudinal.K_I,
        dt=args_longitudinal.dt,
    )
    self._lat_controller = PIDLateralController(
        self.vehicle,
        K_P=args_lateral.K_P,
        K_D=args_lateral.K_D,
        K_I=args_lateral.K_I,
        dt=args_lateral.dt,
    )
    self.logger.debug("PID Controller initiated")

def run_step(
    self, vehicle: Vehicle, next_waypoint: Transform, **kwargs
) -> VehicleControl:
    """
    Execute one step of control invoking both lateral and longitudinal
    PID controllers to reach a target waypoint at a given target_speed.

    Args:
        vehicle: New vehicle state
        next_waypoint:  target location encoded as a waypoint
        **kwargs:

    Returns:
        Next Vehicle Control
    """
    super(VehiclePIDController, self).run_step(vehicle, next_waypoint)
    curr_speed = Vehicle.get_speed(self.vehicle)
    if curr_speed < 60:
        self._lat_controller.k_d = OPTIMIZED_LATERAL_PID_VALUES[60].K_D
        self._lat_controller.k_i = OPTIMIZED_LATERAL_PID_VALUES[60].K_I
        self._lat_controller.k_p = OPTIMIZED_LATERAL_PID_VALUES[60].K_P
    elif curr_speed < 100:
        self._lat_controller.k_d = OPTIMIZED_LATERAL_PID_VALUES[100].K_D
        self._lat_controller.k_i = OPTIMIZED_LATERAL_PID_VALUES[100].K_I
        self._lat_controller.k_p = OPTIMIZED_LATERAL_PID_VALUES[100].K_P
    elif curr_speed < 150:
        self._lat_controller.k_d = OPTIMIZED_LATERAL_PID_VALUES[150].K_D
        self._lat_controller.k_i = OPTIMIZED_LATERAL_PID_VALUES[150].K_I
        self._lat_controller.k_p = OPTIMIZED_LATERAL_PID_VALUES[150].K_P

    acceptable_target_speed = self.target_speed
    if abs(self.vehicle.control.steering) < 0.05:
        acceptable_target_speed += 20  # eco boost

    acceleration = self._lon_controller.run_step(acceptable_target_speed)
    current_steering = self._lat_controller.run_step(next_waypoint)
    control = VehicleControl()

    if acceleration >= 0.0:
        control.throttle = min(acceleration, self.max_throttle)
        # control.brake = 0.0
    else:
        control.throttle = 0
        # control.brake = min(abs(acceleration), self.max_brake)

    # Steering regulation: changes cannot happen abruptly, can't steer too much.
    if current_steering > self.past_steering + 0.1:
        current_steering = self.past_steering + 0.1
    elif current_steering < self.past_steering - 0.1:
        current_steering = self.past_steering - 0.1

    if current_steering >= 0:
        steering = min(self.max_steer, current_steering)
    else:
        steering = max(-self.max_steer, current_steering)
    if abs(current_steering) > 0.03 and curr_speed > 110:
        # if i am doing a sharp (>0.5) turn, i do not want to step on full gas
        control.throttle = -1

    control.steering = steering
    self.past_steering = steering
    return control

def sync_data(self, vehicle) -> None:
    super(VehiclePIDController, self).sync_data(vehicle=vehicle)
    self._lon_controller.vehicle = self.vehicle
    self._lat_controller.vehicle = self.vehicle

def __init__(
        self, vehicle: Vehicle, look_ahead_gain: float,
        look_ahead_distance: float
):
    self.vehicle = vehicle
    self.look_ahead_gain = look_ahead_gain
    self.look_ahead_distance = look_ahead_distance

def run_step(self, vehicle: Vehicle, next_waypoint: Transform) -> float:
    self.sync(vehicle=vehicle)
    target_y = next_waypoint.location.y
    target_x = next_waypoint.location.x
    angle_difference = math.atan2(
        target_y - self.vehicle.transform.location.y,
        target_x - self.vehicle.transform.location.x,
    ) - np.radians(self.vehicle.transform.rotation.yaw)
    curr_look_forward = (
            self.look_ahead_gain * Vehicle.get_speed(vehicle=vehicle)
            + self.look_ahead_distance
    )
    lateral_difference = math.atan2(
        2.0
        * self.vehicle.wheel_base
        * math.sin(angle_difference)
        / curr_look_forward,
        1.0,
    )
    return VehicleControl.clamp(lateral_difference, -1, 1)

def sync(self, vehicle: Vehicle):
    self.vehicle = vehicle

def __init__(self, vehicle: Vehicle, target_speed=60, kp=0.1):
    self.vehicle = vehicle
    self.target_speed = target_speed
    self.kp = kp

def run_step(self, vehicle: Vehicle) -> float:
    self.sync(vehicle=vehicle)
    return float(
        VehicleControl.clamp(
            self.kp * (self.target_speed - Vehicle.get_speed(vehicle)), 0,
            1
        )
    )

def sync(self, vehicle: Vehicle):
    self.vehicle = vehicle

def __init__(
        self,
        vehicle: Vehicle,
        look_ahead_gain: float = 0.1,
        look_ahead_distance: float = 2,
        target_speed=60,
):
    """

    Args:
        vehicle: Vehicle information
        look_ahead_gain: Look ahead factor
        look_ahead_distance: look ahead distance
        target_speed: desired longitudinal speed to maintain
    """

    super(PurePursuitController, self).__init__(vehicle=vehicle)
    self.target_speed = target_speed
    self.look_ahead_gain = look_ahead_gain
    self.look_ahead_distance = look_ahead_distance
    self.latitunal_controller = LatitunalPurePursuitController(
        vehicle=self.vehicle,
        look_ahead_gain=look_ahead_gain,
        look_ahead_distance=look_ahead_distance,
    )
    self.longitunal_controller = LongitunalPurePursuitController(
        vehicle=self.vehicle, target_speed=target_speed
    )

def run_step(
        self, vehicle: Vehicle, next_waypoint: Transform, **kwargs
) -> VehicleControl:
    """
    run one step of Pure Pursuit Control

    Args:
        vehicle: current vehicle state
        next_waypoint: Next waypoint, Transform
        **kwargs:

    Returns:
        Vehicle Control

    """
    control = VehicleControl(
        throttle=self.longitunal_controller.run_step(vehicle=vehicle),
        steering=self.latitunal_controller.run_step(
            vehicle=vehicle, next_waypoint=next_waypoint
        ),
    )
    return control