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 convert_control_from_source_to_agent( self, source: carla.VehicleControl) -> VehicleControl: """Convert CARLA raw vehicle control to VehicleControl(throttle,steering).""" return VehicleControl( throttle=-1 * source.throttle if source.reverse else source.throttle, steering=source.steer, )
def run_step(self, vehicle: Vehicle, sensors_data: SensorsData) -> VehicleControl: super(PIDAgent, self).run_step(vehicle=vehicle, sensors_data=sensors_data) self.transform_history.append(self.vehicle.transform) if self.local_planner.is_done(): control = VehicleControl() self.logger.debug("Path Following Agent is Done. Idling.") else: control = self.local_planner.run_step(vehicle=vehicle) return control
def run_step(self, vehicle: Vehicle, sensors_data: SensorsData) -> VehicleControl: super(GPDAgent, self).run_step(vehicle=vehicle, sensors_data=sensors_data) self.gpd_detector.run_step() if self.gpd_detector.curr_segmentation is not None: world_cords = img_to_world2(depth_img=self.front_depth_camera.data, segmentation=self.gpd_detector.curr_segmentation, criteria=self.gpd_detector.GROUND, intrinsics_matrix=self.front_depth_camera.intrinsics_matrix, extrinsics_matrix= self.front_depth_camera.transform.get_matrix() @ self.vehicle.transform.get_matrix())[:2] print(np.shape(world_cords)) return VehicleControl()
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 run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl: """ Receive Sensor Data and vehicle state information on every step and return a control Args: sensors_data: sensor data on this frame vehicle: vehicle state on this frame Returns: Vehicle Control """ self.time_counter += 1 self.sync_data(sensors_data=sensors_data, vehicle=vehicle) if self.should_save_sensor_data: self.save_sensor_data() return VehicleControl()
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 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
def run_step(self, vehicle: Vehicle) -> VehicleControl: """ Run step for the local planner Procedure: 1. Sync data 2. get the correct look ahead for current speed 3. get the correct next waypoint 4. feed waypoint into controller 5. return result from controller Args: vehicle: current vehicle state Returns: next control that the local think the agent should execute. """ self.sync_data(vehicle=vehicle) # on every run step, sync first if (len(self.mission_planner.mission_plan) == 0 and len(self.way_points_queue) == 0): return VehicleControl() # get vehicle's location vehicle_transform: Union[Transform, None] = self.vehicle.transform if vehicle_transform is None: raise AgentException( "I do not know where I am, I cannot proceed forward") # redefine closeness level based on speed curr_speed = Vehicle.get_speed(self.vehicle) if curr_speed < 60: self.closeness_threshold = 5 elif curr_speed < 80: self.closeness_threshold = 15 elif curr_speed < 120: self.closeness_threshold = 20 else: self.closeness_threshold = 50 # print(f"Curr closeness threshold = {self.closeness_threshold}") # get current waypoint curr_closest_dist = float("inf") while True: if len(self.way_points_queue) == 0: self.logger.info("Destination reached") return VehicleControl() waypoint: Transform = self.way_points_queue[0] curr_dist = vehicle_transform.location.distance(waypoint.location) if curr_dist < curr_closest_dist: # if i find a waypoint that is closer to me than before # note that i will always enter here to start the calculation for curr_closest_dist curr_closest_dist = curr_dist elif curr_dist < self.closeness_threshold: # i have moved onto a waypoint, remove that waypoint from the queue self.way_points_queue.popleft() else: break target_waypoint = self.way_points_queue[0] # target_waypoint = Transform.average(self.way_points_queue[0], self.way_points_queue[1]) # target_waypoint = Transform.average(self.way_points_queue[2], target_waypoint) control: VehicleControl = self.controller.run_step( vehicle=vehicle, next_waypoint=target_waypoint) # self.logger.debug( # f"Target_Location {target_waypoint.location} " # f"| Curr_Location {vehicle_transform.location} " # f"| Distance {int(curr_closest_dist)}") return control
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) -> VehicleControl: return VehicleControl()
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