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