diff --git a/agents/__init__.py b/agents/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/agents/navigation/__init__.py b/agents/navigation/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/agents/navigation/basic_agent.py b/agents/navigation/basic_agent.py new file mode 100644 index 000000000..9f34fef79 --- /dev/null +++ b/agents/navigation/basic_agent.py @@ -0,0 +1,507 @@ +# Copyright (c) # Copyright (c) 2018-2020 CVC. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +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 specified route +""" + +import carla +from shapely.geometry import Polygon + +from agents.navigation.local_planner import LocalPlanner, RoadOption +from agents.navigation.global_route_planner import GlobalRoutePlanner +from agents.tools.misc import (get_speed, is_within_distance, + get_trafficlight_trigger_location) + +from agents.tools.hints import ObstacleDetectionResult, TrafficLightDetectionResult + + +class BasicAgent: + """ + BasicAgent implements an agent that navigates the scene. + This agent respects traffic lights and other vehicles, but ignores stop signs. + It has several functions available to specify the route that the agent must follow, + 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): + """ + 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. + + """ + self._vehicle = vehicle + self._world = self._vehicle.get_world() + if map_inst: + if isinstance(map_inst, carla.Map): + self._map = map_inst + else: + print("Warning: Ignoring the given map as it is not a 'carla.Map'") + self._map = self._world.get_map() + else: + 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'] + + # Initialize the planners + self._local_planner = LocalPlanner(self._vehicle, opt_dict=opt_dict, map_inst=self._map) + if grp_inst: + if isinstance(grp_inst, GlobalRoutePlanner): + 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) + else: + self._global_planner = GlobalRoutePlanner(self._map, self._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 corresponding to its trigger volume location + + def add_emergency_stop(self, control): + """ + 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 + + :param speed (carl.VehicleControl): control to be modified + """ + control.throttle = 0.0 + control.brake = self._max_brake + control.hand_brake = False + return control + + 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) + + def follow_speed_limits(self, value=True): + """ + If active, the agent will dynamically change the target speed according to the speed limits + + :param value (bool): whether or not to activate this behavior + """ + self._local_planner.follow_speed_limits(value) + + def get_local_planner(self): + """Get method for protected member local planner""" + return self._local_planner + + def get_global_planner(self): + """Get method for protected member local planner""" + return self._global_planner + + def set_destination(self, end_location, start_location=None, clean_queue=True): + # type: (carla.Location, carla.Location | None, bool) -> None + """ + This method creates a list of waypoints between a starting and ending location, + based on the route returned by the global router, and adds it to the local planner. + If no starting location is passed and `clean_queue` is True, the vehicle local planner's + target location is chosen, which corresponds (by default), to a location about 5 meters + in front of the vehicle. + If `clean_queue` is False the newly planned route will be appended to the current route. + + :param end_location (carla.Location): final location of the route + :param start_location (carla.Location): starting location of the route + :param clean_queue (bool): Whether to clear or append to the currently planned route + """ + if not start_location: + if clean_queue and self._local_planner.target_waypoint: + # Plan from the waypoint in front of the vehicle onwards + start_location = self._local_planner.target_waypoint.transform.location + elif not clean_queue and self._local_planner._waypoints_queue: + # Append to the current plan + start_location = self._local_planner._waypoints_queue[-1][0].transform.location + else: + # no target_waypoint or _waypoints_queue empty, use vehicle location + start_location = self._vehicle.get_location() + start_waypoint = self._map.get_waypoint(start_location) + end_waypoint = self._map.get_waypoint(end_location) + + route_trace = self.trace_route(start_waypoint, end_waypoint) + self._local_planner.set_global_plan(route_trace, clean_queue=clean_queue) + + def set_global_plan(self, plan, stop_waypoint_creation=True, clean_queue=True): + """ + Adds a specific plan to the agent. + + :param plan: list of [carla.Waypoint, RoadOption] representing the route to be followed + :param stop_waypoint_creation: stops the automatic random creation of waypoints + :param clean_queue: resets the current agent's plan + """ + self._local_planner.set_global_plan( + plan, + stop_waypoint_creation=stop_waypoint_creation, + clean_queue=clean_queue + ) + + def trace_route(self, start_waypoint, end_waypoint): + """ + Calculates the shortest route between a starting and ending waypoint. + + :param start_waypoint (carla.Waypoint): initial waypoint + :param end_waypoint (carla.Waypoint): final waypoint + """ + start_location = start_waypoint.transform.location + end_location = end_waypoint.transform.location + return self._global_planner.trace_route(start_location, end_location) + + def run_step(self): + """Execute one step of navigation.""" + hazard_detected = False + + # Retrieve all relevant actors + vehicle_list = self._world.get_actors().filter("*vehicle*") + + vehicle_speed = get_speed(self._vehicle) / 3.6 + + # Check for possible vehicle obstacles + max_vehicle_distance = self._base_vehicle_threshold + self._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 + affected_by_tlight, _ = self._affected_by_traffic_light(self._lights_list, max_tlight_distance) + if affected_by_tlight: + hazard_detected = True + + control = self._local_planner.run_step() + if hazard_detected: + control = self.add_emergency_stop(control) + + return control + + def done(self): + """Check whether the agent has reached its destination.""" + return self._local_planner.done() + + def ignore_traffic_lights(self, active=True): + """(De)activates the checks for traffic lights""" + self._ignore_traffic_lights = active + + def ignore_stop_signs(self, active=True): + """(De)activates the checks for stop signs""" + self._ignore_stop_signs = active + + def ignore_vehicles(self, active=True): + """(De)activates the checks for stop signs""" + self._ignore_vehicles = active + + def set_offset(self, offset): + """Sets an offset for the vehicle""" + self._local_planner.set_offset(offset) + + def lane_change(self, direction, same_lane_time=0, other_lane_time=0, lane_change_time=2): + """ + Changes the path so that the vehicle performs a lane change. + Use 'direction' to specify either a 'left' or 'right' lane change, + and the other 3 fine tune the maneuver + """ + speed = self._vehicle.get_velocity().length() + path = self._generate_lane_change_path( + self._map.get_waypoint(self._vehicle.get_location()), + direction, + same_lane_time * speed, + other_lane_time * speed, + lane_change_time * speed, + False, + 1, + self._sampling_resolution + ) + if not path: + print("WARNING: Ignoring the lane change as no path was found") + + self.set_global_plan(path) + + def _affected_by_traffic_light(self, lights_list=None, max_distance=None): + """ + Method to check if there is a red light affecting the vehicle. + + :param lights_list (list of carla.TrafficLight): list containing TrafficLight objects. + If None, all traffic lights in the scene are used + :param max_distance (float): max distance for traffic lights to be considered relevant. + If None, the base threshold value is used + """ + if self._ignore_traffic_lights: + return TrafficLightDetectionResult(False, None) + + if not lights_list: + lights_list = self._world.get_actors().filter("*traffic_light*") + + if not max_distance: + max_distance = self._base_tlight_threshold + + if self._last_traffic_light: + if self._last_traffic_light.state != carla.TrafficLightState.Red: + self._last_traffic_light = None + else: + return TrafficLightDetectionResult(True, self._last_traffic_light) + + ego_vehicle_location = self._vehicle.get_location() + ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) + + for traffic_light in lights_list: + if traffic_light.id in self._lights_map: + trigger_wp = self._lights_map[traffic_light.id] + else: + trigger_location = get_trafficlight_trigger_location(traffic_light) + trigger_wp = self._map.get_waypoint(trigger_location) + self._lights_map[traffic_light.id] = trigger_wp + + if trigger_wp.transform.location.distance(ego_vehicle_location) > max_distance: + continue + + if trigger_wp.road_id != ego_vehicle_waypoint.road_id: + continue + + ve_dir = ego_vehicle_waypoint.transform.get_forward_vector() + wp_dir = trigger_wp.transform.get_forward_vector() + dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z + + if dot_ve_wp < 0: + continue + + if traffic_light.state != carla.TrafficLightState.Red: + continue + + if is_within_distance(trigger_wp.transform, self._vehicle.get_transform(), max_distance, [0, 90]): + self._last_traffic_light = traffic_light + return TrafficLightDetectionResult(True, traffic_light) + + return TrafficLightDetectionResult(False, None) + + def _vehicle_obstacle_detected(self, vehicle_list=None, max_distance=None, up_angle_th=90, low_angle_th=0, lane_offset=0): + """ + Method to check if there is a vehicle in front of the agent blocking its path. + + :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 + """ + 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_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) + route_bb.extend([[p1.x, p1.y, p1.z], [p2.x, p2.y, p2.z]]) + + for wp, _ in self._local_planner.get_plan(): + if ego_location.distance(wp.transform.location) > max_distance: + break + + r_vec = wp.transform.get_right_vector() + p1 = wp.transform.location + carla.Location(r_ext * r_vec.x, r_ext * r_vec.y) + p2 = wp.transform.location + carla.Location(l_ext * r_vec.x, l_ext * r_vec.y) + route_bb.extend([[p1.x, p1.y, p1.z], [p2.x, p2.y, p2.z]]) + + # Two points don't create a polygon, nothing to check + if len(route_bb) < 3: + return None + + return Polygon(route_bb) + + if self._ignore_vehicles: + return ObstacleDetectionResult(False, None, -1) + + if vehicle_list is None: + vehicle_list = self._world.get_actors().filter("*vehicle*") + if len(vehicle_list) == 0: + return ObstacleDetectionResult(False, None, -1) + + if not max_distance: + max_distance = self._base_vehicle_threshold + + ego_transform = self._vehicle.get_transform() + ego_location = ego_transform.location + ego_wpt = self._map.get_waypoint(ego_location) + + # Get the right offset + if ego_wpt.lane_id < 0 and lane_offset != 0: + lane_offset *= -1 + + # Get the transform of the front of the ego + ego_front_transform = ego_transform + 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 + + # Get the route bounding box + route_polygon = get_route_polygon() + + for target_vehicle in vehicle_list: + if target_vehicle.id == self._vehicle.id: + continue + + target_transform = target_vehicle.get_transform() + if target_transform.location.distance(ego_location) > max_distance: + continue + + target_wpt = self._map.get_waypoint(target_transform.location, lane_type=carla.LaneType.Any) + + # General approach for junctions and vehicles invading other lanes due to the offset + if (use_bbs or target_wpt.is_junction) and route_polygon: + + target_bb = target_vehicle.bounding_box + target_vertices = target_bb.get_world_vertices(target_vehicle.get_transform()) + target_list = [[v.x, v.y, v.z] for v in target_vertices] + target_polygon = Polygon(target_list) + + if route_polygon.intersects(target_polygon): + return ObstacleDetectionResult(True, target_vehicle, target_vehicle.get_location().distance(ego_location)) + + # Simplified approach, using only the plan waypoints (similar to TM) + else: + + if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id + lane_offset: + next_wpt = self._local_planner.get_incoming_waypoint_and_direction(steps=3)[0] + if not next_wpt: + continue + if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id + lane_offset: + continue + + target_forward_vector = target_transform.get_forward_vector() + target_extent = target_vehicle.bounding_box.extent.x + target_rear_transform = target_transform + target_rear_transform.location -= carla.Location( + x=target_extent * target_forward_vector.x, + y=target_extent * target_forward_vector.y, + ) + + if is_within_distance(target_rear_transform, ego_front_transform, max_distance, [low_angle_th, up_angle_th]): + return ObstacleDetectionResult(True, target_vehicle, target_transform.location.distance(ego_transform.location)) + + return ObstacleDetectionResult(False, None, -1) + + @staticmethod + def _generate_lane_change_path(waypoint, direction='left', distance_same_lane=10, + distance_other_lane=25, lane_change_distance=25, + check=True, lane_changes=1, step_distance=2): + # type: (carla.Waypoint, str, float, float, float, bool, int, float) -> list[tuple[carla.Waypoint, RoadOption]] + """ + This methods generates a path that results in a lane change. + Use the different distances to fine-tune the maneuver. + If the lane change is impossible, the returned path will be empty. + """ + distance_same_lane = max(distance_same_lane, 0.1) + distance_other_lane = max(distance_other_lane, 0.1) + lane_change_distance = max(lane_change_distance, 0.1) + + plan = [] + plan.append((waypoint, RoadOption.LANEFOLLOW)) # start position + + option = RoadOption.LANEFOLLOW + + # Same lane + distance = 0 + while distance < distance_same_lane: + next_wps = plan[-1][0].next(step_distance) + if not next_wps: + return [] + next_wp = next_wps[0] + distance += next_wp.transform.location.distance(plan[-1][0].transform.location) + plan.append((next_wp, RoadOption.LANEFOLLOW)) + + if direction == 'left': + option = RoadOption.CHANGELANELEFT + elif direction == 'right': + option = RoadOption.CHANGELANERIGHT + else: + # ERROR, input value for change must be 'left' or 'right' + return [] + + lane_changes_done = 0 + lane_change_distance = lane_change_distance / lane_changes + + # Lane change + while lane_changes_done < lane_changes: + + # Move forward + next_wps = plan[-1][0].next(lane_change_distance) + if not next_wps: + return [] + next_wp = next_wps[0] + + # Get the side lane + if direction == 'left': + if check and str(next_wp.lane_change) not in ['Left', 'Both']: + return [] + side_wp = next_wp.get_left_lane() + else: + if check and str(next_wp.lane_change) not in ['Right', 'Both']: + return [] + side_wp = next_wp.get_right_lane() + + if not side_wp or side_wp.lane_type != carla.LaneType.Driving: + return [] + + # Update the plan + plan.append((side_wp, option)) + lane_changes_done += 1 + + # Other lane + distance = 0 + while distance < distance_other_lane: + next_wps = plan[-1][0].next(step_distance) + if not next_wps: + return [] + next_wp = next_wps[0] + distance += next_wp.transform.location.distance(plan[-1][0].transform.location) + plan.append((next_wp, RoadOption.LANEFOLLOW)) + + return plan diff --git a/agents/navigation/behavior_agent.py b/agents/navigation/behavior_agent.py new file mode 100644 index 000000000..f4cc1bf27 --- /dev/null +++ b/agents/navigation/behavior_agent.py @@ -0,0 +1,317 @@ +# Copyright (c) # Copyright (c) 2018-2020 CVC. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + + +""" This module implements an agent that roams around a track following random +waypoints and avoiding other vehicles. The agent also responds to traffic lights, +traffic signs, and has different possible configurations. """ + +import numpy as np +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.tools.misc import get_speed, positive + +class BehaviorAgent(BasicAgent): + """ + BehaviorAgent implements an agent that navigates scenes to reach a given + target destination, by computing the shortest possible path to it. + This agent can correctly follow traffic signs, speed limitations, + traffic lights, while also taking into account nearby vehicles. Lane changing + decisions can be taken by analyzing the surrounding environment such as tailgating avoidance. + Adding to these are possible behaviors, the agent can also keep safety distance + from a car in front of it by tracking the instantaneous time to collision + and keeping it in a certain range. Finally, different sets of behaviors + 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): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :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._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 + + # Parameters for agent behavior + if behavior == 'cautious': + self._behavior = Cautious() + + elif behavior == 'normal': + self._behavior = Normal() + + 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._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._incoming_waypoint, self._incoming_direction = self._local_planner.get_incoming_waypoint_and_direction( + steps=self._look_ahead_steps) + if self._incoming_direction is None: + self._incoming_direction = RoadOption.LANEFOLLOW + + def traffic_light_manager(self): + """ + This method is in charge of behaviors for red lights. + """ + actor_list = self._world.get_actors() + lights_list = actor_list.filter("*traffic_light*") + affected, _ = self._affected_by_traffic_light(lights_list) + + return affected + + def _tailgating(self, waypoint, vehicle_list): + """ + This method is in charge of tailgating behaviors. + + :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._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): + 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) + if not new_vehicle_state: + print("Tailgating, moving to the right!") + end_waypoint = self._local_planner.target_waypoint + self._behavior.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) + if not new_vehicle_state: + print("Tailgating, moving to the left!") + end_waypoint = self._local_planner.target_waypoint + self._behavior.tailgate_counter = 200 + self.set_destination(end_waypoint.transform.location, + left_wpt.transform.location) + + def collision_and_car_avoid_manager(self, waypoint): + """ + This module is in charge of warning in case of a collision + and managing possible tailgating chances. + + :param location: current location of the agent + :param waypoint: current waypoint of the agent + :return vehicle_state: True if there is a vehicle nearby, False if not + :return vehicle: nearby vehicle + :return distance: distance to nearby vehicle + """ + + vehicle_list = self._world.get_actors().filter("*vehicle*") + def dist(v): return v.get_location().distance(waypoint.transform.location) + vehicle_list = [v for v in vehicle_list if dist(v) < 45 and v.id != self._vehicle.id] + + 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) + 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) + 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) + + # 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: + self._tailgating(waypoint, vehicle_list) + + return vehicle_state, vehicle, distance + + def pedestrian_avoid_manager(self, waypoint): + """ + This module is in charge of warning in case of a collision + with any pedestrian. + + :param location: current location of the agent + :param waypoint: current waypoint of the agent + :return vehicle_state: True if there is a walker nearby, False if not + :return vehicle: nearby walker + :return distance: distance to nearby walker + """ + + walker_list = self._world.get_actors().filter("*walker.pedestrian*") + def dist(w): return w.get_location().distance(waypoint.transform.location) + walker_list = [w for w in walker_list if dist(w) < 10] + + 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) + 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) + 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) + + return walker_state, walker, distance + + def car_following_manager(self, vehicle, distance, debug=False): + """ + Module in charge of car-following behaviors when there's + someone in front of us. + + :param vehicle: car to follow + :param distance: distance from vehicle + :param debug: boolean for debugging + :return control: carla.VehicleControl + """ + + 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.) + + # Under safety time distance, slow down. + if self._behavior.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]) + 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: + target_speed = min([ + max(self._min_speed, vehicle_speed), + self._behavior.max_speed, + self._speed_limit - self._behavior.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._local_planner.set_speed(target_speed) + control = self._local_planner.run_step(debug=debug) + + return control + + def run_step(self, debug=False): + """ + Execute one step of navigation. + + :param debug: boolean for debugging + :return control: carla.VehicleControl + """ + self._update_information() + + control = None + if self._behavior.tailgate_counter > 0: + self._behavior.tailgate_counter -= 1 + + ego_vehicle_loc = self._vehicle.get_location() + ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc) + + # 1: Red lights and stops behavior + if self.traffic_light_manager(): + return self.emergency_stop() + + # 2.1: Pedestrian avoidance behaviors + walker_state, walker, w_distance = self.pedestrian_avoid_manager(ego_vehicle_wp) + + if walker_state: + # Distance is computed from the center of the two cars, + # we use bounding boxes to calculate the actual distance + distance = w_distance - max( + walker.bounding_box.extent.y, walker.bounding_box.extent.x) - max( + 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: + return self.emergency_stop() + + # 2.2: Car following behaviors + vehicle_state, vehicle, distance = self.collision_and_car_avoid_manager(ego_vehicle_wp) + + if vehicle_state: + # Distance is computed from the center of the two cars, + # we use bounding boxes to calculate the actual distance + distance = distance - max( + vehicle.bounding_box.extent.y, vehicle.bounding_box.extent.x) - max( + 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: + return self.emergency_stop() + else: + control = self.car_following_manager(vehicle, distance) + + # 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._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._local_planner.set_speed(target_speed) + control = self._local_planner.run_step(debug=debug) + + return control + + def emergency_stop(self): + """ + 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 + + :param speed (carl.VehicleControl): control to be modified + """ + control = carla.VehicleControl() + control.throttle = 0.0 + control.brake = self._max_brake + control.hand_brake = False + return control diff --git a/agents/navigation/behavior_types.py b/agents/navigation/behavior_types.py new file mode 100644 index 000000000..9eb81dd21 --- /dev/null +++ b/agents/navigation/behavior_types.py @@ -0,0 +1,37 @@ +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" This module contains the different parameters sets for each behavior. """ + + +class Cautious: + """Class for Cautious agent.""" + max_speed = 40 + speed_lim_dist = 6 + speed_decrease = 12 + safety_time = 3 + min_proximity_threshold = 12 + braking_distance = 6 + tailgate_counter = 0 + + +class Normal: + """Class for Normal agent.""" + max_speed = 50 + speed_lim_dist = 3 + speed_decrease = 10 + safety_time = 3 + min_proximity_threshold = 10 + braking_distance = 5 + tailgate_counter = 0 + + +class Aggressive: + """Class for Aggressive agent.""" + max_speed = 70 + speed_lim_dist = 1 + speed_decrease = 8 + safety_time = 3 + min_proximity_threshold = 8 + braking_distance = 4 + tailgate_counter = -1 diff --git a/agents/navigation/constant_velocity_agent.py b/agents/navigation/constant_velocity_agent.py new file mode 100644 index 000000000..c6e0e2446 --- /dev/null +++ b/agents/navigation/constant_velocity_agent.py @@ -0,0 +1,129 @@ +# Copyright (c) # Copyright (c) 2018-2020 CVC. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" +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 specified route +""" + +import carla + +from agents.navigation.basic_agent import BasicAgent + +class ConstantVelocityAgent(BasicAgent): + """ + ConstantVelocityAgent implements an agent that navigates the scene at a fixed velocity. + This agent will fail if asked to perform turns that are impossible are the desired speed. + This includes lane changes. When a collision is detected, the constant velocity will stop, + wait for a bit, and then start again. + """ + + def __init__(self, vehicle, target_speed=20, opt_dict={}, 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) + + 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 + + self._restart_time = float('inf') # Time after collision before the constant velocity behavior starts again + + if 'restart_time' in opt_dict: + self._restart_time = opt_dict['restart_time'] + if 'use_basic_behavior' in opt_dict: + self._use_basic_behavior = opt_dict['use_basic_behavior'] + + 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) + + def stop_constant_velocity(self): + """Stops the constant velocity behavior""" + self.is_constant_velocity_active = False + self._vehicle.disable_constant_velocity() + self._constant_velocity_stop_time = self._world.get_snapshot().timestamp.elapsed_seconds + + 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) + + def _set_constant_velocity(self, speed): + """Forces the agent to drive at the specified speed""" + self._vehicle.enable_constant_velocity(carla.Vector3D(speed, 0, 0)) + + def run_step(self): + """Execute one step of navigation.""" + if not self.is_constant_velocity_active: + if self._world.get_snapshot().timestamp.elapsed_seconds - self._constant_velocity_stop_time > self._restart_time: + self.restart_constant_velocity() + self.is_constant_velocity_active = True + elif self._use_basic_behavior: + return super(ConstantVelocityAgent, self).run_step() + else: + return carla.VehicleControl() + + hazard_detected = False + + # Retrieve all relevant actors + actor_list = self._world.get_actors() + vehicle_list = actor_list.filter("*vehicle*") + lights_list = actor_list.filter("*traffic_light*") + + vehicle_speed = self._vehicle.get_velocity().length() + + max_vehicle_distance = self._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() + if vehicle_velocity.length() == 0: + hazard_speed = 0 + else: + 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._base_tlight_threshold + 0.3 * vehicle_speed + affected_by_tlight, _ = self._affected_by_traffic_light(lights_list, max_tlight_distance) + if affected_by_tlight: + hazard_speed = 0 + hazard_detected = True + + # The longitudinal PID is overwritten by the constant velocity but it is + # 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) + else: + self._set_constant_velocity(self._target_speed) + + return control + + def _set_collision_sensor(self): + blueprint = self._world.get_blueprint_library().find('sensor.other.collision') + self._collision_sensor = self._world.spawn_actor(blueprint, carla.Transform(), attach_to=self._vehicle) + self._collision_sensor.listen(lambda event: self.stop_constant_velocity()) + + def destroy_sensor(self): + if self._collision_sensor: + self._collision_sensor.destroy() + self._collision_sensor = None diff --git a/agents/navigation/controller.py b/agents/navigation/controller.py new file mode 100644 index 000000000..0b23d8571 --- /dev/null +++ b/agents/navigation/controller.py @@ -0,0 +1,266 @@ +# Copyright (c) # Copyright (c) 2018-2020 CVC. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" This module contains PID controllers to perform lateral and longitudinal control. """ + +from collections import deque +import math +import numpy as np +import carla +from agents.tools.misc import get_speed + + +class VehiclePIDController: + """ + VehiclePIDController is the combination of two PID controllers + (lateral and longitudinal) to perform the + 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): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param args_lateral: 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 + 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. + 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._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) + + def run_step(self, target_speed, 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) + current_steering = self._lat_controller.run_step(waypoint) + control = carla.VehicleControl() + if acceleration >= 0.0: + control.throttle = min(acceleration, self.max_throt) + control.brake = 0.0 + else: + control.throttle = 0.0 + control.brake = min(abs(acceleration), self.max_brake) + + # Steering regulation: changes cannot happen abruptly, can't steer too much. + + if current_steering > self.past_steering + 0.1: + current_steering = self.past_steering + 0.1 + elif current_steering < self.past_steering - 0.1: + current_steering = self.past_steering - 0.1 + + if current_steering >= 0: + steering = min(self.max_steer, current_steering) + else: + steering = max(-self.max_steer, current_steering) + + control.steer = steering + control.hand_brake = False + control.manual_gear_shift = False + self.past_steering = steering + + return control + + + def change_longitudinal_PID(self, args_longitudinal): + """Changes the parameters of the PIDLongitudinalController""" + self._lon_controller.change_parameters(**args_longitudinal) + + def change_lateral_PID(self, args_lateral): + """Changes the parameters of the PIDLateralController""" + self._lat_controller.change_parameters(**args_lateral) + + def set_offset(self, offset): + """Changes the offset""" + self._lat_controller.set_offset(offset) + + +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): + """ + 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 + """ + 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): + """ + 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 + :return: throttle control + """ + current_speed = get_speed(self._vehicle) + + if debug: + print('Current speed = {}'.format(current_speed)) + + return self._pid_control(target_speed, current_speed) + + def _pid_control(self, target_speed, current_speed): + """ + 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 + 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 + 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) + + 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 + + +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): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param offset: distance to the center line. If might cause issues if the value + is large enough to make the vehicle invade other lanes. + :param K_P: Proportional term + :param K_D: Differential term + :param K_I: Integral term + :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) + + def run_step(self, waypoint): + """ + Execute one step of lateral control to steer + the vehicle towards a certain waypoint. + + :param waypoint: target waypoint + :return: steering control in the range [-1, 1] where: + -1 maximum steering to left + +1 maximum steering to right + """ + return self._pid_control(waypoint, self._vehicle.get_transform()) + + def set_offset(self, offset): + """Changes the offset""" + self._offset = offset + + def _pid_control(self, waypoint, vehicle_transform): + """ + Estimate the steering angle of the vehicle based on the PID equations + + :param waypoint: target waypoint + :param vehicle_transform: current transform of the vehicle + :return: steering control in the range [-1, 1] + """ + # Get the ego's location and forward vector + ego_loc = vehicle_transform.location + v_vec = vehicle_transform.get_forward_vector() + v_vec = np.array([v_vec.x, v_vec.y, 0.0]) + + # Get the vector vehicle-target_wp + if self._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) + else: + w_loc = waypoint.transform.location + + w_vec = np.array([w_loc.x - ego_loc.x, + w_loc.y - ego_loc.y, + 0.0]) + + wv_linalg = np.linalg.norm(w_vec) * np.linalg.norm(v_vec) + if wv_linalg == 0: + _dot = 1 + else: + _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / (wv_linalg), -1.0, 1.0)) + _cross = np.cross(v_vec, w_vec) + if _cross[2] < 0: + _dot *= -1.0 + + 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 + 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) + + 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 diff --git a/agents/navigation/global_route_planner.py b/agents/navigation/global_route_planner.py new file mode 100644 index 000000000..47310b84c --- /dev/null +++ b/agents/navigation/global_route_planner.py @@ -0,0 +1,441 @@ +# Copyright (c) # Copyright (c) 2018-2020 CVC. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + + +""" +This module provides GlobalRoutePlanner implementation. +""" + +import math +import numpy as np +import networkx as nx + +import carla +from agents.navigation.local_planner import RoadOption + +# Python 2 compatibility +TYPE_CHECKING = False +if TYPE_CHECKING: + import sys + if sys.version_info >= (3, 11): + from typing import TypedDict, NotRequired + elif sys.version_info >= (3, 8): + from typing import TypedDict + from typing_extensions import NotRequired + else: + from typing_extensions import TypedDict, NotRequired + + TopologyDict = TypedDict('TopologyDict', + { + 'entry': carla.Waypoint, + 'exit': carla.Waypoint, + 'entryxyz': tuple[float, float, float], + 'exitxyz': tuple[float, float, float], + 'path': list[carla.Waypoint] + }) + + EdgeDict = TypedDict('EdgeDict', + { + 'length': int, + 'path': list[carla.Waypoint], + 'entry_waypoint': carla.Waypoint, + 'exit_waypoint': carla.Waypoint, + 'entry_vector': np.ndarray, + 'exit_vector': np.ndarray, + 'net_vector': list[float], + 'intersection': bool, + 'type': RoadOption, + 'change_waypoint': NotRequired[carla.Waypoint] + }) + +class GlobalRoutePlanner: + """ + This class provides a very high level route plan. + """ + + def __init__(self, wmap, sampling_resolution): + # type: (carla.Map, float) -> None + self._sampling_resolution = sampling_resolution + self._wmap = wmap + self._topology = [] # type: list[TopologyDict] + self._graph = None # type: nx.DiGraph # type: ignore[assignment] + self._id_map = None # type: dict[tuple[float, float, float], int] # type: ignore[assignment] + self._road_id_to_edge = None # type: dict[int, dict[int, dict[int, tuple[int, int]]]] # type: ignore[assignment] + + self._intersection_end_node = -1 + self._previous_decision = RoadOption.VOID + + # Build the graph + self._build_topology() + self._build_graph() + self._find_loose_ends() + self._lane_change_link() + + def trace_route(self, origin, destination): + # type: (carla.Location, carla.Location) -> list[tuple[carla.Waypoint, RoadOption]] + """ + This method returns list of (carla.Waypoint, RoadOption) + from origin to destination + """ + route_trace = [] # type: list[tuple[carla.Waypoint, RoadOption]] + route = self._path_search(origin, destination) + current_waypoint = self._wmap.get_waypoint(origin) + destination_waypoint = self._wmap.get_waypoint(destination) + + for i in range(len(route) - 1): + road_option = self._turn_decision(i, route) + edge = self._graph.edges[route[i], route[i + 1]] # type: EdgeDict + path = [] # type: list[carla.Waypoint] + + if edge['type'] != RoadOption.LANEFOLLOW and edge['type'] != RoadOption.VOID: + route_trace.append((current_waypoint, road_option)) + exit_wp = edge['exit_waypoint'] + n1, n2 = self._road_id_to_edge[exit_wp.road_id][exit_wp.section_id][exit_wp.lane_id] + next_edge = self._graph.edges[n1, n2] # type: EdgeDict + if next_edge['path']: + closest_index = self._find_closest_in_list(current_waypoint, next_edge['path']) + closest_index = min(len(next_edge['path']) - 1, closest_index + 5) + current_waypoint = next_edge['path'][closest_index] + else: + current_waypoint = next_edge['exit_waypoint'] + route_trace.append((current_waypoint, road_option)) + + else: + path = path + [edge['entry_waypoint']] + edge['path'] + [edge['exit_waypoint']] + closest_index = self._find_closest_in_list(current_waypoint, path) + for waypoint in path[closest_index:]: + current_waypoint = waypoint + route_trace.append((current_waypoint, road_option)) + if len(route) - i <= 2 and waypoint.transform.location.distance( + destination) < 2 * self._sampling_resolution: + break + elif len( + route) - i <= 2 and current_waypoint.road_id == destination_waypoint.road_id and current_waypoint.section_id == destination_waypoint.section_id and current_waypoint.lane_id == destination_waypoint.lane_id: + destination_index = self._find_closest_in_list(destination_waypoint, path) + if closest_index > destination_index: + break + + return route_trace + + def _build_topology(self): + """ + This function retrieves topology from the server as a list of + road segments as pairs of waypoint objects, and processes the + topology into a list of dictionary objects with the following attributes + + - entry (carla.Waypoint): waypoint of entry point of road segment + - entryxyz (tuple): (x,y,z) of entry point of road segment + - exit (carla.Waypoint): waypoint of exit point of road segment + - exitxyz (tuple): (x,y,z) of exit point of road segment + - path (list of carla.Waypoint): list of waypoints between entry to exit, separated by the resolution + """ + self._topology = [] + # Retrieving waypoints to construct a detailed topology + for segment in self._wmap.get_topology(): + wp1, wp2 = segment[0], segment[1] + l1, l2 = wp1.transform.location, wp2.transform.location + # Rounding off to avoid floating point imprecision + x1, y1, z1, x2, y2, z2 = np.round([l1.x, l1.y, l1.z, l2.x, l2.y, l2.z], 0) + wp1.transform.location, wp2.transform.location = l1, l2 + seg_dict = dict() # type: TopologyDict # type: ignore[assignment] + seg_dict['entry'], seg_dict['exit'] = wp1, wp2 + seg_dict['entryxyz'], seg_dict['exitxyz'] = (x1, y1, z1), (x2, y2, z2) + 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: + break + w = next_ws[0] + else: + next_wps = wp1.next(self._sampling_resolution) + if len(next_wps) == 0: + continue + seg_dict['path'].append(next_wps[0]) + self._topology.append(seg_dict) + + def _build_graph(self): + """ + This function builds a networkx graph representation of topology, creating several class attributes: + - graph (networkx.DiGraph): networkx graph representing the world map, with: + Node properties: + vertex: (x,y,z) position in world map + Edge properties: + entry_vector: unit vector along tangent at entry point + exit_vector: unit vector along tangent at exit point + net_vector: unit vector of the chord from entry to exit + intersection: boolean indicating if the edge belongs to an intersection + - id_map (dictionary): mapping from (x,y,z) to node id + - road_id_to_edge (dictionary): map from road id to edge in the graph + """ + + self._graph = nx.DiGraph() + self._id_map = dict() # Map with structure {(x,y,z): id, ... } + self._road_id_to_edge = dict() # Map with structure {road_id: {lane_id: edge, ... }, ... } + + for segment in self._topology: + entry_xyz, exit_xyz = segment['entryxyz'], segment['exitxyz'] + path = segment['path'] + entry_wp, exit_wp = segment['entry'], segment['exit'] + intersection = entry_wp.is_junction + road_id, section_id, lane_id = entry_wp.road_id, entry_wp.section_id, entry_wp.lane_id + + for vertex in entry_xyz, exit_xyz: + # Adding unique nodes and populating id_map + if vertex not in self._id_map: + new_id = len(self._id_map) + self._id_map[vertex] = new_id + self._graph.add_node(new_id, vertex=vertex) + n1 = self._id_map[entry_xyz] + n2 = self._id_map[exit_xyz] + if road_id not in self._road_id_to_edge: + self._road_id_to_edge[road_id] = dict() + if section_id not in self._road_id_to_edge[road_id]: + self._road_id_to_edge[road_id][section_id] = dict() + self._road_id_to_edge[road_id][section_id][lane_id] = (n1, n2) + + entry_carla_vector = entry_wp.transform.rotation.get_forward_vector() + exit_carla_vector = exit_wp.transform.rotation.get_forward_vector() + net_carla_vector = (exit_wp.transform.location - entry_wp.transform.location).make_unit_vector() + + # Adding edge with attributes + self._graph.add_edge( + n1, n2, + length=len(path) + 1, path=path, + entry_waypoint=entry_wp, exit_waypoint=exit_wp, + entry_vector=np.array( + [entry_carla_vector.x, entry_carla_vector.y, entry_carla_vector.z]), + exit_vector=np.array( + [exit_carla_vector.x, exit_carla_vector.y, exit_carla_vector.z]), + net_vector=[net_carla_vector.x, net_carla_vector.y, net_carla_vector.z], + intersection=intersection, type=RoadOption.LANEFOLLOW) + + def _find_loose_ends(self): + """ + This method finds road segments that have an unconnected end, and + adds them to the internal graph representation + """ + count_loose_ends = 0 + hop_resolution = self._sampling_resolution + for segment in self._topology: + end_wp = segment['exit'] + exit_xyz = segment['exitxyz'] + road_id, section_id, lane_id = end_wp.road_id, end_wp.section_id, end_wp.lane_id + if road_id in self._road_id_to_edge \ + and section_id in self._road_id_to_edge[road_id] \ + and lane_id in self._road_id_to_edge[road_id][section_id]: + pass + else: + count_loose_ends += 1 + if road_id not in self._road_id_to_edge: + self._road_id_to_edge[road_id] = dict() + if section_id not in self._road_id_to_edge[road_id]: + self._road_id_to_edge[road_id][section_id] = dict() + n1 = self._id_map[exit_xyz] + n2 = -1 * count_loose_ends + self._road_id_to_edge[road_id][section_id][lane_id] = (n1, n2) + next_wp = end_wp.next(hop_resolution) + path = [] # type: list[carla.Waypoint] + while next_wp is not None and next_wp \ + and next_wp[0].road_id == road_id \ + and next_wp[0].section_id == section_id \ + and next_wp[0].lane_id == lane_id: + path.append(next_wp[0]) + next_wp = next_wp[0].next(hop_resolution) + if path: + n2_xyz = (path[-1].transform.location.x, + path[-1].transform.location.y, + path[-1].transform.location.z) + self._graph.add_node(n2, vertex=n2_xyz) + self._graph.add_edge( + n1, n2, + length=len(path) + 1, path=path, + entry_waypoint=end_wp, exit_waypoint=path[-1], + entry_vector=None, exit_vector=None, net_vector=None, + intersection=end_wp.is_junction, type=RoadOption.LANEFOLLOW) + + def _lane_change_link(self): + """ + This method places zero cost links in the topology graph + representing availability of lane changes. + """ + + for segment in self._topology: + left_found, right_found = False, False + + for waypoint in segment['path']: + if not segment['entry'].is_junction: + next_waypoint, next_road_option, next_segment = None, None, None + + if waypoint.right_lane_marking and waypoint.right_lane_marking.lane_change & carla.LaneChange.Right and not right_found: + next_waypoint = waypoint.get_right_lane() + if next_waypoint is not None \ + and next_waypoint.lane_type == carla.LaneType.Driving \ + and waypoint.road_id == next_waypoint.road_id: + next_road_option = RoadOption.CHANGELANERIGHT + next_segment = self._localize(next_waypoint.transform.location) + if next_segment is not None: + self._graph.add_edge( + self._id_map[segment['entryxyz']], next_segment[0], entry_waypoint=waypoint, + exit_waypoint=next_waypoint, intersection=False, exit_vector=None, + path=[], length=0, type=next_road_option, change_waypoint=next_waypoint) + right_found = True + if waypoint.left_lane_marking and waypoint.left_lane_marking.lane_change & carla.LaneChange.Left and not left_found: + next_waypoint = waypoint.get_left_lane() + if next_waypoint is not None \ + and next_waypoint.lane_type == carla.LaneType.Driving \ + and waypoint.road_id == next_waypoint.road_id: + next_road_option = RoadOption.CHANGELANELEFT + next_segment = self._localize(next_waypoint.transform.location) + if next_segment is not None: + self._graph.add_edge( + self._id_map[segment['entryxyz']], next_segment[0], entry_waypoint=waypoint, + exit_waypoint=next_waypoint, intersection=False, exit_vector=None, + path=[], length=0, type=next_road_option, change_waypoint=next_waypoint) + left_found = True + if left_found and right_found: + break + + def _localize(self, location): + # type: (carla.Location) -> None | tuple[int, int] + """ + This function finds the road segment that a given location + is part of, returning the edge it belongs to + """ + waypoint = self._wmap.get_waypoint(location) + edge = None # type: None | tuple[int, int] + try: + edge = self._road_id_to_edge[waypoint.road_id][waypoint.section_id][waypoint.lane_id] + except KeyError: + pass + return edge + + def _distance_heuristic(self, n1, n2): + """ + Distance heuristic calculator for path searching + in self._graph + """ + l1 = np.array(self._graph.nodes[n1]['vertex']) + l2 = np.array(self._graph.nodes[n2]['vertex']) + return np.linalg.norm(l1 - l2) + + def _path_search(self, origin, destination): + # type: (carla.Location, carla.Location) -> list[int] + """ + This function finds the shortest path connecting origin and destination + using A* search with distance heuristic. + origin : carla.Location object of start position + destination : carla.Location object of of end position + return : path as list of node ids (as int) of the graph self._graph + connecting origin and destination + """ + start, end = self._localize(origin), self._localize(destination) + + route = nx.astar_path( + self._graph, source=start[0], target=end[0], + heuristic=self._distance_heuristic, weight='length') + route.append(end[1]) + return route + + def _successive_last_intersection_edge(self, index, route): + # type: (int, list[int]) -> tuple[int | None, EdgeDict | None] + """ + This method returns the last successive intersection edge + from a starting index on the route. + This helps moving past tiny intersection edges to calculate + proper turn decisions. + """ + + last_intersection_edge = None # type: EdgeDict | None + last_node = None + for node1, node2 in [(route[i], route[i + 1]) for i in range(index, len(route) - 1)]: + candidate_edge = self._graph.edges[node1, node2] # type: EdgeDict + if node1 == route[index]: + last_intersection_edge = candidate_edge + if candidate_edge['type'] == RoadOption.LANEFOLLOW and candidate_edge['intersection']: + last_intersection_edge = candidate_edge + last_node = node2 + else: + break + + return last_node, last_intersection_edge + + def _turn_decision(self, index, route, threshold=math.radians(35)): + # type: (int, list[int], float) -> RoadOption + """ + This method returns the turn decision (RoadOption) for pair of edges + around current index of route list + """ + + decision = None + previous_node = route[index - 1] + current_node = route[index] + next_node = route[index + 1] + next_edge = self._graph.edges[current_node, next_node] # type: EdgeDict + if index > 0: + if self._previous_decision != RoadOption.VOID \ + and self._intersection_end_node > 0 \ + and self._intersection_end_node != previous_node \ + and next_edge['type'] == RoadOption.LANEFOLLOW \ + and next_edge['intersection']: + decision = self._previous_decision + else: + self._intersection_end_node = -1 + current_edge = self._graph.edges[previous_node, current_node] # type: EdgeDict + calculate_turn = current_edge['type'] == RoadOption.LANEFOLLOW and not current_edge[ + 'intersection'] and next_edge['type'] == RoadOption.LANEFOLLOW and next_edge['intersection'] + if calculate_turn: + last_node, tail_edge = self._successive_last_intersection_edge(index, route) + self._intersection_end_node = last_node + if tail_edge is not None: + next_edge = tail_edge + cv, nv = current_edge['exit_vector'], next_edge['exit_vector'] + if cv is None or nv is None: + return next_edge['type'] + cross_list = [] + for neighbor in self._graph.successors(current_node): + select_edge = self._graph.edges[current_node, neighbor] + if select_edge['type'] == RoadOption.LANEFOLLOW: + if neighbor != route[index + 1]: + sv = select_edge['net_vector'] + cross_list.append(np.cross(cv, sv)[2]) + next_cross = np.cross(cv, nv)[2] + deviation = math.acos(np.clip( + np.dot(cv, nv) / (np.linalg.norm(cv) * np.linalg.norm(nv)), -1.0, 1.0)) + if not cross_list: + cross_list.append(0) + if deviation < threshold: + decision = RoadOption.STRAIGHT + elif cross_list and next_cross < min(cross_list): + decision = RoadOption.LEFT + elif cross_list and next_cross > max(cross_list): + decision = RoadOption.RIGHT + elif next_cross < 0: + decision = RoadOption.LEFT + elif next_cross > 0: + decision = RoadOption.RIGHT + else: + decision = next_edge['type'] + + else: + decision = next_edge['type'] + + self._previous_decision = decision + return decision + + def _find_closest_in_list(self, current_waypoint, waypoint_list): + min_distance = float('inf') + closest_index = -1 + for i, waypoint in enumerate(waypoint_list): + distance = waypoint.transform.location.distance( + current_waypoint.transform.location) + if distance < min_distance: + min_distance = distance + closest_index = i + + return closest_index diff --git a/agents/navigation/local_planner.py b/agents/navigation/local_planner.py new file mode 100644 index 000000000..25833cecf --- /dev/null +++ b/agents/navigation/local_planner.py @@ -0,0 +1,352 @@ +# Copyright (c) # Copyright (c) 2018-2020 CVC. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" This module contains a local planner to perform low-level waypoint following based on PID controllers. """ + +from enum import IntEnum +from collections import deque +import random + +import carla +from agents.navigation.controller import VehiclePIDController +from agents.tools.misc import draw_waypoints, get_speed + + +class RoadOption(IntEnum): + """ + RoadOption represents the possible topological configurations when moving from a segment of lane to other. + + """ + VOID = -1 + LEFT = 1 + RIGHT = 2 + STRAIGHT = 3 + LANEFOLLOW = 4 + CHANGELANELEFT = 5 + CHANGELANERIGHT = 6 + + +class LocalPlanner: + """ + LocalPlanner implements the basic behavior of following a + trajectory of waypoints that is generated on-the-fly. + + The low-level motion of the vehicle is computed by using two PID controllers, + one is used for the lateral control and the other for the longitudinal control (cruise speed). + + When multiple paths are available (intersections) this local planner makes a random choice, + unless a given global plan has already been specified. + """ + + def __init__(self, vehicle, opt_dict={}, map_inst=None): + """ + :param vehicle: actor to apply to local planner logic onto + :param opt_dict: dictionary of arguments with different parameters: + dt: time between simulation steps + target_speed: desired cruise speed in Km/h + sampling_radius: distance between the waypoints part of the plan + lateral_control_dict: values of the lateral PID controller + longitudinal_control_dict: values of the longitudinal PID controller + max_throttle: maximum throttle applied to the vehicle + max_brake: maximum brake applied to the vehicle + max_steering: maximum steering applied to the vehicle + offset: distance between the route waypoints and the center of the lane + :param map_inst: carla.Map instance to avoid the expensive call of getting it. + """ + self._vehicle = vehicle + self._world = self._vehicle.get_world() + if map_inst: + if isinstance(map_inst, carla.Map): + self._map = map_inst + else: + print("Warning: Ignoring the given map as it is not a 'carla.Map'") + self._map = self._world.get_map() + else: + self._map = self._world.get_map() + + self._vehicle_controller = None + self.target_waypoint = None + self.target_road_option = None + + self._waypoints_queue = deque(maxlen=10000) + self._min_waypoint_queue_length = 100 + self._stop_waypoint_creation = False + + # Base parameters + self._dt = 1.0 / 20.0 + self._target_speed = 20.0 # Km/h + self._sampling_radius = 2.0 + self._args_lateral_dict = {'K_P': 1.95, 'K_I': 0.05, 'K_D': 0.2, 'dt': self._dt} + self._args_longitudinal_dict = {'K_P': 1.0, 'K_I': 0.05, 'K_D': 0, 'dt': self._dt} + self._max_throt = 0.75 + self._max_brake = 0.3 + self._max_steer = 0.8 + self._offset = 0 + self._base_min_distance = 3.0 + self._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'] + + # initializing controller + self._init_controller() + + def reset_vehicle(self): + """Reset the ego-vehicle""" + self._vehicle = None + + def _init_controller(self): + """Controller initialization""" + self._vehicle_controller = VehiclePIDController(self._vehicle, + args_lateral=self._args_lateral_dict, + args_longitudinal=self._args_longitudinal_dict, + offset=self._offset, + max_throttle=self._max_throt, + max_brake=self._max_brake, + max_steering=self._max_steer) + + # Compute the current vehicle waypoint + current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) + self.target_waypoint, self.target_road_option = (current_waypoint, RoadOption.LANEFOLLOW) + self._waypoints_queue.append((self.target_waypoint, self.target_road_option)) + + def set_speed(self, speed): + """ + Changes the target speed + + :param speed: new target speed in Km/h + :return: + """ + if self._follow_speed_limits: + print("WARNING: The max speed is currently set to follow the speed limits. " + "Use 'follow_speed_limits' to deactivate this") + self._target_speed = speed + + def follow_speed_limits(self, value=True): + """ + Activates a flag that makes the max speed dynamically vary according to the speed limits + + :param value: bool + :return: + """ + self._follow_speed_limits = value + + def _compute_next_waypoints(self, k=1): + """ + Add new waypoints to the trajectory queue. + + :param k: how many waypoints to compute + :return: + """ + # check we do not overflow the queue + available_entries = self._waypoints_queue.maxlen - len(self._waypoints_queue) + k = min(available_entries, k) + + for _ in range(k): + last_waypoint = self._waypoints_queue[-1][0] + next_waypoints = list(last_waypoint.next(self._sampling_radius)) + + if len(next_waypoints) == 0: + break + elif len(next_waypoints) == 1: + # only one option available ==> lanefollowing + next_waypoint = next_waypoints[0] + road_option = RoadOption.LANEFOLLOW + else: + # random choice between the possible options + road_options_list = _retrieve_options( + next_waypoints, last_waypoint) + road_option = random.choice(road_options_list) + next_waypoint = next_waypoints[road_options_list.index( + road_option)] + + self._waypoints_queue.append((next_waypoint, road_option)) + + def set_global_plan(self, current_plan, stop_waypoint_creation=True, clean_queue=True): + """ + Adds a new plan to the local planner. A plan must be a list of [carla.Waypoint, RoadOption] pairs + The 'clean_queue` parameter erases the previous plan if True, otherwise, it adds it to the old one + The 'stop_waypoint_creation' flag stops the automatic creation of random waypoints + + :param current_plan: list of (carla.Waypoint, RoadOption) + :param stop_waypoint_creation: bool + :param clean_queue: bool + :return: + """ + if clean_queue: + self._waypoints_queue.clear() + + # Remake the waypoints queue if the new plan has a higher length than the queue + new_plan_length = len(current_plan) + len(self._waypoints_queue) + if new_plan_length > self._waypoints_queue.maxlen: + new_waypoint_queue = deque(maxlen=new_plan_length) + for wp in self._waypoints_queue: + new_waypoint_queue.append(wp) + self._waypoints_queue = new_waypoint_queue + + for elem in current_plan: + self._waypoints_queue.append(elem) + + self._stop_waypoint_creation = stop_waypoint_creation + + def set_offset(self, offset): + """Sets an offset for the vehicle""" + self._vehicle_controller.set_offset(offset) + + def run_step(self, debug=False): + """ + Execute one step of local planning which involves running the longitudinal and lateral PID controllers to + follow the waypoints trajectory. + + :param debug: boolean flag to activate waypoints debugging + :return: control to be applied + """ + if self._follow_speed_limits: + self._target_speed = self._vehicle.get_speed_limit() + + # Add more waypoints too few in the horizon + if not self._stop_waypoint_creation and len(self._waypoints_queue) < self._min_waypoint_queue_length: + self._compute_next_waypoints(k=self._min_waypoint_queue_length) + + # Purge the queue of obsolete waypoints + veh_location = self._vehicle.get_location() + vehicle_speed = get_speed(self._vehicle) / 3.6 + self._min_distance = self._base_min_distance + self._distance_ratio * vehicle_speed + + num_waypoint_removed = 0 + for waypoint, _ in self._waypoints_queue: + + if len(self._waypoints_queue) - num_waypoint_removed == 1: + min_distance = 1 # Don't remove the last waypoint until very close by + else: + min_distance = self._min_distance + + if veh_location.distance(waypoint.transform.location) < min_distance: + num_waypoint_removed += 1 + else: + break + + if num_waypoint_removed > 0: + for _ in range(num_waypoint_removed): + self._waypoints_queue.popleft() + + # Get the target waypoint and move using the PID controllers. Stop if no target waypoint + if len(self._waypoints_queue) == 0: + control = carla.VehicleControl() + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 1.0 + control.hand_brake = False + control.manual_gear_shift = False + else: + self.target_waypoint, self.target_road_option = self._waypoints_queue[0] + control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint) + + if debug: + draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], 1.0) + + return control + + def get_incoming_waypoint_and_direction(self, steps=3): + """ + Returns direction and waypoint at a distance ahead defined by the user. + + :param steps: number of steps to get the incoming waypoint. + """ + if len(self._waypoints_queue) > steps: + return self._waypoints_queue[steps] + + else: + try: + wpt, direction = self._waypoints_queue[-1] + return wpt, direction + except IndexError: + return None, RoadOption.VOID + + def get_plan(self): + """Returns the current plan of the local planner""" + return self._waypoints_queue + + def done(self): + """ + Returns whether or not the planner has finished + + :return: boolean + """ + return len(self._waypoints_queue) == 0 + + +def _retrieve_options(list_waypoints, current_waypoint): + """ + Compute the type of connection between the current active waypoint and the multiple waypoints present in + list_waypoints. The result is encoded as a list of RoadOption enums. + + :param list_waypoints: list with the possible target waypoints in case of multiple options + :param current_waypoint: current active waypoint + :return: list of RoadOption enums representing the type of connection from the active waypoint to each + candidate in list_waypoints + """ + options = [] + for next_waypoint in list_waypoints: + # this is needed because something we are linking to + # the beginning of an intersection, therefore the + # variation in angle is small + next_next_waypoint = next_waypoint.next(3.0)[0] + link = _compute_connection(current_waypoint, next_next_waypoint) + options.append(link) + + return options + + +def _compute_connection(current_waypoint, next_waypoint, threshold=35): + """ + Compute the type of topological connection between an active waypoint (current_waypoint) and a target waypoint + (next_waypoint). + + :param current_waypoint: active waypoint + :param next_waypoint: target waypoint + :return: the type of topological connection encoded as a RoadOption enum: + RoadOption.STRAIGHT + RoadOption.LEFT + RoadOption.RIGHT + """ + n = next_waypoint.transform.rotation.yaw + n = n % 360.0 + + c = current_waypoint.transform.rotation.yaw + c = c % 360.0 + + diff_angle = (n - c) % 180.0 + if diff_angle < threshold or diff_angle > (180 - threshold): + return RoadOption.STRAIGHT + elif diff_angle > 90.0: + return RoadOption.LEFT + else: + return RoadOption.RIGHT diff --git a/agents/requirements.txt b/agents/requirements.txt new file mode 100644 index 000000000..9c8a9b531 --- /dev/null +++ b/agents/requirements.txt @@ -0,0 +1,3 @@ +numpy<2.0.0 +networkx +Shapely diff --git a/agents/tools/__init__.py b/agents/tools/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/agents/tools/hints.py b/agents/tools/hints.py new file mode 100644 index 000000000..6a6bf520e --- /dev/null +++ b/agents/tools/hints.py @@ -0,0 +1,34 @@ +""" +Module to add high-level semantic return types for obstacle and traffic light detection results via named tuples. + +The code is compatible with Python 2.7, <3.6 and >=3.6. The later uses the typed version of named tuples. +""" + + +import sys +if sys.version_info < (3, 6): + from collections import namedtuple + ObstacleDetectionResult = namedtuple('ObstacleDetectionResult', ['obstacle_was_found', 'obstacle', 'distance']) + TrafficLightDetectionResult = namedtuple('TrafficLightDetectionResult', ['traffic_light_was_found', 'traffic_light']) +else: + from typing import NamedTuple, Union, TYPE_CHECKING + from carla import Actor, TrafficLight + """ + # Python 3.6+, incompatible with Python 2.7 syntax + class ObstacleDetectionResult(NamedTuple): + obstacle_was_found : bool + obstacle : Union[Actor, None] + distance : float + # distance : Union[float, Literal[-1]] # Python 3.8+ only + + class TrafficLightDetectionResult(NamedTuple): + traffic_light_was_found : bool + traffic_light : Union[TrafficLight, None] + """ + if TYPE_CHECKING: + from typing import Literal + ObstacleDetectionResult = NamedTuple('ObstacleDetectionResult', [('obstacle_was_found', bool), ('obstacle', Union[Actor, None]), ('distance', Union[float, Literal[-1]])]) + else: + ObstacleDetectionResult = NamedTuple('ObstacleDetectionResult', [('obstacle_was_found', bool), ('obstacle', Union[Actor, None]), ('distance', float)]) + + TrafficLightDetectionResult = NamedTuple('TrafficLightDetectionResult', [('traffic_light_was_found', bool), ('traffic_light', Union[TrafficLight, None])]) diff --git a/agents/tools/misc.py b/agents/tools/misc.py new file mode 100644 index 000000000..9400e1517 --- /dev/null +++ b/agents/tools/misc.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python + +# Copyright (c) 2018 Intel Labs. +# authors: German Ros (german.ros@intel.com) +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" Module with auxiliary functions. """ + +import math +import numpy as np +import carla + +_EPS = np.finfo(float).eps + +def draw_waypoints(world, waypoints, z=0.5): + """ + Draw a list of waypoints at a certain height given in z. + + :param world: carla.world object + :param waypoints: list or iterable container with the waypoints to draw + :param z: height in meters + """ + for wpt in waypoints: + wpt_t = wpt.transform + begin = wpt_t.location + carla.Location(z=z) + angle = math.radians(wpt_t.rotation.yaw) + end = begin + carla.Location(x=math.cos(angle), y=math.sin(angle)) + world.debug.draw_arrow(begin, end, arrow_size=0.3, life_time=1.0) + + +def get_speed(vehicle): + """ + Compute speed of a vehicle in Km/h. + + :param vehicle: the vehicle for which speed is calculated + :return: speed as a float in Km/h + """ + vel = vehicle.get_velocity() + + return 3.6 * math.sqrt(vel.x ** 2 + vel.y ** 2 + vel.z ** 2) + +def get_trafficlight_trigger_location(traffic_light): + """ + Calculates the yaw of the waypoint that represents the trigger volume of the traffic light + """ + def rotate_point(point, radians): + """ + rotate a given point by a given angle + """ + rotated_x = math.cos(radians) * point.x - math.sin(radians) * point.y + rotated_y = math.sin(radians) * point.x - math.cos(radians) * point.y + + return carla.Vector3D(rotated_x, rotated_y, point.z) + + base_transform = traffic_light.get_transform() + base_rot = base_transform.rotation.yaw + area_loc = base_transform.transform(traffic_light.trigger_volume.location) + area_ext = traffic_light.trigger_volume.extent + + point = rotate_point(carla.Vector3D(0, 0, area_ext.z), math.radians(base_rot)) + point_location = area_loc + carla.Location(x=point.x, y=point.y) + + return carla.Location(point_location.x, point_location.y, point_location.z) + + +def is_within_distance(target_transform, reference_transform, max_distance, angle_interval=None): + """ + Check if a location is both within a certain distance from a reference object. + By using 'angle_interval', the angle between the location and reference transform + will also be tkaen into account, being 0 a location in front and 180, one behind. + + :param target_transform: location of the target object + :param reference_transform: location of the reference object + :param max_distance: maximum allowed distance + :param angle_interval: only locations between [min, max] angles will be considered. This isn't checked by default. + :return: boolean + """ + target_vector = np.array([ + target_transform.location.x - reference_transform.location.x, + target_transform.location.y - reference_transform.location.y + ]) + norm_target = np.linalg.norm(target_vector) + + # If the vector is too short, we can simply stop here + if norm_target < 0.001: + return True + + # Further than the max distance + if norm_target > max_distance: + return False + + # We don't care about the angle, nothing else to check + if not angle_interval: + return True + + min_angle = angle_interval[0] + max_angle = angle_interval[1] + + fwd = reference_transform.get_forward_vector() + forward_vector = np.array([fwd.x, fwd.y]) + angle = math.degrees(math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.))) + + return min_angle < angle < max_angle + + +def compute_magnitude_angle(target_location, current_location, orientation): + """ + Compute relative angle and distance between a target_location and a current_location + + :param target_location: location of the target object + :param current_location: location of the reference object + :param orientation: orientation of the reference object + :return: a tuple composed by the distance to the object and the angle between both objects + """ + target_vector = np.array([target_location.x - current_location.x, target_location.y - current_location.y]) + norm_target = np.linalg.norm(target_vector) + + forward_vector = np.array([math.cos(math.radians(orientation)), math.sin(math.radians(orientation))]) + d_angle = math.degrees(math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.))) + + return (norm_target, d_angle) + + +def distance_vehicle(waypoint, vehicle_transform): + """ + Returns the 2D distance from a waypoint to a vehicle + + :param waypoint: actual waypoint + :param vehicle_transform: transform of the target vehicle + """ + loc = vehicle_transform.location + x = waypoint.transform.location.x - loc.x + y = waypoint.transform.location.y - loc.y + + return math.sqrt(x * x + y * y) + + +def vector(location_1, location_2): + """ + Returns the unit vector from location_1 to location_2 + + :param location_1, location_2: carla.Location objects + + .. note:: + Alternatively you can use: + `(location_2 - location_1).make_unit_vector()` + """ + x = location_2.x - location_1.x + y = location_2.y - location_1.y + z = location_2.z - location_1.z + norm = np.linalg.norm([x, y, z]) + _EPS + return [x / norm, y / norm, z / norm] + + +def compute_distance(location_1, location_2): + """ + Euclidean distance between 3D points + + :param location_1, location_2: 3D points + + .. deprecated:: 0.9.13 + Use `location_1.distance(location_2)` instead + """ + x = location_2.x - location_1.x + y = location_2.y - location_1.y + z = location_2.z - location_1.z + norm = np.linalg.norm([x, y, z]) + _EPS + return norm + + +def positive(num): + """ + Return the given number if positive, else 0 + + :param num: value to check + """ + return num if num > 0.0 else 0.0 diff --git a/scenario_runner.py b/scenario_runner.py index 345f52756..ad225bbbf 100755 --- a/scenario_runner.py +++ b/scenario_runner.py @@ -1,7 +1,9 @@ #!/usr/bin/env python +# 1. 必须是第一条代码语句 (Shebang和编码声明除外) +from __future__ import print_function + # Copyright (c) 2018-2020 Intel Corporation -# # This work is licensed under the terms of the MIT license. # For a copy, see . @@ -13,8 +15,6 @@ and finally triggers the scenario execution. """ -from __future__ import print_function - import glob import traceback import argparse diff --git a/setup_env.sh b/setup_env.sh new file mode 100755 index 000000000..3a9795e95 --- /dev/null +++ b/setup_env.sh @@ -0,0 +1,31 @@ +#!/bin/bash + +# 激活虚拟环境 +source /home/zhang/carla_env/bin/activate + +# 设置CARLA根目录，请根据你的安装路径修改 +CARLA_ROOT=/home/zhang/下载/CARLA_0.9.16 + +# 设置Scenario Runner根目录 +SCENARIO_RUNNER_ROOT=/home/zhang/scenario_runner + +# 设置 EGG 文件路径。注意：Scenario Runner 通常需要的是 .egg 文件，而不是 .whl 文件。 +# 使用通配符 * 来匹配版本和 Python 编译版本。 +EGG_FILE=$(ls $CARLA_ROOT/PythonAPI/carla/dist/carla-*-py3.10-*.egg 2>/dev/null) + +# 如果找不到 .egg，尝试查找 .whl (某些安装可能只有 .whl) +if [ -z "$EGG_FILE" ]; then + EGG_FILE=$(ls $CARLA_ROOT/PythonAPI/carla/dist/*.whl 2>/dev/null) +fi + +# 检查是否找到文件 +if [ -z "$EGG_FILE" ]; then + echo "错误: 无法在 $CARLA_ROOT/PythonAPI/carla/dist/ 中找到 CARLA Python 包 (.egg 或 .whl)。请检查 CARLA 安装版本和 Python 版本是否匹配。" +fi + +# 设置PYTHONPATH +export PYTHONPATH=$PYTHONPATH:$EGG_FILE +export PYTHONPATH=$PYTHONPATH:$CARLA_ROOT/PythonAPI/carla +export PYTHONPATH=$PYTHONPATH:$CARLA_ROOT/PythonAPI/agents +export PYTHONPATH=$PYTHONPATH:$SCENARIO_RUNNER_ROOT + diff --git a/srunner/data/my_takeover_route.xml b/srunner/data/my_takeover_route.xml new file mode 100644 index 000000000..f23b2a929 --- /dev/null +++ b/srunner/data/my_takeover_route.xml @@ -0,0 +1,15 @@ + + + + + + + + \ No newline at end of file diff --git a/srunner/scenarios/actor_flow.py b/srunner/scenarios/actor_flow.py index 491612276..9f99090c3 100644 --- a/srunner/scenarios/actor_flow.py +++ b/srunner/scenarios/actor_flow.py @@ -7,18 +7,18 @@ """ Scenarios in which another (opposite) vehicle 'illegally' takes -priority, e.g. by running a red traffic light. +priority, e.g. by running a red traffic light.#其他车辆违规有限通行，例如对方闯红灯 """ - -from __future__ import print_function - -import py_trees +#基础模块和兼容性 +from __future__ import print_function#兼容python3和2,保证print行为一致 +import py_trees#行为树库，用于编排复杂有逻辑顺序的行为 import carla - from agents.navigation.local_planner import RoadOption + from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ActorFlow, ScenarioTimeout, WaitForever + from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, ScenarioTimeoutTest from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation, WaitEndIntersection, @@ -42,13 +42,14 @@ def convert_dict_to_location(actor_dict): ) return location -def get_value_parameter(config, name, p_type, default): +def get_value_parameter(config, name, p_type, default):#config代表所有的配置信息，name即名字，比如说speed，p_type代表类型，如int，float，default代表默认值 if name in config.other_parameters: return p_type(config.other_parameters[name]['value']) else: return default def get_interval_parameter(config, name, p_type, default): + print(dir(config)) if name in config.other_parameters: return [ p_type(config.other_parameters[name]['from']), @@ -56,7 +57,9 @@ def get_interval_parameter(config, name, p_type, default): ] else: return default - +######################### +#辅助函数 +######################### class EnterActorFlow(BasicScenario): """ This class holds everything required for a scenario in which another vehicle runs a red light @@ -98,21 +101,23 @@ def _create_behavior(self): Hero vehicle is entering a junction in an urban area, at a signalized intersection, while another actor runs a red lift, forcing the ego to break. """ - source_wp = self._map.get_waypoint(self._start_actor_flow) + source_wp = self._map.get_waypoint(self._start_actor_flow)#获得起点终点 sink_wp = self._map.get_waypoint(self._end_actor_flow) # Get all lanes - source_wps = get_same_dir_lanes(source_wp) + source_wps = get_same_dir_lanes(source_wp)#辅助函数，获取所有的同向车道 sink_wps = get_same_dir_lanes(sink_wp) root = py_trees.composites.Parallel( - policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) + policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)#创建一个parallel行为树节点，只要有一个任务成功，整个并行任务成功 for source_wp, sink_wp in zip(source_wps, sink_wps): + #第一个子任务，条件节点 root.add_child( InTriggerDistanceToLocation( self.ego_vehicles[0], sink_wp.transform.location, + self._sink_distance, ) ) diff --git a/srunner/scenarios/my_complex_scenario.py b/srunner/scenarios/my_complex_scenario.py new file mode 100644 index 000000000..a7ad9c1ed --- /dev/null +++ b/srunner/scenarios/my_complex_scenario.py @@ -0,0 +1,144 @@ +#!/usr/bin/env python + +# 导入所有必要的模块 +import py_trees +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (LaneChange, + StopVehicle, + ActorDestroy, + WaitForever) +from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest +# 我们将使用更可靠的区域触发，而不是距离触发 +from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerRegion +from srunner.scenarios.basic_scenario import BasicScenario +# 导入生成背景车辆的帮助函数 +from srunner.tools.scenario_helper import get_waypoint_in_distance, spawn_surrounding_actors + +# 1. 定义我们自己的场景类，它必须继承自 BasicScenario +class SuddenCutInScenario(BasicScenario): + """ + 一个经过整合和增强的自定义接管场景： + 1. 包含可配置数量的背景交通流，使世界更真实。 + 2. 一辆危险车辆会在主车进入指定区域时，突然切入。 + 3. 切入后，该危险车辆会立即紧急刹车，制造更危险的状况。 + """ + + # 2. __init__: 准备阶段，从XML配置文件中读取参数 + def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True, timeout=120): + """ + 初始化所有场景参数 + """ + print("----- [SCENARIO INIT] 开始初始化场景 -----") + # 从XML读取参数，并进行类型转换 + self._target_speed = float(config.target_speed) + self._cut_in_distance = float(config.cut_in_distance) + # 新增参数：从XML读取触发区域和背景车辆数量 + self._trigger_region = config.trigger_region # 格式应为 "x_min,x_max,y_min,y_max" + self._num_background_vehicles = int(config.num_background_vehicles) + + print(f"[SCENARIO INIT] 参数加载成功: 目标速度={self._target_speed}, 切入距离={self._cut_in_distance}") + print(f"[SCENARIO INIT] 触发区域='{self._trigger_region}', 背景车辆数={self._num_background_vehicles}") + + # 调用父类的构造函数，这是必须的 + super(SuddenCutInScenario, self).__init__("SuddenCutInScenario", + ego_vehicles, + config, + world, + debug_mode, + criteria_enable=criteria_enable, + timeout=timeout) + + # 3. _initialize_actors: 创建所有需要的车辆 + def _initialize_actors(self, config): + """ + 在场景开始时，创建危险车辆和背景交通流 + """ + print("----- [DEBUG] 开始执行 _initialize_actors -----") + + # Part 1: 创建核心的危险车辆 (用于切入和刹车) + # 在主车前方较远处生成，给它留足准备空间 + start_wp = self.ego_vehicles[0].get_map().get_waypoint(self.ego_vehicles[0].get_location()) + waypoint_ahead, _ = get_waypoint_in_distance(start_wp, 80) + # 优先选择右侧车道，如果没有，就在当前车道前方 + self.other_actor_wp = waypoint_ahead.get_right_lane() + if self.other_actor_wp is None: + print("[DEBUG] 警告: 在目标位置找不到右侧车道，将在当前车道前方生成危险车辆。") + self.other_actor_wp = waypoint_ahead + + other_actor_transform = self.other_actor_wp.transform + other_actor_transform.location.z += 0.1 # 稍微抬高以防生成时与地面碰撞 + + # 请求CARLA生成车辆 + self.other_actor = CarlaDataProvider.request_new_actor( + 'vehicle.tesla.model3', other_actor_transform, rolename='other_actor') + + if self.other_actor is None: + raise RuntimeError("无法生成危险车辆，请检查地图和生成点位置。") + + self.other_actor.set_simulate_physics(enabled=True) + self.other_actors.append(self.other_actor) # 添加到列表，以便场景结束时自动销毁 + print(f"[DEBUG] 成功创建危险车辆, Actor ID: {self.other_actor.id} at {other_actor_transform.location}") + + # Part 2: 创建背景交通流 + background_actors = spawn_surrounding_actors(self.ego_vehicles[0], self._num_background_vehicles) + for actor in background_actors: + if actor: + self.other_actors.append(actor) + actor.set_autopilot(True) # 设置为自动驾驶模式 + + print(f"[DEBUG] 成功创建 {len(background_actors)} 辆背景交通车辆。") + print("----- [DEBUG] _initialize_actors 执行完毕 -----") + + # 4. _create_behavior: 定义核心行为逻辑 + def _create_behavior(self): + """ + 定义危险车辆的行为：等待触发 -> 突然切入 -> 紧急刹车 + """ + print("----- [DEBUG] 正在创建场景行为树 -----") + # 解析从XML读取的触发区域坐标字符串 + try: + region_coords = [float(x) for x in self._trigger_region.split(',')] + except (ValueError, IndexError): + raise ValueError(f"XML中的trigger_region格式错误: '{self._trigger_region}'. 应为 'x_min,x_max,y_min,y_max'") + + # 行为树是一个序列 (Sequence)，任务会按顺序执行 + sequence = py_trees.composites.Sequence("CutInAndBrakeAction") + + # 任务1: 等待主车进入指定的触发区域 (更可靠的触发方式) + sequence.add_child(InTriggerRegion( + self.ego_vehicles[0], + region_coords[0], region_coords[1], # x_min, x_max + region_coords[2], region_coords[3] # y_min, y_max + )) + + # 任务2: 执行向左变道切入动作 + sequence.add_child(LaneChange( + self.other_actor, + speed=self._target_speed, + direction='left', + distance_same_lane=self._cut_in_distance + )) + + # 任务3: 切入完成后，立即紧急刹车，并保持静止5秒 + sequence.add_child(StopVehicle(self.other_actor, 5.0)) + + # 任务4: 保持场景继续运行，以便进行碰撞检测 + sequence.add_child(WaitForever()) + + print("----- [DEBUG] 行为树创建成功 -----") + return sequence + + # 5. _create_test_criteria: 定义成功/失败的规则 + def _create_test_criteria(self): + """ + 定义核心的测试标准：碰撞检测 + """ + return [CollisionTest(self.ego_vehicles[0])] + + # 6. __del__: 场景结束后的清理工作 + def __del__(self): + """ + 场景结束时，移除所有创建的演员 + """ + print("----- [SCENARIO END] 正在清理所有场景演员 -----") + self.remove_all_actors() \ No newline at end of file diff --git a/srunner/scenarios/takeover.py b/srunner/scenarios/takeover.py new file mode 100644 index 000000000..9c4bb0415 --- /dev/null +++ b/srunner/scenarios/takeover.py @@ -0,0 +1 @@ +import carla diff --git "a/\347\273\210\347\253\257\346\211\223\345\274\200\346\226\271\345\274\217" "b/\347\273\210\347\253\257\346\211\223\345\274\200\346\226\271\345\274\217" new file mode 100644 index 000000000..206b354c2 --- /dev/null +++ "b/\347\273\210\347\253\257\346\211\223\345\274\200\346\226\271\345\274\217" @@ -0,0 +1,37 @@ +终端里面打开carla +cd ~/download/CARLA_0.9.16 +./CarlaUE4.sh + +~/.bashrc + +python scenario_runner.py --openscenario /home/zhang/下载/scenario_runner/srunner/examples/FollowLeadingVehicle.xosc --reloadWorld + +# 在终端2 +# 注意目录是 CARLA 的 examples 目录 +cd ~/download/CARLA_0.9.16/PythonAPI/examples/ +source /home/zhang/carla_env/bin/activate +# -n 10 代表10辆车, -w 5 代表5个行人。没有 --map 参数！ +python generate_traffic.py -n 10 -w 5 --tm-port 8000 + + + +python /home/zhang/下载/scenario_runner/manual_control.py --rolename hero + +在文件内容中，找到您想运行的那个 标签 + + + + + + + +#carla端口被占用 +1.列出所有的端口 +sudo lsof -i :2000 +接下来会出现： +COMMAND PID USER FD TYPE DEVICE SIZE/OFF NODE NAME +CarlaUE4 12345 zhang 8u IPv4 135790 0t0 TCP *:2000 (LISTEN) +``` * `COMMAND`: 程序的名字，这里应该是 `CarlaUE4`。 +* `PID`: **这是最关键的信息！** 它是进程的ID号，在这个例子里是 `12345`。 +在同一个终端里面结束再打开 +sudo kill -9 12345