From 7ac96373532ea57ffb81795b527bb2c8c10c3a9c Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 27 Feb 2024 16:00:54 +0100 Subject: [PATCH 01/32] np.nextafter as constant --- PythonAPI/carla/agents/navigation/behavior_agent.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py index 3e52fafd39..45b55622e2 100644 --- a/PythonAPI/carla/agents/navigation/behavior_agent.py +++ b/PythonAPI/carla/agents/navigation/behavior_agent.py @@ -193,6 +193,8 @@ def dist(w): return w.get_location().distance(waypoint.transform.location) return walker_state, walker, distance + _epsilon = np.nextafter(0., 1.) + def car_following_manager(self, vehicle, distance, debug=False): """ Module in charge of car-following behaviors when there's @@ -206,7 +208,7 @@ def car_following_manager(self, vehicle, distance, debug=False): vehicle_speed = get_speed(vehicle) delta_v = max(1, (self._speed - vehicle_speed) / 3.6) - ttc = distance / delta_v if delta_v != 0 else distance / np.nextafter(0., 1.) + ttc = distance / delta_v if delta_v != 0 else distance / self._epsilon # Under safety time distance, slow down. if self._behavior.safety_time > ttc > 0.0: From d8b68a8a702c40b6b3c4dbc338bfce7302b5946a Mon Sep 17 00:00:00 2001 From: Daniel Date: Fri, 1 Mar 2024 12:34:16 +0100 Subject: [PATCH 02/32] Moved behavior types to conf dir --- PythonAPI/carla/agents/{navigation => conf}/behavior_types.py | 0 PythonAPI/carla/agents/navigation/behavior_agent.py | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename PythonAPI/carla/agents/{navigation => conf}/behavior_types.py (100%) diff --git a/PythonAPI/carla/agents/navigation/behavior_types.py b/PythonAPI/carla/agents/conf/behavior_types.py similarity index 100% rename from PythonAPI/carla/agents/navigation/behavior_types.py rename to PythonAPI/carla/agents/conf/behavior_types.py diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py index 45b55622e2..890fc05140 100644 --- a/PythonAPI/carla/agents/navigation/behavior_agent.py +++ b/PythonAPI/carla/agents/navigation/behavior_agent.py @@ -13,7 +13,7 @@ import carla from agents.navigation.basic_agent import BasicAgent from agents.navigation.local_planner import RoadOption -from agents.navigation.behavior_types import Cautious, Aggressive, Normal +from agents.conf.behavior_types import Cautious, Aggressive, Normal from agents.tools.misc import get_speed, positive, is_within_distance, compute_distance From 25cfc8a5868421fe0c4f9f760fbcb245f10f6401 Mon Sep 17 00:00:00 2001 From: Daniel Date: Fri, 1 Mar 2024 18:46:01 +0100 Subject: [PATCH 03/32] Added Agent Settings backend --- .../agents/conf/agent_settings_backend.py | 649 ++++++++++++++++++ 1 file changed, 649 insertions(+) create mode 100644 PythonAPI/carla/agents/conf/agent_settings_backend.py diff --git a/PythonAPI/carla/agents/conf/agent_settings_backend.py b/PythonAPI/carla/agents/conf/agent_settings_backend.py new file mode 100644 index 0000000000..a0482888b9 --- /dev/null +++ b/PythonAPI/carla/agents/conf/agent_settings_backend.py @@ -0,0 +1,649 @@ +# DO NOT USE from __future__ import annotations ! This would break the dataclass interface. + +from functools import partial +from dataclasses import dataclass, field +from typing import ClassVar, Dict, Optional, Tuple, Type, Union + +import carla +from agents.navigation.local_planner import RoadOption + +MISSING = "???" + +# TODO: Behaviour _tailgate_counter is not implemented in the settings. + +# --------------------- +# Helper methods +# --------------------- + +class class_or_instance_method: + """Decorator to transform a method into both a regular and class method""" + + def __init__(self, call): + self.__wrapped__ = call + self._wrapper = lambda x : x # TODO: functools.partial and functools.wraps shadow the signature, this reveals it again. + + def __get__(self, instance : Union[None,"AgentConfig"], owner : Type["AgentConfig"]): + if instance is None: # called on class + return self._wrapper(partial(self.__wrapped__, owner)) + return self._wrapper(partial(self.__wrapped__, instance)) # called on instance + + +# --------------------- +# Configs +# --------------------- + +class AgentConfig: + overwrites: "Optional[Dict[str, Union[dict, AgentConfig]]]" = None + + @classmethod + def get_defaults(cls) -> "AgentConfig": + """Returns the global default options.""" + return cls + + def update(self, options : "Union[dict, AgentConfig]", clean=True): + """Updates the options with a new dictionary.""" + if isinstance(options, AgentConfig): + key_values = options.__dataclass_fields__.items() + else: + key_values = options.items() + for k, v in key_values: + if isinstance(getattr(self, k), AgentConfig): + getattr(self, k).update(v) + else: + setattr(self, k, v) + if clean: + self._clean_options() + + def _clean_options(self): + """Postprocessing of possibly wrong values""" + NotImplemented + + def __post_init__(self): + """ + Assures that if a dict is passed the values overwrite the defaults. + + # NOTE: Will be used for dataclass derived children + """ + self._clean_options() + if self.overwrites is None: + return + for key, value in self.overwrites.items(): + if key in self.__annotations__: + if issubclass(self.__annotations__[key], AgentConfig): + getattr(self, key).update(value) + else: + setattr(self, key, value) + else: + print(f"Key {key} not found in {self.__class__.__name__} options.") + +# --------------------- +# Live Info +# --------------------- + +@dataclass +class LiveInfo(AgentConfig): + current_speed : float = MISSING + current_speed_limit : float = MISSING + velocity_vector : "carla.Vector3D" = MISSING + direction : RoadOption = MISSING + + # NOTE: Not ported to OmegaConf + @property + def speed(self): + return self.current_speed + + @property + def speed_limit(self): + return self.current_speed_limit + +# --------------------- +# Speed +# --------------------- + + +@dataclass +class BasicAgentSpeedSettings(AgentConfig): + target_speed: float = 20 + """desired cruise speed in Km/h; overwritten by SpeedLimit if follow_speed_limit is True""" + + follow_speed_limits: bool = False + """If the agent should follow the speed limit. *NOTE:* SpeedLimit overwrites target_speed if True (local_planner.py)""" + +@dataclass +class BehaviorAgentSpeedSettings(BasicAgentSpeedSettings): + """ + The three situations they adjust their speed; # SEE: `behavior_agent.car_following_manager` + + Case A car in front and getting closer : slow down; slower than car in front + Take minium from, speed decrease, speed limit adjustment and target_speed + `target_speed` = min( other_vehicle_speed - self._behavior.speed_decrease, # <-- slow down BELOW the other car + self._behavior.max_speed # use target_speed instead + self._speed_limit - self._behavior.speed_lim_dist]) + Case B car in front but safe distance : match speed + `target_speed` = min([ + max(self._min_speed, other_vehicle_speed), # <- match speed + self._behavior.max_speed, + self._speed_limit - self._behavior.speed_lim_dist]) + Case C front is clear + `target_speed` = min([ + self._behavior.max_speed, + self._speed_limit - self._behavior.speed_lim_dist]) + """ + # TODO: deprecated max_speed use target_speed instead # NOTE: Behavior agents are more flexible in their speed. + max_speed : float = 50 + """The maximum speed in km/h your vehicle will be able to reach. + From normal behavior. This supersedes the target_speed when following the BehaviorAgent logic.""" + + # CASE A + speed_decrease : float = 12 + """other_vehicle_speed""" + + safety_time : float = 3 + """Time in s before a collision at the same speed -> apply speed_decrease""" + + # CASE B + min_speed : float = 5 + """Implement als variable, currently hard_coded""" + + # All Cases + speed_lim_dist : float = 6 + """ + Difference to speed limit. + NOTE: For negative values the car drives above speed limit + """ + + intersection_speed_decrease: float = 5.0 + """Reduction of the targeted_speed when approaching an intersection""" + + +@dataclass +class AutopilotSpeedSettings(AgentConfig): + vehicle_percentage_speed_difference : float = 30 # in percent + """ + Sets the difference the vehicle's intended speed and its current speed limit. + Speed limits can be exceeded by setting the percentage to a negative value. + Default is 30. + + Exceeding a speed limit can be done using negative percentages. + """ + +# --------------------- +# Distance +# --------------------- + + +@dataclass +class BasicAgentDistanceSettings(AgentConfig): + """ + Calculation of the minimum distance for # XXX + min_distance = base_min_distance + distance_ratio * vehicle_speed + + see local_planner.py `run_step` + """ + + base_min_distance : float = 3.0 + """ + Base value of the distance to keep + """ + + distance_ratio : float = 0.5 + """Increases minimum distance multiplied by speed""" + + +@dataclass +class BehaviorAgentDistanceSettings(BasicAgentDistanceSettings): + """ + Collision Avoidance ----- + + Distance in which for vehicles are checked + max(min_proximity_threshold, self._speed_limit / (2 if LANE CHANGE else 3 ) ) + TODO: The secondary speed limit is hardcoded, make adjustable and optional + automatic_proximity_threshold = {RoadOption.CHANGELANELEFT: 2, "same_lane" : 3, "right_lane" : 2} + """ + + min_proximity_threshold : float = 12 + """Range in which cars are detected. NOTE: Speed limit overwrites""" + + braking_distance : float = 6 + """Emergency Stop Distance Trigger""" + + +@dataclass +class AutopilotDistanceSettings(AgentConfig): + distance_to_leading_vehicle : float = 5.0 + """ + Sets the minimum distance in meters that a vehicle has to keep with the others. + The distance is in meters and will affect the minimum moving distance. It is computed from front to back of the vehicle objects. + """ + +# --------------------- +# Lane Change +# --------------------- + +@dataclass +class BasicAgentLaneChangeSettings(AgentConfig): + """ + Timings in seconds to finetune the lane change behavior. + + NOTE: see: `BasicAgent.lane_change` and `BasicAgent._generate_lane_change_path` + """ + same_lane_time : float = 0.0 + other_lane_time : float = 0.0 + lane_change_time : float = 2.0 + +@dataclass +class BehaviorAgentLaneChangeSettings(BasicAgentLaneChangeSettings): + pass + +@dataclass +class AutopilotLaneChangeSettings(AgentConfig): + auto_lane_change: bool = True + """Turns on or off lane changing behavior for a vehicle.""" + + random_left_lanechange_percentage: float = 0.1 + """ + Adjust probability that in each timestep the actor will perform a left/right lane change, + dependent on lane change availability. + """ + random_right_lanechange_percentage : float = 0.1 + """ + Adjust probability that in each timestep the actor will perform a left/right lane change, + dependent on lane change availability. + """ + + keep_right_rule_percentage: float = 0.7 + """ + During the localization stage, this method sets a percent chance that vehicle will follow the keep right rule, + and stay in the right lane. + """ + +# --------------------- +# Obstacles +# --------------------- + +@dataclass +class BasicAgentObstacleDetectionAngles(AgentConfig): + """ + Detection Angles for the BasicAgent used in the `BasicAgent._vehicle_obstacle_detected` method. + + The angle between the location and reference object. + Being 0 a location in front and 180, one behind, i.e, the vector between has to satisfy: + low_angle_th < angle < up_angle_th. + """ + + walkers_lane_change : Tuple[float, float] = (0., 90.) + """Detection angle of walkers when staying in the same lane""" + + walkers_same_lane : Tuple[float, float] = (0., 60.) + """Detection angle of walkers when changing lanes""" + + cars_lane_change : Tuple[float, float] = (0., 180.) + """Detection angle of cars when staying in the same lane""" + + cars_same_lane : Tuple[float, float] = (0., 30.) + """Detection angle of cars when changing lanes""" + +@dataclass +class BasicAgentObstacleSettings(AgentConfig): + """ + -------------------------- + Agent Level + see _affected_by_traffic_light and _affected_by_vehicle in basic_agent.py + -------------------------- + Agents is aware of the vehicles and traffic lights within its distance parameters + optionally can always ignore them. + """ + + ignore_vehicles : bool = False + """Whether the agent should ignore vehicles""" + + ignore_traffic_lights : bool = False + """Whether the agent should ignore traffic lights""" + + ignore_stop_signs : bool = False + """ + Whether the agent should ignore stop signs + + NOTE: No usage implemented! + """ + + use_bbs_detection : bool = False + """ + True: Whether to use a general approach to detect vehicles invading other lanes due to the offset. + + False: Simplified approach, using only the plan waypoints (similar to TM) + + See `BasicAgent._vehicle_obstacle_detected` + """ + + base_tlight_threshold : float = 5.0 + """ + Base distance to traffic lights to check if they affect the vehicle + + USAGE: max_vehicle_distance = base_vehicle_threshold + detection_speed_ratio * vehicle_speed + USAGE: max_tlight_distance = base_tlight_threshold + detection_speed_ratio * vehicle_speed + """ + + base_vehicle_threshold : float = 5.0 + """ + Base distance to vehicles to check if they affect the vehicle + + USAGE: max_vehicle_distance = base_vehicle_threshold + detection_speed_ratio * vehicle_speed + USAGE: max_tlight_distance = base_tlight_threshold + detection_speed_ratio * vehicle_speed + """ + + detection_speed_ratio : float = 1.0 + """ + Increases detection range based on speed + + USAGE: max_vehicle_distance = base_vehicle_threshold + detection_speed_ratio * vehicle_speed + USAGE: max_tlight_distance = base_tlight_threshold + detection_speed_ratio * vehicle_speed + """ + + detection_angles : BasicAgentObstacleDetectionAngles = field(default_factory=BasicAgentObstacleDetectionAngles) + +@dataclass +class BehaviorAgentObstacleSettings(BasicAgentObstacleSettings): + pass + +@dataclass +class AutopilotObstacleSettings(AgentConfig): + ignore_lights_percentage : float = 0.0 + """ + Percentage of time to ignore traffic lights + """ + + ignore_signs_percentage : float = 0.0 + """ + Percentage of time to ignore stop signs + """ + + ignore_walkers_percentage : float = 0.0 + """ + Percentage of time to ignore pedestrians + """ + + +# --------------------- +# ControllerSettings +# --------------------- + +@dataclass +class BasicAgentControllerSettings(AgentConfig): + """Limitations of the controls used one the PIDController Level""" + + max_brake : float = 0.5 + """ + Vehicle control how strong the brake is used, + + NOTE: Also used in emergency stop + """ + max_throttle : float = 0.75 + """maximum throttle applied to the vehicle""" + max_steering : float = 0.8 + """maximum steering applied to the vehicle""" + + # Aliases used: + #@property + #def max_throt(self): + # return self.max_throttle + + #@property + #def max_steer(self): + # return self.max_steering + +@dataclass +class BehaviorAgentControllerSettings(BasicAgentControllerSettings): + pass + +@dataclass +class AutopilotControllerSettings(AgentConfig): + vehicle_lane_offset : float = 0 + """ + Sets a lane offset displacement from the center line. Positive values imply a right offset while negative ones mean a left one. + Default is 0. Numbers high enough to cause the vehicle to drive through other lanes might break the controller. + """ + +# --------------------- +# PlannerSettings +# --------------------- + +@dataclass +class PIDControllerDict: + K_P : float + K_D : float + K_I : float = 0.05 + dt : float = 1.0 / 20.0 + """time differential in seconds""" + +@dataclass +class BasicAgentPlannerSettings(AgentConfig): + """ + PID controller using the following semantics: + K_P -- Proportional term + K_D -- Differential term + K_I -- Integral term + dt -- time differential in seconds + offset: If different than zero, the vehicle will drive displaced from the center line. + Positive values imply a right offset while negative ones mean a left one. + Numbers high enough to cause the vehicle to drive through other lanes might break the controller. + + Notes: + `sampling_resolution` is used by the global planner to build a graph of road segments, also to get a path of waypoints from A to B + + `sampling_radius` is similar but only used only by the local_planner to compute the next waypoints forward. The distance of those is the sampling_radius. + """ + + dt : float = 1.0 / 20.0 + """time differential in seconds""" + + lateral_control_dict : PIDControllerDict = field(default_factory=lambda:PIDControllerDict(**{'K_P': 1.95, 'K_I': 0.05, 'K_D': 0.2})) + """values of the lateral PID controller""" + + longitudinal_control_dict : PIDControllerDict = field(default_factory=lambda:PIDControllerDict(**{'K_P': 1.0, 'K_I': 0.05, 'K_D': 0})) + """values of the longitudinal PID controller""" + + offset : float = 0.0 + """ + If different than zero, the vehicle will drive displaced from the center line. + + Positive values imply a right offset while negative ones mean a left one. Numbers high enough + to cause the vehicle to drive through other lanes might break the controller. + """ + + sampling_radius : float = 2.0 + """ + Distance between waypoints when planning a path in `local_planner._compute_next_waypoints` + + Used with Waypoint.next(sampling_radius) + """ + + sampling_resolution : float = 2.0 + """ + Distance between waypoints in `BasicAgent._generate_lane_change_path` + Furthermore in the GlobalRoutePlanner to build the topology and for path planning. + + Used with the Waypoint.next(sampling_radius) and distance between waypoints. + """ + + +@dataclass +class BehaviorAgentPlannerSettings(BasicAgentPlannerSettings): + sampling_resolution : float = 4.5 + """ + Distance between waypoints in `BasicAgent._generate_lane_change_path` + and GlobalRoutePlanner to build the topology and path planning. + + Used with the Waypoint.next(sampling_radius) + """ + +# --------------------- +# Emergency +# --------------------- + +@dataclass +class BasicAgentEmergencySettings(AgentConfig): + throttle : float = 0.0 + max_emergency_brake : float = 0.5 + hand_brake : bool = False + + +@dataclass +class BehaviorAgentEmergencySettings(BasicAgentEmergencySettings): + pass + + +# --------------------- + +@dataclass +class AutopilotBehavior(AgentConfig): + """ + These are settings from the autopilot carla.TrafficManager which are not exposed or not used by the original carla agents. + NOTE: That default values do not exist for most settings; we should set it to something reasonable. + """ + + auto_lane_change: bool = True + """Turns on or off lane changing behavior for a vehicle.""" + + vehicle_lane_offset : float = 0 + """ + Sets a lane offset displacement from the center line. + + Positive values imply a right offset while negative ones mean a left one. + Default is 0. + + NOTE: Numbers high enough to cause the vehicle to drive through other lanes might break the controller. + """ + + random_left_lanechange_percentage: float = 0.1 + """ + Adjust probability that in each timestep the actor will perform a left/right lane change, + dependent on lane change availability. + """ + random_right_lanechange_percentage : float = 0.1 + """ + Adjust probability that in each timestep the actor will perform a left/right lane change, + dependent on lane change availability. + """ + + keep_right_rule_percentage: float = 0.7 + """ + During the localization stage, this method sets a percent chance that vehicle will follow the keep right rule, + and stay in the right lane. + """ + + distance_to_leading_vehicle : float = 5.0 + """ + Sets the minimum distance in meters that a vehicle has to keep with the others. + The distance is in meters and will affect the minimum moving distance. It is computed from front to back of the vehicle objects. + """ + + vehicle_percentage_speed_difference : float = 30 + """ + Sets the difference the vehicle's intended speed and its current speed limit. + Speed limits can be exceeded by setting the percentage to a negative value. + Exceeding a speed limit can be done using negative percentages. + + NOTE: unit is in percent. + Default is 30. + """ + + ignore_lights_percentage : float = 0.0 + ignore_signs_percentage : float = 0.0 + ignore_walkers_percentage : float = 0.0 + + update_vehicle_lights : bool = False + """Sets if the Traffic Manager is responsible of updating the vehicle lights, or not.""" + + +class SimpleConfig(object): + """ + A class that allows a more simple way to initialize settings and port them to a specified + base class. + + :param _base_settings: The base class to port the settings to. + + TODO: NOTE: This class assumes that there are NO DUPLICATE keys in the underlying base + """ + + _base_settings: ClassVar["AgentConfig"] = None + + def __new__(cls, overwrites:Optional[Dict[str, Union[dict, AgentConfig]]]=None) -> "AgentConfig": + """ + Transforms a SimpleConfig class into the given _base_settings + + :param overwrites: That allow overwrites during initialization. + """ + if cls is SimpleConfig: + raise TypeError("SimpleConfig class may not be instantiated") + if cls._base_settings is None: + raise TypeError("{cls.__name__} must have a class set in the `_base_settings` to initialize to.") + the_inst = super().__new__(cls) + simple_settings = {k:v for k,v in cls.__dict__.items() if not k.startswith("_") } # Removes all private attributes + if overwrites: + simple_settings.update(overwrites) + return the_inst.to_nested_config(simple_settings) + + @class_or_instance_method + def to_nested_config(self, simple_overwrites:dict=None) -> AgentConfig: + """ + Initializes the _base_settings with the given overwrites. + + Maps the keys of simple_overwrites to the base settings. + + NOTE: Assumes unique keys over all settings! + # TODO: add a warning if a non-unique key is found in the overwrites. + """ + if isinstance(self, type): # called on class + return self() + keys = set(simple_overwrites.keys()) + overwrites = {} + for name, base in self._base_settings.__annotations__.items(): + if not isinstance(base, type) or not issubclass(base, AgentConfig): # First out non AgentConfig attributes + continue + matching = keys.intersection(base.__dataclass_fields__.keys()) + print(str(base.__class__.__name__), "matches in", matching) + insert = {k: getattr(self, k) for k in matching} + if len(insert) > 0: + overwrites[name] = {k: getattr(self, k) for k in matching} + keys -= matching + if len(keys) != 0: + # TODO: turn into a warning and find way to include these keys in the config anyway! + raise ValueError(f"Unmatched keys {keys} in {self.__class__.__name__} not contained in base {self._base_settings.__name__}") + return self._base_settings(overwrites=overwrites) + # TODO: could add new keys after post-processing. + + +@dataclass +class BasicAgentSettings(AgentConfig): + overwrites : Optional[Dict[str, dict]] = field(default_factory=dict, repr=False) # type: Optional[Dict[str, Union[dict|AgentConfig]]] + live_info : LiveInfo = field(default_factory=LiveInfo, init=False) + speed : BasicAgentSpeedSettings = field(default_factory=BasicAgentSpeedSettings, init=False) + distance : BasicAgentDistanceSettings = field(default_factory=BasicAgentDistanceSettings, init=False) + lane_change : BasicAgentLaneChangeSettings = field(default_factory=BasicAgentLaneChangeSettings, init=False) + obstacles : BasicAgentObstacleSettings = field(default_factory=BasicAgentObstacleSettings, init=False) + controls : BasicAgentControllerSettings = field(default_factory=BasicAgentControllerSettings, init=False) + planner : BasicAgentPlannerSettings = field(default_factory=BasicAgentPlannerSettings, init=False) + emergency : BasicAgentEmergencySettings = field(default_factory=BasicAgentEmergencySettings, init=False) + +@dataclass +class BehaviorAgentSettings(AgentConfig): + overwrites : Optional[Dict[str, dict]] = field(default_factory=dict, repr=False) # type: Optional[Dict[str, Union[dict|AgentConfig]]] + live_info : LiveInfo = field(default_factory=LiveInfo, init=False) + speed : BehaviorAgentSpeedSettings = field(default_factory=BehaviorAgentSpeedSettings, init=False) + distance : BehaviorAgentDistanceSettings = field(default_factory=BehaviorAgentDistanceSettings, init=False) + lane_change : BehaviorAgentLaneChangeSettings = field(default_factory=BehaviorAgentLaneChangeSettings, init=False) + obstacles : BehaviorAgentObstacleSettings = field(default_factory=BehaviorAgentObstacleSettings, init=False) + controls : BehaviorAgentControllerSettings = field(default_factory=BehaviorAgentControllerSettings, init=False) + planner : BehaviorAgentPlannerSettings = field(default_factory=BehaviorAgentPlannerSettings, init=False) + emergency : BehaviorAgentEmergencySettings = field(default_factory=BehaviorAgentEmergencySettings, init=False) + +@dataclass +class SimpleBasicAgentSettings(SimpleConfig, LiveInfo, BasicAgentSpeedSettings, BasicAgentDistanceSettings, BasicAgentLaneChangeSettings, BasicAgentObstacleSettings, BasicAgentControllerSettings, BasicAgentPlannerSettings, BasicAgentEmergencySettings): + _base_settings :ClassVar[BasicAgentSettings] = BasicAgentSettings + +@dataclass +class SimpleBehaviorAgentSettings(SimpleConfig, LiveInfo, BehaviorAgentSpeedSettings, BehaviorAgentDistanceSettings, BehaviorAgentLaneChangeSettings, BehaviorAgentObstacleSettings, BehaviorAgentControllerSettings, BehaviorAgentPlannerSettings, BehaviorAgentEmergencySettings): + _base_settings :ClassVar[BehaviorAgentSettings] = BehaviorAgentSettings + + From f7a7d147ee12feba7ecd4c33738386d52c6236e4 Mon Sep 17 00:00:00 2001 From: Daniel Date: Fri, 1 Mar 2024 18:46:45 +0100 Subject: [PATCH 04/32] Port of behavior types --- PythonAPI/carla/agents/conf/behavior_types.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/PythonAPI/carla/agents/conf/behavior_types.py b/PythonAPI/carla/agents/conf/behavior_types.py index 3008f9f73b..3b666326a6 100644 --- a/PythonAPI/carla/agents/conf/behavior_types.py +++ b/PythonAPI/carla/agents/conf/behavior_types.py @@ -3,8 +3,11 @@ """ This module contains the different parameters sets for each behavior. """ +from dataclasses import dataclass +from agents.conf.agent_settings_backend import SimpleBehaviorAgentSettings -class Cautious(object): +@dataclass +class Cautious(SimpleBehaviorAgentSettings): """Class for Cautious agent.""" max_speed = 40 speed_lim_dist = 6 @@ -14,8 +17,8 @@ class Cautious(object): braking_distance = 6 tailgate_counter = 0 - -class Normal(object): +@dataclass +class Normal(SimpleBehaviorAgentSettings): """Class for Normal agent.""" max_speed = 50 speed_lim_dist = 3 @@ -25,8 +28,8 @@ class Normal(object): braking_distance = 5 tailgate_counter = 0 - -class Aggressive(object): +@dataclass +class Aggressive(SimpleBehaviorAgentSettings): """Class for Aggressive agent.""" max_speed = 70 speed_lim_dist = 1 From f92824b875be8b81fcfe07f266877e4d9800cec5 Mon Sep 17 00:00:00 2001 From: Daniel Date: Fri, 1 Mar 2024 18:57:53 +0100 Subject: [PATCH 05/32] Ported Local planner and controler to new settings --- .../carla/agents/navigation/controller.py | 136 ++++++++---------- .../carla/agents/navigation/local_planner.py | 68 ++------- 2 files changed, 75 insertions(+), 129 deletions(-) diff --git a/PythonAPI/carla/agents/navigation/controller.py b/PythonAPI/carla/agents/navigation/controller.py index a87488c748..8b0ef8761a 100644 --- a/PythonAPI/carla/agents/navigation/controller.py +++ b/PythonAPI/carla/agents/navigation/controller.py @@ -11,6 +11,8 @@ import carla from agents.tools.misc import get_speed +STEERING_UPDATE_SPEED = 0.1 +ERROR_BUFFER_LENGTH = 10 class VehiclePIDController(): """ @@ -19,70 +21,69 @@ class VehiclePIDController(): low level control a vehicle from client side """ - - def __init__(self, vehicle, args_lateral, args_longitudinal, offset=0, max_throttle=0.75, max_brake=0.3, - max_steering=0.8): + def __init__(self, vehicle: carla.Vehicle, config): """ Constructor method. :param vehicle: actor to apply to local planner logic onto - :param args_lateral: dictionary of arguments to set the lateral PID controller + :param config (BasicAgentSettings): configuration parameters for the agent (must contain a planner attribute). + + config.planner.lateral_control_dict: dictionary of arguments to set the lateral PID controller using the following semantics: K_P -- Proportional term K_D -- Differential term K_I -- Integral term - :param args_longitudinal: dictionary of arguments to set the longitudinal + + config.planner.longitudinal_control_dict: dictionary of arguments to set the longitudinal PID controller using the following semantics: K_P -- Proportional term K_D -- Differential term K_I -- Integral term - :param offset: If different than zero, the vehicle will drive displaced from the center line. + + config.planner.offset: If different than zero, the vehicle will drive displaced from the center line. Positive values imply a right offset while negative ones mean a left one. Numbers high enough to cause the vehicle to drive through other lanes might break the controller. """ - - self.max_brake = max_brake - self.max_throt = max_throttle - self.max_steer = max_steering + + self.config = config self._vehicle = vehicle self._world = self._vehicle.get_world() self.past_steering = self._vehicle.get_control().steer - self._lon_controller = PIDLongitudinalController(self._vehicle, **args_longitudinal) - self._lat_controller = PIDLateralController(self._vehicle, offset, **args_lateral) + self._lon_controller = PIDLongitudinalController(vehicle, config) + self._lat_controller = PIDLateralController(vehicle, config) - def run_step(self, target_speed, waypoint): + def run_step(self, waypoint): """ Execute one step of control invoking both lateral and longitudinal PID controllers to reach a target waypoint at a given target_speed. - :param target_speed: desired vehicle speed :param waypoint: target location encoded as a waypoint :return: distance (in meters) to the waypoint """ - - acceleration = self._lon_controller.run_step(target_speed) + + acceleration = self._lon_controller.run_step() current_steering = self._lat_controller.run_step(waypoint) control = carla.VehicleControl() if acceleration >= 0.0: - control.throttle = min(acceleration, self.max_throt) + control.throttle = min(acceleration, self.config.controls.max_throttle) control.brake = 0.0 else: control.throttle = 0.0 - control.brake = min(abs(acceleration), self.max_brake) + control.brake = min(abs(acceleration), self.config.controls.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 > self.past_steering + STEERING_UPDATE_SPEED: + current_steering = self.past_steering + STEERING_UPDATE_SPEED + elif current_steering < self.past_steering - STEERING_UPDATE_SPEED: + current_steering = self.past_steering - STEERING_UPDATE_SPEED if current_steering >= 0: - steering = min(self.max_steer, current_steering) + steering = min(self.config.controls.max_steering, current_steering) else: - steering = max(-self.max_steer, current_steering) + steering = max(-self.config.controls.max_steering, current_steering) control.steer = steering control.hand_brake = False @@ -91,7 +92,6 @@ def run_step(self, target_speed, waypoint): return control - def change_longitudinal_PID(self, args_longitudinal): """Changes the parameters of the PIDLongitudinalController""" self._lon_controller.change_parameters(**args_longitudinal) @@ -110,65 +110,59 @@ class PIDLongitudinalController(): PIDLongitudinalController implements longitudinal control using a PID. """ - def __init__(self, vehicle, K_P=1.0, K_I=0.0, K_D=0.0, dt=0.03): + def __init__(self, vehicle, config): """ Constructor method. :param vehicle: actor to apply to local planner logic onto - :param K_P: Proportional term - :param K_D: Differential term - :param K_I: Integral term - :param dt: time differential in seconds + :param config (BasicAgentSettings): configuration parameters for the agent. """ self._vehicle = vehicle - self._k_p = K_P - self._k_i = K_I - self._k_d = K_D - self._dt = dt - self._error_buffer = deque(maxlen=10) - - def run_step(self, target_speed, debug=False): + self.config = config + self._error_buffer = deque(maxlen=ERROR_BUFFER_LENGTH) + + def run_step(self, debug=False): """ Execute one step of longitudinal control to reach a given target speed. - :param target_speed: target speed in Km/h - :param debug: boolean for debugging + Assumes the config.live_info was updated with the current speed before + calling this function. + + :param debug: boolean for debugging. Prints the current speed. :return: throttle control """ - current_speed = get_speed(self._vehicle) if debug: - print('Current speed = {}'.format(current_speed)) + print('Current speed = {}'.format(self.config.live_info.current_speed)) - return self._pid_control(target_speed, current_speed) + return self._pid_control() - def _pid_control(self, target_speed, current_speed): + def _pid_control(self): """ Estimate the throttle/brake of the vehicle based on the PID equations - :param target_speed: target speed in Km/h - :param current_speed: current speed of the vehicle in Km/h :return: throttle/brake control """ - error = target_speed - current_speed + target_speed = self.config.speed.target_speed + error = target_speed - self.config.live_info.current_speed self._error_buffer.append(error) if len(self._error_buffer) >= 2: - _de = (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt - _ie = sum(self._error_buffer) * self._dt + _de = (self._error_buffer[-1] - self._error_buffer[-2]) / self.config.planner.dt + _ie = sum(self._error_buffer) * self.config.planner.dt else: _de = 0.0 _ie = 0.0 - return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) + return np.clip((self.config.planner.longitudinal_control_dict.K_P * error) + (self.config.planner.longitudinal_control_dict.K_D * _de) + (self.config.planner.longitudinal_control_dict.K_I * _ie), -1.0, 1.0) def change_parameters(self, K_P, K_I, K_D, dt): """Changes the PID parameters""" - self._k_p = K_P - self._k_i = K_I - self._k_d = K_D - self._dt = dt + self.config.planner.longitudinal_control_dict.K_P = K_P + self.config.planner.longitudinal_control_dict.K_I = K_I + self.config.planner.longitudinal_control_dict.K_D = K_D + self.config.planner.dt = dt class PIDLateralController(): @@ -176,7 +170,7 @@ class PIDLateralController(): PIDLateralController implements lateral control using a PID. """ - def __init__(self, vehicle, offset=0, K_P=1.0, K_I=0.0, K_D=0.0, dt=0.03): + def __init__(self, vehicle, config): """ Constructor method. @@ -189,17 +183,13 @@ def __init__(self, vehicle, offset=0, K_P=1.0, K_I=0.0, K_D=0.0, dt=0.03): :param dt: time differential in seconds """ self._vehicle = vehicle - self._k_p = K_P - self._k_i = K_I - self._k_d = K_D - self._dt = dt - self._offset = offset - self._e_buffer = deque(maxlen=10) + self.config = config + self._e_buffer = deque(maxlen=ERROR_BUFFER_LENGTH) def run_step(self, waypoint): """ Execute one step of lateral control to steer - the vehicle towards a certain waypoin. + the vehicle towards a certain waypoint. :param waypoint: target waypoint :return: steering control in the range [-1, 1] where: @@ -208,9 +198,9 @@ def run_step(self, waypoint): """ return self._pid_control(waypoint, self._vehicle.get_transform()) - def set_offset(self, offset): + def set_offset(self, offset: float): """Changes the offset""" - self._offset = offset + self.config.planner.offset = offset def _pid_control(self, waypoint, vehicle_transform): """ @@ -226,12 +216,12 @@ def _pid_control(self, waypoint, vehicle_transform): v_vec = np.array([v_vec.x, v_vec.y, 0.0]) # Get the vector vehicle-target_wp - if self._offset != 0: + if self.config.planner.offset != 0: # Displace the wp to the side w_tran = waypoint.transform r_vec = w_tran.get_right_vector() - w_loc = w_tran.location + carla.Location(x=self._offset*r_vec.x, - y=self._offset*r_vec.y) + w_loc = w_tran.location + carla.Location(x=self.config.planner.offset*r_vec.x, + y=self.config.planner.offset*r_vec.y) else: w_loc = waypoint.transform.location @@ -250,17 +240,17 @@ def _pid_control(self, waypoint, vehicle_transform): self._e_buffer.append(_dot) if len(self._e_buffer) >= 2: - _de = (self._e_buffer[-1] - self._e_buffer[-2]) / self._dt - _ie = sum(self._e_buffer) * self._dt + _de = (self._e_buffer[-1] - self._e_buffer[-2]) / self.config.planner.dt + _ie = sum(self._e_buffer) * self.config.planner.dt else: _de = 0.0 _ie = 0.0 - return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) + return np.clip((self.config.planner.lateral_control_dict.K_P * _dot) + (self.config.planner.lateral_control_dict.K_D * _de) + (self.config.planner.lateral_control_dict.K_I * _ie), -1.0, 1.0) def change_parameters(self, K_P, K_I, K_D, dt): """Changes the PID parameters""" - self._k_p = K_P - self._k_i = K_I - self._k_d = K_D - self._dt = dt + self.config.planner.lateral_control_dict.K_P = K_P + self.config.planner.lateral_control_dict.K_I = K_I + self.config.planner.lateral_control_dict.K_D = K_D + self.config.planner.dt = dt diff --git a/PythonAPI/carla/agents/navigation/local_planner.py b/PythonAPI/carla/agents/navigation/local_planner.py index e4f4b7fb71..df71dbf75a 100644 --- a/PythonAPI/carla/agents/navigation/local_planner.py +++ b/PythonAPI/carla/agents/navigation/local_planner.py @@ -74,46 +74,7 @@ def __init__(self, vehicle, opt_dict={}, map_inst=None): 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._distance_ratio = 0.5 - 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 'distance_ratio' in opt_dict: - self._distance_ratio = opt_dict['distance_ratio'] - if 'follow_speed_limits' in opt_dict: - self._follow_speed_limits = opt_dict['follow_speed_limits'] + self.config = opt_dict # initializing controller self._init_controller() @@ -124,13 +85,7 @@ def reset_vehicle(self): 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) + self._vehicle_controller = VehiclePIDController(self._vehicle, self.config) # Compute the current vehicle waypoint current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) @@ -144,10 +99,10 @@ def set_speed(self, speed): :param speed: new target speed in Km/h :return: """ - if self._follow_speed_limits: + if self.config.speed.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 + self.config.speed.target_speed = speed def follow_speed_limits(self, value=True): """ @@ -156,7 +111,7 @@ def follow_speed_limits(self, value=True): :param value: bool :return: """ - self._follow_speed_limits = value + self.config.speed.follow_speed_limits = value def _compute_next_waypoints(self, k=1): """ @@ -171,7 +126,7 @@ def _compute_next_waypoints(self, k=1): for _ in range(k): last_waypoint = self._waypoints_queue[-1][0] - next_waypoints = list(last_waypoint.next(self._sampling_radius)) + next_waypoints = list(last_waypoint.next(self.config.planner.sampling_radius)) if len(next_waypoints) == 0: break @@ -228,8 +183,8 @@ def run_step(self, debug=False): :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() + if self.config.speed.follow_speed_limits: + self.config.speed.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: @@ -237,8 +192,9 @@ def run_step(self, debug=False): # 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 + self._distance_ratio * vehicle_speed + self.config.live_info.current_speed = get_speed(self._vehicle) # km/h # Could be removed if we assume that it was updated beforehand by the agent. + vehicle_speed = self.config.live_info.current_speed / 3.6 # km/h -> m/s + self._min_distance = self.config.distance.base_min_distance + self.config.distance.distance_ratio * vehicle_speed num_waypoint_removed = 0 for waypoint, _ in self._waypoints_queue: @@ -267,7 +223,7 @@ def run_step(self, debug=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) + control = self._vehicle_controller.run_step(self.target_waypoint) if debug: draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], 1.0) From 3b17c75f7cfa687e417e468a5827282efcfca67f Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 11:31:56 +0100 Subject: [PATCH 06/32] Updates Basic Agent to new settings Fix --- .../carla/agents/navigation/basic_agent.py | 88 +++++++------------ 1 file changed, 30 insertions(+), 58 deletions(-) diff --git a/PythonAPI/carla/agents/navigation/basic_agent.py b/PythonAPI/carla/agents/navigation/basic_agent.py index 2d0b9b91a1..7b0d93c215 100644 --- a/PythonAPI/carla/agents/navigation/basic_agent.py +++ b/PythonAPI/carla/agents/navigation/basic_agent.py @@ -17,6 +17,7 @@ from agents.tools.misc import (get_speed, is_within_distance, get_trafficlight_trigger_location, compute_distance) +from agents.conf.agent_settings_backend import BasicAgentSettings from agents.tools.hints import ObstacleDetectionResult, TrafficLightDetectionResult @@ -29,7 +30,7 @@ class BasicAgent(object): as well as to change its parameters in case a different driving mode is desired. """ - def __init__(self, vehicle, target_speed=20, opt_dict={}, map_inst=None, grp_inst=None): + def __init__(self, vehicle, opt_dict=BasicAgentSettings(), map_inst=None, grp_inst=None): """ Initialization the agent paramters, the local and the global planner. @@ -53,41 +54,7 @@ def __init__(self, vehicle, target_speed=20, opt_dict={}, map_inst=None, grp_ins self._map = self._world.get_map() self._last_traffic_light = None - # Base parameters - self._ignore_traffic_lights = False - self._ignore_stop_signs = False - self._ignore_vehicles = False - self._use_bbs_detection = False - self._target_speed = target_speed - self._sampling_resolution = 2.0 - self._base_tlight_threshold = 5.0 # meters - self._base_vehicle_threshold = 5.0 # meters - self._speed_ratio = 1 - self._max_brake = 0.5 - self._offset = 0 - - # Change parameters according to the dictionary - opt_dict['target_speed'] = target_speed - if 'ignore_traffic_lights' in opt_dict: - self._ignore_traffic_lights = opt_dict['ignore_traffic_lights'] - if 'ignore_stop_signs' in opt_dict: - self._ignore_stop_signs = opt_dict['ignore_stop_signs'] - if 'ignore_vehicles' in opt_dict: - self._ignore_vehicles = opt_dict['ignore_vehicles'] - if 'use_bbs_detection' in opt_dict: - self._use_bbs_detection = opt_dict['use_bbs_detection'] - if 'sampling_resolution' in opt_dict: - self._sampling_resolution = opt_dict['sampling_resolution'] - if 'base_tlight_threshold' in opt_dict: - self._base_tlight_threshold = opt_dict['base_tlight_threshold'] - if 'base_vehicle_threshold' in opt_dict: - self._base_vehicle_threshold = opt_dict['base_vehicle_threshold'] - if 'detection_speed_ratio' in opt_dict: - self._speed_ratio = opt_dict['detection_speed_ratio'] - if 'max_brake' in opt_dict: - self._max_brake = opt_dict['max_brake'] - if 'offset' in opt_dict: - self._offset = opt_dict['offset'] + self.config = opt_dict # Initialize the planners self._local_planner = LocalPlanner(self._vehicle, opt_dict=opt_dict, map_inst=self._map) @@ -96,13 +63,14 @@ def __init__(self, vehicle, target_speed=20, opt_dict={}, map_inst=None, grp_ins self._global_planner = grp_inst else: print("Warning: Ignoring the given map as it is not a 'carla.Map'") - self._global_planner = GlobalRoutePlanner(self._map, self._sampling_resolution) + self._global_planner = GlobalRoutePlanner(self._map, self.config.planner.sampling_resolution) else: - self._global_planner = GlobalRoutePlanner(self._map, self._sampling_resolution) + self._global_planner = GlobalRoutePlanner(self._map, self.config.planner.sampling_resolution) # Get the static elements of the scene self._lights_list = self._world.get_actors().filter("*traffic_light*") - self._lights_map = {} # Dictionary mapping a traffic light to a wp corrspoing to its trigger volume location + # Dictionary mapping a traffic light to a Waypoint corresponding to its trigger volume location + self._lights_map: "dict[int, carla.Waypoint]" = {} def add_emergency_stop(self, control): """ @@ -111,9 +79,9 @@ def add_emergency_stop(self, control): :param speed (carl.VehicleControl): control to be modified """ - control.throttle = 0.0 - control.brake = self._max_brake - control.hand_brake = False + control.throttle = self.config.emergency.throttle + control.brake = self.config.emergency.max_emergency_brake + control.hand_brake = self.config.emergency.hand_brake return control def set_target_speed(self, speed): @@ -121,8 +89,10 @@ def set_target_speed(self, speed): Changes the target speed of the agent :param speed (float): target speed in Km/h """ - self._target_speed = speed - self._local_planner.set_speed(speed) + self.config.speed.target_speed = speed + if self.config.speed.follow_speed_limits: + print("WARNING: The max speed is currently set to follow the speed limits. " + "Use 'follow_speed_limits' to deactivate this") def follow_speed_limits(self, value=True): """ @@ -130,7 +100,7 @@ def follow_speed_limits(self, value=True): :param value (bool): whether or not to activate this behavior """ - self._local_planner.follow_speed_limits(value) + self.config.speed.follow_speed_limits = value def get_local_planner(self): """Get method for protected member local planner""" @@ -154,6 +124,7 @@ def set_destination(self, end_location, start_location=None, clean_queue=True): :param start_location (carla.Location): starting location of the route :param clean_queue (bool): Whether to clear or append to the currently planned route """ + #TODO: This needs an overhaul to be done. Currently start_location is not used if not start_location: if clean_queue and self._local_planner.target_waypoint: # Plan from the waypoint in front of the vehicle onwards @@ -202,16 +173,17 @@ def run_step(self): # Retrieve all relevant actors vehicle_list = self._world.get_actors().filter("*vehicle*") - vehicle_speed = get_speed(self._vehicle) / 3.6 + self.config.live_info.current_speed = get_speed(self._vehicle) # km/s + vehicle_speed = self.config.live_info.current_speed / 3.6 # Check for possible vehicle obstacles - max_vehicle_distance = self._base_vehicle_threshold + self._speed_ratio * vehicle_speed + max_vehicle_distance = self.config.obstacles.base_vehicle_threshold + self.config.obstacles.detection_speed_ratio * vehicle_speed affected_by_vehicle, _, _ = self._vehicle_obstacle_detected(vehicle_list, max_vehicle_distance) if affected_by_vehicle: hazard_detected = True # Check if the vehicle is affected by a red traffic light - max_tlight_distance = self._base_tlight_threshold + self._speed_ratio * vehicle_speed + max_tlight_distance = self.config.obstacles.base_tlight_threshold + self.config.obstacles.detection_speed_ratio * vehicle_speed affected_by_tlight, _ = self._affected_by_traffic_light(self._lights_list, max_tlight_distance) if affected_by_tlight: hazard_detected = True @@ -228,15 +200,15 @@ def done(self): def ignore_traffic_lights(self, active=True): """(De)activates the checks for traffic lights""" - self._ignore_traffic_lights = active + self.config.obstacles.ignore_traffic_lights = active def ignore_stop_signs(self, active=True): """(De)activates the checks for stop signs""" - self._ignore_stop_signs = active + self.config.obstacles.ignore_stop_signs = active def ignore_vehicles(self, active=True): """(De)activates the checks for stop signs""" - self._ignore_vehicles = active + self.config.obstacles.ignore_vehicles = active def set_offset(self, offset): """Sets an offset for the vehicle""" @@ -257,7 +229,7 @@ def lane_change(self, direction, same_lane_time=0, other_lane_time=0, lane_chang lane_change_time * speed, False, 1, - self._sampling_resolution + self.config.planner.sampling_resolution ) if not path: print("WARNING: Ignoring the lane change as no path was found") @@ -280,7 +252,7 @@ def _affected_by_traffic_light(self, lights_list=None, max_distance=None): lights_list = self._world.get_actors().filter("*traffic_light*") if not max_distance: - max_distance = self._base_tlight_threshold + max_distance = self.config.obstacles.base_tlight_threshold if self._last_traffic_light: if self._last_traffic_light.state != carla.TrafficLightState.Red: @@ -333,8 +305,8 @@ def _vehicle_obstacle_detected(self, vehicle_list=None, max_distance=None, up_an def get_route_polygon(): route_bb = [] extent_y = self._vehicle.bounding_box.extent.y - r_ext = extent_y + self._offset - l_ext = -extent_y + self._offset + r_ext = extent_y + self.config.planner.offset + l_ext = -extent_y + self.config.planner.offset r_vec = ego_transform.get_right_vector() p1 = ego_location + carla.Location(r_ext * r_vec.x, r_ext * r_vec.y) p2 = ego_location + carla.Location(l_ext * r_vec.x, l_ext * r_vec.y) @@ -364,7 +336,7 @@ def get_route_polygon(): return ObstacleDetectionResult(False, None, -1) if not max_distance: - max_distance = self._base_vehicle_threshold + max_distance = self.config.obstacles.base_vehicle_threshold ego_transform = self._vehicle.get_transform() ego_location = ego_transform.location @@ -379,8 +351,8 @@ def get_route_polygon(): ego_front_transform.location += carla.Location( self._vehicle.bounding_box.extent.x * ego_transform.get_forward_vector()) - opposite_invasion = abs(self._offset) + self._vehicle.bounding_box.extent.y > ego_wpt.lane_width / 2 - use_bbs = self._use_bbs_detection or opposite_invasion or ego_wpt.is_junction + opposite_invasion = abs(self.config.planner.offset) + self._vehicle.bounding_box.extent.y > ego_wpt.lane_width / 2 + use_bbs = self.config.obstacles.use_bbs_detection or opposite_invasion or ego_wpt.is_junction # Get the route bounding box route_polygon = get_route_polygon() From 9d43265aa40db89c2471fae4c1c62c5734e116d8 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 12:17:15 +0100 Subject: [PATCH 07/32] Behavior agent to new settings added init fix fixes --- .../carla/agents/navigation/behavior_agent.py | 112 +++++++++--------- 1 file changed, 58 insertions(+), 54 deletions(-) diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py index 890fc05140..45b12cddd4 100644 --- a/PythonAPI/carla/agents/navigation/behavior_agent.py +++ b/PythonAPI/carla/agents/navigation/behavior_agent.py @@ -14,6 +14,7 @@ from agents.navigation.basic_agent import BasicAgent from agents.navigation.local_planner import RoadOption from agents.conf.behavior_types import Cautious, Aggressive, Normal +from agents.conf.agent_settings_backend import BehaviorAgentSettings, AgentConfig, SimpleConfig from agents.tools.misc import get_speed, positive, is_within_distance, compute_distance @@ -30,7 +31,7 @@ class BehaviorAgent(BasicAgent): are encoded in the agent, from cautious to a more aggressive ones. """ - def __init__(self, vehicle, behavior='normal', opt_dict={}, map_inst=None, grp_inst=None): + def __init__(self, vehicle, behavior='normal', opt_dict=BehaviorAgentSettings(), map_inst=None, grp_inst=None): """ Constructor method. @@ -42,38 +43,41 @@ def __init__(self, vehicle, behavior='normal', opt_dict={}, map_inst=None, grp_i self._look_ahead_steps = 0 # Vehicle information - self._speed = 0 - self._speed_limit = 0 - self._direction = None - self._incoming_direction = None - self._incoming_waypoint = None - self._min_speed = 5 - self._behavior = None - self._sampling_resolution = 4.5 + self.config = None # Parameters for agent behavior - if behavior == 'cautious': - self._behavior = Cautious() - - elif behavior == 'normal': - self._behavior = Normal() + if isinstance(behavior, str): + if behavior == 'cautious': + self.config = Cautious() + + elif behavior == 'normal': + self.config = Normal() + + elif behavior == 'aggressive': + self.config = Aggressive() + elif isinstance(behavior, AgentConfig): + self.config = behavior + elif isinstance(behavior, type) and issubclass(behavior, (AgentConfig, SimpleConfig)): + self.config = behavior() + else: + raise TypeError("Behavior must be a string or a subclass of AgentConfig or a SimpleConfig") + if not hasattr(self.config, "tailgate_counter"): # TODO: maybe use an attribute here instead, or choose another option. + self.config.tailgate_counter = 0 - elif behavior == 'aggressive': - self._behavior = Aggressive() def _update_information(self): """ This method updates the information regarding the ego vehicle based on the surrounding world. """ - self._speed = get_speed(self._vehicle) - self._speed_limit = self._vehicle.get_speed_limit() - self._local_planner.set_speed(self._speed_limit) + self.config.live_info.current_speed = get_speed(self._vehicle) + self.config.live_info.current_speed_limit = self._vehicle.get_speed_limit() + self._local_planner.set_speed(self.config.live_info.current_speed_limit) # note: currently redundant, but will print warning. self._direction = self._local_planner.target_road_option if self._direction is None: self._direction = RoadOption.LANEFOLLOW - self._look_ahead_steps = int((self._speed_limit) / 10) + self._look_ahead_steps = int((self.config.live_info.current_speed_limit) / 10) self._incoming_waypoint, self._incoming_direction = self._local_planner.get_incoming_waypoint_and_direction( steps=self._look_ahead_steps) @@ -106,25 +110,25 @@ def _tailgating(self, waypoint, vehicle_list): right_wpt = waypoint.get_right_lane() behind_vehicle_state, behind_vehicle, _ = self._vehicle_obstacle_detected(vehicle_list, max( - self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, low_angle_th=160) - if behind_vehicle_state and self._speed < get_speed(behind_vehicle): + self.config.distance.min_proximity_threshold, self.config.live_info.current_speed_limit / 2), up_angle_th=180, low_angle_th=160) + if behind_vehicle_state and self.config.live_info.current_speed < get_speed(behind_vehicle): if (right_turn == carla.LaneChange.Right or right_turn == carla.LaneChange.Both) and waypoint.lane_id * right_wpt.lane_id > 0 and right_wpt.lane_type == carla.LaneType.Driving: new_vehicle_state, _, _ = self._vehicle_obstacle_detected(vehicle_list, max( - self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=1) + self.config.distance.min_proximity_threshold, self.config.live_info.current_speed_limit / 2), up_angle_th=180, lane_offset=1) if not new_vehicle_state: print("Tailgating, moving to the right!") end_waypoint = self._local_planner.target_waypoint - self._behavior.tailgate_counter = 200 + self.config.tailgate_counter = 200 self.set_destination(end_waypoint.transform.location, right_wpt.transform.location) elif left_turn == carla.LaneChange.Left and waypoint.lane_id * left_wpt.lane_id > 0 and left_wpt.lane_type == carla.LaneType.Driving: new_vehicle_state, _, _ = self._vehicle_obstacle_detected(vehicle_list, max( - self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=-1) + self.config.distance.min_proximity_threshold, self.config.live_info.current_speed_limit / 2), up_angle_th=180, lane_offset=-1) if not new_vehicle_state: print("Tailgating, moving to the left!") end_waypoint = self._local_planner.target_waypoint - self._behavior.tailgate_counter = 200 + self.config.tailgate_counter = 200 self.set_destination(end_waypoint.transform.location, left_wpt.transform.location) @@ -147,20 +151,20 @@ def dist(v): return v.get_location().distance(waypoint.transform.location) if self._direction == RoadOption.CHANGELANELEFT: vehicle_state, vehicle, distance = self._vehicle_obstacle_detected( vehicle_list, max( - self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=-1) + self.config.distance.min_proximity_threshold, self.config.live_info.current_speed_limit / 2), up_angle_th=180, lane_offset=-1) elif self._direction == RoadOption.CHANGELANERIGHT: vehicle_state, vehicle, distance = self._vehicle_obstacle_detected( vehicle_list, max( - self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=1) + self.config.distance.min_proximity_threshold, self.config.live_info.current_speed_limit / 2), up_angle_th=180, lane_offset=1) else: vehicle_state, vehicle, distance = self._vehicle_obstacle_detected( vehicle_list, max( - self._behavior.min_proximity_threshold, self._speed_limit / 3), up_angle_th=30) + self.config.distance.min_proximity_threshold, self.config.live_info.current_speed_limit / 3), up_angle_th=30) # Check for tailgating if not vehicle_state and self._direction == RoadOption.LANEFOLLOW \ - and not waypoint.is_junction and self._speed > 10 \ - and self._behavior.tailgate_counter == 0: + and not waypoint.is_junction and self.config.live_info.current_speed > 10 \ + and self.config.tailgate_counter == 0: self._tailgating(waypoint, vehicle_list) return vehicle_state, vehicle, distance @@ -183,13 +187,13 @@ def dist(w): return w.get_location().distance(waypoint.transform.location) if self._direction == RoadOption.CHANGELANELEFT: walker_state, walker, distance = self._vehicle_obstacle_detected(walker_list, max( - self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=90, lane_offset=-1) + self.config.distance.min_proximity_threshold, self.config.live_info.current_speed_limit / 2), up_angle_th=90, lane_offset=-1) elif self._direction == RoadOption.CHANGELANERIGHT: walker_state, walker, distance = self._vehicle_obstacle_detected(walker_list, max( - self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=90, lane_offset=1) + self.config.distance.min_proximity_threshold, self.config.live_info.current_speed_limit / 2), up_angle_th=90, lane_offset=1) else: walker_state, walker, distance = self._vehicle_obstacle_detected(walker_list, max( - self._behavior.min_proximity_threshold, self._speed_limit / 3), up_angle_th=60) + self.config.distance.min_proximity_threshold, self.config.live_info.current_speed_limit / 3), up_angle_th=60) return walker_state, walker, distance @@ -207,32 +211,32 @@ def car_following_manager(self, vehicle, distance, debug=False): """ vehicle_speed = get_speed(vehicle) - delta_v = max(1, (self._speed - vehicle_speed) / 3.6) + delta_v = max(1, (self.config.live_info.current_speed - vehicle_speed) / 3.6) ttc = distance / delta_v if delta_v != 0 else distance / self._epsilon # Under safety time distance, slow down. - if self._behavior.safety_time > ttc > 0.0: + if self.config.speed.safety_time > ttc > 0.0: target_speed = min([ - positive(vehicle_speed - self._behavior.speed_decrease), - self._behavior.max_speed, - self._speed_limit - self._behavior.speed_lim_dist]) + positive(vehicle_speed - self.config.speed.speed_decrease), + self.config.speed.max_speed, + self.config.live_info.current_speed_limit - self.config.speed.speed_lim_dist]) self._local_planner.set_speed(target_speed) control = self._local_planner.run_step(debug=debug) # Actual safety distance area, try to follow the speed of the vehicle in front. - elif 2 * self._behavior.safety_time > ttc >= self._behavior.safety_time: + elif 2 * self.config.speed.safety_time > ttc >= self.config.speed.safety_time: target_speed = min([ - max(self._min_speed, vehicle_speed), - self._behavior.max_speed, - self._speed_limit - self._behavior.speed_lim_dist]) + max(self.config.speed.min_speed, vehicle_speed), + self.config.speed.max_speed, + self.config.live_info.current_speed_limit - self.config.speed.speed_lim_dist]) self._local_planner.set_speed(target_speed) control = self._local_planner.run_step(debug=debug) # Normal behavior. else: target_speed = min([ - self._behavior.max_speed, - self._speed_limit - self._behavior.speed_lim_dist]) + self.config.speed.max_speed, + self.config.live_info.current_speed_limit - self.config.speed.speed_lim_dist]) self._local_planner.set_speed(target_speed) control = self._local_planner.run_step(debug=debug) @@ -248,8 +252,8 @@ def run_step(self, debug=False): self._update_information() control = None - if self._behavior.tailgate_counter > 0: - self._behavior.tailgate_counter -= 1 + if self.config.tailgate_counter > 0: + self.config.tailgate_counter -= 1 ego_vehicle_loc = self._vehicle.get_location() ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc) @@ -269,7 +273,7 @@ def run_step(self, debug=False): self._vehicle.bounding_box.extent.y, self._vehicle.bounding_box.extent.x) # Emergency brake if the car is very close. - if distance < self._behavior.braking_distance: + if distance < self.config.distance.braking_distance: return self.emergency_stop() # 2.2: Car following behaviors @@ -283,7 +287,7 @@ def run_step(self, debug=False): self._vehicle.bounding_box.extent.y, self._vehicle.bounding_box.extent.x) # Emergency brake if the car is very close. - if distance < self._behavior.braking_distance: + if distance < self.config.distance.braking_distance: return self.emergency_stop() else: control = self.car_following_manager(vehicle, distance) @@ -291,16 +295,16 @@ def run_step(self, debug=False): # 3: Intersection behavior elif self._incoming_waypoint.is_junction and (self._incoming_direction in [RoadOption.LEFT, RoadOption.RIGHT]): target_speed = min([ - self._behavior.max_speed, - self._speed_limit - 5]) + self.config.speed.max_speed, + self.config.live_info.current_speed_limit - 5]) self._local_planner.set_speed(target_speed) control = self._local_planner.run_step(debug=debug) # 4: Normal behavior else: target_speed = min([ - self._behavior.max_speed, - self._speed_limit - self._behavior.speed_lim_dist]) + self.config.speed.max_speed, + self.config.live_info.current_speed_limit - self.config.speed.speed_lim_dist]) self._local_planner.set_speed(target_speed) control = self._local_planner.run_step(debug=debug) @@ -315,6 +319,6 @@ def emergency_stop(self): """ control = carla.VehicleControl() control.throttle = 0.0 - control.brake = self._max_brake + control.brake = self.config.controls.max_brake control.hand_brake = False return control From 7097272fc7858c4341297f79311e006fb8506ef7 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 11:44:07 +0100 Subject: [PATCH 08/32] Behavior agent settings to Normal Template --- .../carla/agents/conf/agent_settings_backend.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/PythonAPI/carla/agents/conf/agent_settings_backend.py b/PythonAPI/carla/agents/conf/agent_settings_backend.py index a0482888b9..a8f6b8226b 100644 --- a/PythonAPI/carla/agents/conf/agent_settings_backend.py +++ b/PythonAPI/carla/agents/conf/agent_settings_backend.py @@ -67,6 +67,7 @@ def __post_init__(self): self._clean_options() if self.overwrites is None: return + # Merge the overwrite dict into the correct ones. for key, value in self.overwrites.items(): if key in self.__annotations__: if issubclass(self.__annotations__[key], AgentConfig): @@ -84,8 +85,8 @@ def __post_init__(self): class LiveInfo(AgentConfig): current_speed : float = MISSING current_speed_limit : float = MISSING - velocity_vector : "carla.Vector3D" = MISSING direction : RoadOption = MISSING + velocity_vector : "carla.Vector3D" = MISSING # NOTE: Not ported to OmegaConf @property @@ -99,8 +100,7 @@ def speed_limit(self): # --------------------- # Speed # --------------------- - - + @dataclass class BasicAgentSpeedSettings(AgentConfig): target_speed: float = 20 @@ -135,7 +135,7 @@ class BehaviorAgentSpeedSettings(BasicAgentSpeedSettings): From normal behavior. This supersedes the target_speed when following the BehaviorAgent logic.""" # CASE A - speed_decrease : float = 12 + speed_decrease : float = 10 """other_vehicle_speed""" safety_time : float = 3 @@ -146,7 +146,7 @@ class BehaviorAgentSpeedSettings(BasicAgentSpeedSettings): """Implement als variable, currently hard_coded""" # All Cases - speed_lim_dist : float = 6 + speed_lim_dist : float = 3 """ Difference to speed limit. NOTE: For negative values the car drives above speed limit @@ -201,10 +201,10 @@ class BehaviorAgentDistanceSettings(BasicAgentDistanceSettings): automatic_proximity_threshold = {RoadOption.CHANGELANELEFT: 2, "same_lane" : 3, "right_lane" : 2} """ - min_proximity_threshold : float = 12 + min_proximity_threshold : float = 10 """Range in which cars are detected. NOTE: Speed limit overwrites""" - braking_distance : float = 6 + braking_distance : float = 5 """Emergency Stop Distance Trigger""" From 0dee3c9cce49b1a0ff9159ebc37519b8d24ad3d7 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 11:56:30 +0100 Subject: [PATCH 09/32] Using opt_dict to overwrite behavior agent settings. also cleaner initialization --- .../carla/agents/navigation/behavior_agent.py | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py index 45b12cddd4..b848bbc215 100644 --- a/PythonAPI/carla/agents/navigation/behavior_agent.py +++ b/PythonAPI/carla/agents/navigation/behavior_agent.py @@ -31,7 +31,7 @@ class BehaviorAgent(BasicAgent): are encoded in the agent, from cautious to a more aggressive ones. """ - def __init__(self, vehicle, behavior='normal', opt_dict=BehaviorAgentSettings(), map_inst=None, grp_inst=None): + def __init__(self, vehicle, behavior='normal', opt_dict={}, map_inst=None, grp_inst=None): """ Constructor method. @@ -39,30 +39,31 @@ def __init__(self, vehicle, behavior='normal', opt_dict=BehaviorAgentSettings(), :param behavior: type of agent to apply """ - super().__init__(vehicle, opt_dict=opt_dict, map_inst=map_inst, grp_inst=grp_inst) self._look_ahead_steps = 0 - # Vehicle information - self.config = None - # Parameters for agent behavior if isinstance(behavior, str): if behavior == 'cautious': - self.config = Cautious() + self.config = Cautious(overwrites=opt_dict) elif behavior == 'normal': - self.config = Normal() + self.config = Normal(overwrites=opt_dict) elif behavior == 'aggressive': - self.config = Aggressive() + self.config = Aggressive(overwrites=opt_dict) elif isinstance(behavior, AgentConfig): self.config = behavior + if opt_dict: + print("Warning: opt_dict is ignored when behavior is an instance of AgentConfig. Initialize the settings with the updates parameters beforehand.") # TODO: maybe can call update. elif isinstance(behavior, type) and issubclass(behavior, (AgentConfig, SimpleConfig)): - self.config = behavior() + self.config = behavior(opt_dict) else: - raise TypeError("Behavior must be a string or a subclass of AgentConfig or a SimpleConfig") - if not hasattr(self.config, "tailgate_counter"): # TODO: maybe use an attribute here instead, or choose another option. + raise TypeError("Behavior must be a string or a subclass of AgentConfig or a SimpleConfig") # TODO: maybe raise a warning instead and assume user passed a fitting structure. + + if not hasattr(self.config, 'tailgate_counter'): # TODO: maybe use an attribute here instead, or choose another option. self.config.tailgate_counter = 0 + + super().__init__(vehicle, opt_dict=self.config, map_inst=map_inst, grp_inst=grp_inst) def _update_information(self): From 0916baae22aec5225215587e34fd4c5219502ced Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 11:57:43 +0100 Subject: [PATCH 10/32] Added type-hints --- PythonAPI/carla/agents/navigation/basic_agent.py | 5 ++--- .../carla/agents/navigation/behavior_agent.py | 4 +++- PythonAPI/carla/agents/navigation/controller.py | 8 ++++++-- .../carla/agents/navigation/local_planner.py | 15 +++++++++------ 4 files changed, 20 insertions(+), 12 deletions(-) diff --git a/PythonAPI/carla/agents/navigation/basic_agent.py b/PythonAPI/carla/agents/navigation/basic_agent.py index 7b0d93c215..a786792385 100644 --- a/PythonAPI/carla/agents/navigation/basic_agent.py +++ b/PythonAPI/carla/agents/navigation/basic_agent.py @@ -72,7 +72,7 @@ def __init__(self, vehicle, opt_dict=BasicAgentSettings(), map_inst=None, grp_in # Dictionary mapping a traffic light to a Waypoint corresponding to its trigger volume location self._lights_map: "dict[int, carla.Waypoint]" = {} - def add_emergency_stop(self, control): + def add_emergency_stop(self, control: carla.VehicleControl): """ Overwrites the throttle a brake values of a control to perform an emergency stop. The steering is kept the same to avoid going out of the lane when stopping during turns @@ -124,7 +124,6 @@ def set_destination(self, end_location, start_location=None, clean_queue=True): :param start_location (carla.Location): starting location of the route :param clean_queue (bool): Whether to clear or append to the currently planned route """ - #TODO: This needs an overhaul to be done. Currently start_location is not used if not start_location: if clean_queue and self._local_planner.target_waypoint: # Plan from the waypoint in front of the vehicle onwards @@ -155,7 +154,7 @@ def set_global_plan(self, plan, stop_waypoint_creation=True, clean_queue=True): clean_queue=clean_queue ) - def trace_route(self, start_waypoint, end_waypoint): + def trace_route(self, start_waypoint: carla.Waypoint, end_waypoint: carla.Waypoint): """ Calculates the shortest route between a starting and ending waypoint. diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py index b848bbc215..0f6f9f34b7 100644 --- a/PythonAPI/carla/agents/navigation/behavior_agent.py +++ b/PythonAPI/carla/agents/navigation/behavior_agent.py @@ -9,6 +9,7 @@ traffic signs, and has different possible configurations. """ import random +from typing import Union import numpy as np import carla from agents.navigation.basic_agent import BasicAgent @@ -30,8 +31,9 @@ class BehaviorAgent(BasicAgent): and keeping it in a certain range. Finally, different sets of behaviors are encoded in the agent, from cautious to a more aggressive ones. """ + config : BehaviorAgentSettings - def __init__(self, vehicle, behavior='normal', opt_dict={}, map_inst=None, grp_inst=None): + def __init__(self, vehicle, behavior: Union[str, AgentConfig]='normal', opt_dict={}, map_inst=None, grp_inst=None): """ Constructor method. diff --git a/PythonAPI/carla/agents/navigation/controller.py b/PythonAPI/carla/agents/navigation/controller.py index 8b0ef8761a..b9760f4bfe 100644 --- a/PythonAPI/carla/agents/navigation/controller.py +++ b/PythonAPI/carla/agents/navigation/controller.py @@ -11,6 +11,10 @@ import carla from agents.tools.misc import get_speed +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from PythonAPI.carla.agents.conf.agent_settings_backend import BasicAgentSettings + STEERING_UPDATE_SPEED = 0.1 ERROR_BUFFER_LENGTH = 10 @@ -186,7 +190,7 @@ def __init__(self, vehicle, config): self.config = config self._e_buffer = deque(maxlen=ERROR_BUFFER_LENGTH) - def run_step(self, waypoint): + def run_step(self, waypoint: carla.Waypoint): """ Execute one step of lateral control to steer the vehicle towards a certain waypoint. @@ -202,7 +206,7 @@ def set_offset(self, offset: float): """Changes the offset""" self.config.planner.offset = offset - def _pid_control(self, waypoint, vehicle_transform): + def _pid_control(self, waypoint: carla.Waypoint, vehicle_transform: carla.Transform): """ Estimate the steering angle of the vehicle based on the PID equations diff --git a/PythonAPI/carla/agents/navigation/local_planner.py b/PythonAPI/carla/agents/navigation/local_planner.py index df71dbf75a..bd2a406669 100644 --- a/PythonAPI/carla/agents/navigation/local_planner.py +++ b/PythonAPI/carla/agents/navigation/local_planner.py @@ -8,11 +8,14 @@ from enum import IntEnum from collections import deque import random +from typing import TYPE_CHECKING import carla from agents.navigation.controller import VehiclePIDController from agents.tools.misc import draw_waypoints, get_speed +if TYPE_CHECKING: + from agents.conf.agent_settings_backend import BasicAgentSettings class RoadOption(IntEnum): """ @@ -39,8 +42,8 @@ class LocalPlanner(object): 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={}, map_inst=None): + + def __init__(self, vehicle: carla.Actor, opt_dict: "BasicAgentSettings", map_inst: carla.Map = None): """ :param vehicle: actor to apply to local planner logic onto :param opt_dict: dictionary of arguments with different parameters: @@ -56,7 +59,7 @@ def __init__(self, vehicle, opt_dict={}, map_inst=None): :param map_inst: carla.Map instance to avoid the expensive call of getting it. """ self._vehicle = vehicle - self._world = self._vehicle.get_world() + self._world : carla.World = self._vehicle.get_world() if map_inst: if isinstance(map_inst, carla.Map): self._map = map_inst @@ -67,10 +70,10 @@ def __init__(self, vehicle, opt_dict={}, map_inst=None): self._map = self._world.get_map() self._vehicle_controller = None - self.target_waypoint = None - self.target_road_option = None + self.target_waypoint: carla.Waypoint = None + self.target_road_option: RoadOption = None - self._waypoints_queue = deque(maxlen=10000) + self._waypoints_queue: "deque[tuple[carla.Waypoint, RoadOption]]" = deque(maxlen=10000) self._min_waypoint_queue_length = 100 self._stop_waypoint_creation = False From 8817144d0eadf609f4c82b127421e8acfa4d96ac Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 11:58:07 +0100 Subject: [PATCH 11/32] Spelling corrections --- PythonAPI/carla/agents/navigation/basic_agent.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/PythonAPI/carla/agents/navigation/basic_agent.py b/PythonAPI/carla/agents/navigation/basic_agent.py index a786792385..57aa9e4011 100644 --- a/PythonAPI/carla/agents/navigation/basic_agent.py +++ b/PythonAPI/carla/agents/navigation/basic_agent.py @@ -6,7 +6,7 @@ """ This module implements an agent that roams around a track following random waypoints and avoiding other vehicles. The agent also responds to traffic lights. -It can also make use of the global route planner to follow a specifed route +It can also make use of the global route planner to follow a specified route """ import carla @@ -32,7 +32,7 @@ class BasicAgent(object): def __init__(self, vehicle, opt_dict=BasicAgentSettings(), map_inst=None, grp_inst=None): """ - Initialization the agent paramters, the local and the global planner. + Initialization the agent parameters, the local and the global planner. :param vehicle: actor to apply to agent logic onto :param target_speed: speed (in Km/h) at which the vehicle will move @@ -296,7 +296,7 @@ def _vehicle_obstacle_detected(self, vehicle_list=None, max_distance=None, up_an """ Method to check if there is a vehicle in front of the agent blocking its path. - :param vehicle_list (list of carla.Vehicle): list contatining vehicle objects. + :param vehicle_list (list of carla.Vehicle): list containing vehicle objects. If None, all vehicle in the scene are used :param max_distance: max freespace to check for obstacles. If None, the base threshold value is used From d05d012247e6716015b562bbd18bbd99c171fc1d Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 12:14:12 +0100 Subject: [PATCH 12/32] Unmatched keys are now a warning and not an error anymore. --- PythonAPI/carla/agents/conf/agent_settings_backend.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/PythonAPI/carla/agents/conf/agent_settings_backend.py b/PythonAPI/carla/agents/conf/agent_settings_backend.py index a8f6b8226b..c4ca1f5d57 100644 --- a/PythonAPI/carla/agents/conf/agent_settings_backend.py +++ b/PythonAPI/carla/agents/conf/agent_settings_backend.py @@ -608,8 +608,10 @@ def to_nested_config(self, simple_overwrites:dict=None) -> AgentConfig: overwrites[name] = {k: getattr(self, k) for k in matching} keys -= matching if len(keys) != 0: + overwrites.update({k: v for k,v in simple_overwrites.items() if k in keys}) # TODO: turn into a warning and find way to include these keys in the config anyway! - raise ValueError(f"Unmatched keys {keys} in {self.__class__.__name__} not contained in base {self._base_settings.__name__}") + #raise ValueError(f"Unmatched keys {keys} in {self.__class__.__name__} not contained in base {self._base_settings.__name__}") + print("Warning: Unmatched keys", keys, "in", self.__class__.__name__, "not contained in base", self._base_settings.__name__) # TODO: remove later, left here for testing. return self._base_settings(overwrites=overwrites) # TODO: could add new keys after post-processing. From df1b2fd71dff07193704d6b15ae967c6f4d35974 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 12:25:42 +0100 Subject: [PATCH 13/32] Basic Agent first parameter allows overwrite of settings Using the SimpleConfig interface --- PythonAPI/carla/agents/navigation/basic_agent.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/PythonAPI/carla/agents/navigation/basic_agent.py b/PythonAPI/carla/agents/navigation/basic_agent.py index 57aa9e4011..c939a2c4ae 100644 --- a/PythonAPI/carla/agents/navigation/basic_agent.py +++ b/PythonAPI/carla/agents/navigation/basic_agent.py @@ -17,7 +17,7 @@ from agents.tools.misc import (get_speed, is_within_distance, get_trafficlight_trigger_location, compute_distance) -from agents.conf.agent_settings_backend import BasicAgentSettings +from agents.conf.agent_settings_backend import BasicAgentSettings, SimpleBasicAgentSettings from agents.tools.hints import ObstacleDetectionResult, TrafficLightDetectionResult @@ -54,6 +54,9 @@ def __init__(self, vehicle, opt_dict=BasicAgentSettings(), map_inst=None, grp_in self._map = self._world.get_map() self._last_traffic_light = None + if isinstance(opt_dict, dict): + opt_dict = SimpleBasicAgentSettings(overwrites=opt_dict) + self.config = opt_dict # Initialize the planners From df428041406d921dc111b2d629658ebac53094a0 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 13:30:46 +0100 Subject: [PATCH 14/32] Constant Velocity Agent to new settings --- .../navigation/constant_velocity_agent.py | 25 +++++++------------ 1 file changed, 9 insertions(+), 16 deletions(-) diff --git a/PythonAPI/carla/agents/navigation/constant_velocity_agent.py b/PythonAPI/carla/agents/navigation/constant_velocity_agent.py index 8d3b1c5aa8..2b47a8a995 100644 --- a/PythonAPI/carla/agents/navigation/constant_velocity_agent.py +++ b/PythonAPI/carla/agents/navigation/constant_velocity_agent.py @@ -12,6 +12,7 @@ import carla from agents.navigation.basic_agent import BasicAgent +from agents.conf.agent_settings_backend import BasicAgentSettings class ConstantVelocityAgent(BasicAgent): """ @@ -21,22 +22,19 @@ class ConstantVelocityAgent(BasicAgent): wait for a bit, and then start again. """ - def __init__(self, vehicle, target_speed=20, opt_dict={}, map_inst=None, grp_inst=None): + def __init__(self, vehicle, opt_dict=BasicAgentSettings(), map_inst=None, grp_inst=None): """ Initialization the agent parameters, the local and the global planner. :param vehicle: actor to apply to agent logic onto - :param target_speed: speed (in Km/h) at which the vehicle will move :param opt_dict: dictionary in case some of its parameters want to be changed. This also applies to parameters related to the LocalPlanner. :param map_inst: carla.Map instance to avoid the expensive call of getting it. :param grp_inst: GlobalRoutePlanner instance to avoid the expensive call of getting it. """ - super().__init__(vehicle, target_speed, opt_dict=opt_dict, map_inst=map_inst, grp_inst=grp_inst) + super().__init__(vehicle, opt_dict=opt_dict, map_inst=map_inst, grp_inst=grp_inst) self._use_basic_behavior = False # Whether or not to use the BasicAgent behavior when the constant velocity is down - self._target_speed = target_speed / 3.6 # [m/s] - self._current_speed = vehicle.get_velocity().length() # [m/s] self._constant_velocity_stop_time = None self._collision_sensor = None @@ -49,12 +47,7 @@ def __init__(self, vehicle, target_speed=20, opt_dict={}, map_inst=None, grp_ins self.is_constant_velocity_active = True self._set_collision_sensor() - self._set_constant_velocity(target_speed) - - def set_target_speed(self, speed): - """Changes the target speed of the agent [km/h]""" - self._target_speed = speed / 3.6 - self._local_planner.set_speed(speed) + self._set_constant_velocity(self.config.speed.target_speed / 3.6) def stop_constant_velocity(self): """Stops the constant velocity behavior""" @@ -65,7 +58,7 @@ def stop_constant_velocity(self): def restart_constant_velocity(self): """Public method to restart the constant velocity""" self.is_constant_velocity_active = True - self._set_constant_velocity(self._target_speed) + self._set_constant_velocity(self.config.speed.target_speed / 3.6) def _set_constant_velocity(self, speed): """Forces the agent to drive at the specified speed""" @@ -91,7 +84,7 @@ def run_step(self): vehicle_speed = self._vehicle.get_velocity().length() - max_vehicle_distance = self._base_vehicle_threshold + vehicle_speed + max_vehicle_distance = self.config.obstacles.base_vehicle_threshold + vehicle_speed affected_by_vehicle, adversary, _ = self._vehicle_obstacle_detected(vehicle_list, max_vehicle_distance) if affected_by_vehicle: vehicle_velocity = self._vehicle.get_velocity() @@ -102,7 +95,7 @@ def run_step(self): hazard_detected = True # Check if the vehicle is affected by a red traffic light - max_tlight_distance = self._base_tlight_threshold + 0.3 * vehicle_speed + max_tlight_distance = self.config.obstacles.base_tlight_threshold + self.config.distance.distance_ratio * vehicle_speed affected_by_tlight, _ = self._affected_by_traffic_light(lights_list, max_tlight_distance) if affected_by_tlight: hazard_speed = 0 @@ -112,9 +105,9 @@ def run_step(self): # still useful to apply it so that the vehicle isn't moving with static wheels control = self._local_planner.run_step() if hazard_detected: - self._set_constant_velocity(hazard_speed) + self._set_constant_velocity(hazard_speed / 3.6) else: - self._set_constant_velocity(self._target_speed) + self._set_constant_velocity(self.config.speed.target_speed / 3.6) return control From 4997a29be51a702ce19fcfb6838aa5bc3417ede5 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 13:33:27 +0100 Subject: [PATCH 15/32] Examples compatible with new AgentSettings --- PythonAPI/examples/automatic_control.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PythonAPI/examples/automatic_control.py b/PythonAPI/examples/automatic_control.py index 330d9afe43..ca75e1dc9d 100755 --- a/PythonAPI/examples/automatic_control.py +++ b/PythonAPI/examples/automatic_control.py @@ -738,10 +738,10 @@ def game_loop(args): world = World(client.get_world(), hud, args) controller = KeyboardControl(world) if args.agent == "Basic": - agent = BasicAgent(world.player, 30) + agent = BasicAgent(world.player, {'target_speed':30}) agent.follow_speed_limits(True) elif args.agent == "Constant": - agent = ConstantVelocityAgent(world.player, 30) + agent = ConstantVelocityAgent(world.player, {'target_speed':30}) ground_loc = world.world.ground_projection(world.player.get_location(), 5) if ground_loc: world.player.set_location(ground_loc.location + carla.Location(z=0.01)) From 25f70ad4b23626fb049a8331110348923baae70b Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 13:38:29 +0100 Subject: [PATCH 16/32] Added __all__ & Small improvements. --- .../agents/conf/agent_settings_backend.py | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/PythonAPI/carla/agents/conf/agent_settings_backend.py b/PythonAPI/carla/agents/conf/agent_settings_backend.py index c4ca1f5d57..855de2b72e 100644 --- a/PythonAPI/carla/agents/conf/agent_settings_backend.py +++ b/PythonAPI/carla/agents/conf/agent_settings_backend.py @@ -7,9 +7,15 @@ import carla from agents.navigation.local_planner import RoadOption -MISSING = "???" +__all__ = ["AgentConfig", + "SimpleConfig", + "BasicAgentSettings", + "BehaviorAgentSettings", + "SimpleBasicAgentSettings", + "SimpleBehaviorAgentSettings" + ] -# TODO: Behaviour _tailgate_counter is not implemented in the settings. +MISSING = "???" # --------------------- # Helper methods @@ -22,14 +28,14 @@ def __init__(self, call): self.__wrapped__ = call self._wrapper = lambda x : x # TODO: functools.partial and functools.wraps shadow the signature, this reveals it again. - def __get__(self, instance : Union[None,"AgentConfig"], owner : Type["AgentConfig"]): + def __get__(self, instance : Union[None, "AgentConfig"], owner : Type["AgentConfig"]): if instance is None: # called on class return self._wrapper(partial(self.__wrapped__, owner)) return self._wrapper(partial(self.__wrapped__, instance)) # called on instance # --------------------- -# Configs +# Base Classes # --------------------- class AgentConfig: @@ -75,7 +81,7 @@ def __post_init__(self): else: setattr(self, key, value) else: - print(f"Key {key} not found in {self.__class__.__name__} options.") + print(f"Warning: Key '{key}' not found in {self.__class__.__name__} default options. Consider updating or creating a new class to avoid this message.") # --------------------- # Live Info @@ -616,6 +622,10 @@ def to_nested_config(self, simple_overwrites:dict=None) -> AgentConfig: # TODO: could add new keys after post-processing. +# --------------------- +# Final Settings +# --------------------- + @dataclass class BasicAgentSettings(AgentConfig): overwrites : Optional[Dict[str, dict]] = field(default_factory=dict, repr=False) # type: Optional[Dict[str, Union[dict|AgentConfig]]] From d01d6672e11fb60d3a7d924d941c851b31788c33 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 14:07:55 +0100 Subject: [PATCH 17/32] SimpleConfig warns if duplicated key is encoutnered. --- .../agents/conf/agent_settings_backend.py | 126 ++++++++++-------- 1 file changed, 67 insertions(+), 59 deletions(-) diff --git a/PythonAPI/carla/agents/conf/agent_settings_backend.py b/PythonAPI/carla/agents/conf/agent_settings_backend.py index 855de2b72e..ea05537bce 100644 --- a/PythonAPI/carla/agents/conf/agent_settings_backend.py +++ b/PythonAPI/carla/agents/conf/agent_settings_backend.py @@ -83,6 +83,71 @@ def __post_init__(self): else: print(f"Warning: Key '{key}' not found in {self.__class__.__name__} default options. Consider updating or creating a new class to avoid this message.") + + +class SimpleConfig(object): + """ + A class that allows a more simple way to initialize settings. + Initializing an instance changes the type the the given base class, defined via `_base_settings`. + + :param _base_settings: The base class to port the settings to. + + TODO: NOTE: This class assumes that there are NO DUPLICATE keys in the underlying base + """ + + _base_settings: ClassVar["AgentConfig"] = None + + def __new__(cls, overwrites:Optional[Dict[str, Union[dict, AgentConfig]]]=None) -> "AgentConfig": + """ + Transforms a SimpleConfig class into the given _base_settings + + :param overwrites: That allow overwrites during initialization. + """ + if cls._base_settings is None: + raise TypeError("{cls.__name__} must have a class set in the `_base_settings` to initialize to.") + if cls is SimpleConfig: + raise TypeError("SimpleConfig class may not be instantiated") + simple_settings = {k:v for k,v in cls.__dict__.items() if not k.startswith("_") } # Removes all private attributes + if overwrites: + simple_settings.update(overwrites) + return super().__new__(cls).to_nested_config(simple_settings) # call from a pseudo instance. + + @class_or_instance_method + def to_nested_config(self, simple_overwrites:dict=None) -> AgentConfig: + """ + Initializes the _base_settings with the given overwrites. + + Maps the keys of simple_overwrites to the base settings. + + More specifically builds a overwrites dict that is compatible with the nested configuration versions. + + NOTE: Assumes unique keys over all settings! + # TODO: add a warning if a non-unique key is found in the overwrites. + """ + if isinstance(self, type): # called on class + return self() + keys = set(simple_overwrites.keys()) + removed_keys = set() # to check for duplicated keys that cannot be set via SimpleConfig unambiguously + overwrites = {} + for name, base in self._base_settings.__annotations__.items(): + if not isinstance(base, type) or not issubclass(base, AgentConfig): # First out non AgentConfig attributes + continue + matching = keys.intersection(base.__dataclass_fields__.keys()) # keys that match from the simple config to the real nested config + if len(matching) > 0: + if removed_keys.intersection(matching): + print("WARNING: Ambiguous key", removed_keys.intersection(matching), "in the SimpleConfig", str(self), "that occurs multiple times in its given base", self._base_settings.__name__+".", "Encountered at", name, base) # TODO: remove later, left here for testing.") + overwrites[name] = {k: getattr(self, k) for k in matching} + keys -= matching + removed_keys.update(matching) + if len(keys) != 0: + overwrites.update({k: v for k,v in simple_overwrites.items() if k in keys}) + print("Warning: Unmatched keys", keys, "in", self.__class__.__name__, "not contained in base", self._base_settings.__name__) # TODO: remove later, left here for testing. + return self._base_settings(overwrites=overwrites) + # TODO: could add new keys after post-processing. + + +# --------------------- + # --------------------- # Live Info # --------------------- @@ -561,65 +626,7 @@ class AutopilotBehavior(AgentConfig): update_vehicle_lights : bool = False """Sets if the Traffic Manager is responsible of updating the vehicle lights, or not.""" - -class SimpleConfig(object): - """ - A class that allows a more simple way to initialize settings and port them to a specified - base class. - - :param _base_settings: The base class to port the settings to. - - TODO: NOTE: This class assumes that there are NO DUPLICATE keys in the underlying base - """ - - _base_settings: ClassVar["AgentConfig"] = None - - def __new__(cls, overwrites:Optional[Dict[str, Union[dict, AgentConfig]]]=None) -> "AgentConfig": - """ - Transforms a SimpleConfig class into the given _base_settings - - :param overwrites: That allow overwrites during initialization. - """ - if cls is SimpleConfig: - raise TypeError("SimpleConfig class may not be instantiated") - if cls._base_settings is None: - raise TypeError("{cls.__name__} must have a class set in the `_base_settings` to initialize to.") - the_inst = super().__new__(cls) - simple_settings = {k:v for k,v in cls.__dict__.items() if not k.startswith("_") } # Removes all private attributes - if overwrites: - simple_settings.update(overwrites) - return the_inst.to_nested_config(simple_settings) - - @class_or_instance_method - def to_nested_config(self, simple_overwrites:dict=None) -> AgentConfig: - """ - Initializes the _base_settings with the given overwrites. - - Maps the keys of simple_overwrites to the base settings. - - NOTE: Assumes unique keys over all settings! - # TODO: add a warning if a non-unique key is found in the overwrites. - """ - if isinstance(self, type): # called on class - return self() - keys = set(simple_overwrites.keys()) - overwrites = {} - for name, base in self._base_settings.__annotations__.items(): - if not isinstance(base, type) or not issubclass(base, AgentConfig): # First out non AgentConfig attributes - continue - matching = keys.intersection(base.__dataclass_fields__.keys()) - print(str(base.__class__.__name__), "matches in", matching) - insert = {k: getattr(self, k) for k in matching} - if len(insert) > 0: - overwrites[name] = {k: getattr(self, k) for k in matching} - keys -= matching - if len(keys) != 0: - overwrites.update({k: v for k,v in simple_overwrites.items() if k in keys}) - # TODO: turn into a warning and find way to include these keys in the config anyway! - #raise ValueError(f"Unmatched keys {keys} in {self.__class__.__name__} not contained in base {self._base_settings.__name__}") - print("Warning: Unmatched keys", keys, "in", self.__class__.__name__, "not contained in base", self._base_settings.__name__) # TODO: remove later, left here for testing. - return self._base_settings(overwrites=overwrites) - # TODO: could add new keys after post-processing. + # --------------------- @@ -649,6 +656,7 @@ class BehaviorAgentSettings(AgentConfig): controls : BehaviorAgentControllerSettings = field(default_factory=BehaviorAgentControllerSettings, init=False) planner : BehaviorAgentPlannerSettings = field(default_factory=BehaviorAgentPlannerSettings, init=False) emergency : BehaviorAgentEmergencySettings = field(default_factory=BehaviorAgentEmergencySettings, init=False) + avoid_tailgators : bool = True @dataclass class SimpleBasicAgentSettings(SimpleConfig, LiveInfo, BasicAgentSpeedSettings, BasicAgentDistanceSettings, BasicAgentLaneChangeSettings, BasicAgentObstacleSettings, BasicAgentControllerSettings, BasicAgentPlannerSettings, BasicAgentEmergencySettings): From c82f463ec1acea24525bbf39dc06c1b4e78f7b43 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 14:09:58 +0100 Subject: [PATCH 18/32] BehaviourAgent make tailgate counter a more clear option and boolean. --- PythonAPI/carla/agents/conf/behavior_types.py | 6 +++--- .../carla/agents/navigation/behavior_agent.py | 16 +++++++++------- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/PythonAPI/carla/agents/conf/behavior_types.py b/PythonAPI/carla/agents/conf/behavior_types.py index 3b666326a6..94f3d211be 100644 --- a/PythonAPI/carla/agents/conf/behavior_types.py +++ b/PythonAPI/carla/agents/conf/behavior_types.py @@ -15,7 +15,7 @@ class Cautious(SimpleBehaviorAgentSettings): safety_time = 3 min_proximity_threshold = 12 braking_distance = 6 - tailgate_counter = 0 + avoid_tailgators = True @dataclass class Normal(SimpleBehaviorAgentSettings): @@ -26,7 +26,7 @@ class Normal(SimpleBehaviorAgentSettings): safety_time = 3 min_proximity_threshold = 10 braking_distance = 5 - tailgate_counter = 0 + avoid_tailgators = True @dataclass class Aggressive(SimpleBehaviorAgentSettings): @@ -37,4 +37,4 @@ class Aggressive(SimpleBehaviorAgentSettings): safety_time = 3 min_proximity_threshold = 8 braking_distance = 4 - tailgate_counter = -1 + avoid_tailgators = False diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py index 0f6f9f34b7..cfc1816734 100644 --- a/PythonAPI/carla/agents/navigation/behavior_agent.py +++ b/PythonAPI/carla/agents/navigation/behavior_agent.py @@ -62,8 +62,10 @@ def __init__(self, vehicle, behavior: Union[str, AgentConfig]='normal', opt_dict else: raise TypeError("Behavior must be a string or a subclass of AgentConfig or a SimpleConfig") # TODO: maybe raise a warning instead and assume user passed a fitting structure. - if not hasattr(self.config, 'tailgate_counter'): # TODO: maybe use an attribute here instead, or choose another option. - self.config.tailgate_counter = 0 + if self.config.avoid_tailgators: + self._tailgate_counter = 0 + else: + self._tailgate_counter = -1 # Does not trigger the == 0 condition. super().__init__(vehicle, opt_dict=self.config, map_inst=map_inst, grp_inst=grp_inst) @@ -122,7 +124,7 @@ def _tailgating(self, waypoint, vehicle_list): if not new_vehicle_state: print("Tailgating, moving to the right!") end_waypoint = self._local_planner.target_waypoint - self.config.tailgate_counter = 200 + self._tailgate_counter = 200 self.set_destination(end_waypoint.transform.location, right_wpt.transform.location) elif left_turn == carla.LaneChange.Left and waypoint.lane_id * left_wpt.lane_id > 0 and left_wpt.lane_type == carla.LaneType.Driving: @@ -131,7 +133,7 @@ def _tailgating(self, waypoint, vehicle_list): if not new_vehicle_state: print("Tailgating, moving to the left!") end_waypoint = self._local_planner.target_waypoint - self.config.tailgate_counter = 200 + self._tailgate_counter = 200 self.set_destination(end_waypoint.transform.location, left_wpt.transform.location) @@ -167,7 +169,7 @@ def dist(v): return v.get_location().distance(waypoint.transform.location) # Check for tailgating if not vehicle_state and self._direction == RoadOption.LANEFOLLOW \ and not waypoint.is_junction and self.config.live_info.current_speed > 10 \ - and self.config.tailgate_counter == 0: + and self._tailgate_counter == 0: self._tailgating(waypoint, vehicle_list) return vehicle_state, vehicle, distance @@ -255,8 +257,8 @@ def run_step(self, debug=False): self._update_information() control = None - if self.config.tailgate_counter > 0: - self.config.tailgate_counter -= 1 + if self._tailgate_counter > 0: + self._tailgate_counter -= 1 ego_vehicle_loc = self._vehicle.get_location() ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc) From 774bab745eca1938e7d290e2d3a99525024809ab Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 14:14:52 +0100 Subject: [PATCH 19/32] Changelog for AgentSettings --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index d743c9c7ef..3d9fa294d6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,7 @@ * PythonAPI `Sensor.is_listening` was defined twice (property and method), cleaned and clarified it as a method. * Added V2X sensors for cooperative awareness message and custom user-defined messages to support vehicle-to-vehicle communication * Added named tuples for BasicAgent.py's detection result to allow for type-hints and better semantics. + * Unified the settings of the Python agents to use a dictionary structure for most of their parameters which further allows more dynamic changes. ## CARLA 0.9.15 From 66e1bb08eef2d5c6b8333dc3d7aa611bf533c20b Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 14:51:23 +0100 Subject: [PATCH 20/32] Updated doc --- PythonAPI/carla/agents/conf/agent_settings_backend.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/PythonAPI/carla/agents/conf/agent_settings_backend.py b/PythonAPI/carla/agents/conf/agent_settings_backend.py index ea05537bce..e6759a59e6 100644 --- a/PythonAPI/carla/agents/conf/agent_settings_backend.py +++ b/PythonAPI/carla/agents/conf/agent_settings_backend.py @@ -39,6 +39,12 @@ def __get__(self, instance : Union[None, "AgentConfig"], owner : Type["AgentConf # --------------------- class AgentConfig: + """ + Base interface for the agent settings. + + Handling the initialization from a nested dataclass and merges in the changes + from the overwrites options. + """ overwrites: "Optional[Dict[str, Union[dict, AgentConfig]]]" = None @classmethod @@ -84,7 +90,6 @@ def __post_init__(self): print(f"Warning: Key '{key}' not found in {self.__class__.__name__} default options. Consider updating or creating a new class to avoid this message.") - class SimpleConfig(object): """ A class that allows a more simple way to initialize settings. From f08752fcd14f41839c92a17d2db76935fed4fe2a Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 15:08:40 +0100 Subject: [PATCH 21/32] Added top-level settings support --- PythonAPI/carla/agents/conf/agent_settings_backend.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/PythonAPI/carla/agents/conf/agent_settings_backend.py b/PythonAPI/carla/agents/conf/agent_settings_backend.py index e6759a59e6..2c1aa65221 100644 --- a/PythonAPI/carla/agents/conf/agent_settings_backend.py +++ b/PythonAPI/carla/agents/conf/agent_settings_backend.py @@ -136,6 +136,10 @@ def to_nested_config(self, simple_overwrites:dict=None) -> AgentConfig: overwrites = {} for name, base in self._base_settings.__annotations__.items(): if not isinstance(base, type) or not issubclass(base, AgentConfig): # First out non AgentConfig attributes + if name in keys: + overwrites[name] = simple_overwrites[name] # if updating a top level attribute + keys.remove(name) + removed_keys.add(name) continue matching = keys.intersection(base.__dataclass_fields__.keys()) # keys that match from the simple config to the real nested config if len(matching) > 0: @@ -145,8 +149,8 @@ def to_nested_config(self, simple_overwrites:dict=None) -> AgentConfig: keys -= matching removed_keys.update(matching) if len(keys) != 0: - overwrites.update({k: v for k,v in simple_overwrites.items() if k in keys}) - print("Warning: Unmatched keys", keys, "in", self.__class__.__name__, "not contained in base", self._base_settings.__name__) # TODO: remove later, left here for testing. + overwrites.update({k: v for k,v in simple_overwrites.items() if k in keys}) # Add them to the top level + print("Warning: Unmatched keys", keys, "in", self.__class__.__name__, "not contained in base", self._base_settings.__name__+".", "Adding them to the top-level of the settings.") # TODO: remove later, left here for testing. return self._base_settings(overwrites=overwrites) # TODO: could add new keys after post-processing. From aed097ce6446f21b4447a3aaae6c2623e8e395bb Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 17:18:00 +0100 Subject: [PATCH 22/32] Dynamic Traffic Light distance BasicAgent and the BehaviourAgent do differ here, this allows both options for both agents. fix constant velocity agent --- PythonAPI/carla/agents/conf/agent_settings_backend.py | 9 +++++++++ PythonAPI/carla/agents/navigation/basic_agent.py | 8 +++++++- PythonAPI/carla/agents/navigation/behavior_agent.py | 10 ++++++++-- .../carla/agents/navigation/constant_velocity_agent.py | 9 ++++++++- 4 files changed, 32 insertions(+), 4 deletions(-) diff --git a/PythonAPI/carla/agents/conf/agent_settings_backend.py b/PythonAPI/carla/agents/conf/agent_settings_backend.py index 2c1aa65221..90119eab6f 100644 --- a/PythonAPI/carla/agents/conf/agent_settings_backend.py +++ b/PythonAPI/carla/agents/conf/agent_settings_backend.py @@ -420,7 +420,16 @@ class BasicAgentObstacleSettings(AgentConfig): USAGE: max_tlight_distance = base_tlight_threshold + detection_speed_ratio * vehicle_speed """ + dynamic_threshold_by_speed : bool = True + """ + Whether to add a dynamic threshold based on the vehicle speed to the base threshold. + + Usage: base_threshold + dynamic_threshold_by_speed * vehicle_speed + """ + detection_angles : BasicAgentObstacleDetectionAngles = field(default_factory=BasicAgentObstacleDetectionAngles) + """Defines detection angles used when checking for obstacles.""" + @dataclass class BehaviorAgentObstacleSettings(BasicAgentObstacleSettings): diff --git a/PythonAPI/carla/agents/navigation/basic_agent.py b/PythonAPI/carla/agents/navigation/basic_agent.py index c939a2c4ae..9bdaef41d0 100644 --- a/PythonAPI/carla/agents/navigation/basic_agent.py +++ b/PythonAPI/carla/agents/navigation/basic_agent.py @@ -185,7 +185,13 @@ def run_step(self): hazard_detected = True # Check if the vehicle is affected by a red traffic light - max_tlight_distance = self.config.obstacles.base_tlight_threshold + self.config.obstacles.detection_speed_ratio * vehicle_speed + if self.config.obstacles.dynamic_threshold_by_speed: + # Basic agent setting: + max_tlight_distance = self.config.obstacles.base_tlight_threshold + self.config.obstacles.detection_speed_ratio * self.config.live_info.current_speed + else: + # Behavior setting: + max_tlight_distance = self.config.obstacles.base_tlight_threshold + affected_by_tlight, _ = self._affected_by_traffic_light(self._lights_list, max_tlight_distance) if affected_by_tlight: hazard_detected = True diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py index cfc1816734..5153b94259 100644 --- a/PythonAPI/carla/agents/navigation/behavior_agent.py +++ b/PythonAPI/carla/agents/navigation/behavior_agent.py @@ -95,7 +95,13 @@ def traffic_light_manager(self): """ actor_list = self._world.get_actors() lights_list = actor_list.filter("*traffic_light*") - affected, _ = self._affected_by_traffic_light(lights_list) + if self.config.obstacles.dynamic_threshold_by_speed: + # Basic agent setting: + max_tlight_distance = self.config.obstacles.base_tlight_threshold + self.config.obstacles.detection_speed_ratio * self.config.live_info.current_speed + else: + # Behavior setting: + max_tlight_distance = self.config.obstacles.base_tlight_threshold + affected, _ = self._affected_by_traffic_light(lights_list, max_distance=max_tlight_distance) return affected @@ -301,7 +307,7 @@ def run_step(self, debug=False): elif self._incoming_waypoint.is_junction and (self._incoming_direction in [RoadOption.LEFT, RoadOption.RIGHT]): target_speed = min([ self.config.speed.max_speed, - self.config.live_info.current_speed_limit - 5]) + self.config.live_info.current_speed_limit - self.config.speed.intersection_speed_decrease]) self._local_planner.set_speed(target_speed) control = self._local_planner.run_step(debug=debug) diff --git a/PythonAPI/carla/agents/navigation/constant_velocity_agent.py b/PythonAPI/carla/agents/navigation/constant_velocity_agent.py index 2b47a8a995..750dfb892a 100644 --- a/PythonAPI/carla/agents/navigation/constant_velocity_agent.py +++ b/PythonAPI/carla/agents/navigation/constant_velocity_agent.py @@ -83,6 +83,7 @@ def run_step(self): lights_list = actor_list.filter("*traffic_light*") vehicle_speed = self._vehicle.get_velocity().length() + self.config.live_info.current_speed = vehicle_speed max_vehicle_distance = self.config.obstacles.base_vehicle_threshold + vehicle_speed affected_by_vehicle, adversary, _ = self._vehicle_obstacle_detected(vehicle_list, max_vehicle_distance) @@ -94,8 +95,14 @@ def run_step(self): hazard_speed = vehicle_velocity.dot(adversary.get_velocity()) / vehicle_velocity.length() hazard_detected = True + # Check if the vehicle is affected by a red traffic light - max_tlight_distance = self.config.obstacles.base_tlight_threshold + self.config.distance.distance_ratio * vehicle_speed + if self.config.obstacles.dynamic_threshold_by_speed: + # Basic agent setting: + max_tlight_distance = self.config.obstacles.base_tlight_threshold + self.config.obstacles.detection_speed_ratio * vehicle_speed + else: + # Behavior setting: + max_tlight_distance = self.config.obstacles.base_tlight_threshold affected_by_tlight, _ = self._affected_by_traffic_light(lights_list, max_tlight_distance) if affected_by_tlight: hazard_speed = 0 From ca48a9197562790b2546d8715873c37852a4c451 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 16:41:05 +0100 Subject: [PATCH 23/32] Made close waypoint removal setting more clear and moved it to Planner settings --- .../agents/conf/agent_settings_backend.py | 25 ++++++++----------- .../carla/agents/navigation/local_planner.py | 2 +- 2 files changed, 12 insertions(+), 15 deletions(-) diff --git a/PythonAPI/carla/agents/conf/agent_settings_backend.py b/PythonAPI/carla/agents/conf/agent_settings_backend.py index 90119eab6f..e1efc15574 100644 --- a/PythonAPI/carla/agents/conf/agent_settings_backend.py +++ b/PythonAPI/carla/agents/conf/agent_settings_backend.py @@ -254,20 +254,7 @@ class AutopilotSpeedSettings(AgentConfig): @dataclass class BasicAgentDistanceSettings(AgentConfig): - """ - Calculation of the minimum distance for # XXX - min_distance = base_min_distance + distance_ratio * vehicle_speed - - see local_planner.py `run_step` - """ - - base_min_distance : float = 3.0 - """ - Base value of the distance to keep - """ - - distance_ratio : float = 0.5 - """Increases minimum distance multiplied by speed""" + pass @dataclass @@ -554,6 +541,16 @@ class BasicAgentPlannerSettings(AgentConfig): Used with the Waypoint.next(sampling_radius) and distance between waypoints. """ + + min_distance_next_waypoint : float = 3.0 + """ + Removes waypoints from the queue that are too close to the vehicle. + + Usage: min_distance = min_distance_next_waypoint + next_waypoint_distance_ratio * vehicle_speed + """ + + next_waypoint_distance_ratio : float = 0.5 + """Increases the minimum distance to the next waypoint based on the vehicles speed.""" @dataclass diff --git a/PythonAPI/carla/agents/navigation/local_planner.py b/PythonAPI/carla/agents/navigation/local_planner.py index bd2a406669..dc1961c4fd 100644 --- a/PythonAPI/carla/agents/navigation/local_planner.py +++ b/PythonAPI/carla/agents/navigation/local_planner.py @@ -197,7 +197,7 @@ def run_step(self, debug=False): veh_location = self._vehicle.get_location() self.config.live_info.current_speed = get_speed(self._vehicle) # km/h # Could be removed if we assume that it was updated beforehand by the agent. vehicle_speed = self.config.live_info.current_speed / 3.6 # km/h -> m/s - self._min_distance = self.config.distance.base_min_distance + self.config.distance.distance_ratio * vehicle_speed + self._min_distance = self.config.planner.min_distance_next_waypoint + self.config.planner.next_waypoint_distance_ratio * vehicle_speed num_waypoint_removed = 0 for waypoint, _ in self._waypoints_queue: From 6ddf6fc10201129b86fb9ab7c476ffafe260cd71 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 17:20:14 +0100 Subject: [PATCH 24/32] Renamed breaking_distance for more semantics --- PythonAPI/carla/agents/conf/agent_settings_backend.py | 2 +- PythonAPI/carla/agents/conf/behavior_types.py | 6 +++--- PythonAPI/carla/agents/navigation/behavior_agent.py | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/PythonAPI/carla/agents/conf/agent_settings_backend.py b/PythonAPI/carla/agents/conf/agent_settings_backend.py index e1efc15574..71ed7fdeda 100644 --- a/PythonAPI/carla/agents/conf/agent_settings_backend.py +++ b/PythonAPI/carla/agents/conf/agent_settings_backend.py @@ -271,7 +271,7 @@ class BehaviorAgentDistanceSettings(BasicAgentDistanceSettings): min_proximity_threshold : float = 10 """Range in which cars are detected. NOTE: Speed limit overwrites""" - braking_distance : float = 5 + emergency_braking_distance : float = 5 """Emergency Stop Distance Trigger""" diff --git a/PythonAPI/carla/agents/conf/behavior_types.py b/PythonAPI/carla/agents/conf/behavior_types.py index 94f3d211be..7d3faba887 100644 --- a/PythonAPI/carla/agents/conf/behavior_types.py +++ b/PythonAPI/carla/agents/conf/behavior_types.py @@ -14,7 +14,7 @@ class Cautious(SimpleBehaviorAgentSettings): speed_decrease = 12 safety_time = 3 min_proximity_threshold = 12 - braking_distance = 6 + emergency_braking_distance = 6 avoid_tailgators = True @dataclass @@ -25,7 +25,7 @@ class Normal(SimpleBehaviorAgentSettings): speed_decrease = 10 safety_time = 3 min_proximity_threshold = 10 - braking_distance = 5 + emergency_braking_distance = 5 avoid_tailgators = True @dataclass @@ -36,5 +36,5 @@ class Aggressive(SimpleBehaviorAgentSettings): speed_decrease = 8 safety_time = 3 min_proximity_threshold = 8 - braking_distance = 4 + emergency_braking_distance = 4 avoid_tailgators = False diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py index 5153b94259..52c21b3b10 100644 --- a/PythonAPI/carla/agents/navigation/behavior_agent.py +++ b/PythonAPI/carla/agents/navigation/behavior_agent.py @@ -284,7 +284,7 @@ def run_step(self, debug=False): self._vehicle.bounding_box.extent.y, self._vehicle.bounding_box.extent.x) # Emergency brake if the car is very close. - if distance < self.config.distance.braking_distance: + if distance < self.config.distance.emergency_braking_distance: return self.emergency_stop() # 2.2: Car following behaviors @@ -298,7 +298,7 @@ def run_step(self, debug=False): self._vehicle.bounding_box.extent.y, self._vehicle.bounding_box.extent.x) # Emergency brake if the car is very close. - if distance < self.config.distance.braking_distance: + if distance < self.config.distance.emergency_braking_distance: return self.emergency_stop() else: control = self.car_following_manager(vehicle, distance) From 542e330698eb6043be67072ef025da1ac5520cdc Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 17:28:09 +0100 Subject: [PATCH 25/32] Doc update --- .../agents/conf/agent_settings_backend.py | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/PythonAPI/carla/agents/conf/agent_settings_backend.py b/PythonAPI/carla/agents/conf/agent_settings_backend.py index 71ed7fdeda..f354ce5c50 100644 --- a/PythonAPI/carla/agents/conf/agent_settings_backend.py +++ b/PythonAPI/carla/agents/conf/agent_settings_backend.py @@ -262,10 +262,8 @@ class BehaviorAgentDistanceSettings(BasicAgentDistanceSettings): """ Collision Avoidance ----- - Distance in which for vehicles are checked - max(min_proximity_threshold, self._speed_limit / (2 if LANE CHANGE else 3 ) ) - TODO: The secondary speed limit is hardcoded, make adjustable and optional - automatic_proximity_threshold = {RoadOption.CHANGELANELEFT: 2, "same_lane" : 3, "right_lane" : 2} + Distance in which for vehicles are checked: + usage: max_distance = max(min_proximity_threshold, self._speed_limit / (2 if else 3 ) ) """ min_proximity_threshold : float = 10 @@ -312,6 +310,7 @@ class AutopilotLaneChangeSettings(AgentConfig): Adjust probability that in each timestep the actor will perform a left/right lane change, dependent on lane change availability. """ + random_right_lanechange_percentage : float = 0.1 """ Adjust probability that in each timestep the actor will perform a left/right lane change, @@ -387,31 +386,33 @@ class BasicAgentObstacleSettings(AgentConfig): """ Base distance to traffic lights to check if they affect the vehicle - USAGE: max_vehicle_distance = base_vehicle_threshold + detection_speed_ratio * vehicle_speed - USAGE: max_tlight_distance = base_tlight_threshold + detection_speed_ratio * vehicle_speed + Usage: max_vehicle_distance = base_vehicle_threshold + detection_speed_ratio * vehicle_speed + Usage: max_tlight_distance = base_tlight_threshold + detection_speed_ratio * vehicle_speed """ base_vehicle_threshold : float = 5.0 """ Base distance to vehicles to check if they affect the vehicle - USAGE: max_vehicle_distance = base_vehicle_threshold + detection_speed_ratio * vehicle_speed - USAGE: max_tlight_distance = base_tlight_threshold + detection_speed_ratio * vehicle_speed + Usage: max_vehicle_distance = base_vehicle_threshold + detection_speed_ratio * vehicle_speed + Usage: max_tlight_distance = base_tlight_threshold + detection_speed_ratio * vehicle_speed """ detection_speed_ratio : float = 1.0 """ Increases detection range based on speed - USAGE: max_vehicle_distance = base_vehicle_threshold + detection_speed_ratio * vehicle_speed - USAGE: max_tlight_distance = base_tlight_threshold + detection_speed_ratio * vehicle_speed + Usage: max_vehicle_distance = base_vehicle_threshold + detection_speed_ratio * vehicle_speed + Usage: max_tlight_distance = base_tlight_threshold + detection_speed_ratio * vehicle_speed """ dynamic_threshold_by_speed : bool = True """ Whether to add a dynamic threshold based on the vehicle speed to the base threshold. - Usage: base_threshold + dynamic_threshold_by_speed * vehicle_speed + Usage: base_threshold + detection_speed_ratio * vehicle_speed + + #NOTE: Currently only applied to traffic lights """ detection_angles : BasicAgentObstacleDetectionAngles = field(default_factory=BasicAgentObstacleDetectionAngles) From 83d84ee18ac5ec479ddebd0e84455feb661a7c41 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 17:57:05 +0100 Subject: [PATCH 26/32] Changes parameter name to make more clear it is boolean --- PythonAPI/carla/agents/conf/agent_settings_backend.py | 2 +- PythonAPI/carla/agents/navigation/basic_agent.py | 2 +- PythonAPI/carla/agents/navigation/behavior_agent.py | 2 +- PythonAPI/carla/agents/navigation/constant_velocity_agent.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/PythonAPI/carla/agents/conf/agent_settings_backend.py b/PythonAPI/carla/agents/conf/agent_settings_backend.py index f354ce5c50..ff58210956 100644 --- a/PythonAPI/carla/agents/conf/agent_settings_backend.py +++ b/PythonAPI/carla/agents/conf/agent_settings_backend.py @@ -406,7 +406,7 @@ class BasicAgentObstacleSettings(AgentConfig): Usage: max_tlight_distance = base_tlight_threshold + detection_speed_ratio * vehicle_speed """ - dynamic_threshold_by_speed : bool = True + use_dynamic_speed_threshold : bool = True """ Whether to add a dynamic threshold based on the vehicle speed to the base threshold. diff --git a/PythonAPI/carla/agents/navigation/basic_agent.py b/PythonAPI/carla/agents/navigation/basic_agent.py index 9bdaef41d0..06684e85b3 100644 --- a/PythonAPI/carla/agents/navigation/basic_agent.py +++ b/PythonAPI/carla/agents/navigation/basic_agent.py @@ -185,7 +185,7 @@ def run_step(self): hazard_detected = True # Check if the vehicle is affected by a red traffic light - if self.config.obstacles.dynamic_threshold_by_speed: + if self.config.obstacles.use_dynamic_speed_threshold: # Basic agent setting: max_tlight_distance = self.config.obstacles.base_tlight_threshold + self.config.obstacles.detection_speed_ratio * self.config.live_info.current_speed else: diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py index 52c21b3b10..c7fe580802 100644 --- a/PythonAPI/carla/agents/navigation/behavior_agent.py +++ b/PythonAPI/carla/agents/navigation/behavior_agent.py @@ -95,7 +95,7 @@ def traffic_light_manager(self): """ actor_list = self._world.get_actors() lights_list = actor_list.filter("*traffic_light*") - if self.config.obstacles.dynamic_threshold_by_speed: + if self.config.obstacles.use_dynamic_speed_threshold: # Basic agent setting: max_tlight_distance = self.config.obstacles.base_tlight_threshold + self.config.obstacles.detection_speed_ratio * self.config.live_info.current_speed else: diff --git a/PythonAPI/carla/agents/navigation/constant_velocity_agent.py b/PythonAPI/carla/agents/navigation/constant_velocity_agent.py index 750dfb892a..52b9c66ebe 100644 --- a/PythonAPI/carla/agents/navigation/constant_velocity_agent.py +++ b/PythonAPI/carla/agents/navigation/constant_velocity_agent.py @@ -97,7 +97,7 @@ def run_step(self): # Check if the vehicle is affected by a red traffic light - if self.config.obstacles.dynamic_threshold_by_speed: + if self.config.obstacles.use_dynamic_speed_threshold: # Basic agent setting: max_tlight_distance = self.config.obstacles.base_tlight_threshold + self.config.obstacles.detection_speed_ratio * vehicle_speed else: From 710082d40262020f27256245e575eaf2161ddc20 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 5 Mar 2024 18:13:08 +0100 Subject: [PATCH 27/32] doc and layout update --- .../agents/conf/agent_settings_backend.py | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/PythonAPI/carla/agents/conf/agent_settings_backend.py b/PythonAPI/carla/agents/conf/agent_settings_backend.py index ff58210956..a6d644c822 100644 --- a/PythonAPI/carla/agents/conf/agent_settings_backend.py +++ b/PythonAPI/carla/agents/conf/agent_settings_backend.py @@ -260,10 +260,12 @@ class BasicAgentDistanceSettings(AgentConfig): @dataclass class BehaviorAgentDistanceSettings(BasicAgentDistanceSettings): """ - Collision Avoidance ----- + Collision Avoidance + ------------------- - Distance in which for vehicles are checked: - usage: max_distance = max(min_proximity_threshold, self._speed_limit / (2 if else 3 ) ) + Distance in which for vehicles are checked. + + Usage: max_distance = max(min_proximity_threshold, self._speed_limit / (2 if else 3 ) ) """ min_proximity_threshold : float = 10 @@ -580,6 +582,8 @@ class BehaviorAgentEmergencySettings(BasicAgentEmergencySettings): pass +# --------------------- +# Final Settings # --------------------- @dataclass @@ -625,7 +629,7 @@ class AutopilotBehavior(AgentConfig): The distance is in meters and will affect the minimum moving distance. It is computed from front to back of the vehicle objects. """ - vehicle_percentage_speed_difference : float = 30 + vehicle_percentage_speed_difference : float = 30 # in percent """ Sets the difference the vehicle's intended speed and its current speed limit. Speed limits can be exceeded by setting the percentage to a negative value. @@ -641,14 +645,8 @@ class AutopilotBehavior(AgentConfig): update_vehicle_lights : bool = False """Sets if the Traffic Manager is responsible of updating the vehicle lights, or not.""" - - -# --------------------- -# Final Settings -# --------------------- - @dataclass class BasicAgentSettings(AgentConfig): overwrites : Optional[Dict[str, dict]] = field(default_factory=dict, repr=False) # type: Optional[Dict[str, Union[dict|AgentConfig]]] @@ -661,6 +659,7 @@ class BasicAgentSettings(AgentConfig): planner : BasicAgentPlannerSettings = field(default_factory=BasicAgentPlannerSettings, init=False) emergency : BasicAgentEmergencySettings = field(default_factory=BasicAgentEmergencySettings, init=False) + @dataclass class BehaviorAgentSettings(AgentConfig): overwrites : Optional[Dict[str, dict]] = field(default_factory=dict, repr=False) # type: Optional[Dict[str, Union[dict|AgentConfig]]] @@ -673,7 +672,7 @@ class BehaviorAgentSettings(AgentConfig): planner : BehaviorAgentPlannerSettings = field(default_factory=BehaviorAgentPlannerSettings, init=False) emergency : BehaviorAgentEmergencySettings = field(default_factory=BehaviorAgentEmergencySettings, init=False) avoid_tailgators : bool = True - + @dataclass class SimpleBasicAgentSettings(SimpleConfig, LiveInfo, BasicAgentSpeedSettings, BasicAgentDistanceSettings, BasicAgentLaneChangeSettings, BasicAgentObstacleSettings, BasicAgentControllerSettings, BasicAgentPlannerSettings, BasicAgentEmergencySettings): _base_settings :ClassVar[BasicAgentSettings] = BasicAgentSettings From db952e80068fa0a2da41d9d3525f3601353ae59f Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 16 Apr 2024 12:48:40 +0200 Subject: [PATCH 28/32] clearer variable names --- PythonAPI/carla/agents/navigation/behavior_agent.py | 6 +++--- .../carla/agents/navigation/global_route_planner.py | 12 ++++++------ 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py index c7fe580802..7e42a6e421 100644 --- a/PythonAPI/carla/agents/navigation/behavior_agent.py +++ b/PythonAPI/carla/agents/navigation/behavior_agent.py @@ -223,10 +223,10 @@ def car_following_manager(self, vehicle, distance, debug=False): vehicle_speed = get_speed(vehicle) delta_v = max(1, (self.config.live_info.current_speed - vehicle_speed) / 3.6) - ttc = distance / delta_v if delta_v != 0 else distance / self._epsilon + time_to_collision = distance / delta_v if delta_v != 0 else distance / self._epsilon # Under safety time distance, slow down. - if self.config.speed.safety_time > ttc > 0.0: + if self.config.speed.safety_time > time_to_collision > 0.0: target_speed = min([ positive(vehicle_speed - self.config.speed.speed_decrease), self.config.speed.max_speed, @@ -235,7 +235,7 @@ def car_following_manager(self, vehicle, distance, debug=False): control = self._local_planner.run_step(debug=debug) # Actual safety distance area, try to follow the speed of the vehicle in front. - elif 2 * self.config.speed.safety_time > ttc >= self.config.speed.safety_time: + elif 2 * self.config.speed.safety_time > time_to_collision >= self.config.speed.safety_time: target_speed = min([ max(self.config.speed.min_speed, vehicle_speed), self.config.speed.max_speed, diff --git a/PythonAPI/carla/agents/navigation/global_route_planner.py b/PythonAPI/carla/agents/navigation/global_route_planner.py index 7bc2a5a93b..8e59ff0eea 100644 --- a/PythonAPI/carla/agents/navigation/global_route_planner.py +++ b/PythonAPI/carla/agents/navigation/global_route_planner.py @@ -107,13 +107,13 @@ def _build_topology(self): seg_dict['path'] = [] endloc = wp2.transform.location if wp1.transform.location.distance(endloc) > self._sampling_resolution: - w = wp1.next(self._sampling_resolution)[0] - while w.transform.location.distance(endloc) > self._sampling_resolution: - seg_dict['path'].append(w) - next_ws = w.next(self._sampling_resolution) - if len(next_ws) == 0: + next_wp = wp1.next(self._sampling_resolution)[0] + while next_wp.transform.location.distance(endloc) > self._sampling_resolution: + seg_dict['path'].append(next_wp) + next_waypoints = next_wp.next(self._sampling_resolution) + if len(next_waypoints) == 0: break - w = next_ws[0] + next_wp = next_waypoints[0] else: next_wps = wp1.next(self._sampling_resolution) if len(next_wps) == 0: From 921310e15ed3af04587b12e7193a726fa077e3da Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 16 Apr 2024 18:54:05 +0200 Subject: [PATCH 29/32] Function to check lane direction --- PythonAPI/carla/agents/tools/misc.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/PythonAPI/carla/agents/tools/misc.py b/PythonAPI/carla/agents/tools/misc.py index 2d3d4c334f..6a28a21071 100644 --- a/PythonAPI/carla/agents/tools/misc.py +++ b/PythonAPI/carla/agents/tools/misc.py @@ -169,3 +169,17 @@ def positive(num): :param num: value to check """ return num if num > 0.0 else 0.0 + + +def lanes_have_same_direction(wp1, wp2) -> bool: + """ + Check if two lanes have the same direction, i.e. their lane ids + have the same sign. + + :param wp1: first waypoint + :param wp2: second waypoint + + Returns: + True if the lanes have the same direction, False otherwise + """ + return wp1.lane_id * wp2.lane_id > 0 \ No newline at end of file From bab4fc35fe5e6f734b2bc440b6aae7d859d51755 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 16 Apr 2024 18:55:11 +0200 Subject: [PATCH 30/32] Type-hints as comments only Backward compatibility --- PythonAPI/carla/agents/navigation/basic_agent.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/PythonAPI/carla/agents/navigation/basic_agent.py b/PythonAPI/carla/agents/navigation/basic_agent.py index 06684e85b3..5c8f56a0f8 100644 --- a/PythonAPI/carla/agents/navigation/basic_agent.py +++ b/PythonAPI/carla/agents/navigation/basic_agent.py @@ -42,7 +42,7 @@ def __init__(self, vehicle, opt_dict=BasicAgentSettings(), map_inst=None, grp_in :param grp_inst: GlobalRoutePlanner instance to avoid the expensive call of getting it. """ - self._vehicle = vehicle + self._vehicle = vehicle # type: carla.Vehicle self._world = self._vehicle.get_world() if map_inst: if isinstance(map_inst, carla.Map): @@ -73,9 +73,10 @@ def __init__(self, vehicle, opt_dict=BasicAgentSettings(), map_inst=None, grp_in # Get the static elements of the scene self._lights_list = self._world.get_actors().filter("*traffic_light*") # Dictionary mapping a traffic light to a Waypoint corresponding to its trigger volume location - self._lights_map: "dict[int, carla.Waypoint]" = {} + self._lights_map = {} # type: "dict[int, carla.Waypoint]" - def add_emergency_stop(self, control: carla.VehicleControl): + def add_emergency_stop(self, control): + # type: (carla.VehicleControl) -> carla.VehicleControl """ Overwrites the throttle a brake values of a control to perform an emergency stop. The steering is kept the same to avoid going out of the lane when stopping during turns @@ -157,7 +158,8 @@ def set_global_plan(self, plan, stop_waypoint_creation=True, clean_queue=True): clean_queue=clean_queue ) - def trace_route(self, start_waypoint: carla.Waypoint, end_waypoint: carla.Waypoint): + def trace_route(self, start_waypoint, end_waypoint): + # type: (carla.Waypoint, carla.Waypoint) -> list[carla.Waypoint] """ Calculates the shortest route between a starting and ending waypoint. From c0758895c05f9b7b9f1e8d3cbd9d64b7f2738ae8 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 16 Apr 2024 18:58:52 +0200 Subject: [PATCH 31/32] Added location and current waypoint to live_info --- .../carla/agents/conf/agent_settings_backend.py | 4 +++- .../carla/agents/navigation/behavior_agent.py | 14 ++++++++++++-- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/PythonAPI/carla/agents/conf/agent_settings_backend.py b/PythonAPI/carla/agents/conf/agent_settings_backend.py index a6d644c822..7bf48c18b7 100644 --- a/PythonAPI/carla/agents/conf/agent_settings_backend.py +++ b/PythonAPI/carla/agents/conf/agent_settings_backend.py @@ -165,8 +165,10 @@ def to_nested_config(self, simple_overwrites:dict=None) -> AgentConfig: class LiveInfo(AgentConfig): current_speed : float = MISSING current_speed_limit : float = MISSING - direction : RoadOption = MISSING velocity_vector : "carla.Vector3D" = MISSING + direction : RoadOption = MISSING + current_location : "carla.Location" = MISSING + current_waypoint : "carla.Waypoint" = MISSING # NOTE: Not ported to OmegaConf @property diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py index 7e42a6e421..2425f1fb45 100644 --- a/PythonAPI/carla/agents/navigation/behavior_agent.py +++ b/PythonAPI/carla/agents/navigation/behavior_agent.py @@ -75,15 +75,25 @@ def _update_information(self): This method updates the information regarding the ego vehicle based on the surrounding world. """ + # Speed self.config.live_info.current_speed = get_speed(self._vehicle) self.config.live_info.current_speed_limit = self._vehicle.get_speed_limit() self._local_planner.set_speed(self.config.live_info.current_speed_limit) # note: currently redundant, but will print warning. + + # Location + self.config.live_info.current_location = self._vehicle.get_location() + self.config.live_info.current_waypoint = self._map.get_waypoint(self.config.live_info.current_location) + + # Direction + # NOTE: This is set at local_planner.run_step and the executed direction from the *last* step self._direction = self._local_planner.target_road_option if self._direction is None: self._direction = RoadOption.LANEFOLLOW + # Upcoming direction self._look_ahead_steps = int((self.config.live_info.current_speed_limit) / 10) + # Will take the direction that is a few meters in front of the vehicle depending on the speed self._incoming_waypoint, self._incoming_direction = self._local_planner.get_incoming_waypoint_and_direction( steps=self._look_ahead_steps) if self._incoming_direction is None: @@ -266,8 +276,8 @@ def run_step(self, debug=False): if self._tailgate_counter > 0: self._tailgate_counter -= 1 - ego_vehicle_loc = self._vehicle.get_location() - ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc) + ego_vehicle_loc = self.config.live_info.current_location + ego_vehicle_wp = self.config.live_info.current_waypoint # 1: Red lights and stops behavior if self.traffic_light_manager(): From f9bc71dcea14c3c5275addea79418679ff62db94 Mon Sep 17 00:00:00 2001 From: Daniel Date: Tue, 16 Apr 2024 18:59:37 +0200 Subject: [PATCH 32/32] Added function to plan a safe lane_change. --- .../carla/agents/navigation/behavior_agent.py | 67 +++++++++++-------- 1 file changed, 39 insertions(+), 28 deletions(-) diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py index 2425f1fb45..d4326a1237 100644 --- a/PythonAPI/carla/agents/navigation/behavior_agent.py +++ b/PythonAPI/carla/agents/navigation/behavior_agent.py @@ -17,7 +17,7 @@ from agents.conf.behavior_types import Cautious, Aggressive, Normal from agents.conf.agent_settings_backend import BehaviorAgentSettings, AgentConfig, SimpleConfig -from agents.tools.misc import get_speed, positive, is_within_distance, compute_distance +from agents.tools.misc import get_speed, positive, is_within_distance, compute_distance, lanes_have_same_direction class BehaviorAgent(BasicAgent): """ @@ -115,44 +115,55 @@ def traffic_light_manager(self): return affected + def plan_lane_change(self, vehicle_list, order=["left", "right"], up_angle_th=180, low_angle_th=0, speed_limit_look_ahead_factor=0.5): + if vehicle_list is None: + vehicle_list = self._world.get_actors().filter("*vehicle*") + waypoint = self.config.live_info.current_waypoint + + for direction in order: + if direction == "right": + right_turn = waypoint.right_lane_marking.lane_change + can_change = (right_turn == carla.LaneChange.Right or right_turn == carla.LaneChange.Both) + other_wpt = waypoint.get_right_lane() + lane_offset = 1 + else: + left_turn = waypoint.left_lane_marking.lane_change + can_change = (left_turn == carla.LaneChange.Left or left_turn == carla.LaneChange.Both) + other_wpt = waypoint.get_left_lane() + lane_offset = -1 + if can_change and lanes_have_same_direction(waypoint, other_wpt) and other_wpt.lane_type == carla.LaneType.Driving: + # Detect if right lane is free + affected_by_vehicle, _, _ = self._vehicle_obstacle_detected(vehicle_list, + max(self.config.distance.min_proximity_threshold, + self.config.live_info.current_speed_limit * speed_limit_look_ahead_factor), + up_angle_th=up_angle_th, + low_angle_th=low_angle_th, + lane_offset=lane_offset) + if not affected_by_vehicle: + print("Changing lane, moving to the %s!", direction) + end_waypoint = self._local_planner.target_waypoint + self.set_destination(end_waypoint.transform.location, + other_wpt.transform.location) + return True + + + def _tailgating(self, waypoint, vehicle_list): """ This method is in charge of tailgating behaviors. + If a faster vehicle is behind the agent it will try to change the lane. - :param location: current location of the agent :param waypoint: current waypoint of the agent :param vehicle_list: list of all the nearby vehicles """ - left_turn = waypoint.left_lane_marking.lane_change - right_turn = waypoint.right_lane_marking.lane_change - - left_wpt = waypoint.get_left_lane() - right_wpt = waypoint.get_right_lane() - behind_vehicle_state, behind_vehicle, _ = self._vehicle_obstacle_detected(vehicle_list, max( self.config.distance.min_proximity_threshold, self.config.live_info.current_speed_limit / 2), up_angle_th=180, low_angle_th=160) if behind_vehicle_state and self.config.live_info.current_speed < get_speed(behind_vehicle): - if (right_turn == carla.LaneChange.Right or right_turn == - carla.LaneChange.Both) and waypoint.lane_id * right_wpt.lane_id > 0 and right_wpt.lane_type == carla.LaneType.Driving: - new_vehicle_state, _, _ = self._vehicle_obstacle_detected(vehicle_list, max( - self.config.distance.min_proximity_threshold, self.config.live_info.current_speed_limit / 2), up_angle_th=180, lane_offset=1) - if not new_vehicle_state: - print("Tailgating, moving to the right!") - end_waypoint = self._local_planner.target_waypoint - self._tailgate_counter = 200 - self.set_destination(end_waypoint.transform.location, - right_wpt.transform.location) - elif left_turn == carla.LaneChange.Left and waypoint.lane_id * left_wpt.lane_id > 0 and left_wpt.lane_type == carla.LaneType.Driving: - new_vehicle_state, _, _ = self._vehicle_obstacle_detected(vehicle_list, max( - self.config.distance.min_proximity_threshold, self.config.live_info.current_speed_limit / 2), up_angle_th=180, lane_offset=-1) - if not new_vehicle_state: - print("Tailgating, moving to the left!") - end_waypoint = self._local_planner.target_waypoint - self._tailgate_counter = 200 - self.set_destination(end_waypoint.transform.location, - left_wpt.transform.location) - + changes_lane = self.plan_lane_change(vehicle_list, order=("right", "left")) + if changes_lane: + self._tailgate_counter = 200 + def collision_and_car_avoid_manager(self, waypoint): """ This module is in charge of warning in case of a collision