diff --git a/carlaAPI/PythonAPI/README.md b/carlaAPI/PythonAPI/README.md deleted file mode 100644 index 2b58442..0000000 --- a/carlaAPI/PythonAPI/README.md +++ /dev/null @@ -1 +0,0 @@ -This directory is from CARLA's offical directory. You can check at this [page](https://github.com/carla-simulator/carla/tree/0.9.15/PythonAPI). (It has little difference from this dir, because our one is from 'release version'.) \ No newline at end of file diff --git a/carlaAPI/PythonAPI/agents/__init__.py b/carlaAPI/PythonAPI/agents/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/carlaAPI/PythonAPI/agents/navigation/__init__.py b/carlaAPI/PythonAPI/agents/navigation/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/carlaAPI/PythonAPI/agents/navigation/basic_agent.py b/carlaAPI/PythonAPI/agents/navigation/basic_agent.py deleted file mode 100644 index 093749d..0000000 --- a/carlaAPI/PythonAPI/agents/navigation/basic_agent.py +++ /dev/null @@ -1,495 +0,0 @@ -# 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 specifed 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, - compute_distance) - - -class BasicAgent(object): - """ - 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 paramters, 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 corrspoing 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): - """ - 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, the vehicle local planner's target location is chosen, - which corresponds (by default), to a location about 5 meters in front of the vehicle. - - :param end_location (carla.Location): final location of the route - :param start_location (carla.Location): starting location of the route - """ - if not start_location: - start_location = self._local_planner.target_waypoint.transform.location - clean_queue = True - else: - start_location = self._vehicle.get_location() - clean_queue = False - - 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 (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 (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 (True, traffic_light) - - return (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 contatining 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 (False, None, -1) - - if not vehicle_list: - vehicle_list = self._world.get_actors().filter("*vehicle*") - - 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 (True, target_vehicle, compute_distance(target_vehicle.get_location(), 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 (True, target_vehicle, compute_distance(target_transform.location, ego_transform.location)) - - return (False, None, -1) - - def _generate_lane_change_path(self, waypoint, direction='left', distance_same_lane=10, - distance_other_lane=25, lane_change_distance=25, - check=True, lane_changes=1, step_distance=2): - """ - 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/carlaAPI/PythonAPI/agents/navigation/behavior_agent.py b/carlaAPI/PythonAPI/agents/navigation/behavior_agent.py deleted file mode 100644 index 3e52faf..0000000 --- a/carlaAPI/PythonAPI/agents/navigation/behavior_agent.py +++ /dev/null @@ -1,318 +0,0 @@ -# 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 random -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, is_within_distance, compute_distance - -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/carlaAPI/PythonAPI/agents/navigation/behavior_types.py b/carlaAPI/PythonAPI/agents/navigation/behavior_types.py deleted file mode 100644 index 3008f9f..0000000 --- a/carlaAPI/PythonAPI/agents/navigation/behavior_types.py +++ /dev/null @@ -1,37 +0,0 @@ -# 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(object): - """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(object): - """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(object): - """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/carlaAPI/PythonAPI/agents/navigation/constant_velocity_agent.py b/carlaAPI/PythonAPI/agents/navigation/constant_velocity_agent.py deleted file mode 100644 index 8d3b1c5..0000000 --- a/carlaAPI/PythonAPI/agents/navigation/constant_velocity_agent.py +++ /dev/null @@ -1,129 +0,0 @@ -# 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 specifed 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/carlaAPI/PythonAPI/agents/navigation/controller.py b/carlaAPI/PythonAPI/agents/navigation/controller.py deleted file mode 100644 index a87488c..0000000 --- a/carlaAPI/PythonAPI/agents/navigation/controller.py +++ /dev/null @@ -1,266 +0,0 @@ -# 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 waypoin. - - :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/carlaAPI/PythonAPI/agents/navigation/global_route_planner.py b/carlaAPI/PythonAPI/agents/navigation/global_route_planner.py deleted file mode 100644 index 7bc2a5a..0000000 --- a/carlaAPI/PythonAPI/agents/navigation/global_route_planner.py +++ /dev/null @@ -1,398 +0,0 @@ -# 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 -from agents.tools.misc import vector - -class GlobalRoutePlanner(object): - """ - This class provides a very high level route plan. - """ - - def __init__(self, wmap, sampling_resolution): - self._sampling_resolution = sampling_resolution - self._wmap = wmap - self._topology = None - self._graph = None - self._id_map = None - self._road_id_to_edge = None - - 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): - """ - This method returns list of (carla.Waypoint, RoadOption) - from origin to destination - """ - route_trace = [] - 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]] - path = [] - - 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] - 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() - 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() - - # 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=vector(entry_wp.transform.location, exit_wp.transform.location), - 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 = [] - 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): - """ - 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 - 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): - """ - 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): - """ - 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 - 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] - 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)): - """ - 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] - 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] - 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/carlaAPI/PythonAPI/agents/navigation/local_planner.py b/carlaAPI/PythonAPI/agents/navigation/local_planner.py deleted file mode 100644 index e4f4b7f..0000000 --- a/carlaAPI/PythonAPI/agents/navigation/local_planner.py +++ /dev/null @@ -1,352 +0,0 @@ -# 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(object): - """ - LocalPlanner implements the basic behavior of following a - trajectory of waypoints that is generated on-the-fly. - - The low-level motion of the vehicle is computed by using two PID controllers, - one is used for the lateral control and the other for the longitudinal control (cruise speed). - - When multiple paths are available (intersections) this local planner makes a random choice, - unless a given global plan has already been specified. - """ - - def __init__(self, vehicle, opt_dict={}, 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 spped limits - - :param value: bool - :return: - """ - self._follow_speed_limits = value - - def _compute_next_waypoints(self, k=1): - """ - Add new waypoints to the trajectory queue. - - :param k: how many waypoints to compute - :return: - """ - # check we do not overflow the queue - available_entries = self._waypoints_queue.maxlen - len(self._waypoints_queue) - k = min(available_entries, k) - - for _ in range(k): - last_waypoint = self._waypoints_queue[-1][0] - next_waypoints = list(last_waypoint.next(self._sampling_radius)) - - if len(next_waypoints) == 0: - break - elif len(next_waypoints) == 1: - # only one option available ==> lanefollowing - next_waypoint = next_waypoints[0] - road_option = RoadOption.LANEFOLLOW - else: - # random choice between the possible options - road_options_list = _retrieve_options( - next_waypoints, last_waypoint) - road_option = random.choice(road_options_list) - next_waypoint = next_waypoints[road_options_list.index( - road_option)] - - self._waypoints_queue.append((next_waypoint, road_option)) - - def set_global_plan(self, current_plan, stop_waypoint_creation=True, clean_queue=True): - """ - Adds a new plan to the local planner. A plan must be a list of [carla.Waypoint, RoadOption] pairs - The 'clean_queue` parameter erases the previous plan if True, otherwise, it adds it to the old one - The 'stop_waypoint_creation' flag stops the automatic creation of random waypoints - - :param current_plan: list of (carla.Waypoint, RoadOption) - :param stop_waypoint_creation: bool - :param clean_queue: bool - :return: - """ - if clean_queue: - self._waypoints_queue.clear() - - # Remake the waypoints queue if the new plan has a higher length than the queue - new_plan_length = len(current_plan) + len(self._waypoints_queue) - if new_plan_length > self._waypoints_queue.maxlen: - new_waypoint_queue = deque(maxlen=new_plan_length) - for wp in self._waypoints_queue: - new_waypoint_queue.append(wp) - self._waypoints_queue = new_waypoint_queue - - for elem in current_plan: - self._waypoints_queue.append(elem) - - self._stop_waypoint_creation = stop_waypoint_creation - - def 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 as i: - return None, RoadOption.VOID - - def get_plan(self): - """Returns the current plan of the local planner""" - return self._waypoints_queue - - def done(self): - """ - Returns whether or not the planner has finished - - :return: boolean - """ - return len(self._waypoints_queue) == 0 - - -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 beggining 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/carlaAPI/PythonAPI/agents/tools/__init__.py b/carlaAPI/PythonAPI/agents/tools/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/carlaAPI/PythonAPI/agents/tools/misc.py b/carlaAPI/PythonAPI/agents/tools/misc.py deleted file mode 100644 index 2d3d4c3..0000000 --- a/carlaAPI/PythonAPI/agents/tools/misc.py +++ /dev/null @@ -1,171 +0,0 @@ -#!/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 - -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 - """ - 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]) + np.finfo(float).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 - """ - 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]) + np.finfo(float).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/carlaAPI/PythonAPI/requirements.txt b/carlaAPI/PythonAPI/requirements.txt deleted file mode 100644 index fa5196e..0000000 --- a/carlaAPI/PythonAPI/requirements.txt +++ /dev/null @@ -1,5 +0,0 @@ -networkx -numpy; python_version < '3.0' -numpy==1.18.4; python_version >= '3.0' -distro -Shapely==1.6.4.post2 diff --git a/carlaAPI/PythonAPI/scene_layout.py b/carlaAPI/PythonAPI/scene_layout.py deleted file mode 100644 index 91d046d..0000000 --- a/carlaAPI/PythonAPI/scene_layout.py +++ /dev/null @@ -1,284 +0,0 @@ -# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . -# Provides map data for users. - -import glob -import os -import sys - -try: - sys.path.append(glob.glob('dist/carla-*%d.%d-%s.egg' % ( - sys.version_info.major, - sys.version_info.minor, - 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) -except IndexError: - pass - -import carla -import random - - -def get_scene_layout(carla_map): - """ - Function to extract the full scene layout to be used as a full scene description to be - given to the user - :return: a dictionary describing the scene. - """ - - def _lateral_shift(transform, shift): - transform.rotation.yaw += 90 - return transform.location + shift * transform.get_forward_vector() - - topology = [x[0] for x in carla_map.get_topology()] - topology = sorted(topology, key=lambda w: w.transform.location.z) - - # A road contains a list of lanes, a each lane contains a list of waypoints - map_dict = dict() - precision = 0.05 - for waypoint in topology: - waypoints = [waypoint] - nxt = waypoint.next(precision) - if len(nxt) > 0: - nxt = nxt[0] - while nxt.road_id == waypoint.road_id: - waypoints.append(nxt) - nxt = nxt.next(precision) - if len(nxt) > 0: - nxt = nxt[0] - else: - break - - left_marking = [_lateral_shift(w.transform, -w.lane_width * 0.5) for w in waypoints] - right_marking = [_lateral_shift(w.transform, w.lane_width * 0.5) for w in waypoints] - - lane = { - "waypoints": waypoints, - "left_marking": left_marking, - "right_marking": right_marking - } - - if map_dict.get(waypoint.road_id) is None: - map_dict[waypoint.road_id] = {} - map_dict[waypoint.road_id][waypoint.lane_id] = lane - - # Generate waypoints graph - waypoints_graph = dict() - for road_key in map_dict: - for lane_key in map_dict[road_key]: - # List of waypoints - lane = map_dict[road_key][lane_key] - - for i in range(0, len(lane["waypoints"])): - next_ids = [w.id for w in lane["waypoints"][i + 1:len(lane["waypoints"])]] - - # Get left and right lane keys - left_lane_key = lane_key - 1 if lane_key - 1 != 0 else lane_key - 2 - right_lane_key = lane_key + 1 if lane_key + 1 != 0 else lane_key + 2 - - # Get left and right waypoint ids only if they are valid - left_lane_waypoint_id = -1 - if left_lane_key in map_dict[road_key]: - left_lane_waypoints = map_dict[road_key][left_lane_key]["waypoints"] - if i < len(left_lane_waypoints): - left_lane_waypoint_id = left_lane_waypoints[i].id - - right_lane_waypoint_id = -1 - if right_lane_key in map_dict[road_key]: - right_lane_waypoints = map_dict[road_key][right_lane_key]["waypoints"] - if i < len(right_lane_waypoints): - right_lane_waypoint_id = right_lane_waypoints[i].id - - # Get left and right margins (aka markings) - lm = carla_map.transform_to_geolocation(lane["left_marking"][i]) - rm = carla_map.transform_to_geolocation(lane["right_marking"][i]) - - # Waypoint Position - wl = carla_map.transform_to_geolocation(lane["waypoints"][i].transform.location) - - # Waypoint Orientation - wo = lane["waypoints"][i].transform.rotation - - # Waypoint dict - waypoint_dict = { - "road_id": road_key, - "lane_id": lane_key, - "position": [wl.latitude, wl.longitude, wl.altitude], - "orientation": [wo.roll, wo.pitch, wo.yaw], - "left_margin_position": [lm.latitude, lm.longitude, lm.altitude], - "right_margin_position": [rm.latitude, rm.longitude, rm.altitude], - "next_waypoints_ids": next_ids, - "left_lane_waypoint_id": left_lane_waypoint_id, - "right_lane_waypoint_id": right_lane_waypoint_id - } - waypoints_graph[map_dict[road_key][lane_key]["waypoints"][i].id] = waypoint_dict - - return waypoints_graph - - -def get_dynamic_objects(carla_world, carla_map): - # Private helper functions - def _get_bounding_box(actor): - bb = actor.bounding_box.extent - corners = [ - carla.Location(x=-bb.x, y=-bb.y), - carla.Location(x=bb.x, y=-bb.y), - carla.Location(x=bb.x, y=bb.y), - carla.Location(x=-bb.x, y=bb.y)] - t = actor.get_transform() - t.transform(corners) - corners = [carla_map.transform_to_geolocation(p) for p in corners] - return corners - - def _get_trigger_volume(actor): - bb = actor.trigger_volume.extent - corners = [carla.Location(x=-bb.x, y=-bb.y), - carla.Location(x=bb.x, y=-bb.y), - carla.Location(x=bb.x, y=bb.y), - carla.Location(x=-bb.x, y=bb.y), - carla.Location(x=-bb.x, y=-bb.y)] - corners = [x + actor.trigger_volume.location for x in corners] - t = actor.get_transform() - t.transform(corners) - corners = [carla_map.transform_to_geolocation(p) for p in corners] - return corners - - def _split_actors(actors): - vehicles = [] - traffic_lights = [] - speed_limits = [] - walkers = [] - stops = [] - static_obstacles = [] - for actor in actors: - if 'vehicle' in actor.type_id: - vehicles.append(actor) - elif 'traffic_light' in actor.type_id: - traffic_lights.append(actor) - elif 'speed_limit' in actor.type_id: - speed_limits.append(actor) - elif 'walker' in actor.type_id: - walkers.append(actor) - elif 'stop' in actor.type_id: - stops.append(actor) - elif 'static.prop' in actor.type_id: - static_obstacles.append(actor) - - - return (vehicles, traffic_lights, speed_limits, walkers, stops, static_obstacles) - - # Public functions - def get_stop_signals(stops): - stop_signals_dict = dict() - for stop in stops: - st_transform = stop.get_transform() - location_gnss = carla_map.transform_to_geolocation(st_transform.location) - st_dict = { - "id": stop.id, - "position": [location_gnss.latitude, location_gnss.longitude, location_gnss.altitude], - "trigger_volume": [[v.longitude, v.latitude, v.altitude] for v in _get_trigger_volume(stop)] - } - stop_signals_dict[stop.id] = st_dict - return stop_signals_dict - - def get_traffic_lights(traffic_lights): - traffic_lights_dict = dict() - for traffic_light in traffic_lights: - tl_transform = traffic_light.get_transform() - location_gnss = carla_map.transform_to_geolocation(tl_transform.location) - tl_dict = { - "id": traffic_light.id, - "state": int(traffic_light.state), - "position": [location_gnss.latitude, location_gnss.longitude, location_gnss.altitude], - "trigger_volume": [[v.longitude, v.latitude, v.altitude] for v in _get_trigger_volume(traffic_light)] - } - traffic_lights_dict[traffic_light.id] = tl_dict - return traffic_lights_dict - - def get_vehicles(vehicles): - vehicles_dict = dict() - for vehicle in vehicles: - v_transform = vehicle.get_transform() - location_gnss = carla_map.transform_to_geolocation(v_transform.location) - v_dict = { - "id": vehicle.id, - "position": [location_gnss.latitude, location_gnss.longitude, location_gnss.altitude], - "orientation": [v_transform.rotation.roll, v_transform.rotation.pitch, v_transform.rotation.yaw], - "bounding_box": [[v.longitude, v.latitude, v.altitude] for v in _get_bounding_box(vehicle)] - } - vehicles_dict[vehicle.id] = v_dict - return vehicles_dict - - def get_hero_vehicle(hero_vehicle): - if hero_vehicle is None: - return hero_vehicle - - hero_waypoint = carla_map.get_waypoint(hero_vehicle.get_location()) - hero_transform = hero_vehicle.get_transform() - location_gnss = carla_map.transform_to_geolocation(hero_transform.location) - - hero_vehicle_dict = { - "id": hero_vehicle.id, - "position": [location_gnss.latitude, location_gnss.longitude, location_gnss.altitude], - "road_id": hero_waypoint.road_id, - "lane_id": hero_waypoint.lane_id - } - return hero_vehicle_dict - - def get_walkers(walkers): - walkers_dict = dict() - for walker in walkers: - w_transform = walker.get_transform() - location_gnss = carla_map.transform_to_geolocation(w_transform.location) - w_dict = { - "id": walker.id, - "position": [location_gnss.latitude, location_gnss.longitude, location_gnss.altitude], - "orientation": [w_transform.rotation.roll, w_transform.rotation.pitch, w_transform.rotation.yaw], - "bounding_box": [[v.longitude, v.latitude, v.altitude] for v in _get_bounding_box(walker)] - } - walkers_dict[walker.id] = w_dict - return walkers_dict - - def get_speed_limits(speed_limits): - speed_limits_dict = dict() - for speed_limit in speed_limits: - sl_transform = speed_limit.get_transform() - location_gnss = carla_map.transform_to_geolocation(sl_transform.location) - sl_dict = { - "id": speed_limit.id, - "position": [location_gnss.latitude, location_gnss.longitude, location_gnss.altitude], - "speed": int(speed_limit.type_id.split('.')[2]) - } - speed_limits_dict[speed_limit.id] = sl_dict - return speed_limits_dict - - def get_static_obstacles(static_obstacles): - static_obstacles_dict = dict() - for static_prop in static_obstacles: - sl_transform = static_prop.get_transform() - location_gnss = carla_map.transform_to_geolocation(sl_transform.location) - sl_dict = { - "id": static_prop.id, - "position": [location_gnss.latitude, location_gnss.longitude, location_gnss.altitude] - } - static_obstacles_dict[static_prop.id] = sl_dict - return static_obstacles_dict - - actors = carla_world.get_actors() - vehicles, traffic_lights, speed_limits, walkers, stops, static_obstacles = _split_actors(actors) - - hero_vehicles = [vehicle for vehicle in vehicles if - 'vehicle' in vehicle.type_id and vehicle.attributes['role_name'] == 'hero'] - hero = None if len(hero_vehicles) == 0 else random.choice(hero_vehicles) - - return { - 'vehicles': get_vehicles(vehicles), - 'hero_vehicle': get_hero_vehicle(hero), - 'walkers': get_walkers(walkers), - 'traffic_lights': get_traffic_lights(traffic_lights), - 'stop_signs': get_stop_signals(stops), - 'speed_limits': get_speed_limits(speed_limits), - 'static_obstacles': get_static_obstacles(static_obstacles) - } diff --git a/docs/assets/carla-error-situation.png b/docs/assets/carla-error-situation.png deleted file mode 100644 index d95e419..0000000 Binary files a/docs/assets/carla-error-situation.png and /dev/null differ diff --git a/docs/installation.md b/docs/installation.md deleted file mode 100644 index 503e6e3..0000000 --- a/docs/installation.md +++ /dev/null @@ -1,263 +0,0 @@ -# Project Environment & Installation - -Our project is based on **`Carla 0.9.14`** version, so other components are based on Carla's system requirements. You can check informatoins about Carla 0.9.14 installation and requirements at [this link](https://carla.readthedocs.io/en/0.9.14/start_quickstart/). - - -# OS: Ubuntu 20.04 focal -```shell -# Carla's official supported platform is 18.04, but it was EOL at May 31 2023. So we've chosen 20.04 instead of. -``` - - -# Python: 3.8 -```shell -# Carla library is compatiable with Python 2.7, 3.6, 3.7, and 3.8. -``` - -## Install Python - -I've installed python by ppa. -```shell -sudo apt update -sudo apt install software-properties-common -sudo add-apt-repository ppa:deadsnakes/ppa -sudo apt install python3.8 python3-pip -``` - -If you wanna check is python ready, please run this command and check the result. -```shell -> python3 --version -Python 3.8.10 - -> pip3 --version -pip 20.0.2 from /usr/lib/python3/dist-packages/pip (python 3.8) -``` - -You need 20.3 or higher for pip to install carla api. So if your pip version is low, run this command: -``` -sudo apt upgrade pip -``` - -If your pip is still old version after update, your new pip is in `~/.local/bin/`. So update $PATH or override pip command by alias. - -## Install Python Libraries and venv - -If your OS and Python is ready to go, you need to install numpy and pygame to use Carla Client. - -```shell -# If pip doesn't work, first try pip3. Still not working? Check did you installed pip correctly. -pip install numpy pygame -sudo apt install python3-venv -``` - - -# Carla: 0.9.14 - -## Install Carla - -### 1. Tips -There are two way of installing Carla. First is by Debian package, second is downloading it from github. In this document, I wrote the second way only. Move to [carla's installation document](https://carla.readthedocs.io/en/0.9.14/start_quickstart/#carla-installation) to check the Debian package way. -### 2. Downloading & Unzip -Move to this **[link](https://github.com/carla-simulator/carla/releases/tag/0.9.14/)** and download Carla compressed file and Additional Maps too. The Carla package is 7.8Gb and 6.2Gb to Additional Maps, so let your environment have good network conditions. -### 3. Env setting -After download, move them to certain location and unzip. Remember where did you cloned and unziped, and put the location at variable `CARLA` of [`env.template`](/settings/env.template). -### 4. Import AdditionalAssets -- Linux - move the package to the Import folder and run the following script to extract the contents: - ```shell - cd path/to/carla/root - ./ImportAssets.sh - ``` -- Windows - Extract the contents directly in the root folder - -## Error solve - - - -If your Carla is too bright and blue, this command will help. Don't forget to put this line in your rc file. `ex: zshrc` -``` -export VK_ICD_FILENAMES="/usr/share/vulkan/icd.d/nvidia_icd.json" -``` - -## Install Carla Client Library - -Several ways to install carla library was introduced in [official document](https://carla.readthedocs.io/en/0.9.14/start_quickstart/#carla-installation). But installing by whl and egg didn't worked, so I used venv and pip way. - -```shell -# This script create and call .env, create venv and install libraries for carla. -./init.sh -``` - - -# ROS Bridge Installation - -You can see full document [here](https://carla.readthedocs.io/projects/ros-bridge/en/latest/ros_installation_ros2/). - -## Prerequisite -- (ROS2 Foxy)[https://docs.ros.org/en/foxy/Installation.html] -- Additional ROS packages(ex: rviz) -- CARLA 0.9.11 or later (We already have done! 😄) - -### ROS2 Foxy -Follow this documentation. [Link](https://docs.ros.org/en/foxy/Installation/Alternatives/Ubuntu-Development-Setup.html) - -1. Locale setting -```shell -locale # check for UTF-8 - -sudo apt update && sudo apt install locales - -sudo locale-gen en_US en_US.UTF-8 - -sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 - -export LANG=en_US.UTF-8 - -locale # verify settings -``` - -2. Add ROS2 repository -```shell -sudo apt install software-properties-common - -sudo add-apt-repository universe - -sudo apt update && sudo apt install curl -y - -sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg - -echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null -``` - -3. Install development tools and ROS tools -```shell -sudo apt update && sudo apt install -y \ - libbullet-dev \ - python3-pip \ - python3-pytest-cov \ - ros-dev-tools - -# install some pip packages needed for testing -python3 -m pip install -U \ - argcomplete \ - flake8-blind-except \ - flake8-builtins \ - flake8-class-newline \ - flake8-comprehensions \ - flake8-deprecated \ - flake8-docstrings \ - flake8-import-order \ - flake8-quotes \ - pytest-repeat \ - pytest-rerunfailures \ - pytest - -# install Fast-RTPS dependencies -sudo apt install --no-install-recommends -y \ - libasio-dev \ - libtinyxml2-dev - -# install Cyclone DDS dependencies -sudo apt install --no-install-recommends -y \ - libcunit1-dev -``` - -4. Get ROS2 Code -```shell -mkdir -p ~/ros2_foxy/src - -cd ~/ros2_foxy - -vcs import --input https://raw.githubusercontent.com/ros2/ros2/foxy/ros2.repos src -``` - -5. Install dependencies using rosdep -```shell -sudo apt upgrade - -sudo rosdep init - -rosdep update - -rosdep install --from-paths src --ignore-src -y --skip-keys "fastcdr rti-connext-dds-5.3.1 urdfdom_headers" - -colcon build --symlink-install -``` - -### RVIZ2 install -``` -sudo apt install ros-foxy-rviz2 -``` - -## Install ROS Bridge -```shell -mkdir -p ~/carla-ros-bridge && cd ~/carla-ros-bridge - -git clone --recurse-submodules https://github.com/carla-simulator/ros-bridge.git src/ros-bridge - -source /opt/ros/foxy/setup.zsh - -sudo apt install ros-foxy-pcl-conversions - -rosdep update - -rosdep install --from-paths src --ignore-src -r - -colcon build -``` - -## Run ROS Bridge - -1. Run your Carla. -```shell -carla -``` - -2. Add the correct CARLA modules to your Python path -```shell -export CARLA_ROOT= - -export PYTHONPATH=$PYTHONPATH:$CARLA_ROOT/PythonAPI/carla/dist/carla-.egg:$CARLA_ROOT/PythonAPI/carla -``` -Don't forget to put this to your rc file. `ex: zshrc` - -3. Add the source path for the ROS bridge workspace -```shell -ros-init # custom alias command -``` - -4. Examples -```shell -ros2 launch carla_ros_bridge carla_ros_bridge.launch.py -ros2 launch carla_ros_bridge carla_ros_bridge_with_example_ego_vehicle.launch.py -``` - -- If your ros2 cannot find the packages, try to run `/opt/ros/$VERSION/setup.zsh`. `~/ros2_foxy/install/local_setup.zsh`, and `~/carla_ros_bridge/install/setup.zsh`. I recommend you to set these in `~/.zshrc` to automate. Here is my zshrc file. (Just as an example) -```shell -# ... - -# CARLA Custom command and vars -alias dev=~/dev -export CARLA=~/dev/CarlaUE4 -export ROS_VERSION=foxy -export ROS2=~/ros2_$ROS_VERSION -alias carla="$CARLA/CarlaUE4.sh" -alias pip3="~/.local/bin/pip3.11" -alias pip="~/.local/bin/pip3.11" -alias ros-init-local="source $ROS2/install/local_setup.zsh" -alias ros-init-opt="source /opt/ros/$ROS_VERSION/setup.zsh" -alias ros-init-bridge="source ~/carla_ros_bridge/install/setup.zsh" - -# CARLA error solve -export VK_ICD_FILENAMES="/usr/share/vulkan/icd.d/nvidia_icd.json" - -# CARLA ROS BRIDGE -export CARLA_ROOT=$CARLA -export PYTHONPATH=$PYTHONPATH:$CARLA_ROOT/PythonAPI/carla/dist/carla-0.9.14-py2.7-linux-x86_64.egg:$CARLA_ROOT/PythonAPI/carla:$ROS2/install - -ros-init-opt -ros-init-local -``` - -- Check `CARLA_VERSION` file in `~/carla-ros-bridge/install/carla_ros_bridge/lib/python3.8/site-packages/carla_ros_bridge`. If the version is different with your CARLA, just change as your carla. (0.9.14 in case of this tutorial) \ No newline at end of file diff --git a/docs/subject.en.md b/docs/subject.en.md index a6b94b7..de7d51b 100644 --- a/docs/subject.en.md +++ b/docs/subject.en.md @@ -191,4 +191,4 @@ This work is licensed under a [cc-by-nc-sa]: http://creativecommons.org/licenses/by-nc-sa/4.0/ [cc-by-nc-sa-image]: https://licensebuttons.net/l/by-nc-sa/4.0/88x31.png -[cc-by-nc-sa-shield]: https://img.shields.io/badge/License-CC%20BY--NC--SA%204.0-lightgrey.svg \ No newline at end of file +[cc-by-nc-sa-shield]: https://img.shields.io/badge/License-CC%20BY--NC--SA%204.0-lightgrey.svg diff --git a/env.template b/env.template deleted file mode 100644 index ae5da29..0000000 --- a/env.template +++ /dev/null @@ -1,3 +0,0 @@ -CARLA=~/dev/CarlaUE4 -PYTHONAPI=carlaAPI/PythonAPI -VENV=venv \ No newline at end of file diff --git a/init.sh b/init.sh deleted file mode 100755 index ec694a6..0000000 --- a/init.sh +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/zsh -# env setting -rm -f .env -cat env.template >> .env -export $(grep -v '^#' .env | xargs -d '\n') - -# venv setting -rm -rf venv -# pip install --upgrade pip -# pip3 install --upgrade pip -python3 -m venv $VENV -source $VENV/bin/activate -pip3 install -qU pip -pip3 install -qr requirement.txt -printf "\nrun this command to activate the virtual environment.\n------------------------------------\nsource $VENV/bin/activate\n" diff --git a/requirement.txt b/requirement.txt deleted file mode 100644 index 91711cc..0000000 --- a/requirement.txt +++ /dev/null @@ -1,2 +0,0 @@ -carla==0.9.15 -pkg_resources==0.0.0 \ No newline at end of file