def run_in_series(self, 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
"""
curr_speed = Vehicle.get_speed(self.agent.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.agent.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_in_series(self, 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
"""
self.update_lon_control_k_values()
self.update_lat_control_k_values()
acceleration = self._lon_controller.run_step(self.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)
control.steering = steering
self.past_steering = steering
abs_throttle = 0.2
return self.throttle_regularization(control=control,
min_throttle=-abs_throttle,
max_throttle=abs_throttle)
def long_pid_control(self, next_waypoint: Transform,
control: VehicleControl):
dist_to_car = next_waypoint.location.z
if dist_to_car < self.distance_to_keep:
self.logger.info("TOO CLOSE BRAKING!")
control.brake = True
control.throttle = 0
else:
error = dist_to_car - self.distance_to_keep
error_dt = 0 if len(self.long_error_queue
) == 0 else error - self.long_error_queue[-1]
self.long_error_queue.append(error)
error_it = sum(self.long_error_queue)
e_p = self.lon_kp * error
e_d = self.lon_kd * error_dt
e_i = self.lon_ki * error_it
long_control = np.clip(e_p + e_d + e_i, -1, self.max_throttle)
control.throttle = long_control
def run_step(self, sensors_data: SensorsData,
vehicle: Vehicle) -> VehicleControl:
super(DepthE2EAgent, self).run_step(sensors_data, vehicle)
control = VehicleControl(throttle=0.5, steering=0)
if self.front_depth_camera.data is not None:
depth_image = self.front_depth_camera.data.copy()
data = np.expand_dims(np.expand_dims(depth_image, 2), 0)
output = self.model.predict(data)
throttle, steering = output[0]
control.throttle, control.steering = float(throttle), float(
steering)
return control
def long_pid_control(self, next_waypoint, control: VehicleControl):
self_point = self.agent.vehicle.transform.to_array()
target_point = next_waypoint.location.to_array()
x_diff = target_point[0] - self_point[0]
y_diff = target_point[1] - self_point[1]
z_diff = target_point[2] - self_point[2]
dist_to_car = math.sqrt(x_diff * x_diff + z_diff * z_diff)
if dist_to_car < self.distance_to_keep:
self.logger.info(
f"TOO CLOSE BRAKING! dist_to_car: {dist_to_car} < self.distance_to_keep:{self.distance_to_keep}"
)
control.brake = True
control.throttle = 0
else:
error = dist_to_car - self.distance_to_keep
error_dt = 0 if len(self.long_error_queue
) == 0 else error - self.long_error_queue[-1]
self.long_error_queue.append(error)
error_it = sum(self.long_error_queue)
e_p = self.lon_kp * error
e_d = self.lon_kd * error_dt
e_i = self.lon_ki * error_it
long_control = np.clip(e_p + e_d + e_i, -1, self.max_throttle)
control.throttle = long_control
def smoothen_control(self, control: VehicleControl):
if abs(control.throttle) > abs(self.prev_control.throttle
) and self.prev_control.throttle > 0.15:
# ensure slower increase, faster decrease. 0.15 barely drives the car
control.throttle = (self.prev_control.throttle * self.throttle_smoothen_factor + control.throttle) / \
(self.throttle_smoothen_factor + 1)
if abs(control.steering) < abs(self.prev_control.steering):
# slowly turn back
control.steering = (
self.prev_control.steering * self.steering_smoothen_factor_backward + control.steering) / \
(self.steering_smoothen_factor_backward + 1)
elif abs(control.steering) < abs(self.prev_control.steering):
control.steering = (self.prev_control.steering * self.steering_smoothen_factor_forward + control.steering) / \
(self.steering_smoothen_factor_backward + 1)
self.prev_control = control
return control
def run_in_series(self, next_waypoint: Transform) -> VehicleControl:
super(VehicleMPCController, self).run_in_series(next_waypoint)
# get vehicle location (x, y)
# location = self.vehicle.transform.location
location = self.agent.vehicle.control.location
x, y = location.x, location.y
# get vehicle rotation
# rotation = self.vehicle.transform.rotation
rotation = self.agent.vehicle.control.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(self.agent.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 throttle_regularization(self, control: VehicleControl, min_throttle,
max_throttle):
control.throttle = np.interp(x=control.throttle,
xp=[-1, 1],
fp=[min_throttle, max_throttle])
return control
