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 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