class SetPointAgent(oatomobile.Agent): """An agent that predicts setpoints and consumes them with a PID controller.""" def __init__( self, environment: oatomobile.Env, *, setpoint_index: int = 5, replan_every_steps: int = 1, lateral_control_dict: Mapping[str, Any] = LATERAL_PID_CONTROLLER_CONFIG, longitudinal_control_dict: Mapping[ str, Any] = LONGITUDINAL_PID_CONTROLLER_CONFIG, fixed_delta_seconds_between_setpoints: Optional[int] = None ) -> None: """Constructs a setpoint-based agent. Args: environment: The navigation environment to spawn the agent. setpoint_index: The index of the point to cut-off the plan. replan_every_steps: The number of steps between subsequent call on the model. lateral_control_dict: The lateral PID controller's config. longitudinal_control_dict: The longitudinal PID controller's config. fixed_delta_seconds_between_setpoints: The time difference (in seconds) between the setpoints. It defaults to the fps of the simulator. """ try: from agents.navigation.controller import VehiclePIDController # pylint: disable=import-error except ImportError: raise ImportError( "Missing CARLA installation, " "make sure the environment variable CARLA_ROOT is provided " "and that the PythonAPI is `easy_install`ed") super(SetPointAgent, self).__init__(environment=environment) # References to the CARLA objects. self._vehicle = self._environment.simulator.hero self._world = self._vehicle.get_world() self._map = self._world.get_map() # Sets up PID controllers. dt = self._vehicle.get_world().get_settings().fixed_delta_seconds lateral_control_dict = lateral_control_dict.copy() lateral_control_dict.update({"dt": dt}) logging.debug( "Lateral PID controller config {}".format(lateral_control_dict)) longitudinal_control_dict = longitudinal_control_dict.copy() longitudinal_control_dict.update({"dt": dt}) logging.debug("Longitudinal PID controller config {}".format( longitudinal_control_dict)) self._vehicle_controller = VehiclePIDController( vehicle=self._vehicle, args_lateral=lateral_control_dict, args_longitudinal=longitudinal_control_dict, ) # Sets agent's hyperparameters. self._setpoint_index = setpoint_index self._replan_every_steps = replan_every_steps self._fixed_delta_seconds_between_setpoints = fixed_delta_seconds_between_setpoints or dt # Inits agent's buffer of setpoints. self._setpoints_buffer = None self._steps_counter = 0 @abc.abstractmethod def __call__(self, observation: oatomobile.Observations, *args, **kwargs) -> np.ndarray: """Returns the predicted plan in ego-coordinates, based on a model.""" def act(self, observation: oatomobile.Observations, *args, **kwargs) -> oatomobile.Action: """Takes in an observation, samples from agent's policy, returns an action.""" from oatomobile.utils import carla as cutil # Current measurements used for local2world2local transformations. current_location = observation["location"] current_rotation = observation["rotation"] if self._setpoints_buffer is None or self._steps_counter % self._replan_every_steps == 0: # Get agent predictions. predicted_plan_ego = self(copy.deepcopy(observation), *args, **kwargs) # [T, 3] # Transform plan to world coordinates predicted_plan_world = cutil.local2world( current_location=current_location, current_rotation=current_rotation, local_locations=predicted_plan_ego, ) # Refreshes buffer. self._setpoints_buffer = predicted_plan_world else: # Pops first setpoint from the buffer. self._setpoints_buffer = self._setpoints_buffer[1:] # Registers setpoints for rendering. self._environment.unwrapped.simulator.sensor_suite.get( "predictions").predictions = cutil.world2local( current_location=current_location, current_rotation=current_rotation, world_locations=self._setpoints_buffer, ) # Increments counter. self._steps_counter += 1 # Calculates target speed by averaging speed in the `setpoint_index` window. target_speed = np.linalg.norm( np.diff(self._setpoints_buffer[:self._setpoint_index], axis=0), axis=1, ).mean() / (self._fixed_delta_seconds_between_setpoints) # Converts plan to PID controller setpoint. setpoint = self._map.get_waypoint( cutil.ndarray_to_location( self._setpoints_buffer[self._setpoint_index])) # Avoids getting stuck when spawned. if self._steps_counter <= 100: target_speed = 20.0 / 3.6 # Run PID step. control = self._vehicle_controller.run_step( target_speed=target_speed * 3.6, # PID controller expects speed in km/h! waypoint=setpoint, ) return control
class LocalPlanner(object): """ LocalPlanner implements the basic behavior of following a trajectory of waypoints that is generated on-the-fly. The low-level motion of the vehicle is computed by using two PID controllers, one is used for the lateral control and the other for the longitudinal control (cruise speed). When multiple paths are available (intersections) this local planner makes a random choice. """ # minimum distance to target waypoint as a percentage (e.g. within 90% of # total distance) MIN_DISTANCE_PERCENTAGE = 0.9 def __init__(self, vehicle, opt_dict={}): """ :param vehicle: actor to apply to local planner logic onto :param opt_dict: dictionary of arguments with the following semantics: dt -- time difference between physics control in seconds. This is typically fixed from server side using the arguments -benchmark -fps=F . In this case dt = 1/F target_speed -- desired cruise speed in Km/h sampling_radius -- search radius for next waypoints in seconds: e.g. 0.5 seconds ahead lateral_control_dict -- dictionary of arguments to setup the lateral PID controller {'K_P':, 'K_D':, 'K_I':, 'dt'} longitudinal_control_dict -- dictionary of arguments to setup the longitudinal PID controller {'K_P':, 'K_D':, 'K_I':, 'dt'} """ self._vehicle = vehicle self._map = self._vehicle.get_world().get_map() self._dt = None self._target_speed = None self._sampling_radius = None self._min_distance = None self._current_waypoint = None self._target_road_option = None self._next_waypoints = None self._target_waypoint = None self._vehicle_controller = None self._global_plan = None # queue with tuples of (waypoint, RoadOption) self._waypoints_queue = deque(maxlen=200) # self.init_controller(opt_dict) def __del__(self): self._vehicle.destroy() print("Destroying ego-vehicle!") def init_controller(self, opt_dict): """ Controller initialization. :param opt_dict: dictionary of arguments. :return: """ # default params self._dt = 1.0 / 20.0 self._target_speed = 20.0 # Km/h self._sampling_radius = self._target_speed * 0.5 / 3.6 # 0.5 seconds horizon self._min_distance = self._sampling_radius * self.MIN_DISTANCE_PERCENTAGE args_lateral_dict = { 'K_P': 1.95, 'K_D': 0.01, 'K_I': 1.4, 'dt': self._dt } args_longitudinal_dict = { 'K_P': 1.0, 'K_D': 0, 'K_I': 1, 'dt': self._dt } # parameters overload if 'dt' in opt_dict: self._dt = opt_dict['dt'] if 'target_speed' in opt_dict: self._target_speed = opt_dict['target_speed'] if 'sampling_radius' in opt_dict: self._sampling_radius = self._target_speed * \ opt_dict['sampling_radius'] / 3.6 if 'lateral_control_dict' in opt_dict: args_lateral_dict = opt_dict['lateral_control_dict'] if 'longitudinal_control_dict' in opt_dict: args_longitudinal_dict = opt_dict['longitudinal_control_dict'] self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) self._vehicle_controller = VehiclePIDController( self._vehicle, args_lateral=args_lateral_dict, args_longitudinal=args_longitudinal_dict) self._global_plan = False # compute initial waypoints self._waypoints_queue.append( (self._current_waypoint.next(self._sampling_radius)[0], RoadOption.LANEFOLLOW)) self._target_road_option = RoadOption.LANEFOLLOW # fill waypoint trajectory queue self._compute_next_waypoints(k=200) def set_speed(self, speed): """ Request new target speed. :param speed: new target speed in Km/h :return: """ self._target_speed = speed def _compute_next_waypoints(self, k=1): """ Add new waypoints to the trajectory queue. :param k: how many waypoints to compute :return: """ # check we do not overflow the queue available_entries = self._waypoints_queue.maxlen - len( self._waypoints_queue) k = min(available_entries, k) for _ in range(k): last_waypoint = self._waypoints_queue[-1][0] next_waypoints = list(last_waypoint.next(self._sampling_radius)) if len(next_waypoints) == 1: # only one option available ==> lanefollowing next_waypoint = next_waypoints[0] road_option = RoadOption.LANEFOLLOW else: # random choice between the possible options road_options_list = retrieve_options(next_waypoints, last_waypoint) road_option = random.choice(road_options_list) next_waypoint = next_waypoints[road_options_list.index( road_option)] self._waypoints_queue.append((next_waypoint, road_option)) def set_global_plan(self, current_plan): self._waypoints_queue.clear() for elem in current_plan: self._waypoints_queue.append(elem) self._target_road_option = RoadOption.LANEFOLLOW self._global_plan = True def run_step(self, debug=True): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. :param debug: boolean flag to activate waypoints debugging :return: """ # not enough waypoints in the horizon? => add more! if len(self._waypoints_queue) < int( self._waypoints_queue.maxlen * 0.5): if not self._global_plan: self._compute_next_waypoints(k=100) if len(self._waypoints_queue) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 0.0 control.hand_brake = False control.manual_gear_shift = False return control # current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) # target waypoint self._target_waypoint, self._target_road_option = self._waypoints_queue[ 0] # move using PID controllers control = self._vehicle_controller.run_step(self._target_speed, self._target_waypoint) # purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 for i, (waypoint, _) in enumerate(self._waypoints_queue): if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoints_queue.popleft() if debug: draw_waypoints(self._vehicle.get_world(), [self._target_waypoint], self._vehicle.get_location().z + 1.0) return control
class LocalPlanner(object): """ LocalPlanner implements the basic behavior of following a trajectory of waypoints that is generated on-the-fly. The low-level motion of the vehicle is computed by using two PID controllers, one is used for the lateral control and the other for the longitudinal control (cruise speed). When multiple paths are available (intersections) this local planner makes a random choice. """ # Minimum distance to target waypoint as a percentage # (e.g. within 80% of total distance) # TODO: Set FPS in constructor # FPS used for dt FPS = 10 def __init__(self, agent): """ :param agent: agent that regulates the vehicle :param vehicle: actor to apply to local planner logic onto """ self._vehicle = agent.vehicle self._map = agent.vehicle.get_world().get_map() self._target_speed = None self.sampling_radius = None self._min_distance = None self._current_waypoint = None self.target_road_option = None self._next_waypoints = None self.target_waypoint = None self._vehicle_controller = None self._global_plan = None self._pid_controller = None # queue with tuples of (waypoint, RoadOption) self.waypoints_queue = deque(maxlen=20000) self._buffer_size = 10 self._waypoint_buffer = deque(maxlen=self._buffer_size) self._prev_waypoint = None self._init_controller() # initializing controller def reset_vehicle(self): """Reset the ego-vehicle""" self._vehicle = None print("Resetting ego-vehicle!") def _init_controller(self): """ Controller initialization. dt -- time difference between physics control in seconds. This is can be fixed from server side using the arguments -benchmark -fps=F, since dt = 1/F target_speed -- desired cruise speed in km/h min_distance -- minimum distance to remove waypoint from queue lateral_dict -- dictionary of arguments to setup the lateral PID controller {'K_P':, 'K_D':, 'K_I':, 'dt'} longitudinal_dict -- dictionary of arguments to setup the longitudinal PID controller {'K_P':, 'K_D':, 'K_I':, 'dt'} """ # Default parameters self.args_lat_hw_dict = { 'K_P': 0.75, 'K_D': 0.02, 'K_I': 0.4, 'dt': 1.0 / self.FPS } self.args_lat_city_dict = { 'K_P': 0.58, 'K_D': 0.02, 'K_I': 0.5, 'dt': 1.0 / self.FPS } self.args_long_hw_dict = { 'K_P': 0.37, 'K_D': 0.024, 'K_I': 0.032, 'dt': 1.0 / self.FPS } self.args_long_city_dict = { 'K_P': 0.15, 'K_D': 0.05, 'K_I': 0.07, 'dt': 1.0 / self.FPS } self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) self._prev_waypoint = self._current_waypoint self._global_plan = False self._target_speed = self._vehicle.get_speed_limit() self._min_distance = 4.5 def set_speed(self, speed): """ Request new target speed. :param speed: new target speed in km/h """ self._target_speed = speed def set_global_plan(self, current_plan, clean=False): """ Sets new global plan. :param current_plan: list of waypoints in the actual plan """ for elem in current_plan: self.waypoints_queue.append(elem) if clean: self._waypoint_buffer.clear() for _ in range(self._buffer_size): if self.waypoints_queue: self._waypoint_buffer.append( self.waypoints_queue.popleft()) else: break # Put waypoints from global queue to buffer self._buffer_waypoints() self._global_plan = True def get_incoming_waypoint_and_direction(self, steps=3): """ Returns direction and waypoint at a distance ahead defined by the user. :param steps: number of steps to get the incoming waypoint. """ if len(self._waypoint_buffer) > steps: return self._waypoint_buffer[steps] else: try: wpt, direction = self._waypoint_buffer[-1] return wpt, direction except IndexError as i: print(i) return None, RoadOption.VOID return None, RoadOption.VOID def run_step(self, target_speed=None, debug=False): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. :param target_speed: desired speed :param debug: boolean flag to activate waypoints debugging :return: control """ if target_speed is not None: self._target_speed = target_speed else: self._target_speed = self._vehicle.get_speed_limit() # Buffer waypoints self._buffer_waypoints() if len(self._waypoint_buffer) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False return control # Current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) speed = get_speed(self._vehicle) # kph look_ahead = max(2, speed / 4.5) # Target waypoint self.target_waypoint, self.target_road_option = self._waypoint_buffer[ 0] look_ahead_loc = self._get_look_ahead_location(look_ahead) if target_speed > 50: args_lat = self.args_lat_hw_dict args_long = self.args_long_hw_dict else: args_lat = self.args_lat_city_dict args_long = self.args_long_city_dict if not self._pid_controller: self._pid_controller = VehiclePIDController( self._vehicle, args_lateral=args_lat, args_longitudinal=args_long) else: self._pid_controller.set_lon_controller_params(**args_long) self._pid_controller.set_lat_controller_params(**args_lat) control = self._pid_controller.run_step(self._target_speed, look_ahead_loc) # Purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 for i, (waypoint, _) in enumerate(self._waypoint_buffer): if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): if i == max_index: self._prev_waypoint = self._waypoint_buffer.popleft()[0] else: self._waypoint_buffer.popleft() if debug: carla_world = self._vehicle.get_world() # Draw current buffered waypoint buffered_waypts = [elem[0] for elem in self._waypoint_buffer] draw_waypoints(carla_world, buffered_waypts) # Draw current look ahead point look_ahead_loc carla_world.debug.draw_line(look_ahead_loc, look_ahead_loc + carla.Location(z=0.2), color=carla.Color(255, 255, 0), thickness=0.2, life_time=1.0) return control def _buffer_waypoints(self): """Put waypoints from global queue into the buffer.""" num_waypoints_to_add = self._buffer_size - len(self._waypoint_buffer) for _ in range(num_waypoints_to_add): if self.waypoints_queue: next_waypoint = self.waypoints_queue.popleft() self._waypoint_buffer.append(next_waypoint) else: break def waypoints_in_buffer(self): """True if waypoints exist in buffer.""" return len(self._waypoint_buffer) > 0 def _get_projection(self): """Get the projection of current vehicle position between prev and target waypoints. Returns: carla.Location: Location of the current projection point. numpy.ndarray: 3D vector formed by the prev and incoming waypoints. """ # Vector between prev and target waypoints waypt_location_diff = self.target_waypoint.transform.location - \ self._prev_waypoint.transform.location vec_waypoints = np.array([ waypt_location_diff.x, waypt_location_diff.y, waypt_location_diff.z ]) # Vector between prev waypoint and current vehicle's location veh_location_diff = self._vehicle.get_location( ) - self._prev_waypoint.transform.location vec_vehicle = np.array( [veh_location_diff.x, veh_location_diff.y, veh_location_diff.z]) proj_pt = vec_vehicle @ vec_waypoints * \ vec_waypoints / np.linalg.norm(vec_waypoints)**2 prev_waypt_loc = self._prev_waypoint.transform.location proj_loc = carla.Location(proj_pt[0] + prev_waypt_loc.x, proj_pt[1] + prev_waypt_loc.y, proj_pt[2] + prev_waypt_loc.z) return proj_loc, vec_waypoints def _get_look_ahead_location(self, look_ahead): """Get location of look ahead point along path formed by waypoints.""" proj_loc, vec_waypoints = self._get_projection() target_loc = self.target_waypoint.transform.location # Distance between current projection point and the incoming target waypoint dist_to_next_waypoint = proj_loc.distance(target_loc) if look_ahead <= dist_to_next_waypoint: # Vector scaled by look ahead distance vec_look_ahead = vec_waypoints / \ np.linalg.norm(vec_waypoints) * look_ahead look_ahead_location = carla.Location( proj_loc.x + vec_look_ahead[0], proj_loc.y + vec_look_ahead[1], proj_loc.z + vec_look_ahead[2]) else: # Loop over buffered waypoints to find the section where the look ahead point # lies in, then compute the location of the look ahead point idx = 0 while True: # If out of waypoints in buffer, use the last waypoint in buffer as look ahead point if idx + 2 > len(self._waypoint_buffer): look_ahead_location = self._waypoint_buffer[-1][ 0].transform.location break # Comput look ahead distacne in the current section look_ahead -= dist_to_next_waypoint # Vector formed by start and end waypoints of the current section vec = self._waypoint_buffer[idx+1][0].transform.location - \ self._waypoint_buffer[idx][0].transform.location vec = np.array([vec.x, vec.y, vec.z]) dist_to_next_waypoint = np.linalg.norm(vec) # If look ahead distance exceeds length of current section, go to next one if look_ahead > dist_to_next_waypoint: idx += 1 continue # Section found, compute look ahead location else: # Vector scaled by remaining look ahead distance vec_look_ahead = vec / dist_to_next_waypoint * look_ahead base_waypt_loc = self._waypoint_buffer[idx][ 0].transform.location look_ahead_location = carla.Location( base_waypt_loc.x + vec_look_ahead[0], base_waypt_loc.y + vec_look_ahead[1], base_waypt_loc.z + vec_look_ahead[2]) break return look_ahead_location
class LocalPlanner(object): """ LocalPlanner implements the basic behavior of following a trajectory of waypoints that is generated on-the-fly. The low-level motion of the vehicle is computed by using two PID controllers, one is used for the lateral control and the other for the longitudinal control (cruise speed). When multiple paths are available (intersections) this local planner makes a random choice. """ # minimum distance to target waypoint as a percentage (e.g. within 90% of # total distance) MIN_DISTANCE_PERCENTAGE = 0.9 def __init__(self, vehicle, opt_dict={}): """ :param vehicle: actor to apply to local planner logic onto :param opt_dict: dictionary of arguments with the following semantics: dt -- time difference between physics control in seconds. This is typically fixed from server side using the arguments -benchmark -fps=F . In this case dt = 1/F target_speed -- desired cruise speed in Km/h sampling_radius -- search radius for next waypoints in seconds: e.g. 0.5 seconds ahead lateral_control_dict -- dictionary of arguments to setup the lateral PID controller {'K_P':, 'K_D':, 'K_I':, 'dt'} longitudinal_control_dict -- dictionary of arguments to setup the longitudinal PID controller {'K_P':, 'K_D':, 'K_I':, 'dt'} """ self._vehicle = vehicle self._map = self._vehicle.get_world().get_map() self._dt = None self._target_speed = None self._sampling_radius = None self._min_distance = None self._current_waypoint = None self._target_road_option = None self._next_waypoints = None self._target_waypoint = None self._vehicle_controller = None self._global_plan = None # queue with tuples of (waypoint, RoadOption) self._waypoints_queue = deque(maxlen=600) self._buffer_size = 5 self.message_waypoints = 3 # 当前阶段完成的waypoints个数 self.finished_waypoints = 0 self._waypoint_buffer = deque(maxlen=self._buffer_size) # initializing controller self.init_controller(opt_dict) def __del__(self): if self._vehicle: self._vehicle.destroy() print("Destroying ego-vehicle!") def init_controller(self, opt_dict): """ Controller initialization. :param opt_dict: dictionary of arguments. :return: """ # default params self._dt = 1.0 / 20.0 self._target_speed = 20.0 # Km/h self._sampling_radius = self._target_speed * 0.5 / 3.6 # 0.5 seconds horizon self._min_distance = self._sampling_radius * self.MIN_DISTANCE_PERCENTAGE args_lateral_dict = { 'K_P': 1.95, 'K_D': 0.01, 'K_I': 1.4, 'dt': self._dt } args_longitudinal_dict = { 'K_P': 1.0, 'K_D': 0, 'K_I': 1, 'dt': self._dt } # parameters overload if 'dt' in opt_dict: self._dt = opt_dict['dt'] if 'target_speed' in opt_dict: self._target_speed = opt_dict['target_speed'] if 'sampling_radius' in opt_dict: self._sampling_radius = self._target_speed * \ opt_dict['sampling_radius'] / 3.6 if 'lateral_control_dict' in opt_dict: args_lateral_dict = opt_dict['lateral_control_dict'] if 'longitudinal_control_dict' in opt_dict: args_longitudinal_dict = opt_dict['longitudinal_control_dict'] self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) # print("current waypoint: ", self._current_waypoint) self._vehicle_controller = VehiclePIDController( self._vehicle, args_lateral=args_lateral_dict, args_longitudinal=args_longitudinal_dict) self._global_plan = False # compute initial waypoints # print("before append len: ", len(self._waypoints_queue)) # self._waypoints_queue.append() # self._waypoints_queue.append( (self._current_waypoint.next(self._sampling_radius)[0], RoadOption.LANEFOLLOW)) # self._target_road_option = RoadOption.LANEFOLLOW # fill waypoint trajectory queue # # 计算初始的waypoints # self._compute_next_waypoints(k=10) # print("after append len: ", len(self._waypoints_queue)) def set_speed(self, speed): """ Request new target speed. :param speed: new target speed in Km/h :return: """ self._target_speed = speed def _compute_next_waypoints(self, k=1): """ Add new waypoints to the trajectory queue. :param k: how many waypoints to compute :return: """ # check we do not overflow the queue available_entries = self._waypoints_queue.maxlen - len( self._waypoints_queue) k = min(available_entries, k) for _ in range(k): last_waypoint = self._waypoints_queue[-1][0] next_waypoints = list(last_waypoint.next(self._sampling_radius)) if len(next_waypoints) == 1: # only one option available ==> lanefollowing next_waypoint = next_waypoints[0] road_option = RoadOption.LANEFOLLOW else: # random choice between the possible options road_options_list = retrieve_options(next_waypoints, last_waypoint) road_option = random.choice(road_options_list) next_waypoint = next_waypoints[road_options_list.index( road_option)] self._waypoints_queue.append((next_waypoint, road_option)) def add_waypoint(self, waypoint): """ Add a new specific waypoint to the waypoints buffer from the SUMO simulation server. :param waypoint: the specific waypoint passed here """ # print("initial waypoint: ", waypoint) # new_point = self._map.get_waypoint(waypoint) new_point = self._map.get_waypoint(waypoint.location) # print("point lane type:", type(new_point.lane_type)) # new_point = new_point.get_left_lane().get_left_lane() # new_point. # print("new f*****g waypoint: ", new_point) # print("type of new point: ", type(new_point)) # new_point = carla.Waypoint() #print("self.current_waypoint: ", self._current_waypoint) # print("waypoint in add_waypoint: ", waypoint) # new_point.transform = waypoint # new_point.transform.location = waypoint.location new_point.transform.rotation = waypoint.rotation # # new_point.transform.location.y = waypoint.location.y # # new_point.transform.location.z = waypoint.location.z # new_point.transform.rotation.pitch = waypoint.rotation.pitch # new_point.transform.rotation.yaw = waypoint.rotation.yaw # new_point.transform.rotation.roll = waypoint.rotation.roll road_option = compute_connection(self._current_waypoint, new_point) # print("f*****g road option: ", road_option) # print("new point: ", new_point) self._waypoints_queue.append((new_point, road_option)) # wp, _ = self._waypoints_queue[-1] # wp2, _ = self._waypoints_queue[0] # print("element in queue[0]: ", wp2) # print("element in queue[-1]: ", wp) # print("queue length: ", len(self._waypoints_queue)) # print("waypoint queue add done in local_planner! current length: ", len(self._waypoints_queue)) def set_global_plan(self, current_plan): self._waypoints_queue.clear() for elem in current_plan: self._waypoints_queue.append(elem) self._target_road_option = RoadOption.LANEFOLLOW self._global_plan = True def get_finished_waypoints(self): ret = self.finished_waypoints if self.finished_waypoints >= self.message_waypoints: self.finished_waypoints = 0 return ret # return True if vehicle is close to the 10th waypoint def reached_final_waypoint(self): pass ''' 收到新的action package后丢弃现有的路点缓冲 ''' def drop_waypoint_buffer(self): self._waypoints_queue.clear() self._waypoint_buffer.clear() self.finished_waypoints = 0 def run_step(self, debug=True): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. :param debug: boolean flag to activate waypoints debugging :return: """ # print("current queue len: ", len(self._waypoints_queue)) # print("maxlen: ", int(self._waypoints_queue.maxlen * 0.5)) # not enough waypoints in the horizon? => add more! # if len(self._waypoints_queue) < int(self._waypoints_queue.maxlen * 0.5): # print("no points here!") # if not self._global_plan: # self._compute_next_waypoints(k=100) # # 队列为空时切换到手动操作模式,待修改 # if len(self._waypoints_queue) == 0: #control = carla.VehicleControl() #control.steer = 0.0 #control.throttle = 0.0 #control.brake = 0.0 #control.hand_brake = False #control.manual_gear_shift = False # return control # Buffering the waypoints # print("queue length inside run_step: ", len(self._waypoints_queue)) if not self._waypoint_buffer: for i in range(self._buffer_size): if self._waypoints_queue: # print("queue length: ", len(self._waypoints_queue)) left_point, left_road = self._waypoints_queue.popleft() self._waypoint_buffer.append((left_point, left_road)) # right_point, _ = self._waypoint_buffer.popleft() # print("right point is ", right_point) else: break # target_waypoint, _ = self._waypoint_buffer[0] # print("target_waypoint:", target_waypoint) # for i, (waypoint, _) in enumerate(self._waypoint_buffer): # print("waypoint is ", waypoint) # time.sleep(2) # current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) if not self._waypoint_buffer: # control = self._vehicle_controller.run_step(0, self._current_waypoint) # return control return None self._target_waypoint, self._target_road_option = self._waypoint_buffer[ 0] # for i in range(len(self._waypoint_buffer)): # wp, _ = self._waypoint_buffer[i] # print("waypoint in buffer is ", wp) # move using PID controllers # print("target_waypoint: ", self._target_waypoint) control = self._vehicle_controller.run_step(self._target_speed, self._target_waypoint) # purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 # print("len of buffer: ", len(self._waypoint_buffer)) # for i in range(len(self._waypoint_buffer)): # waypoint = self for i, (waypoint, _) in enumerate(self._waypoint_buffer): distance = distance_vehicle(waypoint, vehicle_transform) # print("min distance: ", self._min_distance, "distance: ", distance) # time.sleep(2) # 当前车辆和路点的距离小于最小距离,认为已经行驶完成 if distance < self._min_distance: # print("waypoint in enumerate is ", waypoint) max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() self.finished_waypoints += 1 print("current finished waypoints is ", self.finished_waypoints) if debug: draw_waypoints(self._vehicle.get_world(), [self._target_waypoint], self._vehicle.get_location().z + 1.0) # if len(self._waypoint_buffer) == 0: # self.finished_waypoints = self.message_waypoints return control
class Colliding_Agent: def __init__(self, world, main_agent): self.world = world self.map = self.world.get_map() self.vehicle = None self.main_agent = main_agent self.target_speed = 20 #Km/Hr # add gaussian dt = 0.05 self.args_lateral_dict = { 'K_P': 1.95, 'K_D': 0.01, 'K_I': 1.4, 'dt': dt } self.args_longitudinal_dict = { 'K_P': 1.0, 'K_D': 0, 'K_I': 1, 'dt': dt } self.vehicle_controller = None self.waypoint = None def create_agent(self, spawn_point): try: blueprints = self.world.get_blueprint_library().filter('vehicle.*') blueprints = [ x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4 ] blueprints = [x for x in blueprints if not x.id.endswith('isetta')] # spawn_points = list(self.world.get_map().get_spawn_points()) # print('found %d spawn points.' % len(spawn_points)) blueprint = random.choice(blueprints) if blueprint.has_attribute('color'): color = random.choice( blueprint.get_attribute('color').recommended_values) blueprint.set_attribute('color', color) blueprint.set_attribute('role_name', 'autopilot') self.vehicle = self.world.try_spawn_actor(blueprint, spawn_point) if self.vehicle is not None: self.vehicle.set_autopilot() print('*Collision* Spawned %r at %s' % (self.vehicle.type_id, spawn_point.location)) else: print("No vehicles to spawn") return False self.vehicle_controller = VehiclePIDController(self.vehicle, args_lateral=self.args_lateral_dict, \ args_longitudinal=self.args_longitudinal_dict) except: print("Unable to import vehicle") def destroy(self): if self.vehicle is not None: self.vehicle.destroy() def control_agent(self): # Control the oncoming vehicle to hit into ego vehicle using Carla's PID if self.find_distance(self.vehicle.get_transform(), self.main_agent.get_transform()) < 35: if self.waypoint is None: # Store the waypoint at the first timestep of lane change self.waypoint = self.map.get_waypoint( self.vehicle.get_transform().location, True) self.vehicle.set_autopilot(False) # print(self.main_agent.get_transform()) # print(waypoint.next(50)[0].get_left_lane()) # Use the waypoint 20 m ahead to come and hit the vehicle () target_waypoint = self.waypoint.next(20)[0].get_left_lane() # print(target_waypoint) control = self.vehicle_controller.run_step(self.target_speed, target_waypoint) self.vehicle.apply_control(control) def find_distance(self, main_agent_T, vehicle_T): distance = math.sqrt((main_agent_T.location.x - vehicle_T.location.x)**2 + (main_agent_T.location.y - vehicle_T.location.y)**2 \ + (main_agent_T.location.z - vehicle_T.location.z)**2) return distance
class PIDAgent(Agent): def __init__(self, vehicle, opt_dict=None): super(PIDAgent, self).__init__(vehicle) # Default agent params: self.target_speed = 20.0 # Default PID params: self.lateral_pid_dict = { 'K_P': 1.0, 'K_D': 0.01, 'K_I': 0.6, 'dt': 0.05 } self.longitudinal_pid_dict = { 'K_P': 1.0, 'K_D': 0, 'K_I': 1, 'dt': 0.05 } if opt_dict: if 'target_speed' in opt_dict: self.target_speed = opt_dict['target_speed'] self.lateral_pid_dict['dt'] = 1.0 / self.target_speed self.longitudinal_pid_dict['dt'] = 1.0 / self.target_speed if 'radius' in opt_dict: self.radius = opt_dict['radius'] if 'min_dist' in opt_dict: self.min_dist = opt_dict['min_dist'] if 'lateral_pid_dict' in opt_dict: self.lateral_pid_dict = opt_dict['lateral_pid_dict'] if 'longitudinal_pid_dict' in opt_dict: self.longitudinal_pid_dict = opt_dict['longitudinal_pid_dict'] self.controller = VehiclePIDController( vehicle, args_lateral=self.lateral_pid_dict, args_longitudinal=self.longitudinal_pid_dict) self.waypoints = [] self.max_waypoints = 200 self.radius = self.target_speed / 3.6 # Radius at which next waypoint is sampled self.min_dist = 0.9 * self.radius # If min_dist away from waypoint[0], move on to next one. def add_next_waypoints(self): def d_angle(a, b): return abs((a - b + 180) % 360 - 180) if not self.waypoints: current_w = self._map.get_waypoint(self._vehicle.get_location()) self.waypoints.append(current_w) while len(self.waypoints) < self.max_waypoints: last_w = self.waypoints[-1] last_heading = last_w.transform.rotation.yaw next_w_list = list(last_w.next(self.radius)) # Go straight if possible. if next_w_list: next_w = min(next_w_list, key=lambda w: d_angle(w.transform.rotation.yaw, last_heading)) else: print('No more waypoints.') return self.waypoints.append(next_w) def run_step(self): transform = self._vehicle.get_transform() if self.waypoints: # If too far off course, reset waypoint queue. if distance_vehicle(self.waypoints[0], transform) > 5.0 * self.radius: self.waypoints = [] # Get more waypoints. if len(self.waypoints) < self.max_waypoints // 2: self.add_next_waypoints() # If no more waypoints, stop. if not self.waypoints: print('Ran out of waypoints; stopping.') control = carla.VehicleControl() control.throttle = 0.0 return control # Remove waypoints we've reached. while distance_vehicle(self.waypoints[0], transform) < self.min_dist: self.waypoints = self.waypoints[1:] # Draw next waypoint draw_waypoints(self._vehicle.get_world(), self.waypoints[:1], self._vehicle.get_location().z + 1.0) return self.controller.run_step(self.target_speed, self.waypoints[0])
class LocalPlanner(object): """ LocalPlanner implements the basic behavior of following a trajectory of waypoints that is generated on-the-fly. The low-level motion of the vehicle is computed by using two PID controllers, one is used for the lateral control and the other for the longitudinal control (cruise speed). When multiple paths are available (intersections) this local planner makes a random choice. """ # Minimum distance to target waypoint as a percentage # (e.g. within 80% of total distance) # FPS used for dt FPS = 20 def __init__(self, agent): """ :param agent: agent that regulates the vehicle :param vehicle: actor to apply to local planner logic onto """ self._vehicle = agent.vehicle self._map = agent.vehicle.get_world().get_map() self._target_speed = None self.sampling_radius = None self._min_distance = None self._current_waypoint = None self.target_road_option = None self._next_waypoints = None self.target_waypoint = None self._vehicle_controller = None self._global_plan = None self._pid_controller = None self.waypoints_queue = deque( maxlen=20000) # queue with tuples of (waypoint, RoadOption) self._buffer_size = 5 self._waypoint_buffer = deque(maxlen=self._buffer_size) self._init_controller() # initializing controller def reset_vehicle(self): """Reset the ego-vehicle""" self._vehicle = None print("Resetting ego-vehicle!") def _init_controller(self): """ Controller initialization. dt -- time difference between physics control in seconds. This is can be fixed from server side using the arguments -benchmark -fps=F, since dt = 1/F target_speed -- desired cruise speed in km/h min_distance -- minimum distance to remove waypoint from queue lateral_dict -- dictionary of arguments to setup the lateral PID controller {'K_P':, 'K_D':, 'K_I':, 'dt'} longitudinal_dict -- dictionary of arguments to setup the longitudinal PID controller {'K_P':, 'K_D':, 'K_I':, 'dt'} """ # Default parameters self.args_lat_hw_dict = { 'K_P': 0.75, 'K_D': 0.02, 'K_I': 0.4, 'dt': 1.0 / self.FPS } self.args_lat_city_dict = { 'K_P': 0.58, 'K_D': 0.02, 'K_I': 0.5, 'dt': 1.0 / self.FPS } self.args_long_hw_dict = { 'K_P': 0.37, 'K_D': 0.024, 'K_I': 0.032, 'dt': 1.0 / self.FPS } self.args_long_city_dict = { 'K_P': 0.15, 'K_D': 0.05, 'K_I': 0.07, 'dt': 1.0 / self.FPS } self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) self._global_plan = False self._target_speed = self._vehicle.get_speed_limit() self._min_distance = 3 def set_speed(self, speed): """ Request new target speed. :param speed: new target speed in km/h """ self._target_speed = speed def set_global_plan(self, current_plan): """ Sets new global plan. :param current_plan: list of waypoints in the actual plan """ for elem in current_plan: self.waypoints_queue.append(elem) self._global_plan = True def get_incoming_waypoint_and_direction(self, steps=3): """ Returns direction and waypoint at a distance ahead defined by the user. :param steps: number of steps to get the incoming waypoint. """ if len(self.waypoints_queue) > steps: return self.waypoints_queue[steps] else: try: wpt, direction = self.waypoints_queue[-1] return wpt, direction except IndexError as i: print(i) return None, RoadOption.VOID return None, RoadOption.VOID def run_step(self, target_speed=None, debug=False): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. :param target_speed: desired speed :param debug: boolean flag to activate waypoints debugging :return: control """ if target_speed is not None: self._target_speed = target_speed else: self._target_speed = self._vehicle.get_speed_limit() if len(self.waypoints_queue) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False return control # Buffering the waypoints if not self._waypoint_buffer: for i in range(self._buffer_size): if self.waypoints_queue: self._waypoint_buffer.append( self.waypoints_queue.popleft()) else: break # Current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) # Target waypoint self.target_waypoint, self.target_road_option = self._waypoint_buffer[ 0] if target_speed > 50: args_lat = self.args_lat_hw_dict args_long = self.args_long_hw_dict else: args_lat = self.args_lat_city_dict args_long = self.args_long_city_dict self._pid_controller = VehiclePIDController( self._vehicle, args_lateral=args_lat, args_longitudinal=args_long) control = self._pid_controller.run_step(self._target_speed, self.target_waypoint) # Purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 for i, (waypoint, _) in enumerate(self._waypoint_buffer): if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() if debug: draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], 1.0) return control
class NaiveAgent: def __init__(self, vehicle, route, target_speed=20): self._vehicle = vehicle self._route = route self._target_speed = target_speed self._driver = IDM(self._vehicle, self._target_speed, TIME_INTERVAL) self._world = self._vehicle.get_world() self._map = self._world.get_map() self._dt = TIME_INTERVAL # it works, but still has a little ossilation args_lateral_dict = { 'K_P': 0.08, 'K_D': 0.008, 'K_I': 0.004, 'dt': self._dt } args_longitudinal_dict = { 'K_P': 1.0, 'K_D': 0, 'K_I': 0, 'dt': self._dt } self._vehicle_controller = VehiclePIDController( self._vehicle, args_lateral=args_lateral_dict, args_longitudinal=args_longitudinal_dict) self._history = pd.DataFrame(columns=[ 'timestamp', 'ego_vehicle_x', 'ego_vehicle_y', 'ego_vehicle_z', 'ego_vehicle_v', 'leader_vehicle_x', 'leader_vehicle_y', 'leader_vehicle_z', 'leader_vehicle_v', ]) def update_history(self, info_dict): self._history = self._history.append(info_dict, ignore_index=True) @staticmethod def get_info_dict(timestamp, ego_vehicle_snapshot, leader_vehicle_snapshot): info_dict = {} info_dict['timestamp'] = timestamp.elapsed_seconds ego_vehicle_location = ego_vehicle_snapshot.get_transform().location info_dict['ego_vehicle_x'] = ego_vehicle_location.x info_dict['ego_vehicle_y'] = ego_vehicle_location.y info_dict['ego_vehicle_z'] = ego_vehicle_location.z info_dict['ego_vehicle_v'] = get_speed(ego_vehicle_snapshot) if leader_vehicle_snapshot: leader_vehicle_location = leader_vehicle_snapshot.get_transform( ).location info_dict['leader_vehicle_x'] = leader_vehicle_location.x info_dict['leader_vehicle_y'] = leader_vehicle_location.y info_dict['leader_vehicle_z'] = leader_vehicle_location.z info_dict['leader_vehicle_v'] = get_speed(leader_vehicle_snapshot) return info_dict def store_history(self): self._history.to_csv('./out/acc{}.csv'.format( time.strftime("%Y%m%d-%H%M%S"))) def set_target_speed(self, target_speed): self._target_speed = target_speed def is_close(self, other_transform, epsilon=0.1): vehicle_transform = self._vehicle.get_transform() loc = vehicle_transform.location dx = other_transform.location.x - loc.x dy = other_transform.location.y - loc.y return np.linalg.norm(np.array([dx, dy])) < epsilon def is_ahead(self, other_transform): """Check if ego vehicle is ahead of other""" vehicle_transform = self._vehicle.get_transform() loc = vehicle_transform.location orientation = vehicle_transform.rotation.yaw dx = other_transform.location.x - loc.x dy = other_transform.location.y - loc.y target_vector = np.array([dx, dy]) norm_target = np.linalg.norm(target_vector) forward_vector = np.array([ math.cos(math.radians(orientation)), math.sin(math.radians(orientation)) ]) cos_vector = np.dot(forward_vector, target_vector) / norm_target if cos_vector > 1: cos_vector = 1 elif cos_vector < -1: cos_vector = -1 d_angle = math.degrees(math.acos(cos_vector)) return d_angle > 90 def get_distance(self, other): ego_vehicle_transform = self._vehicle.get_transform() other_transform = other.get_transform() sign = -1 if self.is_ahead(other_transform) else 1 ego_location = ego_vehicle_transform.location other_location = other_transform.location dx = ego_location.x - other_location.x dy = ego_location.y - other_location.y dz = ego_location.z - other_location.z target_vector = np.array([dx, dy, dz]) norm_target = np.linalg.norm(target_vector) return sign * norm_target def get_front_vehicle_state(self): actor_list = self._world.get_actors() vehicle_list = actor_list.filter("*vehicle*") ego_vehicle_location = self._vehicle.get_location() ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) front_vehicles_info = [] for target_vehicle in vehicle_list: # do not account for the ego vehicle if target_vehicle.id == self._vehicle.id: continue # if the object is not in our lane it's not an obstacle target_vehicle_waypoint = self._map.get_waypoint( target_vehicle.get_location()) # consider absolute value of lane_id is enough for the experiment if abs(target_vehicle_waypoint.lane_id) != abs( ego_vehicle_waypoint.lane_id): continue # get the state of front vehiles target_distance = self.get_distance(target_vehicle) target_speed = get_speed(target_vehicle) target_id = target_vehicle.id if target_distance > 0: front_vehicles_info.append( (target_distance, target_speed, target_id)) if front_vehicles_info: front_vehicles_info = sorted(front_vehicles_info, key=lambda x: x[0]) return True, front_vehicles_info[0] else: return False, None def run_step(self, debug=False): # stop if no route to follow if not self._route: return emergency_stop() # purge obsolete waypoints in the route index = 0 for i, waypoint in enumerate(self._route): if self.is_close(waypoint.transform): if debug: print("Vehicle: {}".format(self._vehicle.get_transform())) print("vehicle is close to obsolete waypoint: {}".format( waypoint)) index += 1 elif self.is_ahead(waypoint.transform): if debug: print("Vehicle: {}".format(self._vehicle.get_transform())) print("vehicle is ahead of obsolete waypoint: {}".format( waypoint)) index += 1 else: break if index != 0: self._route = self._route[index:] if debug: print("waypoint remain: {}".format(len(self._route))) if not self._route: return emergency_stop() # follow next waypoint target_waypoint = self._route[0] if debug: print("target waypoint: {}".format(target_waypoint)) print("waypoint lane id: {}".format(target_waypoint.lane_id)) # find leader vehicle has_front_vehicle, front_vehicle_state = self.get_front_vehicle_state() # prepare to add info to history world_snapshot = self._world.get_snapshot() timestamp = world_snapshot.timestamp ego_vehicle_snapshot = world_snapshot.find(self._vehicle.id) front_vehicle_snapshot = None # get control based on front vehicle state if has_front_vehicle: front_vehicle_distance, front_vehicle_speed, front_vehicle_id = front_vehicle_state front_vehicle_snapshot = world_snapshot.find(front_vehicle_id) print("obstacle distance: {:3f}m, speed: {:1f}km/h".format( front_vehicle_distance, front_vehicle_speed)) # stop directly if too close if front_vehicle_distance < self._driver.minimum_distance: control = emergency_stop() else: self._driver.set_leader_info(front_vehicle_distance, front_vehicle_speed) desired_speed = self._driver.calc_desired_speed() print("target speed: {:1f}".format(desired_speed)) control = self._vehicle_controller.run_step( desired_speed, target_waypoint) else: print("no obstacle, default speed: {}".format(self._target_speed)) control = self._vehicle_controller.run_step( self._target_speed, target_waypoint) self.update_history( self.get_info_dict(timestamp, ego_vehicle_snapshot, front_vehicle_snapshot)) return control
class LocalPlanner(object): """ LocalPlanner implements the basic behavior of following a trajectory of waypoints that is generated on-the-fly. The low-level motion of the vehicle is computed by using two PID controllers, one is used for the lateral control and the other for the longitudinal control (cruise speed). When multiple paths are available (intersections) this local planner makes a random choice. """ # minimum distance to target waypoint as a percentage (e.g. within 90% of # total distance) MIN_DISTANCE_PERCENTAGE = 0.9 def __init__(self, vehicle, dest, opt_dict=None): """ :param vehicle: actor to apply to local planner logic onto :param dest: destination of the route (carla.Location Object) :param opt_dict: dictionary of arguments with the following semantics: dt -- time difference between physics control in seconds. This is typically fixed from server side using the arguments -benchmark -fps=F . In this case dt = 1/F target_speed -- desired cruise speed in Km/h sampling_radius -- search radius for next waypoints in seconds: e.g. 0.5 seconds ahead lateral_control_dict -- dictionary of arguments to setup the lateral PID controller {'K_P':, 'K_D':, 'K_I':, 'dt'} longitudinal_control_dict -- dictionary of arguments to setup the longitudinal PID controller {'K_P':, 'K_D':, 'K_I':, 'dt'} """ self._vehicle = vehicle self._map = self._vehicle.get_world().get_map() # self._world = world self._dt = None self._target_speed = None self._sampling_radius = None self._min_distance = None self._current_waypoint = None self._target_road_option = None self._next_waypoints = None self.target_waypoint = None self._vehicle_controller = None self._global_plan = None # queue with tuples of (waypoint, RoadOption) self._waypoints_queue = deque(maxlen=20000) self._buffer_size = 5 self._waypoint_buffer = deque(maxlen=self._buffer_size) # initializing controller self._init_controller(dest, opt_dict) def _init_controller(self, dest, opt_dict): """ Controller initialization. :param opt_dict: dictionary of arguments. :return: """ # default params self._dt = 1.0 / 10.0 self._target_speed = 20.0 # Km/h self._sampling_radius = self._target_speed * 1 / 3.6 # 1 seconds horizon self._min_distance = self._sampling_radius * self.MIN_DISTANCE_PERCENTAGE args_lateral_dict = { 'K_P': 1.0, 'K_D': 0.0, 'K_I': 0.5, 'dt': self._dt } args_longitudinal_dict = { 'K_P': 1.0, 'K_D': 0, 'K_I': 1, 'dt': self._dt } # parameters overload if opt_dict: print("opt_dict is passed into route_planner") if 'dt' in opt_dict: self._dt = opt_dict['dt'] if 'target_speed' in opt_dict: self._target_speed = opt_dict['target_speed'] print("target speed modified to " + str(self._target_speed)) if 'sampling_radius' in opt_dict: self._sampling_radius = self._target_speed * \ opt_dict['sampling_radius'] / 3.6 if 'lateral_control_dict' in opt_dict: args_lateral_dict = opt_dict['lateral_control_dict'] if 'longitudinal_control_dict' in opt_dict: args_longitudinal_dict = opt_dict['longitudinal_control_dict'] self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) self._vehicle_controller = VehiclePIDController( self._vehicle, args_lateral=args_lateral_dict, args_longitudinal=args_longitudinal_dict) self._global_plan = False # compute initial waypoints self._waypoints_queue.append( (self._current_waypoint.next(self._sampling_radius)[0], RoadOption.LANEFOLLOW)) self._target_road_option = RoadOption.LANEFOLLOW # fill waypoint trajectory queue self._compute_next_waypoints(dest, k=200) def set_speed(self, speed): """ Request new target speed. :param speed: new target speed in Km/h :return: """ self._target_speed = speed def _compute_next_waypoints(self, dest, k=1): """ Add new waypoints to the trajectory queue. :param k: how many waypoints to compute :return: """ # check we do not overflow the queue available_entries = self._waypoints_queue.maxlen - len( self._waypoints_queue) k = min(available_entries, k) for _ in range(k): last_waypoint = self._waypoints_queue[-1][0] # print("the last waypoint is: " + str(last_waypoint) + "\n") next_waypoints = list(last_waypoint.next(self._sampling_radius)) if len(next_waypoints) == 1: #print("only one waypoint available") # only one option available ==> lanefollowing next_waypoint = next_waypoints[0] road_option = RoadOption.LANEFOLLOW else: # choose a waypoint towards the destination road_options_list = _retrieve_options(next_waypoints, last_waypoint) #location = self._current_waypoint.transform.location #print("choosing waypoints at x={}, y={}".format(location.x, location.y)) min_dist = -1 road_option = RoadOption.LANEFOLLOW next_waypoint = next_waypoints[0] if (self._current_waypoint.is_junction): #print("vehicle is at a junction") for option in road_options_list: # if option == RoadOption.LEFT: # print("left") # elif option == RoadOption.RIGHT: # print("right") # elif option == RoadOption.LANEFOLLOW: # print("follow") next_waypoint = next_waypoints[road_options_list.index( option)] dist = dest.distance(next_waypoint.transform.location) if min_dist < 0 or dist < min_dist: road_option = option min_dist = dist #road_option = random.choice(road_options_list) #else: #next_waypoint = next_waypoints[0] # default to lanefollow #print("final road option: " + str(road_option)) next_waypoint = next_waypoints[road_options_list.index( road_option)] self._waypoints_queue.append((next_waypoint, road_option)) def set_global_plan(self, current_plan): self._waypoints_queue.clear() for elem in current_plan: self._waypoints_queue.append(elem) self._target_road_option = RoadOption.LANEFOLLOW self._global_plan = True def run_step(self, dest, lane_change, world, debug=False): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. :param debug: boolean flag to activate waypoints debugging :return: """ # if(lane_change == "right"): # print('switch into the right lane') # # change target waypoint to a point on the right lane # right_lane = self._current_waypoint.get_right_lane() # new_waypoint = right_lane.next(5)[0] # # self.target_waypoint = new_waypoint # self._waypoints_queue.clear() # self._waypoint_buffer.clear() # self._waypoints_queue.append((new_waypoint, RoadOption.LANEFOLLOW)) # elif(lane_change == "left"): # print('switch into the left lane') # # change target waypoint to a point on the right lane # left_lane = self._current_waypoint.get_left_lane() # new_waypoint = left_lane.next(5)[0] # # self.target_waypoint = new_waypoint # self._waypoints_queue.clear() # self._waypoint_buffer.clear() # print(len(self._waypoints_queue)) # # print("new waypoint at " + str(new_waypoint)) # self._waypoints_queue.append((new_waypoint, RoadOption.LANEFOLLOW)) # not enough waypoints in the horizon? => add more! if not self._global_plan and len(self._waypoints_queue) < int( self._waypoints_queue.maxlen * 0.5): # print("current waypoint: " + str(self._current_waypoint)) # print("add 100 additional waypoints") self._compute_next_waypoints(dest, k=100) current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) if len(self._waypoints_queue) == 0: if not current_waypoint.is_intersection: self._target_road_option = RoadOption.LANEFOLLOW print("way points queue is empty") control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False return control # Buffering the waypoints if not self._waypoint_buffer: for i in range(self._buffer_size): if self._waypoints_queue: self._waypoint_buffer.append( self._waypoints_queue.popleft()) else: break # current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) # target waypoint self.target_waypoint, self._target_road_option = self._waypoint_buffer[ 0] # print("target waypoint at " + str(self.target_waypoint)) if (lane_change == "right"): # print('switch into the right lane') # change target waypoint to a point on the right lane right_lane = self._current_waypoint.get_right_lane() if (not right_lane): print("cannot switch into the right lane") else: self.target_waypoint = right_lane.next(5)[-1] # self.target_waypoint = new_waypoint elif (lane_change == "left"): # print('switch into the left lane') # change target waypoint to a point on the right lane left_lane = self._current_waypoint.get_left_lane() if (not left_lane): print("cannot switch into the right lane") else: self.target_waypoint = left_lane.next(5)[-1] # # set the target speed # speed_limit = self._vehicle.get_speed_limit() # #set highway driving speed to 40 kmh # if(speed_limit > 30): # self.set_speed(30) # # otherwise, set driving speed to 20 kmh # else: # self.set_speed(20) # move using PID controllers #print("target_speed: " + str(self._target_speed)) control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint) # purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 for i, (waypoint, _) in enumerate(self._waypoint_buffer): if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() # if debug: draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], self._vehicle.get_location().z + 1.0) # if self._target_road_option != RoadOption.LANEFOLLOW not current_waypoint.is_intersection: # self._target_road_option = RoadOption.LANEFOLLOW return control
class LocalPlanner(object): """ LocalPlanner implements the basic behavior of following a trajectory of waypoints that is generated on-the-fly. The low-level motion of the vehicle is computed by using two PID controllers, one is used for the lateral control and the other for the longitudinal control (cruise speed). When multiple paths are available (intersections) this local planner makes a random choice, unless a given global plan has already been specified. """ def __init__(self, vehicle, opt_dict={}): """ :param vehicle: actor to apply to local planner logic onto :param opt_dict: dictionary of arguments with different parameters: dt: time between simulation steps target_speed: desired cruise speed in Km/h sampling_radius: distance between the waypoints part of the plan lateral_control_dict: values of the lateral PID controller longitudinal_control_dict: values of the longitudinal PID controller max_throttle: maximum throttle applied to the vehicle max_brake: maximum brake applied to the vehicle max_steering: maximum steering applied to the vehicle offset: distance between the route waypoints and the center of the lane """ self._vehicle = vehicle self._world = self._vehicle.get_world() self._map = self._world.get_map() self._vehicle_controller = None self.target_waypoint = None self.target_road_option = None self._waypoints_queue = deque(maxlen=10000) self._min_waypoint_queue_length = 100 self._stop_waypoint_creation = False # Base parameters self._dt = 1.0 / 20.0 self._target_speed = 20.0 # Km/h self._sampling_radius = 2.0 self._args_lateral_dict = { 'K_P': 1.95, 'K_I': 0.05, 'K_D': 0.2, 'dt': self._dt } self._args_longitudinal_dict = { 'K_P': 1.0, 'K_I': 0.05, 'K_D': 0, 'dt': self._dt } self._max_throt = 0.75 self._max_brake = 0.3 self._max_steer = 0.8 self._offset = 0 self._base_min_distance = 3.0 self._follow_speed_limits = False # Overload parameters if opt_dict: if 'dt' in opt_dict: self._dt = opt_dict['dt'] if 'target_speed' in opt_dict: self._target_speed = opt_dict['target_speed'] if 'sampling_radius' in opt_dict: self._sampling_radius = opt_dict['sampling_radius'] if 'lateral_control_dict' in opt_dict: self._args_lateral_dict = opt_dict['lateral_control_dict'] if 'longitudinal_control_dict' in opt_dict: self._args_longitudinal_dict = opt_dict[ 'longitudinal_control_dict'] if 'max_throttle' in opt_dict: self._max_throt = opt_dict['max_throttle'] if 'max_brake' in opt_dict: self._max_brake = opt_dict['max_brake'] if 'max_steering' in opt_dict: self._max_steer = opt_dict['max_steering'] if 'offset' in opt_dict: self._offset = opt_dict['offset'] if 'base_min_distance' in opt_dict: self._base_min_distance = opt_dict['base_min_distance'] if 'follow_speed_limits' in opt_dict: self._follow_speed_limits = opt_dict['follow_speed_limits'] # initializing controller self._init_controller() def reset_vehicle(self): """Reset the ego-vehicle""" self._vehicle = None def _init_controller(self): """Controller initialization""" self._vehicle_controller = VehiclePIDController( self._vehicle, args_lateral=self._args_lateral_dict, args_longitudinal=self._args_longitudinal_dict, offset=self._offset, max_throttle=self._max_throt, max_brake=self._max_brake, max_steering=self._max_steer) # Compute the current vehicle waypoint current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) self.target_waypoint, self.target_road_option = (current_waypoint, RoadOption.LANEFOLLOW) self._waypoints_queue.append( (self.target_waypoint, self.target_road_option)) def set_speed(self, speed): """ Changes the target speed :param speed: new target speed in Km/h :return: """ if self._follow_speed_limits: print( "WARNING: The max speed is currently set to follow the speed limits. " "Use 'follow_speed_limits' to deactivate this") self._target_speed = speed def follow_speed_limits(self, value=True): """ Activates a flag that makes the max speed dynamically vary according to the spped limits :param value: bool :return: """ self._follow_speed_limits = value def _compute_next_waypoints(self, k=1): """ Add new waypoints to the trajectory queue. :param k: how many waypoints to compute :return: """ # check we do not overflow the queue available_entries = self._waypoints_queue.maxlen - len( self._waypoints_queue) k = min(available_entries, k) for _ in range(k): last_waypoint = self._waypoints_queue[-1][0] next_waypoints = list(last_waypoint.next(self._sampling_radius)) if len(next_waypoints) == 0: break elif len(next_waypoints) == 1: # only one option available ==> lanefollowing next_waypoint = next_waypoints[0] road_option = RoadOption.LANEFOLLOW else: # random choice between the possible options road_options_list = _retrieve_options(next_waypoints, last_waypoint) road_option = random.choice(road_options_list) next_waypoint = next_waypoints[road_options_list.index( road_option)] self._waypoints_queue.append((next_waypoint, road_option)) def set_global_plan(self, current_plan, stop_waypoint_creation=True, clean_queue=True): """ Adds a new plan to the local planner. A plan must be a list of [carla.Waypoint, RoadOption] pairs The 'clean_queue` parameter erases the previous plan if True, otherwise, it adds it to the old one The 'stop_waypoint_creation' flag stops the automatic creation of random waypoints :param current_plan: list of (carla.Waypoint, RoadOption) :param stop_waypoint_creation: bool :param clean_queue: bool :return: """ if clean_queue: self._waypoints_queue.clear() # Remake the waypoints queue if the new plan has a higher length than the queue new_plan_length = len(current_plan) + len(self._waypoints_queue) if new_plan_length > self._waypoints_queue.maxlen: new_waypoint_queue = deque(maxlen=new_plan_length) for wp in self._waypoints_queue: new_waypoint_queue.append(wp) self._waypoints_queue = new_waypoint_queue for elem in current_plan: self._waypoints_queue.append(elem) self._stop_waypoint_creation = stop_waypoint_creation def run_step(self, debug=False): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. :param debug: boolean flag to activate waypoints debugging :return: control to be applied """ if self._follow_speed_limits: self._target_speed = self._vehicle.get_speed_limit() # Add more waypoints too few in the horizon if not self._stop_waypoint_creation and len( self._waypoints_queue) < self._min_waypoint_queue_length: self._compute_next_waypoints(k=self._min_waypoint_queue_length) # Purge the queue of obsolete waypoints veh_location = self._vehicle.get_location() vehicle_speed = get_speed(self._vehicle) / 3.6 self._min_distance = self._base_min_distance + 0.5 * vehicle_speed num_waypoint_removed = 0 for waypoint, _ in self._waypoints_queue: if len(self._waypoints_queue) - num_waypoint_removed == 1: min_distance = 1 # Don't remove the last waypoint until very close by else: min_distance = self._min_distance if veh_location.distance( waypoint.transform.location) < min_distance: num_waypoint_removed += 1 else: break if num_waypoint_removed > 0: for _ in range(num_waypoint_removed): self._waypoints_queue.popleft() # Get the target waypoint and move using the PID controllers. Stop if no target waypoint if len(self._waypoints_queue) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False else: self.target_waypoint, self.target_road_option = self._waypoints_queue[ 0] control = self._vehicle_controller.run_step( self._target_speed, self.target_waypoint) if debug: draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], 1.0) return control def get_incoming_waypoint_and_direction(self, steps=3): """ Returns direction and waypoint at a distance ahead defined by the user. :param steps: number of steps to get the incoming waypoint. """ if len(self._waypoints_queue) > steps: return self._waypoints_queue[steps] else: try: wpt, direction = self._waypoints_queue[-1] return wpt, direction except IndexError as i: return None, RoadOption.VOID def get_plan(self): """Returns the current plan of the local planner""" return self._waypoints_queue def done(self): """ Returns whether or not the planner has finished :return: boolean """ return len(self._waypoints_queue) == 0
class LocalPlanner(object): """ LocalPlanner implements the basic behavior of following a trajectory of waypoints that is generated on-the-fly. The low-level motion of the vehicle is computed by using two PID controllers, one is used for the lateral control and the other for the longitudinal control (cruise speed). When multiple paths are available (intersections) this local planner makes a random choice. """ # minimum distance to target waypoint as a percentage (e.g. within 90% of # total distance) MIN_DISTANCE_PERCENTAGE = 0.9 def __init__(self, vehicle, opt_dict={}): """ :param vehicle: actor to apply to local planner logic onto :param opt_dict: dictionary of arguments with the following semantics: dt -- time difference between physics control in seconds. This is typically fixed from server side using the arguments -benchmark -fps=F . In this case dt = 1/F target_speed -- desired cruise speed in Km/h sampling_radius -- search radius for next waypoints in seconds: e.g. 0.5 seconds ahead lateral_control_dict -- dictionary of arguments to setup the lateral PID controller {'K_P':, 'K_D':, 'K_I':, 'dt'} longitudinal_control_dict -- dictionary of arguments to setup the longitudinal PID controller {'K_P':, 'K_D':, 'K_I':, 'dt'} """ self._vehicle = vehicle self._map = self._vehicle.get_world().get_map() self._dt = None self._target_speed = None self._sampling_radius = None self._min_distance = None self._current_waypoint = None self._target_road_option = None self._next_waypoints = None self._target_waypoint = None self._vehicle_controller = None self._global_plan = None # queue with tuples of (waypoint, RoadOption) self._waypoints_queue = deque(maxlen=600) self._buffer_size = 10 self._waypoint_buffer = deque(maxlen=self._buffer_size) # initializing controller self.init_controller(opt_dict) def __del__(self): self._vehicle.destroy() print("Destroying ego-vehicle!") def init_controller(self, opt_dict): """ Controller initialization. :param opt_dict: dictionary of arguments. :return: """ # default params self._dt = 1.0 / 5.0 self._target_speed = 35.0 # Km/h self._sampling_radius = 30 * 0.5 / 3.6 #self._target_speed * 0.5 / 3.6 # 0.5 seconds horizon self._min_distance = self._sampling_radius * self.MIN_DISTANCE_PERCENTAGE args_lateral_dict = { 'K_P': .5, 'K_D': 0.01, 'K_I': 0.4, 'dt': self._dt } args_longitudinal_dict = { 'K_P': 0.2, 'K_D': 0.014, 'K_I': 0.0258, 'dt': self._dt } # parameters overload if 'dt' in opt_dict: self._dt = opt_dict['dt'] if 'target_speed' in opt_dict: self._target_speed = opt_dict['target_speed'] if 'sampling_radius' in opt_dict: self._sampling_radius = self._target_speed * \ opt_dict['sampling_radius'] / 3.6 if 'lateral_control_dict' in opt_dict: args_lateral_dict = opt_dict['lateral_control_dict'] if 'longitudinal_control_dict' in opt_dict: args_longitudinal_dict = opt_dict['longitudinal_control_dict'] self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) self._vehicle_controller = VehiclePIDController( self._vehicle, args_lateral=args_lateral_dict, args_longitudinal=args_longitudinal_dict) self._global_plan = False # compute initial waypoints self._waypoints_queue.append( (self._current_waypoint.next(self._sampling_radius)[0], RoadOption.LANEFOLLOW)) self._target_road_option = RoadOption.LANEFOLLOW # fill waypoint trajectory queue self._compute_next_waypoints(k=200) def set_speed(self, speed): """ Request new target speed. :param speed: new target speed in Km/h :return: """ self._target_speed = speed def _compute_next_waypoints(self, k=1): """ Add new waypoints to the trajectory queue. :param k: how many waypoints to compute :return: """ # check we do not overflow the queue available_entries = self._waypoints_queue.maxlen - len( self._waypoints_queue) k = min(available_entries, k) for _ in range(k): last_waypoint = self._waypoints_queue[-1][0] next_waypoints = list(last_waypoint.next(self._sampling_radius)) if len(next_waypoints) == 1: # only one option available ==> lanefollowing next_waypoint = next_waypoints[0] road_option = RoadOption.LANEFOLLOW else: # random choice between the possible options road_options_list = retrieve_options(next_waypoints, last_waypoint) road_option = random.choice(road_options_list) next_waypoint = next_waypoints[road_options_list.index( road_option)] self._waypoints_queue.append((next_waypoint, road_option)) def set_global_plan(self, current_plan): self._waypoints_queue.clear() for elem in current_plan: self._waypoints_queue.append(elem) self._target_road_option = RoadOption.LANEFOLLOW self._global_plan = True def get_filled_waypoint_buffer(self) -> List[carla.Waypoint]: while len(self._waypoint_buffer) < self._buffer_size: if self._waypoints_queue: # [0] -> get only carla.Waypoint, ignore RoadOption next_waypoint = self._waypoints_queue.popleft()[0] self._waypoint_buffer.append(next_waypoint) else: break return self._waypoint_buffer def run_step(self, debug=True): # not enough waypoints in the horizon? => add more! if len(self._waypoints_queue) < int( self._waypoints_queue.maxlen * 0.5): if not self._global_plan: self._compute_next_waypoints(k=100) if len(self._waypoints_queue) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 0.0 control.hand_brake = False control.manual_gear_shift = False return control veh_location = self._vehicle.get_location() # type: carla.Location veh_waypoint = self._map.get_waypoint( veh_location) # type: carla.Waypoint veh_yaw = self._vehicle.get_transform( ).rotation.yaw # TODO type: float, range TODO local_plan = self.get_filled_waypoint_buffer( ) # type: List[carla.Waypoint] # Calculate best waypoint to follow considering current yaw L = 2.9 fx = veh_location.x + L * np.cos(veh_yaw) fy = veh_location.y + L * np.sin(veh_yaw) distances = [] for waypoint in local_plan: wp = waypoint.transform.location dx = fx - wp.x dy = fy - wp.y distance = np.sqrt(dx**2 + dy**2) distances.append(distance) target_idx = np.argmin(distances) closest_error = distances[target_idx] self._target_waypoint = local_plan[target_idx] # Calculate path curvature waypoints_to_look_ahead = 6 reference_waypoint = local_plan[target_idx + waypoints_to_look_ahead] ref_location = reference_waypoint.transform.location # delta_x = ref_location.x - veh_location.x # delta_y = ref_location.y - veh_location.y # theta_radians = math.atan2(delta_y, delta_x) # FIXME Sometimes yaw oscilates from 179 to -179 which leads to temporarily bad calculations distance, relative_angle = compute_magnitude_angle( ref_location, veh_location, veh_yaw) #np.rad2deg(theta_radians) - veh_yaw # plt.cla() # plt.plot([self._vehicle.get_transform().location.x, lookahead_waypoint.transform.location.x], [self._vehicle.get_transform().location.y, lookahead_waypoint.transform.location.y], "-r", label="debug") # # plt.plot(, , ".b", label="lookahead") # plt.axis("equal") # plt.legend() # plt.grid(True) # plt.title("Rel angle: {}, yaw {}".format(str(angle), yaw)) # plt.pause(0.0001) if abs(relative_angle) < 15: target_speed = 50 elif abs(relative_angle) < 20: target_speed = 40 elif abs(relative_angle) < 30: target_speed = 30 else: target_speed = 20 print( 'Relative angle to reference waypoint: {:3d} | Vehicle yaw angle: {:3d} | Target speed {} km/h' .format(int(relative_angle), int(veh_yaw), target_speed)) control = self._vehicle_controller.run_step(target_speed, self._target_waypoint) # purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 # i, (waypoint, _) for i, waypoint in enumerate(self._waypoint_buffer): if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() if debug: #draw_waypoints(self._vehicle.get_world(), [self._target_waypoint], self._vehicle.get_location().z + 1.0, color=carla.Color(0, 255, 0)) draw_waypoints(self._vehicle.get_world(), [reference_waypoint], veh_location.z + 1.0) return control