From d1dcfc45850a7f820161c131d675f2e68f6c06ab Mon Sep 17 00:00:00 2001 From: djunru Date: Mon, 28 Nov 2022 22:03:39 -0500 Subject: [PATCH] push --- legacy/gui_interface/backend/carla_env.py | 732 +++++++++ legacy/gui_interface/backend/demo_setting2 | 24 + .../backend/full_path_vehicle.py | 421 +++++ .../backend/generate_path_omit_regulation.py | 286 ++++ .../backend/human_ego_control.py | 131 ++ .../backend/initial_intersection.py | 752 +++++++++ .../backend/intersection_backend.py | 438 +++++ .../backend/intersection_definition.py | 1450 +++++++++++++++++ .../backend/intersection_settings_helper.py | 197 +++ .../backend/multiple_vehicle_control.py | 629 +++++++ .../backend/multiple_vehicle_control_debug.py | 590 +++++++ ...pursuit_with_auto_trajectory_generation.py | 498 ++++++ .../backend/section_definition.py | 369 +++++ .../backend/section_environment.py | 898 ++++++++++ .../backend/section_init_definition.py | 594 +++++++ .../backend/section_vehicle_control.py | 329 ++++ .../backend/third_intersection_setting | 12 + .../backend/vehicle_length_config.txt | 27 + scripts/freeway_test.py | 77 +- .../sim_backend/experiments/experiment.py | 31 +- umich_sim/sim_backend/helpers.py | 4 +- 21 files changed, 8477 insertions(+), 12 deletions(-) create mode 100644 legacy/gui_interface/backend/carla_env.py create mode 100644 legacy/gui_interface/backend/demo_setting2 create mode 100644 legacy/gui_interface/backend/full_path_vehicle.py create mode 100644 legacy/gui_interface/backend/generate_path_omit_regulation.py create mode 100644 legacy/gui_interface/backend/human_ego_control.py create mode 100644 legacy/gui_interface/backend/initial_intersection.py create mode 100644 legacy/gui_interface/backend/intersection_backend.py create mode 100644 legacy/gui_interface/backend/intersection_definition.py create mode 100644 legacy/gui_interface/backend/intersection_settings_helper.py create mode 100644 legacy/gui_interface/backend/multiple_vehicle_control.py create mode 100644 legacy/gui_interface/backend/multiple_vehicle_control_debug.py create mode 100644 legacy/gui_interface/backend/pure_pursuit_with_auto_trajectory_generation.py create mode 100644 legacy/gui_interface/backend/section_definition.py create mode 100644 legacy/gui_interface/backend/section_environment.py create mode 100644 legacy/gui_interface/backend/section_init_definition.py create mode 100644 legacy/gui_interface/backend/section_vehicle_control.py create mode 100644 legacy/gui_interface/backend/third_intersection_setting create mode 100644 legacy/gui_interface/backend/vehicle_length_config.txt diff --git a/legacy/gui_interface/backend/carla_env.py b/legacy/gui_interface/backend/carla_env.py new file mode 100644 index 0000000..19409fd --- /dev/null +++ b/legacy/gui_interface/backend/carla_env.py @@ -0,0 +1,732 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Sun May 24 13:45:54 2020 + +@author: shijiliu +""" +import glob +import os +import sys +import random + +try: + sys.path.append(glob.glob('../carla/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 numpy as np +from configobj import ConfigObj +import math + +# color for debug use +red = carla.Color(255, 0, 0) +green = carla.Color(0, 255, 0) +blue = carla.Color(47, 210, 231) +cyan = carla.Color(0, 255, 255) +yellow = carla.Color(255, 255, 0) +orange = carla.Color(255, 162, 0) +white = carla.Color(255, 255, 255) + +# API Helper - relocated on 2/16/2022 to Helpers.py +def config_world(world, synchrony = True, delta_seconds = 0.02): + ''' + Effects + ------- + Config the carla world's synchrony and time-step + tutorial: https://carla.readthedocs.io/en/latest/adv_synchrony_timestep/ + + Parameters + ---------- + synchrony : TYPE, optional + DESCRIPTION. The default is True. + delta_seconds : TYPE, optional + DESCRIPTION. The default is 0.02. + + Returns + ------- + synchrony, delta_seconds + ''' + + + settings = world.get_settings() + settings.synchronous_mode = synchrony + settings.fixed_delta_seconds = delta_seconds + world.apply_settings(settings) + return synchrony, delta_seconds + +class CARLA_ENV(): + # Move this information into the Experiment class + def __init__(self, world): + + self.world = world + self.blueprint_library = self.world.get_blueprint_library() + + self.vehicle_dict = {} + self.walker_dict = {} + self.sensor_dict = {} + self.config_env(synchrony = False) + self.distance_between_vehicles = ConfigObj() # store the distance between vehicles + + # get the length of all vehicles + vehicle_length_config_directory = os.path.join('backend', 'vehicle_length_config.txt') + + currentDirectory = os.path.join(os.path.dirname(os.getcwd()), vehicle_length_config_directory) + print(currentDirectory) + self.vehicle_model_length_config = ConfigObj(currentDirectory) + print(self.vehicle_model_length_config) + + # Remove, redundant code + def config_env(self, synchrony = False, delta_seconds = 0.02): + + self.synchrony = synchrony + self.delta_seconds = delta_seconds + settings = self.world.get_settings() + settings.synchronous_mode = synchrony + settings.fixed_delta_seconds = delta_seconds + self.world.apply_settings(settings) + + # Static Method in Vehicle Class + def spawn_vehicle(self, model_name = None, spawn_point = None, color = None): + ''' + Parameters + ---------- + model_name : str TYPE, optional + DESCRIPTION: The default is None. + spawn_point : carla.Transform() TYPE, optional + DESCRIPTION. The default is None. + color : str, optional + the color of the vehicle + + Returns + ------- + Uniquename of the actor. + + ''' + if model_name == None: + bp = random.choice(self.blueprint_library.filter('vehicle.*.*')) + else: + bp = random.choice(self.blueprint_library.filter(model_name)) + + if spawn_point == None: + spawn_point = random.choice(self.world.get_map().get_spawn_points()) + + if color != None and bp.has_attribute('color'): + bp.set_attribute('color',color) + + + vehicle = self.world.spawn_actor(bp,spawn_point) + self.vehicle_dict[vehicle.type_id + '_' + str(vehicle.id)] = vehicle + return vehicle.type_id + '_' + str(vehicle.id) + + # Method in Vehicle Class - relocated on 2/16/22 to Vehicle.py + def move_vehicle_location(self, uniquename, spawn_point): + ''' + + + Parameters + ---------- + uniquename : string + uniquename of a vehicle. + spawn_point : carla.Transform() + new spawn point of the vehicle + + Returns + ------- + None. + + ''' + + + vehicle = self.vehicle_dict[uniquename] + vehicle.set_transform(spawn_point) + + # Method in Experiment Class - marked as redundant on 2/16/22 + def destroy_vehicle(self, uniquename): + if uniquename in self.vehicle_dict: + self.vehicle_dict[uniquename].destroy() # destroy the vehicle in carla + self.vehicle_dict.pop(uniquename) # remove the vehicle from dictionary + + # Method in Vehicle Class - relocated on 2/16/22 to Vehicle.py + def get_vehicle_model_length(self, model_name): + ''' + + + Parameters + ---------- + model_name : string + the model name of a vehicle model, or type_id of a vehicle + + Returns + ------- + the length of the vehicle, or bounding_box.extent.x + + ''' + length = None + if model_name in self.vehicle_model_length_config: + length = float(self.vehicle_model_length_config[model_name]) + else: + print(model_name) + print("Error: invalid model_name entered to get vehicle model length") + + return length + + # Method in Vehicle Class - marked as redundant on 2/26/2022 + def get_vehicle_bounding_box(self, uniquename): + ''' + + + Parameters + ---------- + uniquename : string + uniquename of a vehicle. + + Returns + ------- + the carla actor corresponding to the uniquename. + None type will be sent is uniquename doesn't exist + + + ''' + ret_vehicle_bb = None + if uniquename in self.vehicle_dict: + ret_vehicle_bb = self.vehicle_dict[uniquename].bounding_box.extent + + return ret_vehicle_bb + + # Method in Experiment Class - relocated on 2/16/22 to Experiment.py + def destroy_actors(self): + ''' + Effects + ------- + Destroy all actors that have been spawned + + Returns + ------- + None. + + ''' + for index in self.vehicle_dict.keys(): + self.vehicle_dict[index].destroy() + for index in self.walker_dict.keys(): + self.walker_dict[index].destroy() + for index in self.sensor_dict.keys(): + self.sensor_dict[index].destroy() + + self.vehicle_dict.clear() + self.walker_dict.clear() + self.sensor_dict.clear() + print("destroyed all actors") + + # Method of Controller class - marked as redundant on 2/16/22 (exists in the base carla.Vehicle class) + def apply_vehicle_control(self, uniquename, vehicle_control): + ''' + Effects: apply control to a specific vehicle + + Parameters + ---------- + uniquename : str TYPE + DESCRIPTION. + vehicle_control : vehicle control TYPE, https://carla.readthedocs.io/en/latest/python_api/#carla.Vehicle + DESCRIPTION. + + Returns + ------- + None. + + ''' + vehicle = self.vehicle_dict[uniquename] + vehicle.apply_control(vehicle_control) + + # Private method of the vehicle class - marked as redundant on 2/16/22 (exists in base carla.Vehicle class "set_target_velocity") + def set_vehicle_velocity(self, uniquename, vehicle_velocity): + ''' + + + Parameters + ---------- + uniquename : string + uniquename of vehicle. + vehicle_velocity : varla.Vector3D + vehicle speed + + Returns + ------- + None. + + ''' + vehicle = self.vehicle_dict[uniquename] + vehicle.set_target_velocity(vehicle_velocity) + + # Method of the Vehicle class - relocated on 2/16/22 to Vehicle.py + def get_forward_speed(self, uniquename): + ''' + Get the forward speed of the vehicle + + Parameters + ---------- + uniquename : TYPE + name of the vehicle. + + Returns + ------- + forward speed of the vehicle. + + ''' + vehicle = self.vehicle_dict[uniquename] + velocity = vehicle.get_velocity() + return (velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2)**0.5 + + # Probably not necessary - marked as redundant on 2/16/22 + def vehicle_available(self, uniquename): + ''' + check whether the vehicle exists + + Parameters + ---------- + uniquename : str + name of the vehicle. + + Returns + ------- + exists : bool + whether the vehicle exists + + ''' + if uniquename in self.vehicle_dict: + return True + else: + return False + + # Method of Vehicle Class - relocation on 2/16/22 to Vehicle.py + def get_transform_2d(self, uniquename): + ''' + + + Parameters + ---------- + uniquename : str + name of the vehicle. + + Returns + ------- + location and orientation of the vehicle. + + ''' + vehicle = self.vehicle_dict[uniquename] + transform = vehicle.get_transform() + location_2d = [transform.location.x, transform.location.y] + yaw = transform.rotation.yaw + + return (location_2d,yaw) + + # Method of Vehicle class - marked as unneeded on 2/16/22 + def get_transform_3d(self, uniquename): + ''' + + + Parameters + ---------- + uniquename : str + name of the vehicle. + + Returns + ------- + 3d transform of the vehicle + + ''' + vehicle = self.vehicle_dict[uniquename] + transform = vehicle.get_transform() + + + return transform + + # Method of Vehicle Class (takes in list of all other vehicles) - relocated on 2/16/22 to Vehicle.py + def update_vehicle_distance(self): + ''' + Update the distance between each 2 vehicles + This function should be called each world.tick() + + Returns + ------- + None. + + ''' + self.distance_between_vehicles.reset() # reset the configuration file each update + + # get all available vehicles + vehicle_uniquenames = [] + for name in self.vehicle_dict: + vehicle_uniquenames.append(name) + self.distance_between_vehicles[name] = {} # create empty storage + + for ii in range(len(vehicle_uniquenames)): + for jj in range(ii,len(vehicle_uniquenames)): + name_1 = vehicle_uniquenames[ii] + name_2 = vehicle_uniquenames[jj] + if name_1 == name_2: + self.distance_between_vehicles[name_1][name_2] = 0.0 # distance with itself, 0.0 + else: + vehicle_1 = self.vehicle_dict[name_1] + vehicle_2 = self.vehicle_dict[name_2] + location_1 = vehicle_1.get_transform().location + location_2 = vehicle_2.get_transform().location + distance = math.sqrt((location_1.x - location_2.x)**2 + (location_1.y - location_2.y)**2) + self.distance_between_vehicles[name_1][name_2] = distance + self.distance_between_vehicles[name_2][name_1] = distance + + # Method of Vehicle Class (Make a single check Vehicle in direction function) (maybe Controller class) + def check_vehicle_in_front(self, uniquename, safety_distance): + ''' + + + Parameters + ---------- + uniquename : str + name of the vehicle. + + safety_distance: float + allowed closest distance between two vehicles + + Returns + ------- + has_vehicle_in_front : bool + whether there exists a vehicle within safety distance + distance: float + distance between this vehicle and the vehicle in the front + + ''' + # get the distance between this vehicle and other vehicles + distance_with_other_vehicle = self.distance_between_vehicles[uniquename] + + # get the bounding box of this vehicle + vehicle_bb = self.vehicle_dict[uniquename].bounding_box.extent + safety_distance += vehicle_bb.y / 2 # add the half length of the vehicle + + has_vehicle_in_front = False + distance = None + + vehicle_1 = self.vehicle_dict[uniquename] + location_1 = vehicle_1.get_transform().location + forward_vector = vehicle_1.get_transform().get_forward_vector() + + for name in distance_with_other_vehicle: + if name != uniquename and distance_with_other_vehicle[name] < safety_distance and name in self.vehicle_dict: # a possible vehicle + location_2 = self.vehicle_dict[name].get_transform().location + vec1_2 = np.array([location_2.x - location_1.x, location_2.y - location_1.y]) + forward_vector_2d = np.array([forward_vector.x, forward_vector.y]) + + norm_vec1_2 = vec1_2 / np.linalg.norm(vec1_2) + norm_forward_vector_2d = forward_vector_2d / np.linalg.norm(forward_vector_2d) + dot_product = np.dot(norm_vec1_2,norm_forward_vector_2d) + angle = np.arccos(dot_product) + + if angle < np.arctan(vehicle_bb.y / vehicle_bb.x):#np.arcsin((vehicle_bb.y + 1) / distance_with_other_vehicle[name]):#np.arctan(vehicle_bb.y / vehicle_bb.x): + has_vehicle_in_front = True + distance = np.dot(vec1_2,forward_vector_2d) + break + + return has_vehicle_in_front, distance + + # Likely redundant but double check - marked as redundant on 2/28/22 + def check_vehicle_in_front_freeway(self, uniquename, safety_distance): + ''' + greatly narrow the front view for freeway + + + Parameters + ---------- + uniquename : str + name of the vehicle. + + safety_distance: float + allowed closest distance between two vehicles + + Returns + ------- + has_vehicle_in_front : bool + whether there exists a vehicle within safety distance + distance: float + distance between this vehicle and the vehicle in the front + + ''' + # get the distance between this vehicle and other vehicles + distance_with_other_vehicle = self.distance_between_vehicles[uniquename] + + # get the bounding box of this vehicle + vehicle_bb = self.vehicle_dict[uniquename].bounding_box.extent + safety_distance += vehicle_bb.x / 2 # add the half length of the vehicle + + has_vehicle_in_front = False + distance = None + + vehicle_1 = self.vehicle_dict[uniquename] + location_1 = vehicle_1.get_transform().location + forward_vector = vehicle_1.get_transform().get_forward_vector() + + for name in distance_with_other_vehicle: + if name != uniquename and distance_with_other_vehicle[name] < safety_distance and name in self.vehicle_dict: # a possible vehicle + location_2 = self.vehicle_dict[name].get_transform().location + vec1_2 = np.array([location_2.x - location_1.x, location_2.y - location_1.y]) + forward_vector_2d = np.array([forward_vector.x, forward_vector.y]) + + norm_vec1_2 = vec1_2 / np.linalg.norm(vec1_2) + norm_forward_vector_2d = forward_vector_2d / np.linalg.norm(forward_vector_2d) + dot_product = np.dot(norm_vec1_2,norm_forward_vector_2d) + angle = np.arccos(dot_product) + + + + if angle < np.pi / 36:#np.arcsin((vehicle_bb.y / 2) / distance_with_other_vehicle[name]):#np.arcsin((vehicle_bb.y + 1) / distance_with_other_vehicle[name]):#np.arctan(vehicle_bb.y / vehicle_bb.x): + has_vehicle_in_front = True + distance = np.dot(vec1_2,forward_vector_2d) + break + + return has_vehicle_in_front, distance + + # Make a single check Vehicle in direction function - marked as redundant on 2/28/22 + def check_vehicle_in_back_freeway(self, uniquename, safety_distance): + ''' + + + + Parameters + ---------- + uniquename : str + name of the vehicle. + + safety_distance: float + allowed closest distance between two vehicles + + Returns + ------- + has_vehicle_in_back : bool + whether there exists a vehicle within safety distance + distance: float + distance between this vehicle and the vehicle in the front + + ''' + # get the distance between this vehicle and other vehicles + distance_with_other_vehicle = self.distance_between_vehicles[uniquename] + + # get the bounding box of this vehicle + vehicle_bb = self.vehicle_dict[uniquename].bounding_box.extent + safety_distance += vehicle_bb.x / 2 # add the half length of the vehicle + + has_vehicle_in_back = False + distance = None + smallest_distance = np.inf + + vehicle_1 = self.vehicle_dict[uniquename] + location_1 = vehicle_1.get_transform().location + forward_vector = vehicle_1.get_transform().get_forward_vector() + + for name in distance_with_other_vehicle: + if name != uniquename and distance_with_other_vehicle[name] < safety_distance and name in self.vehicle_dict: # a possible vehicle + location_2 = self.vehicle_dict[name].get_transform().location + vec1_2 = np.array([location_2.x - location_1.x, location_2.y - location_1.y]) + forward_vector_2d = np.array([forward_vector.x, forward_vector.y]) + + norm_vec1_2 = vec1_2 / np.linalg.norm(vec1_2) + norm_forward_vector_2d = forward_vector_2d / np.linalg.norm(forward_vector_2d) + dot_product = np.dot(norm_vec1_2,norm_forward_vector_2d) + angle = np.arccos(dot_product) + + + + if angle > np.pi - np.arcsin((vehicle_bb.y / 2) / distance_with_other_vehicle[name]):#np.arcsin((vehicle_bb.y + 1) / distance_with_other_vehicle[name]):#np.arctan(vehicle_bb.y / vehicle_bb.x): + has_vehicle_in_back = True + distance = np.dot(vec1_2,forward_vector_2d) + if abs(distance) < abs(smallest_distance): + smallest_distance = distance + + return has_vehicle_in_back, smallest_distance + + # Make single check vehicle in direction function - relocated to Vehicle.py on 2/28/22 + def check_vehicle_in_right(self, uniquename, safety_distance = 6): + ''' + function checking whether a right vehicle is too close to the current vehicle + this function is primarily designed to help decide whether a vehicle can safely change lane + + Parameters + ---------- + uniquename : str + name of the vehicle. + + safety_distance: float + allowed closest distance between two vehicles + + Returns + ------- + has_vehicle_in_right : bool + whether there exists a vehicle within safety distance + distance: float + distance between this vehicle and the vehicle to the right + + ''' + # get the distance between this vehicle and other vehicles + distance_with_other_vehicle = self.distance_between_vehicles[uniquename] + + # get the bounding box of this vehicle + vehicle_bb = self.vehicle_dict[uniquename].bounding_box.extent + safety_distance += vehicle_bb.x / 2 # add the half length of the vehicle + + has_vehicle_in_right = False + distance = None + + smallest_distance = np.inf + + vehicle_1 = self.vehicle_dict[uniquename] + location_1 = vehicle_1.get_transform().location + forward_vector = vehicle_1.get_transform().get_forward_vector() + forward_vector_2d = np.array([forward_vector.x, forward_vector.y]) + forward_vector_3d = np.array([forward_vector.x, forward_vector.y, 0]) + + for name in distance_with_other_vehicle: + if name != uniquename and distance_with_other_vehicle[name] < safety_distance and name in self.vehicle_dict: # a possible vehicle + print(distance_with_other_vehicle[name]) + + location_2 = self.vehicle_dict[name].get_transform().location + vec1_2 = np.array([location_2.x - location_1.x, location_2.y - location_1.y, 0]) + vec1_2_2d = np.array([location_2.x - location_1.x, location_2.y - location_1.y]) + cross_product = np.cross(forward_vector_3d, vec1_2) + if cross_product[2] > 0: # left hand system + has_vehicle_in_right = True + distance = np.dot(forward_vector_2d, vec1_2_2d) + if abs(distance) <= smallest_distance: + smallest_distance = distance + + + return has_vehicle_in_right, smallest_distance + + # Make single check vehicle in direction function - relocated to Vehicle.py on 2/28/22 + def check_vehicle_in_left(self, uniquename, safety_distance = 6): + ''' + function checking whether a left vehicle is too close to the current vehicle + this function is primarily designed to help decide whether a vehicle can safely change lane + + Parameters + ---------- + uniquename : str + name of the vehicle. + + safety_distance: float + allowed closest distance between two vehicles + + Returns + ------- + has_vehicle_in_left : bool + whether there exists a vehicle within safety distance + distance: float + distance between this vehicle and the vehicle to the right + + ''' + # get the distance between this vehicle and other vehicles + distance_with_other_vehicle = self.distance_between_vehicles[uniquename] + + # get the bounding box of this vehicle + vehicle_bb = self.vehicle_dict[uniquename].bounding_box.extent + safety_distance += vehicle_bb.x / 2 # add the half length of the vehicle + + has_vehicle_in_left = False + distance = None + smallest_distance = np.inf + + vehicle_1 = self.vehicle_dict[uniquename] + location_1 = vehicle_1.get_transform().location + forward_vector = vehicle_1.get_transform().get_forward_vector() + forward_vector_2d = np.array([forward_vector.x, forward_vector.y]) + forward_vector_3d = np.array([forward_vector.x, forward_vector.y, 0]) + + for name in distance_with_other_vehicle: + if name != uniquename and distance_with_other_vehicle[name] < safety_distance and name in self.vehicle_dict: # a possible vehicle + print(distance_with_other_vehicle[name]) + + location_2 = self.vehicle_dict[name].get_transform().location + vec1_2 = np.array([location_2.x - location_1.x, location_2.y - location_1.y, 0]) + vec1_2_2d = np.array([location_2.x - location_1.x, location_2.y - location_1.y]) + cross_product = np.cross(forward_vector_3d, vec1_2) + if cross_product[2] < 0: # this is the only difference between the check_vehicle_in_left / check_vehicle_in_right function + has_vehicle_in_left = True + distance = np.dot(forward_vector_2d, vec1_2_2d) + if abs(distance) <= smallest_distance: + smallest_distance = distance + + return has_vehicle_in_left, smallest_distance + + # Method of Vehicle Class (already implemented in the CARLA API), marked as redundant on 2/28/22 + def get_traffic_light_state(self, uniquename): + ''' + + + Parameters + ---------- + uniquename : str + name of the vehicle.. + + Returns + ------- + The traffic light state corresponding to this vehicle. Each vehicle + tracks the traffic light that is directly in front of it. + If no traffic light available, return None + + ''' + vehicle = self.vehicle_dict[uniquename] + state = None + if vehicle.is_at_traffic_light(): + light = vehicle.get_traffic_light() + state = light.get_state() + + return state + + # Method of Vehicle Class + def draw_waypoints(self, trajectory, points): + ''' + Draw the way points and trajectory for the vehicle to follow + + Parameters + ---------- + trajectory : numpy 2d array + the interpolated trajectory of a vehicle. + points : list of (x,y) + waypoints to highlight + + Returns + ------- + None. + + ''' + for ii in range(len(points) - 1): + location = carla.Location(x = points[ii][0], y = points[ii][1], z = 5.0) + self.world.debug.draw_point(location, size = 0.1, color = orange, life_time=0.0, persistent_lines=True) + + location = carla.Location(x = points[-1][0], y = points[-1][1], z = 5.0) + self.world.debug.draw_point(location, size = 0.1, color = red, life_time=0.0, persistent_lines=True) + + for ii in range(1,len(trajectory)): + begin = carla.Location(x = trajectory[ii - 1][0], y = trajectory[ii - 1][1], z = 5.0) + end = carla.Location(x = trajectory[ii][0], y = trajectory[ii][1], z = 5.0) + self.world.debug.draw_line(begin, end, thickness=0.8, color=orange, life_time=0.0, persistent_lines=True) + + # Doesn't appear to be used (maybe keep for debug purposes) + def draw_real_trajectory(self, real_trajectory): + ''' + Draw the real trajectory + + Parameters + ---------- + real_trajectory : a deque of 2 (x,y) tuple + stores the current and previous 2d location of the vehicle + + Returns + ------- + None. + + ''' + begin = carla.Location(x = real_trajectory[0][0], y = real_trajectory[0][1], z = 5.0) + end = carla.Location(x = real_trajectory[1][0], y = real_trajectory[1][1], z = 5.0) + #self.world.debug.draw_arrow(begin, end, thickness=1.0, arrow_size=1.0, color = green, life_time=0.0, persistent_lines=True) diff --git a/legacy/gui_interface/backend/demo_setting2 b/legacy/gui_interface/backend/demo_setting2 new file mode 100644 index 0000000..e6ba7c9 --- /dev/null +++ b/legacy/gui_interface/backend/demo_setting2 @@ -0,0 +1,24 @@ +subject_vehicle = , +left_vehicle = "{'model': 'vehicle.tesla.model3', 'gap': 5, 'command': 'straight', 'obey_traffic_lights': True, 'run': True, 'safety_distance': 15.0, 'choice': 'left', 'traffic_light': , 'uniquename': 'vehicle.tesla.model3_571', 'ref_waypoint': None, 'location': None, 'rotation': None, 'trajectory': None, 'ref_speed_list': None, 'bounding_box': None, 'vehicle_type': 'other', 'stop_choice': 'normal', 'penetrate_distance': None, 'stop_ref_point': None}", "{'model': 'vehicle.tesla.model3', 'gap': 5, 'command': 'left', 'obey_traffic_lights': True, 'run': True, 'safety_distance': 15.0, 'choice': 'left', 'traffic_light': , 'uniquename': 'vehicle.tesla.model3_572', 'ref_waypoint': None, 'location': None, 'rotation': None, 'trajectory': None, 'ref_speed_list': None, 'bounding_box': None, 'vehicle_type': 'other', 'stop_choice': 'normal', 'penetrate_distance': None, 'stop_ref_point': None}", "{'model': 'vehicle.tesla.model3', 'gap': 5, 'command': 'right', 'obey_traffic_lights': True, 'run': True, 'safety_distance': 15.0, 'choice': 'left', 'traffic_light': , 'uniquename': 'vehicle.tesla.model3_573', 'ref_waypoint': None, 'location': None, 'rotation': None, 'trajectory': None, 'ref_speed_list': None, 'bounding_box': None, 'vehicle_type': 'other', 'stop_choice': 'normal', 'penetrate_distance': None, 'stop_ref_point': None}" +right_vehicle = "{'model': 'vehicle.tesla.model3', 'gap': 10.0, 'command': 'straight', 'obey_traffic_lights': True, 'run': True, 'safety_distance': 15.0, 'choice': 'right', 'traffic_light': , 'uniquename': 'vehicle.tesla.model3_574', 'ref_waypoint': None, 'location': None, 'rotation': None, 'trajectory': None, 'ref_speed_list': None, 'bounding_box': None, 'vehicle_type': 'other', 'stop_choice': 'normal', 'penetrate_distance': None, 'stop_ref_point': None}", "{'model': 'vehicle.tesla.model3', 'gap': 10.0, 'command': 'left', 'obey_traffic_lights': True, 'run': True, 'safety_distance': 15.0, 'choice': 'right', 'traffic_light': , 'uniquename': 'vehicle.tesla.model3_575', 'ref_waypoint': None, 'location': None, 'rotation': None, 'trajectory': None, 'ref_speed_list': None, 'bounding_box': None, 'vehicle_type': 'other', 'stop_choice': 'normal', 'penetrate_distance': None, 'stop_ref_point': None}", "{'model': 'vehicle.tesla.model3', 'gap': 10.0, 'command': 'right', 'obey_traffic_lights': True, 'run': True, 'safety_distance': 15.0, 'choice': 'right', 'traffic_light': , 'uniquename': 'vehicle.tesla.model3_576', 'ref_waypoint': None, 'location': None, 'rotation': None, 'trajectory': None, 'ref_speed_list': None, 'bounding_box': None, 'vehicle_type': 'other', 'stop_choice': 'normal', 'penetrate_distance': None, 'stop_ref_point': None}" +ahead_vehicle = "{'model': 'vehicle.tesla.model3', 'gap': 10.0, 'command': 'straight', 'obey_traffic_lights': True, 'run': True, 'safety_distance': 15.0, 'choice': 'ahead', 'traffic_light': , 'uniquename': 'vehicle.tesla.model3_577', 'ref_waypoint': None, 'location': None, 'rotation': None, 'trajectory': None, 'ref_speed_list': None, 'bounding_box': None, 'vehicle_type': 'other', 'stop_choice': 'normal', 'penetrate_distance': None, 'stop_ref_point': None}", "{'model': 'vehicle.tesla.model3', 'gap': 10.0, 'command': 'left', 'obey_traffic_lights': True, 'run': True, 'safety_distance': 15.0, 'choice': 'ahead', 'traffic_light': , 'uniquename': 'vehicle.tesla.model3_578', 'ref_waypoint': None, 'location': None, 'rotation': None, 'trajectory': None, 'ref_speed_list': None, 'bounding_box': None, 'vehicle_type': 'other', 'stop_choice': 'normal', 'penetrate_distance': None, 'stop_ref_point': None}", "{'model': 'vehicle.tesla.model3', 'gap': 10.0, 'command': 'right', 'obey_traffic_lights': True, 'run': True, 'safety_distance': 15.0, 'choice': 'ahead', 'traffic_light': , 'uniquename': 'vehicle.tesla.model3_579', 'ref_waypoint': None, 'location': None, 'rotation': None, 'trajectory': None, 'ref_speed_list': None, 'bounding_box': None, 'vehicle_type': 'other', 'stop_choice': 'normal', 'penetrate_distance': None, 'stop_ref_point': None}" +subject_light = yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green +left_light = yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green +right_light = yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green +ahead_light = yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, yellow, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, red, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green, green +[subject_light_time] +red = 1000.0 +yellow = 1000.0 +green = 1000.0 +[left_light_time] +red = 1000.0 +yellow = 1000.0 +green = 1000.0 +[right_light_time] +red = 1000.0 +yellow = 1000.0 +green = 1000.0 +[ahead_light_time] +red = 1000.0 +yellow = 1000.0 +green = 1000.0 diff --git a/legacy/gui_interface/backend/full_path_vehicle.py b/legacy/gui_interface/backend/full_path_vehicle.py new file mode 100644 index 0000000..789648a --- /dev/null +++ b/legacy/gui_interface/backend/full_path_vehicle.py @@ -0,0 +1,421 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Wed Jul 8 11:06:32 2020 + +@author: shijiliu +""" + + +import carla +import numpy as np +from collections import deque +import math + +import control # the python-control package, install first + +from backend.intersection_definition import get_trajectory +from backend.multiple_vehicle_control import VehicleControl + +import copy + + +# color for debug use +red = carla.Color(255, 0, 0) +green = carla.Color(0, 255, 0) +blue = carla.Color(47, 210, 231) +cyan = carla.Color(0, 255, 255) +yellow = carla.Color(255, 255, 0) +orange = carla.Color(255, 162, 0) +white = carla.Color(255, 255, 255) + +# LeadVehicleController class (derives from Vehicle Controller) +class LeadVehicleControl(VehicleControl): + # the LeadVehicleControl has two different modes: + # normal full path mode: vehicle follows normal full path + # pause mode: vehicle stop to the right of the lane, + # waiting for the ego vehicle to come + + def __init__(self,env,vehicle_config, delta_seconds, allow_collision = True): + super().__init__(env, vehicle_config, delta_seconds, allow_collision) + + # copy the full path trajectory + self.full_path_trajectory = copy.copy(self.trajectory) + self.full_path_index = copy.copy(self.index) + self.full_path_ref_speed_list = copy.copy(self.ref_speed_list ) + + # generate waypoints for pausing the car + self._get_pause_waypoints() + self.mode = "normal" + + # Method in either Vehicle or Controller class + def _get_pause_waypoints(self): + ''' + generate a list of waypoints for the pause path + the waypoints are in vehicle right-hand coordinates, the heading + direction of the vehicle is x-axis + + Returns + ------- + None. + + ''' + bb = self.vehicle_config["bounding_box"] + # Makes a weird curve to the right (currently direction restricted, assumes direction of vehicle is straight ahead) + self.pause_waypoint_list = [(0.0,0.0),(bb.x, -bb.y ),(bb.x * 2,-bb.y * 2),(bb.x * 3, -bb.y * 3),(bb.x * 4, -bb.y * 3.5),(bb.x * 5, -bb.y * 3.5),(bb.x * 6, -bb.y * 3.5),(bb.x * 7, -bb.y * 3.5)] + + # Method in Vehicle Class + def _get_unit_left_vector(self,yaw): + # get the left vector (y axis) + right_yaw = (yaw + 90) % 360 + rad_yaw = math.radians(right_yaw) + left_vector = np.array([math.cos(rad_yaw),math.sin(rad_yaw)]) + left_vector = left_vector / np.linalg.norm(left_vector) + return left_vector + + # Controller class + def _generate_pause_path(self): + ''' + generate a path for the vehicle to right shift a certain value and then stop. + assume the lead vehicle is always heading in the straight direction + + Returns + -- + None + ''' + # get world transform of the lead vehicle + world_transform = self.env.get_transform_3d(self.model_uniquename) + location = world_transform.location + location_2d = np.array([location.x,location.y]) + forward_vector = world_transform.get_forward_vector() + forward_vector_2d = np.array([forward_vector.x,forward_vector.y]) + left_vector_2d = self._get_unit_left_vector(world_transform.rotation.yaw) + + # transform local waypoints into global coordinates + world_waypoints = [] + for pt in self.pause_waypoint_list: + world_pt = location_2d + pt[0] * forward_vector_2d + pt[1] * left_vector_2d + world_waypoints.append( ((world_pt[0],world_pt[1]),5.0) ) # vehicle will stop at 5 m/s + if self.debug_vehicle: + loc = carla.Location(x = world_pt[0],y = world_pt[1], z = 0.0) + self.env.world.debug.draw_point(loc, size = 0.2, color = white, life_time=0.0, persistent_lines=True) + + # form trajectory + smoothed_full_trajectory, ref_speed_list = get_trajectory(world_waypoints) + + self.pause_path_trajectory = smoothed_full_trajectory + self.pause_path_ref_speed = ref_speed_list + + self.pause_path_index = 0 + + if self.debug_vehicle: + + for ii in range(1,len(smoothed_full_trajectory)): + loc1 = carla.Location(x = smoothed_full_trajectory[ii - 1][0], y = smoothed_full_trajectory[ii - 1][1], z = 0.0) + loc2 = carla.Location(x = smoothed_full_trajectory[ii][0], y = smoothed_full_trajectory[ii][1], z = 0.0) + self.env.world.debug.draw_arrow(loc1, loc2, thickness = 0.05, arrow_size = 0.1, color = red, life_time=0.0, persistent_lines=True) + + # Method of Controller class + def change_mode(self, mode): + ''' + change vehicle mode + + the LeadVehicleControl has two different modes: + normal full path mode: vehicle follows normal full path + pause mode: vehicle stop to the right of the lane, + waiting for the ego vehicle to come + + Parameters + ---------- + mode : string + the mode. valid valuse are "normal","pause" + + Returns + ------- + None. + + ''' + if mode == "normal": + self.index = copy.copy(self.full_path_index) + self.ref_speed_list = copy.copy(self.full_path_ref_speed_list) + self.trajectory = copy.copy(self.full_path_trajectory) + self.mode = mode + elif mode == "pause": + self._generate_pause_path() # generate path when switching mode + self.index = copy.copy(self.pause_path_index) + self.ref_speed_list = copy.copy(self.pause_path_ref_speed) + self.trajectory = copy.copy(self.pause_path_trajectory) + self.mode = mode + + # Control class + def pure_pursuit_control_wrapper(self): + ''' + Apply one step control to the vehicle, store essential information for further use + + Note: this is an overriden version of pure_pursuit_control_wrapper + to avoid vehicle being removed from the environment when reach the end of pause + + Returns + ------- + end_trajectory : bool + whether this vehicle reaches its end + + ''' + + curr_speed = self.env.get_forward_speed(self.model_uniquename) + vehicle_pos_2d = self.env.get_transform_2d(self.model_uniquename) # the (x,y) location and yaw angle of the vehicle + self.speed.append(curr_speed) + self.curr_speeds.append(curr_speed) + + # draw real trajectory if debug is enabled + if self.debug_vehicle: + self.vehicle_pose.append(vehicle_pos_2d[0]) + if len(self.vehicle_pose) == 2: + self.env.draw_real_trajectory(self.vehicle_pose) + self._display_vehicle_type() + + # use pure-pursuit model to get the steer angle (in radius) + delta, current_ref_speed, index, end_trajectory = self.pure_pursuit_control(vehicle_pos_2d, curr_speed, self.trajectory, self.ref_speed_list, self.index) + self.index = index + steer = np.clip(delta,-1.0,1.0) + + + # If vehicle has safety distance set, check whether a vehicle is in the front + current_ref_speed = self._obey_safety_distance(current_ref_speed) + + # If vehicle obey traffic lights and is going straight / turning left, check the traffic light state + current_ref_speed = self._obey_traffic_light(current_ref_speed) + + self.ref_speeds.append(current_ref_speed) + self.reference_speed.append(current_ref_speed) + + # get throttle to get the next reference speed + throttle = self.speed_control() # get the throttle control based on reference and current speed + throttle = np.clip(throttle,0,1) # throttle value is [0,1] + self.throttles.append(throttle) # for visualization + + # check whether we are reaching the destination or not + # this part is different from the original version + if end_trajectory and self.mode == "normal": + vehicle_control = carla.VehicleControl(throttle = 0.0,steer=steer,brake = 1.0) # immediately stop the car + self.env.apply_vehicle_control(self.model_uniquename, vehicle_control) # apply control to vehicle + self.run = False + self._destroy_vehicle() + return end_trajectory + elif end_trajectory and self.mode == "pause": + vehicle_control = carla.VehicleControl(throttle = 0.0,steer=steer,brake = 1.0) + self.env.apply_vehicle_control(self.model_uniquename, vehicle_control) + return False + + + + # apply throttle-steer-brake control + if curr_speed <= current_ref_speed: + vehicle_control = carla.VehicleControl(throttle = throttle,steer=steer) + else: + vehicle_control = carla.VehicleControl(throttle = throttle,steer=steer,brake = 0.5) + self.env.apply_vehicle_control(self.model_uniquename, vehicle_control) # apply control to vehicle + return end_trajectory + +# FollowVehicleController (derives from VehicleController) +class FollowVehicleControl(VehicleControl): + # the FollowVehicle class is created for both ego and follow vehicle + # this kind of vehicle has 2 modes: + # - speed control mode, when the vehicle is not following another vehicle + # - distance control mode, when the vehicle is following another vehicle + + def __init__(self,env,vehicle_config, delta_seconds, allow_collision = True): + super().__init__(env, vehicle_config, delta_seconds, allow_collision) + + # control mode + self.mode = "speed" + + # Method of Control Class + def _get_distance_controller(self,delta_seconds): + ''' + Effects: create distance controller + ''' + KP_1 = 1.0#1.0 + KI_1 = 1.0#1.0 + + num_pi = [-KP_1, -KI_1] # numerator of the PI transfer function (KP*s + KI) + den_pi = [1.0, 0.01*KI_1/KP_1] # denominator of PI transfer function (s + 0.01*KI/KP) + + sys = control.tf(num_pi,den_pi) # get transfer function for PI controller (since the denominator has a small term 0.01*KI/KP, it is actually a lag-compensator) + sys = control.sample_system(sys, delta_seconds) # discretize the transfer function (from s-domain which is continuous to z-domain) + #since our simulation is discrete + sys = control.tf2ss(sys) # transform transfer function into state space. + + + self.distance_sys = sys + + # Controller class + def _get_distance_control_reference_speed(self): + ''' + + + Parameters + ---------- + self.distance_sys : control.ss + state space controller. + self.ref_distance : deque(maxlen=2), float + the reference distance. + self.curr_distance : deque(maxlen=2), float + current distance between two vehicles. + init_values : the initial_values of the system + + Returns + ------- + None. + + ''' + U0 = np.array(self.ref_distance) - np.array(self.curr_distance) + #print(U0) + _,y0,x0 = control.forced_response(self.distance_sys,U = U0,X0 = self.distance_init_values[0]) # y0 is the next values, x0 is the state evolution + # see https://python-control.readthedocs.io/en/0.8.3/generated/control.forced_response.html#control.forced_response + self.distance_init_values.append(x0[-1]) + ref_speed = y0[-1] + + if self.ref_distance[1] > self.curr_distance[1]: + ref_speed = 0 + + # apply a boundary to the reference speed + # to avoid vehicle approaching too high a speed + # when trying to catch up the target + ref_speed = np.clip(ref_speed,0,15) + + #print(ref_speed) + + return ref_speed + + # Controller class + def use_distance_mode(self, follow_distance): + # change the mode + self.mode = "distance" + + # store the follow_distance + self.follow_distance = follow_distance + self.L # self.L is the length of the bounding box + + # storage for distance controller + self.distance_init_values = deque(maxlen = 2) + self.ref_distance = deque(maxlen = 2) + self.curr_distance = deque(maxlen = 2) + + # initialize those storage + self.distance_init_values.append(0) + self.ref_distance.append(self.follow_distance) + self.curr_distance.append(self.follow_distance) + + # get the controller for distance control + self._get_distance_controller(self.env.delta_seconds) + + # Controller class (maybe make one set_mode function) + def use_speed_mode(self): + self.mode = "speed" + + # Not sure, maybe Vehicle class + def get_current_distance(self, target_transform): + ''' + get the real distance between the vehicle and the one it is following + + Parameters + ---------- + target_transform : carla.Transform + the transformation of the target. + + Returns + ------- + None. + + ''' + # distance control seems problematic, temporarily still keep using speed control + self.mode = "speed"#"distance" + + target_location = target_transform.location + # get the local transformation + local_transform = self.env.get_transform_3d(self.model_uniquename) + local_location = local_transform.location + forward_vector = local_transform.get_forward_vector() + forward_vector_2d = np.array([forward_vector.x,forward_vector.y]) + unit_forward_vector_2d = forward_vector_2d / np.linalg.norm(forward_vector_2d) + + vec_loc_target = np.array([target_location.x - local_location.x,target_location.y - local_location.y]) + + # get the distance in the direction of local vehicle heading + distance = np.dot(vec_loc_target,unit_forward_vector_2d) + if self.vehicle_config["vehicle_type"] == "follow": + #print(distance) + pass + + self.ref_distance.append(self.follow_distance) + self.curr_distance.append(distance) + + # Controller class (not quite sure what's going on here) + def pure_pursuit_control(self,vehicle_pos_2d, current_forward_speed, trajectory, ref_speed_list, prev_index): + + # override the pure_pursuit_control method + # use self.mode to decide how we are going to + # assign the model reference speed + + ''' + + + Parameters + ---------- + vehicle_pos_2d : (location_2d,yaw) + tuple of vehicle location and heading in 2d. + location_2d : (x,y), both x and y are in meter + yaw : heading angle **Note** yaw is in degree + current_forward_speed : float + the current velocity of the vehicle. + trajectory : numpy 2d array + interpolated waypoints. + ref_speed_list : list + the reference speed corresponding to each way point + prev_index : int + the previous index + Returns + ------- + delta : float + steer angle of the vehicle. + current_ref_speed : the reference speed + DESCRIPTION. + index : int + the index of the target. + end_trajectory : boolean + whether we have reached clos enough to the destination. + + ''' + + + + location_2d, yaw = vehicle_pos_2d + yaw = np.deg2rad(yaw) # change the unit the radius + index, end_trajectory = self.get_target_index(location_2d, current_forward_speed, trajectory) + + if prev_index >= index: + index = prev_index + + if index < len(trajectory): + tx = trajectory[index][0] + ty = trajectory[index][1] + else: + tx = trajectory[-1][0] + ty = trajectory[-1][1] + + alpha = math.atan2(ty - location_2d[1],tx - location_2d[0]) - yaw + + if current_forward_speed < 0: #back, should not happen in our case + alpha = math.pi - alpha + + Lf = self.k * current_forward_speed + self.Lfc + + delta = math.atan2(2.0 * self.L * math.sin(alpha) / Lf, 1.0) + #print("delta == ", delta, "yaw == ", yaw) + + if self.mode == "speed": + current_ref_speed = ref_speed_list[index] + elif self.mode == "distance": + current_ref_speed = self._get_distance_control_reference_speed() + + return delta, current_ref_speed, index, end_trajectory \ No newline at end of file diff --git a/legacy/gui_interface/backend/generate_path_omit_regulation.py b/legacy/gui_interface/backend/generate_path_omit_regulation.py new file mode 100644 index 0000000..8837eeb --- /dev/null +++ b/legacy/gui_interface/backend/generate_path_omit_regulation.py @@ -0,0 +1,286 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Mon Jun 15 09:21:45 2020 + +@author: shijiliu +""" + + +#!/usr/bin/env python + +# 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 . + +import glob +import os +import sys + +try: + sys.path.append(glob.glob('../carla/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 math +import time + +from backend.carla_env import CARLA_ENV # self-written class that provides help functions, should be in the same folder + +# color for debugging +red = carla.Color(255, 0, 0) +green = carla.Color(0, 255, 0) +blue = carla.Color(47, 210, 231) +cyan = carla.Color(0, 255, 255) +yellow = carla.Color(255, 255, 0) +orange = carla.Color(255, 162, 0) +white = carla.Color(255, 255, 255) + +# enable/disable debug mode to draw the trajectory finding process in carla +DEBUG = True +display_trajectory = False + +# Not sure yet +def waypoint_exists(new_point, waypoint_list): + for pt in waypoint_list: + if pt.id == new_point.id: + return True + return False + +# Not sure yet +def get_junction_points(junction): + junction_w = junction.get_waypoints(carla.LaneType.Any) + junction_points = [] + for pair_w in junction_w: + for w in pair_w: + if not waypoint_exists(w , junction_points): + junction_points.append(w) + + + return junction_points + +# Not sure yet +def get_next_w(current_w, waypoint_separation): + ''' + code adapted from "lane_explorer.py" available in carla-simulator/PythonAPI/util + + Parameters + ---------- + current_w : carla.Waypoint() + the source point. + + Returns + ------- + potential_w : carla.Waypoint() + potential way point that can be connected by the current waypoint + + ''' + # list of potential next waypoints + potential_w = list(current_w.next(waypoint_separation)) + + # list of potential waypoints in opposite direction + #potential_w += list(current_w.previous(waypoint_separation)) + + if current_w.is_junction: + junction = current_w.get_junction() + junction_points = get_junction_points(junction) + + curr_x = current_w.transform.location.x + curr_y = current_w.transform.location.y + + for pt in junction_points: + distance = math.sqrt((curr_x - pt.transform.location.x)**2 + (curr_y - pt.transform.location.y)**2) + if distance < waypoint_separation: + potential_w.append(pt) + + return potential_w + +# Not sure yet +def end_of_search(current_w, destination, waypoint_separation): + ''' + + + Parameters + ---------- + current_w : carla.Waypoint() + the current waypoint . + destination : carla.Waypoint() + the destination we want to reach. + waypoint_separation : float + distance between the way points. + + Returns + ------- + Boolean value indicating whether the current waypoint is close enough to the destination + + ''' + + current_location = current_w.transform.location + destination_location = destination.transform.location + distance = math.sqrt( (current_location.x - destination_location.x)**2 + (current_location.y - destination_location.y)**2 ) + if distance <= waypoint_separation / 2: + print(distance) + + return True + else: + return False + +# This is the only function used in other places in the backend (Controller class) +def generate_path(env, start, destination, waypoint_separation = 4): + ''' + Apply Dijkstra's algorithm to find the path connecting the start point and destination point with smallest + number of autogenerated waypoints involved. + + Note: there is no guarantee the find path is the shortest path between the start and destination, but the + path should approximately be the shortest. + + + Parameters + ---------- + start : carla.Waypoint() + the start point of a path. + destination : carla.Waypoint() + the destination we want to reach. + Note: the search will stop once we reach a point within "waypoint_separation" away from the destination + waypoint_separation : float + distance between the way points. + Returns + ------- + trajectory : list of 2d points + a trajectory that can be used for interpolation and vehicle control. + + ''' + trajectory = [] # the trajectory we will return + + explored_w_list = [] # store waypoints that have been explored + explored_w_traceback_index = [] # each explored point was arrived by a previous point, + # store the index of the previous point for traceback + + potential_w_list = [] # store the waypoints to be explored + potential_w_traceback_index = [] # each potential point was arrived by a previous point, + # store the index of the previous point for traceback + + # initialization + potential_w_list.append(start) + potential_w_traceback_index.append(-1) + index = 0 # index that will be stored into the 2 index list + + # main loop for exploration + while True: + current_w = potential_w_list.pop(0) # get the first element of the list + current_w_index = potential_w_traceback_index.pop(0) # get the traceback index of the current waypoint + + # DEBUG + if DEBUG: + env.world.debug.draw_point(current_w.transform.location, size = 0.08, color = green, life_time = 2.0) + + + # check whether we have arrived at the destination + if end_of_search(current_w, destination, waypoint_separation): + # trace back + while True: + location = current_w.transform.location + trajectory.insert(0,(location.x,location.y)) + if current_w_index == -1: # reach the start point, note the order of the code + break + current_w = explored_w_list[current_w_index] + current_w_index = explored_w_traceback_index[current_w_index] + + print("find a path with " + str(len(trajectory)) + " waypoints") + + if display_trajectory: + ''' + for point in trajectory: + loc = carla.Location(x = point[0], y = point[1], z = 0.0) + env.world.debug.draw_point(loc, size = 0.15, color = green, life_time=0.0, persistent_lines=True) + ''' + for ii in range(1,len(trajectory)): + loc1 = carla.Location(x = trajectory[ii - 1][0], y = trajectory[ii - 1][1], z = 0.0) + loc2 = carla.Location(x = trajectory[ii][0], y = trajectory[ii][1], z = 0.0) + env.world.debug.draw_arrow(loc1, loc2, thickness = 0.5, arrow_size = 0.5, color = yellow, life_time=0.0, persistent_lines=True) + break + + # add the point to explored list + explored_w_list.append(current_w) + explored_w_traceback_index.append(current_w_index) + + # DEBUG + if DEBUG: + env.world.debug.draw_point(current_w.transform.location, size = 0.08, color = yellow, life_time= 2.0) + + # we haven't reach the end, search based on the current waypoint + potential_w = get_next_w(current_w, waypoint_separation) # get potential waypoint that can be connected by this point + + # check whether the potential point has not been connected + for pt in potential_w: + if not waypoint_exists(pt,potential_w_list) and not waypoint_exists(pt,explored_w_list) : # this point has not been explored + potential_w_list.append(pt) + potential_w_traceback_index.append(index) + + if DEBUG: + env.world.debug.draw_point(current_w.transform.location, size = 0.08, color = white, life_time= 2.0) + + + if len(potential_w_list) == 0: # no more potential points + print("failed to find the trajectory") + break + + index += 1 + + return trajectory + +def main(): + try: + client = carla.Client("localhost",2000) + client.set_timeout(10.0) + world = client.load_world('Town05') + + # set the weather + world = client.get_world() + weather = carla.WeatherParameters( + cloudiness=10.0, + precipitation=0.0, + sun_altitude_angle=90.0) + world.set_weather(weather) + + # set the spectator position for demo purpose + spectator = world.get_spectator() + spectator.set_transform(carla.Transform(carla.Location(x=0.0, y=0.0, z=20.0), carla.Rotation(pitch=-31.07, yaw= -90.868, roll=1.595))) # plain ground + + env = CARLA_ENV(world) + time.sleep(2) + + # get map + carla_map = env.world.get_map() + + start_raw = carla.Location(x=-0.19, y=6.65, z=0.0) + destination_raw = carla.Location(x=24.18125, y=35.5, z=0.0) + + world.debug.draw_point(start_raw, size = 0.15, color = red, life_time = 0.0, persistent_lines=True ) + world.debug.draw_point(destination_raw, size = 0.15, color = red, life_time = 0.0, persistent_lines=True ) + + start = carla_map.get_waypoint(start_raw) + destination = carla_map.get_waypoint(destination_raw) + + world.debug.draw_point(start.transform.location, size = 0.15, color = blue, life_time = 0.0, persistent_lines=True ) + world.debug.draw_point(destination.transform.location, size = 0.15, color = blue, life_time = 0.0, persistent_lines=True ) + + trajectory = generate_path(env,start, destination, waypoint_separation = 10) + + finally: + return trajectory + +if __name__ == '__main__': + try: + trajectory = main() + except KeyboardInterrupt: + print('\nExit by user.') + finally: + print('\nExit.') + \ No newline at end of file diff --git a/legacy/gui_interface/backend/human_ego_control.py b/legacy/gui_interface/backend/human_ego_control.py new file mode 100644 index 0000000..3ad98bb --- /dev/null +++ b/legacy/gui_interface/backend/human_ego_control.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Tue Aug 11 10:54:49 2020 + +@author: shijiliu +""" + + +import socket +import time + +# This stuff doesn't even function and there's a million better ways to handle user input that this +class HumanEgoControlServer(): + def __init__(self): + + self.command_num = 3 # throttle, steer, brake, in total 3 + + HOST = '' # Symbolic name meaning all available interfaces + PORT = 50007 # Arbitrary non-privileged port + + # create the server + self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.s.bind((HOST, PORT)) + + # wait until a client is online, start connection with the client + self.s.listen(1) + + + print("Server initialized") + + def get_human_command(self): + ''' + get the command from user + the command is a string containting the vehicle throttle, steer and brake + + Note: + the server will be waiting for the latest command + + + Returns + ------- + human_command: list [float,float,float] + the throttle, steer and brake value + + ''' + conn, addr = self.s.accept() + with conn: + + while True: + data = conn.recv(32) + if data: + #print("Received ", repr(data)) + human_command = self._decode_command(data) + conn.sendall(b'Command received') + conn.close() + print("human command: throttle == ", human_command[0], " steer == ", human_command[1], " brake == ", human_command[2]) + return human_command + else: + print("Invalid input, waiting for valid commands") + + def _decode_command(self,data): + data = repr(data) + splitted_command = data.split(",") + command = [] + for ii in range(self.command_num): + val_str = splitted_command[ii].split(":")[1] + command.append(float(val_str)) + return command + + def __del__(self): + self.s.close() + +class HumanEgoControlClient(): + def __init__(self): + self.HOST = '' # The remote host + self.PORT = 50007 # The same port as used by the server + + # create the client + # create the server + #self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + #self.s.connect((self.HOST, self.PORT)) + print("Client Initialized") + + def apply_ego_commands(self, throttle = 0.0, steer = 0.0, brake = 0.0): + ''' + helper function for the user to drive ego vehicle by + applying throttle, steer and brake value + + Parameters + ---------- + throttle : float + throttle of the vehicle, within [0.0,1.0] + steer : float + the steer of the vehicle, within [-1.0,1.0] + brake : float + the brake of the vehicle, within [0.0,1.0]. + + Returns + ------- + None. + + ''' + s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + s.connect((self.HOST, self.PORT)) + + # get command string + command_string = self.encode_ego_commands(throttle, steer, brake) + s.sendall(command_string.encode()) + data = s.recv(32) + print("Received ", repr(data)) + s.close() + return + + def encode_ego_commands(self, throttle = 0.0, steer = 0.0, brake = 0.0): + # encode the commands into a string + # all float input will be formatted into a 2 digit float + throttle = format(throttle,'.2f') + steer = format(steer,'.2f') + brake = format(brake,'.2f') + command_string = 't:' + str(throttle) + ',' + 's:' + str(steer) + ',' + 'b:' + str(brake) + ',\n' + return command_string + +def main(): + server = HumanEgoControlServer() + for ii in range(20): + human_command = server.get_human_command() + print(human_command) + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/legacy/gui_interface/backend/initial_intersection.py b/legacy/gui_interface/backend/initial_intersection.py new file mode 100644 index 0000000..f4b2395 --- /dev/null +++ b/legacy/gui_interface/backend/initial_intersection.py @@ -0,0 +1,752 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Tue Jul 7 15:12:58 2020 + +@author: shijiliu +""" +import sys +sys.path.append("..") + +import carla +import time + +from backend.generate_path_omit_regulation import generate_path +from backend.intersection_definition import Intersection, get_traffic_lights, get_trajectory, smooth_trajectory +from backend.carla_env import CARLA_ENV # self-written class that provides help functions, should be in the same folder +from configobj import ConfigObj +from backend.multiple_vehicle_control import VehicleControl + +import copy + +# color for debug use +red = carla.Color(255, 0, 0) +green = carla.Color(0, 255, 0) +blue = carla.Color(47, 210, 231) +cyan = carla.Color(0, 255, 255) +yellow = carla.Color(255, 255, 0) +orange = carla.Color(255, 162, 0) +white = carla.Color(255, 255, 255) + + +# Really not sure if this needs to be a separate intersection class +class Init_Intersection(Intersection): + def __init__(self, env, world_pos, traffic_light_list, waypoint_list, subject_traffic_light_list, navigation_speed = 10.0): + super().__init__(env, world_pos, traffic_light_list,navigation_speed = navigation_speed) + self.waypoint_list = waypoint_list + self.start_sim = True # the init intersection will start as soon as the simulation begins + self.subject_traffic_light_list = subject_traffic_light_list + self.subject_traffic_light_list.insert(0,self.subject_light) + + # 3 special vehicles + self.ego_vehicle = None + self.lead_vehicle = None + self.follow_vehicle = None + + # generate the full path that's going to be shared between the ego, lead and follow vehicles + self._generate_intersection_path() + + # Intersection Class + def add_ego_vehicle(self, gap = 10.0,model_name = "vehicle.tesla.model3", stop_choice = "abrupt", penetrate_distance = None, obey_traffic_lights = True, run = True, safety_distance = 0.0, vehicle_color = None): + + self._add_full_path_vehicle("ego",gap = gap, model_name = model_name, obey_traffic_lights = obey_traffic_lights, run = run, safety_distance = safety_distance, vehicle_color = vehicle_color) + self.ego_vehicle["lead_distance"] = 0.0 + self.ego_vehicle["follow_distance"] = 0.0 + self.ego_vehicle["index"] = len(self.subject_vehicle) - 1 # the index of the ego vehicle in the subject + self.ego_vehicle["vehicle_type"] = "ego" + self.ego_vehicle["stop_choice"] = stop_choice + self.ego_vehicle["penetrate_distance"] = penetrate_distance + self.ego_vehicle["stop_ref_point"] = self._generate_full_path_stop_ref(stop_choice,penetrate_distance) + + return self.ego_vehicle['uniquename'] + + # Intersection class + def add_lead_vehicle(self, lead_distance ,gap = 10.0,model_name = "vehicle.tesla.model3", stop_choice = "abrupt", penetrate_distance = None, obey_traffic_lights = True, run = True, safety_distance = 15.0, vehicle_color = None): + # get all the vehicles that's going to be after the lead vehicle + ego_index = self.ego_vehicle["index"] + vehicle_after_lead = self.subject_vehicle[ego_index : ] + self.ego_vehicle["lead_distance"] = lead_distance + shift_distance = -(gap + 5) # should be gap + model length + + # shift all vehicles starting from the ego vehicle + self._shift_vehicles(shift_distance,index = ego_index) + + # only keep the vehicle before the ego + self.subject_vehicle = self.subject_vehicle[:ego_index] + + # add the lead vehicle + self._add_full_path_vehicle("lead",gap = gap, model_name = model_name, obey_traffic_lights = obey_traffic_lights, run = run, safety_distance = safety_distance, vehicle_color = vehicle_color) + + # put back ego and vehicles after ego + self.subject_vehicle += vehicle_after_lead + + # generate path for ego vehicle + start_waypoint = self.ego_vehicle["ref_waypoint"] + trajectory, ref_speed_list = self._generate_full_path(start_waypoint) + self.ego_vehicle["trajectory"] = trajectory + self.ego_vehicle["ref_speed_list"] = ref_speed_list + + self.lead_vehicle["vehicle_type"] = "lead" + self.ego_vehicle["index"] = ego_index + 1 # add one car in front of ego vehicle + + # stop settings for lead + self.lead_vehicle["stop_choice"] = stop_choice + self.lead_vehicle["penetrate_distance"] = penetrate_distance + self.lead_vehicle["stop_ref_point"] = self._generate_full_path_stop_ref(stop_choice,penetrate_distance) + return self.lead_vehicle['uniquename'] + + # Intersection Class + def add_follow_vehicle(self, follow_distance ,gap = 10.0,model_name = "vehicle.tesla.model3", stop_choice = "abrupt", penetrate_distance = None, obey_traffic_lights = True, run = True, safety_distance = 15.0, vehicle_color = None): + # get all the vehicles that's going to be after the lead vehicle + ego_index = self.ego_vehicle["index"] + vehicle_after_ego = self.subject_vehicle[ego_index + 1 : ] + self.ego_vehicle["follow_distance"] = follow_distance + shift_distance = -(gap + 5) # should be gap + model length + + # shift all vehicles starting after the ego vehicle + self._shift_vehicles(shift_distance,index = ego_index + 1) + + # only keep the vehicle until the ego + self.subject_vehicle = self.subject_vehicle[:ego_index + 1] + + # add the follow vehicle + self._add_full_path_vehicle("follow",gap = gap, model_name = model_name, obey_traffic_lights = obey_traffic_lights, run = run, safety_distance = safety_distance, vehicle_color = vehicle_color) + + # put back vehicles after follow + self.subject_vehicle += vehicle_after_ego + + + self.follow_vehicle["vehicle_type"] = "follow" + + # stop settings for follow vehicle + self.follow_vehicle["stop_choice"] = stop_choice + self.follow_vehicle["penetrate_distance"] = penetrate_distance + self.follow_vehicle["stop_ref_point"] = self._generate_full_path_stop_ref(stop_choice,penetrate_distance) + return self.follow_vehicle['uniquename'] + + + # Intersection Class (figure out if full_path vehicles need to be separated out) + def _add_full_path_vehicle(self, vehicle_type, gap = 10.0,model_name = "vehicle.tesla.model3", obey_traffic_lights = True, run = True, safety_distance = 0.0, vehicle_color = None): + ''' + + + Parameters + ---------- + vehicle_type : string + the type of the vehicle, valid input values are "ego","lead","follow" + + gap : float, optional + initial distance between this vehicle and the vehicle in front of it/ border. The default is 10.0. + model_name : string, optional + vehicle model. The default is "vehicle.tesla.model3". + obey_traffic_lights : bool, optional + whether ego vehicle obeys traffic light. The default is True. + run : bool, optional + whether ego vehicle starts immediately. The default is True. + safety_distance : float, optional + safety distance between ego vehicle and other vehicle. The default is 0.0 since ego vehicle is allowed to crash + + Returns + ------- + None. + + ''' + right_shift_value = 0.0#1.6 + + vehicle = ConfigObj() + + if vehicle_type == "ego": + self.ego_vehicle = vehicle + elif vehicle_type == "lead": + self.lead_vehicle = vehicle + elif vehicle_type == "follow": + self.follow_vehicle = vehicle + + + vehicle["model"] = model_name + vehicle["gap"] = gap + vehicle["obey_traffic_lights"] = obey_traffic_lights + vehicle["run"] = run + vehicle["safety_distance"] = safety_distance + vehicle["command"] = "straight" + vehicle["traffic_light"] = self.subject_traffic_light_list + ref_waypoint = self.subject_lane_ref + vehicle_set = self.subject_vehicle + + if len(vehicle_set) != 0: + ref_waypoint = vehicle_set[-1]["ref_waypoint"] + #previous_uniquename = vehicle_set[-1]["uniquename"] + #bb = self.env.get_vehicle_bounding_box(previous_uniquename) + bb = vehicle_set[-1]["bounding_box"] + + right_shift_value = right_shift_value #- bb.y / 2 + + + + curr_length = self.env.get_vehicle_model_length(model_name) + + gap += bb.x / 2 + curr_length / 2 + + + # use the original reference point to get the new reference point + # reference point is in the middle of the lane + # function same as self._get_next_waypoint + forward_vector = ref_waypoint.transform.get_forward_vector() + + location = ref_waypoint.transform.location + raw_spawn_point = carla.Location(x = location.x - gap * forward_vector.x , y = location.y - gap * forward_vector.y , z = location.z + 0.1) + + new_ref_waypoint = self.carla_map.get_waypoint(raw_spawn_point) + + # right shift the spawn point + # right is with respect to the direction of vehicle navigation + ref_yaw = new_ref_waypoint.transform.rotation.yaw + + right_vector = self._get_unit_right_vector(ref_yaw) + + new_location = new_ref_waypoint.transform.location + + spawn_location = carla.Location(x = new_location.x - right_shift_value * right_vector[0], y = new_location.y - right_shift_value * right_vector[1], z = new_location.z + 0.2) + spawn_rotation = new_ref_waypoint.transform.rotation + + uniquename = self.env.spawn_vehicle(model_name = model_name,spawn_point = carla.Transform(spawn_location,spawn_rotation), color = vehicle_color) + vehicle["uniquename"] = uniquename + vehicle["ref_waypoint"] = new_ref_waypoint + vehicle["location"] = spawn_location + vehicle["rotation"] = spawn_rotation + if vehicle_color == None: + vehicle["vehicle_color"] = vehicle_color + else: + vehicle["vehicle_color"] = vehicle_color.replace(',',';') # replace , by ; to avoid error when importing from file + + # generate the full path + trajectory, ref_speed_list = self._generate_full_path(new_ref_waypoint) + + vehicle["trajectory"] = trajectory + vehicle["ref_speed_list"] = ref_speed_list + + # get the bounding box of the new vehicle + + new_bb = self.env.get_vehicle_bounding_box(uniquename) + vehicle["bounding_box"] = new_bb + + vehicle_set.append(vehicle) + + # Controller class + def _generate_full_path_stop_ref(self, stop_choice, penetrate_distance): + ''' + generate the reference point for stoping the vehicle + + Returns + ------- + stop_ref_list : list [(float,float),(float,float),...] + a list of stop reference points + + ''' + + + stop_ref_waypoint = [] + stop_ref_waypoint.append(self.subject_lane_ref) + + stop_ref_list = [] + + for ii in range(0,len(self.waypoint_list),3): + stop_ref_waypoint.append(self.waypoint_list[ii]) # the first point of each intersection + + if stop_choice == "normal": + for ref_waypoint in stop_ref_waypoint: + stop_point = self._get_next_waypoint(ref_waypoint,distance = -3.0) + stop_ref_list.append(stop_point.transform) + elif stop_choice == "penetrate": + for ref_waypoint in stop_ref_waypoint: + stop_point = self._get_next_waypoint(ref_waypoint,distance = penetrate_distance) + stop_ref_list.append(stop_point.transform) + else: + for ref_waypoint in stop_ref_waypoint: + stop_ref_list.append(ref_waypoint.transform) + + return stop_ref_list + + # Controller class + def _generate_intersection_path(self): + ''' + generate the path from the initial intersection to the end intersection + + Returns + ------- + None. + + ''' + first_waypoint = self.subject_lane_ref + second_waypoint = self.ahead_in[0] # can also be [1], choosing the left lane + third_waypoint = self._get_next_waypoint(second_waypoint,30) + waypoint_list = copy.copy(self.waypoint_list) + waypoint_list.insert(0,third_waypoint) + waypoint_list.insert(0,second_waypoint) + waypoint_list.insert(0,first_waypoint) + + full_trajectory = generate_path(self.env, waypoint_list[0] , waypoint_list[1], waypoint_separation = 4) + for ii in range(1,len(waypoint_list) - 1): + trajectory = generate_path(self.env, waypoint_list[ii] , waypoint_list[ii + 1], waypoint_separation = 4) + full_trajectory += trajectory[1:] + + self.intersection_trajectory = full_trajectory + + # Controller class + def _generate_full_path(self,start_waypoint): + ''' + + + Parameters + ---------- + start_waypoint : carla.Waypoint + initial waypoint of the vehicle. + + Returns + ------- + None. + + ''' + color = cyan + + trajectory = generate_path(self.env, start_waypoint, self.subject_lane_ref, waypoint_separation = 4) + + full_trajectory = trajectory + self.intersection_trajectory[1:] + + whole_trajectory = [((pt[0],pt[1]),10.0) for pt in full_trajectory] + + smoothed_full_trajectory, ref_speed_list = get_trajectory(whole_trajectory) + + if self.DEBUG_TRAJECTORY: + for ii in range(1,len(smoothed_full_trajectory)): + loc1 = carla.Location(x = smoothed_full_trajectory[ii - 1][0], y = smoothed_full_trajectory[ii - 1][1], z = 0.0) + loc2 = carla.Location(x = smoothed_full_trajectory[ii][0], y = smoothed_full_trajectory[ii][1], z = 0.0) + self.env.world.debug.draw_arrow(loc1, loc2, thickness = 0.05, arrow_size = 0.1, color = color, life_time=0.0, persistent_lines=True) + + return smoothed_full_trajectory, ref_speed_list + + # Intersection class + def export_settings(self): + ''' + export all settings for a specific intersection + note: this method overrides the method in Intersection + by not exporting the settings for subject lane + + + Returns + ------- + intersection_settings : ConfigObj + settings of the intersection + + ''' + intersection_settings = ConfigObj() + + # general + intersection_settings["navigation_speed"] = self.navigation_speed + + # vehicles + intersection_settings["subject_vehicle"] = [] + intersection_settings["left_vehicle"] = [] + intersection_settings["right_vehicle"] = [] + intersection_settings["ahead_vehicle"] = [] + + # do not export subject lane settings + ''' + for vehicle in self.subject_vehicle: + # deep copy the vehicle settings + new_vehicle = self._copy_vehicle_settings(vehicle) + intersection_settings["subject_vehicle"].append(new_vehicle) + ''' + + for vehicle in self.left_vehicle: + # deep copy the vehicle settings + new_vehicle = self._copy_vehicle_settings(vehicle) + intersection_settings["left_vehicle"].append(new_vehicle) + + for vehicle in self.right_vehicle: + # deep copy the vehicle settings + new_vehicle = self._copy_vehicle_settings(vehicle) + intersection_settings["right_vehicle"].append(new_vehicle) + + for vehicle in self.ahead_vehicle: + # deep copy the vehicle settings + new_vehicle = self._copy_vehicle_settings(vehicle) + intersection_settings["ahead_vehicle"].append(new_vehicle) + + # lights + intersection_settings["subject_light"] = copy.copy(self.light_config["subject"]) + intersection_settings["subject_light_time"] = copy.copy(self.light_config["subject_time"]) + + intersection_settings["left_light"] = copy.copy(self.light_config["left"]) + intersection_settings["left_light_time"] = copy.copy(self.light_config["left_time"]) + + intersection_settings["right_light"] = copy.copy(self.light_config["right"]) + intersection_settings["right_light_time"] = copy.copy(self.light_config["right_time"]) + + intersection_settings["ahead_light"] = copy.copy(self.light_config["ahead"]) + intersection_settings["ahead_light_time"] = copy.copy(self.light_config["ahead_time"]) + + return intersection_settings + + # Intersection class + def import_settings(self,intersection_settings): + ''' + note: this method overrides the import_settings in Intersection + by add the part of removing the ego, lead and follow vehicle + + Parameters + ---------- + intersection_settings : ConfigObj + the intersection settings we want to import + + Returns + ------- + new_intersection_setting : ConfigObj + settings of the intersection + this will be generated by call self.export_settings() after finishing import + output these settings are for the purpose of creating the front-end gui + ''' + + # remove ego, lead and follow settings + self.ego_vehicle = None + self.lead_vehicle = None + self.follow_vehicle = None + + # remove all vehicle in this intersection + # if any vehicle has been added + for ii in range(len(self.subject_vehicle) - 1, -1, -1): # go through the array in reverse order + uniquename = self.subject_vehicle[ii]['uniquename'] + self.remove_vehicle(uniquename) + + for ii in range(len(self.left_vehicle) - 1, -1, -1): # go through the array in reverse order + uniquename = self.left_vehicle[ii]['uniquename'] + self.remove_vehicle(uniquename) + + for ii in range(len(self.right_vehicle) - 1, -1, -1): # go through the array in reverse order + uniquename = self.right_vehicle[ii]['uniquename'] + self.remove_vehicle(uniquename) + + for ii in range(len(self.ahead_vehicle) - 1, -1, -1): # go through the array in reverse order + uniquename = self.ahead_vehicle[ii]['uniquename'] + self.remove_vehicle(uniquename) + + # import all settings + + self.navigation_speed = float(intersection_settings["navigation_speed"]) + + ''' + for vehicle_config in intersection_settings["subject_vehicle"]: + # add vehicles according to imported settings + self.add_vehicle(gap = vehicle_config["gap"], + model_name = vehicle_config["model"], + choice = vehicle_config['choice'], + command = vehicle_config['command'], + stop_choice = vehicle_config['stop_choice'], + penetrate_distance = vehicle_config['penetrate_distance'], + obey_traffic_lights = vehicle_config['obey_traffic_lights'], + run = vehicle_config['run'], + safety_distance = vehicle_config['safety_distance'], + vehicle_color = vehicle_config['vehicle_color'].replace(';',',')) + ''' + + for vehicle_config in intersection_settings["left_vehicle"]: + # add vehicles according to imported settings + if vehicle_config['vehicle_color'] != None: + vehicle_config['vehicle_color'] = vehicle_config['vehicle_color'].replace(';',',') + + self.add_vehicle(gap = vehicle_config["gap"], + model_name = vehicle_config["model"], + choice = vehicle_config['choice'], + command = vehicle_config['command'], + stop_choice = vehicle_config['stop_choice'], + penetrate_distance = vehicle_config['penetrate_distance'], + obey_traffic_lights = vehicle_config['obey_traffic_lights'], + run = vehicle_config['run'], + safety_distance = vehicle_config['safety_distance'], + vehicle_color = vehicle_config['vehicle_color']) + + for vehicle_config in intersection_settings["right_vehicle"]: + # add vehicles according to imported settings + if vehicle_config['vehicle_color'] != None: + vehicle_config['vehicle_color'] = vehicle_config['vehicle_color'].replace(';',',') + + self.add_vehicle(gap = vehicle_config["gap"], + model_name = vehicle_config["model"], + choice = vehicle_config['choice'], + command = vehicle_config['command'], + stop_choice = vehicle_config['stop_choice'], + penetrate_distance = vehicle_config['penetrate_distance'], + obey_traffic_lights = vehicle_config['obey_traffic_lights'], + run = vehicle_config['run'], + safety_distance = vehicle_config['safety_distance'], + vehicle_color = vehicle_config['vehicle_color']) + + for vehicle_config in intersection_settings["ahead_vehicle"]: + # add vehicles according to imported settings + if vehicle_config['vehicle_color'] != None: + vehicle_config['vehicle_color'] = vehicle_config['vehicle_color'].replace(';',',') + + self.add_vehicle(gap = vehicle_config["gap"], + model_name = vehicle_config["model"], + choice = vehicle_config['choice'], + command = vehicle_config['command'], + stop_choice = vehicle_config['stop_choice'], + penetrate_distance = vehicle_config['penetrate_distance'], + obey_traffic_lights = vehicle_config['obey_traffic_lights'], + run = vehicle_config['run'], + safety_distance = vehicle_config['safety_distance'], + vehicle_color = vehicle_config['vehicle_color']) + + self.light_config['subject'] = intersection_settings['subject_light'] + self.light_config['subject_time'] = intersection_settings['subject_light_time'] + + self.light_config['left'] = intersection_settings['left_light'] + self.light_config['left_time'] = intersection_settings['left_light_time'] + + self.light_config['right'] = intersection_settings['right_light'] + self.light_config['right_time'] = intersection_settings['right_light_time'] + + self.light_config['ahead'] = intersection_settings['ahead_light'] + self.light_config['ahead_time'] = intersection_settings['ahead_light_time'] + + new_intersection_setting = self.export_settings() + return new_intersection_setting + + # Experiment class +def create_intersections(env, number_of_intersections, traffic_light_list, navigation_speed): + ''' + + + Parameters + ---------- + env : CARLA_ENV + sself-written simulation help class. + number_of_intersections : int + number of intersection. + navigation_speed : float + the navigation speed of the vehicle + + Returns + ------- + Intersections : list of intersections, [Init_Intersection,Intersection,Intersection,...,Intersection] + + ''' + + # note: due to the limit of map, number_of_intersections can be at most 4 at present + world_pos_list = [(-190.0,0.0),(-133.0,0.0),(-55.0,0.0),(25.4,0.0)] # Hardcoded intersection list + number_of_intersections = min(4,number_of_intersections) + waypoint_list = [] # way points that form the full path + subject_traffic_light_list = [] # all subject traffic lights along the full path + intersection_list = [] + + for ii in range(1,number_of_intersections): + normal_intersection = Intersection(env,world_pos_list[ii],traffic_light_list,navigation_speed = navigation_speed) + waypoint_list += normal_intersection.get_subject_waypoints() # get the three points representing the path + subject_traffic_light_list.append(normal_intersection.get_subject_traffic_light()) + intersection_list.append(normal_intersection) + + + + init_intersection = Init_Intersection(env,world_pos_list[0],traffic_light_list,waypoint_list,subject_traffic_light_list, navigation_speed = navigation_speed) + intersection_list.insert(0,init_intersection) + return intersection_list + + # Vehicle class +def get_ego_spectator(ego_transform,distance = -10): + ''' + + + Parameters + ---------- + ego_transform : carla.Transform + transform for the ego vehicle. + distance : float, optional + "distance" between ego vehicle and spectator. The default is -10. + + Returns + ------- + next_waypoint : carla.Waypoint + next waypoint, "distance" away from curr_waypoint, in the direction of the current way point + ''' + forward_vector = ego_transform.get_forward_vector() + + location = ego_transform.location + spectator_location = carla.Location(x = location.x + distance * forward_vector.x , y = location.y + distance * forward_vector.y , z = location.z + 5.0) + spectator_transform = carla.Transform(spectator_location,ego_transform.rotation) + + return spectator_transform + + # Vehicle Class +def get_ego_left_spectator(ego_transform,distance = -20): + forward_vector = ego_transform.get_forward_vector() + + location = ego_transform.location + spectator_location = carla.Location(x = location.x + distance * forward_vector.y , y = location.y + distance * forward_vector.x , z = location.z + 5.0) + spectator_rotation = carla.Rotation(roll = ego_transform.rotation.roll,pitch = ego_transform.rotation.pitch, yaw = (ego_transform.rotation.yaw + 90) % 360) + + spectator_transform = carla.Transform(spectator_location,spectator_rotation) + return spectator_transform + + # Vehicle Class +def get_ego_driving_spectator(ego_transform, bounding_box): + + distance = bounding_box.x + left_shift = -bounding_box.y * 0.3 + + forward_vector = ego_transform.get_forward_vector() + + location = ego_transform.location + spectator_location = carla.Location(x = location.x + distance * forward_vector.x + left_shift * forward_vector.y , y = location.y + distance * forward_vector.y + left_shift * forward_vector.x, z = location.z + bounding_box.z * 1.5) + spectator_transform = carla.Transform(spectator_location,ego_transform.rotation) + + return spectator_transform + + # Redundant, please delete +def IntersectionBackend(env,intersection_list): + vehicle_list = [] + started_intersection_list = [] + ego_vehicle = intersection_list[0].ego_vehicle #init_intersection.ego_vehicle + lead_vehicle = intersection_list[0].lead_vehicle + follow_vehicle = intersection_list[0].follow_vehicle + + spectator = env.world.get_spectator() + + # assign the first full path vehicle, to determine whether + # each intersection should start + if not lead_vehicle == None: + first_full_path_vehicle_name = lead_vehicle["uniquename"] + else: + first_full_path_vehicle_name = ego_vehicle["uniquename"] + + # assign the vehicle for the spectator to follow + if follow_vehicle != None: + spectator_vehicle = follow_vehicle + else: + spectator_vehicle = ego_vehicle + # get the init intersection + init_intersection = intersection_list.pop(0) + + for vehicle_config in init_intersection.subject_vehicle: + # initialize vehicles by different type (ego,lead,follow,other) + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + for vehicle_config in init_intersection.left_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + for vehicle_config in init_intersection.right_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + for vehicle_config in init_intersection.ahead_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + + while True: + env.world.tick() + + # update the distance between vehicles after each tick + env.update_vehicle_distance() + + # update the ego spectator + if env.vehicle_available(spectator_vehicle["uniquename"]): + spectator_vehicle_transform = env.get_transform_3d(spectator_vehicle["uniquename"]) + spectator_transform = get_ego_spectator(spectator_vehicle_transform,distance = -10) + spectator.set_transform(spectator_transform) + + #else: + # spectator_transform = carla.Transform(carla.Location(x= 25.4, y=1.29, z=75.0), carla.Rotation(pitch=-88.0, yaw= -1.85, roll=1.595)) + #spectator.set_transform(spectator_transform) + + + for ii in range(len(intersection_list)-1,-1,-1): + # check whether the intersection should start + intersection_list[ii].start_simulation(first_full_path_vehicle_name) + if intersection_list[ii].start_sim: + for vehicle_config in intersection_list[ii].subject_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + for vehicle_config in intersection_list[ii].left_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + for vehicle_config in intersection_list[ii].right_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + for vehicle_config in intersection_list[ii].ahead_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + # move the intersection to started intersection list + intersection = intersection_list.pop(ii) + started_intersection_list.append(intersection) + + if len(vehicle_list) == 0: + break + + for jj in range(len(vehicle_list) -1, -1, -1): + vehicle = vehicle_list[jj] + if vehicle.run: + end_trajectory = vehicle.pure_pursuit_control_wrapper() + if end_trajectory: + vehicle_list.pop(jj) + +def main(): + try: + client = carla.Client("localhost",2000) + client.set_timeout(10.0) + world = client.load_world('Town05') + + # set the weather + weather = carla.WeatherParameters( + cloudiness=10.0, + precipitation=0.0, + sun_altitude_angle=90.0) + world.set_weather(weather) + + # set the spectator position for demo purpose + spectator = world.get_spectator() + spectator.set_transform(carla.Transform(carla.Location(x=-190, y=1.29, z=75.0), carla.Rotation(pitch=-88.0, yaw= -1.85, roll=1.595))) # top view of intersection + + env = CARLA_ENV(world) + time.sleep(2) # sleep for 2 seconds, wait the initialization to finish + + traffic_light_list = get_traffic_lights(world.get_actors()) + + intersection_list = create_intersections(env, 4, traffic_light_list) + init_intersection = intersection_list[0] + normal_intersections = intersection_list[1:] + init_intersection.add_ego_vehicle(safety_distance = 15.0 ) + init_intersection.add_follow_vehicle(follow_distance = 20.0) + init_intersection.add_lead_vehicle(lead_distance = 20.0) + init_intersection.add_vehicle(choice = "left") + init_intersection.add_vehicle(choice = "right",command="left") + init_intersection.add_vehicle(choice = "ahead",command="left") + init_intersection.add_vehicle(choice = "ahead",command = "right") + + intersection_list[1].add_vehicle(choice = "ahead") + intersection_list[1].add_vehicle(choice = "left",command="left") + intersection_list[1].add_vehicle(choice = "right",command = "left") + intersection_list[1].add_vehicle(choice = "right",command = "right") + intersection_list[1]._shift_vehicles(-10, choice = "right",index = 0) + + intersection_list[2].add_vehicle(choice = "ahead") + intersection_list[2].add_vehicle(choice = "left",command="left") + intersection_list[2].add_vehicle(choice = "right",command = "left") + intersection_list[2].add_vehicle(choice = "right",command = "right") + + intersection_list[3].add_vehicle(command = "left") + intersection_list[3].add_vehicle() + + + IntersectionBackend(env,intersection_list) + finally: + time.sleep(10) + env.destroy_actors() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/legacy/gui_interface/backend/intersection_backend.py b/legacy/gui_interface/backend/intersection_backend.py new file mode 100644 index 0000000..4500462 --- /dev/null +++ b/legacy/gui_interface/backend/intersection_backend.py @@ -0,0 +1,438 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Wed Jul 8 14:20:00 2020 + +@author: shijiliu & zhiychen +""" +import sys +sys.path.append("..") + +import carla +import time +from openpyxl import load_workbook +from openpyxl import Workbook + +from backend.intersection_definition import get_traffic_lights +from backend.carla_env import CARLA_ENV # self-written class that provides help functions, should be in the same folder +from backend.multiple_vehicle_control import VehicleControl + +from backend.initial_intersection import create_intersections, get_ego_spectator, get_ego_left_spectator, get_ego_driving_spectator +from backend.full_path_vehicle import LeadVehicleControl, FollowVehicleControl + +from backend.intersection_settings_helper import write_intersection_settings, read_intersection_settings + +from backend.human_ego_control import HumanEgoControlServer + + +# Simulate loop in Base Experiment class (please break this up into sub-functions for the love of god) +def IntersectionBackend(env,intersection_list, allow_collision = True, spectator_mode = None, enable_human_control = False): + + ''' + back end function for the Intersection + + Parameters + ---------- + spectator_mode : string, optional + the spectator mode, valid value is "first_person" or "left". The default is None. + + allow_collision : bool, optional + whether collision is allowed during simulation + + enable_human_control : bool, optional + whether ego vehicle is controlled by human + + Returns + ------- + None. + + ''' + + vehicle_list = [] # list of "other" type vehicle + started_intersection_list = [] + ego_vehicle_config = intersection_list[0].ego_vehicle #init_intersection.ego_vehicle + lead_vehicle_config = intersection_list[0].lead_vehicle + follow_vehicle_config = intersection_list[0].follow_vehicle + + spectator = env.world.get_spectator() + + # if enable_human_control, get the ego vehicle from the environment + if enable_human_control: + ego_vehicle_uniquename = ego_vehicle_config["uniquename"] + human_control_server = HumanEgoControlServer() # create the server for receiving the human command + spectator_mode = "human_driving" + + # assign the first full path vehicle, to determine whether + # each intersection should start + if lead_vehicle_config != None: + first_full_path_vehicle_name = lead_vehicle_config["uniquename"] + + lead_vehicle = LeadVehicleControl(env,lead_vehicle_config,env.delta_seconds,allow_collision) + ego_vehicle = FollowVehicleControl(env, ego_vehicle_config, env.delta_seconds,allow_collision) + end_lead = False + ego_vehicle.use_distance_mode(ego_vehicle_config["lead_distance"]) #use distance control mode with lead_distance + + else: + first_full_path_vehicle_name = ego_vehicle_config["uniquename"] + ego_vehicle = FollowVehicleControl(env, ego_vehicle_config, env.delta_seconds,allow_collision) + end_lead = True + ego_vehicle.use_speed_mode() + + # assign the vehicle for the spectator to follow + if follow_vehicle_config != None: + spectator_vehicle = follow_vehicle_config + spectator_bb = follow_vehicle_config["bounding_box"] + follow_vehicle = FollowVehicleControl(env, follow_vehicle_config, env.delta_seconds,allow_collision) + end_follow = False + follow_vehicle.use_distance_mode(ego_vehicle_config["follow_distance"]) + else: + spectator_vehicle = ego_vehicle_config + spectator_bb = ego_vehicle_config["bounding_box"] + end_follow = True + + if enable_human_control: + spectator_bb = ego_vehicle_config["bounding_box"] + + + + end_ego = False + # get the init intersection + init_intersection = intersection_list.pop(0) + started_intersection_list.append(init_intersection) + + for vehicle_config in init_intersection.subject_vehicle: + # initialize vehicles by different type (ego,lead,follow,other) + if vehicle_config["vehicle_type"] == "other": + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds,allow_collision) + vehicle_list.append(vehicle) + + + for vehicle_config in init_intersection.left_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds,allow_collision) + vehicle_list.append(vehicle) + + for vehicle_config in init_intersection.right_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds,allow_collision) + vehicle_list.append(vehicle) + + for vehicle_config in init_intersection.ahead_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds,allow_collision) + + + RECORD_ENABLE = False + + if RECORD_ENABLE: + # format of ego_vehicle = vehichle_type_id + "_" + uniqname + ego_uniquename = ego_vehicle_config["uniquename"] + timestr = time.strftime("%Y%m%d-%H%M%S") + + # record 1: data on an plain text file + filename = "../data_collection/Urb" + timestr + ".xlsx" + file = open("../data_collection/Urb" + timestr + ".txt", "w+" ) + + world_snapshot = env.world.get_snapshot() + tm = world_snapshot.timestamp + file.write("the experiment starts from " + str(round(tm.elapsed_seconds, 3)) + "(seconds)\n") + print("start urban recordings: ") + + # record 2: data on an excel file + header_row = ['timestamp(sec)'] + for key in env.vehicle_dict.keys(): + key = key[7:] + header_row += [key+'-lane_id', key+'-location_x(m)', key+'-location_y(m)', key+'location_z(m)', key+'-roll(degrees)', key+'-pitch(degrees)', key+'yaw(degrees)', + key+'-velocity_x(m/s)', key+'-velocity_y(m/s)', key+'velocity_z(m/s)', key+'-acceleration_x(m/s2)', key+'-acceleration_y(m/s2)', key+'acceleration_z(m/s2)'] + try: + wb = load_workbook(filename) + ws = wb.worksheets[0] + except FileNotFoundError: + wb = Workbook() + ws = wb.active + ws.append(header_row) + wb.save(filename) + + while True: + + if RECORD_ENABLE: + world_snapshot = env.world.get_snapshot() + ego_id = (int)(ego_uniquename.split("_")[1]) + ego_actor = world_snapshot.find(ego_id) + + tm = world_snapshot.timestamp + file.write("time: " + str(round(tm.elapsed_seconds, 3))+"(seconds)\n") + + data = [str(round(tm.elapsed_seconds, 3))] + for key in env.vehicle_dict.keys(): + vehicle_data = [] + key = key[8:] + id = (int)(key.split("_")[1]) + if(id == ego_id): + file.write("ego id: " + key + "\n") + else: + file.write("vehicle id: " + key + "\n") + actor = world_snapshot.find(id) + + actor_location = actor.get_transform().location + lane = map.get_waypoint(actor_location).lane_id + file.write("lane: " + str(lane) + "\n") + vehicle_data+=[lane] + + actor_transform = actor.get_transform() + x = round(actor_transform.location.x, 2) + y = round(actor_transform.location.y, 2) + z = round(actor_transform.location.z, 2) + vehicle_data+=[x, y, z] + file.write("location: x=" + str(x) + " y=" + str(y) + " z=" +str(z) + "(meters)\n" ) + + x = round(actor_transform.rotation.roll, 2) + y = round(actor_transform.rotation.pitch, 2) + z = round(actor_transform.rotation.yaw, 2) + vehicle_data+=[x, y, z] + file.write("Rotation: roll=" + str(x) + " pitch=" + str(y) + " yaw=" +str(z) + "(degrees)\n") + + + actor_velocity = actor.get_velocity() + x = round(actor_velocity.x, 2) + y = round(actor_velocity.y, 2) + z = round(actor_velocity.z, 2) + vehicle_data+=[x, y, z] + file.write("Velocity: x=" + str(x) + " y=" + str(y) + " z=" +str(z) + "(m/s)\n") + + actor_acceleration = actor.get_acceleration() + x = round(actor_acceleration.x, 2) + y = round(actor_acceleration.y, 2) + z = round(actor_acceleration.z, 2) + vehicle_data+=[x, y, z] + file.write("Acceleration: x=" + str(x) + " y=" + str(y) + " z=" +str(z) + "(m/s2)\n") + + data+=vehicle_data + ws.append(data) + wb.save(filename) + + + # Lane Logic + # waypoint = env.world.get_map().get_waypoint(vehicle.get_location(),project_to_road=True, lane_type=(carla.LaneType.Driving | carla.LaneType.Shoulder | carla.LaneType.Sidewalk)) + # print("Current lane type: " + str(waypoint.lane_type)) + # # Check current lane change allowed + # print("Current Lane change: " + str(waypoint.lane_change)) + # # Left and Right lane markings + # print("L lane marking type: " + str(waypoint.left_lane_marking.type)) + # print("L lane marking change: " + str(waypoint.left_lane_marking.lane_change)) + # print("R lane marking type: " + str(waypoint.right_lane_marking.type)) + # print("R lane marking change: " + str(waypoint.right_lane_marking.lane_change)) + # ... + env.world.tick() + # update the distance between vehicles after each tick + env.update_vehicle_distance() + + # update the ego spectator + if env.vehicle_available(spectator_vehicle["uniquename"]): + spectator_vehicle_transform = env.get_transform_3d(spectator_vehicle["uniquename"]) + #spectator_transform = get_ego_spectator(spectator_vehicle_transform,distance = -10) + if spectator_mode == "first_person": + spectator_transform = get_ego_spectator(spectator_vehicle_transform, distance = -10) + spectator.set_transform(spectator_transform) + elif spectator_mode == "left": + if env.vehicle_available(ego_vehicle_config["uniquename"]): + spectator_vehicle_transform = env.get_transform_3d(ego_vehicle_config["uniquename"]) + spectator_transform = get_ego_left_spectator(spectator_vehicle_transform) + spectator.set_transform(spectator_transform) + elif spectator_mode == "human_driving": + if env.vehicle_available(ego_vehicle_config["uniquename"]): + spectator_vehicle_transform = env.get_transform_3d(ego_vehicle_config["uniquename"]) + #spectator_vehicle_transform = env.get_transform_3d(ego_vehicle_uniquename) + spectator_transform = get_ego_driving_spectator( spectator_vehicle_transform, spectator_bb) + spectator.set_transform(spectator_transform) + #else: + # spectator_transform = carla.Transform(carla.Location(x= 25.4, y=1.29, z=75.0), carla.Rotation(pitch=-88.0, yaw= -1.85, roll=1.595)) + #spectator.set_transform(spectator_transform) + + + for ii in range(len(intersection_list)-1,-1,-1): + # check whether the intersection should start + intersection_list[ii].start_simulation(first_full_path_vehicle_name) + if intersection_list[ii].start_sim: + for vehicle_config in intersection_list[ii].subject_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds,allow_collision) + vehicle_list.append(vehicle) + + for vehicle_config in intersection_list[ii].left_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds,allow_collision) + vehicle_list.append(vehicle) + + for vehicle_config in intersection_list[ii].right_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds,allow_collision) + vehicle_list.append(vehicle) + + for vehicle_config in intersection_list[ii].ahead_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds,allow_collision) + vehicle_list.append(vehicle) + + # move the intersection to started intersection list + intersection = intersection_list.pop(ii) + started_intersection_list.append(intersection) + + ego_stop_at_light = False + + # set the traffic lights based on traffic light setting + for started_intsection in started_intersection_list: + started_intsection.set_intersection_traffic_lights() + + # check the current location of the lead vehicle and ego vehicle if they are available + # so as to update the curr_distance for ego, and follow vehicle + lead_transform = None + ego_transform = None + if not end_lead: + lead_transform = lead_vehicle.get_vehicle_transform() + + if not end_ego: + ego_transform = ego_vehicle.get_vehicle_transform() + if lead_transform != None: + ego_vehicle.get_current_distance(lead_transform) + else: + ego_vehicle.use_speed_mode() # no lead available, change to speed control + + if not end_follow: + if ego_transform != None: + follow_vehicle.get_current_distance(ego_transform) + else: + follow_vehicle.use_speed_mode()# no ego available, change to speed control + + + + + # apply control to ego vehicle, get whether it stops at traffic light + if not end_ego: + if not enable_human_control: + # use the automatic control provided by the back-end, which is set as default + end_ego = ego_vehicle.pure_pursuit_control_wrapper() + ego_stop_at_light = ego_vehicle.blocked_by_light + else: + # get control from human + human_command = human_control_server.get_human_command() + # format the command into carla.VehicleControl + ego_vehicle_control = carla.VehicleControl(throttle = human_command[0] ,steer=human_command[1],brake = human_command[2]) + # apply control to the vehicle + env.apply_vehicle_control(ego_vehicle_uniquename , ego_vehicle_control) + + + end_ego = ego_vehicle.fake_pure_pursuit_control_wrapper() # change all internal settings as the real wrapper, but + # don't apply control to vehicle + ego_stop_at_light = ego_vehicle.blocked_by_light + + # apply control to lead vehicle + if not end_lead: + if ego_stop_at_light and lead_vehicle.mode != "pause" : # lead is still in full path mode when ego stops + lead_vehicle.change_mode("pause") + elif not ego_stop_at_light and lead_vehicle.mode == "pause": + lead_vehicle.change_mode("normal") + + end_lead = lead_vehicle.pure_pursuit_control_wrapper() + + # apply control to follow vehicle + if not end_follow: + end_follow = follow_vehicle.pure_pursuit_control_wrapper() + + + + + + if len(vehicle_list) == 0 and end_lead and end_ego and end_follow: # all vehicle has stopped + break + + for jj in range(len(vehicle_list) -1, -1, -1): + vehicle = vehicle_list[jj] + if vehicle.run: + end_trajectory = vehicle.pure_pursuit_control_wrapper() + if end_trajectory: + vehicle_list.pop(jj) + +def main(): + try: + client = carla.Client("localhost",2000) + client.set_timeout(10.0) + world = client.load_world('Town05') + + # set the weather + weather = carla.WeatherParameters( + cloudiness=10.0, + precipitation=0.0, + sun_altitude_angle=90.0) + world.set_weather(weather) + + # set the spectator position for demo purpose + spectator = world.get_spectator() + spectator.set_transform(carla.Transform(carla.Location(x=-190, y=1.29, z=75.0), carla.Rotation(pitch=-88.0, yaw= -1.85, roll=1.595))) # top view of intersection + + env = CARLA_ENV(world) + time.sleep(2) # sleep for 2 seconds, wait the initialization to finish + + traffic_light_list = get_traffic_lights(world.get_actors()) + + intersection_list = create_intersections(env, 4, traffic_light_list, navigation_speed = 30.0) + init_intersection = intersection_list[0] + normal_intersections = intersection_list[1:] + init_intersection.add_ego_vehicle(safety_distance = 15.0, stop_choice = "abrupt", vehicle_color = '255,255,255') + init_intersection.add_follow_vehicle(follow_distance = 20.0, stop_choice = "penetrate", penetrate_distance = 2.0) + init_intersection.add_lead_vehicle(lead_distance = 20.0, stop_choice = "abrupt", vehicle_color = '255,255,255') + init_intersection.add_vehicle(choice = "left", stop_choice = "abrupt", vehicle_color = '255,255,255') + init_intersection.add_vehicle(choice = "right",command="left") + + # test edit settings + name1 = init_intersection.add_vehicle(choice = "ahead",command="left") + name2 = init_intersection.add_vehicle(choice = "ahead",command = "right") + + name1 = init_intersection.edit_vehicle_settings(name1, choice = "ahead", vehicle_color = '128,128,128') + name2 = init_intersection.edit_vehicle_settings(name2, choice = "ahead", gap = 15.0, vehicle_color = '128,128,128') + + vehicle_settings_entered = init_intersection.get_vehicle_settings(name2) + print(vehicle_settings_entered["gap"]) + + #init_intersection.edit_traffic_light("subject") + #init_intersection.edit_traffic_light("left",red_start = 40.0,red_end = 60.0,yellow_start=30.0,yellow_end=40.0,green_start=0.0,green_end = 30.0) + #init_intersection.edit_traffic_light("right",red_start = 0.0,red_end = 10.0,yellow_start=10.0,yellow_end=20.0,green_start=20.0,green_end = 40.0) + #init_intersection.edit_traffic_light("ahead",red_start = 20.0,red_end = 40.0,yellow_start=10.0,yellow_end=20.0,green_start=0.0,green_end = 10.0) + + # get bounding box + bb = init_intersection.get_vehicle_bounding_box(name1) + print("bb.x = %f, bb.y = %f, bb.z = %f" % (bb.x, bb.y, bb.z)) + + intersection_list[1].add_vehicle(choice = "ahead") + intersection_list[1].add_vehicle(choice = "left",command="left") + intersection_list[1].add_vehicle(choice = "right",command = "left") + intersection_list[1].add_vehicle(choice = "right") + intersection_list[1]._shift_vehicles(-10, choice = "right",index = 0) + #intersection_list[1].edit_traffic_light("left"), + + intersection_list[2].add_vehicle(choice = "ahead") + intersection_list[2].add_vehicle(choice = "left",command="left") + intersection_list[2].add_vehicle(choice = "right",command = "left") + intersection_list[2].add_vehicle(choice = "right",command = "right") + #intersection_list[2].edit_traffic_light("right") + + intersection_list[3].add_vehicle(command = "left") + intersection_list[3].add_vehicle() + #intersection_list[3].edit_traffic_light("ahead") + + # test import/export + init_setting = init_intersection.export_settings() + + intersection_list[3].import_settings(init_setting) + intersection_list[3].add_vehicle(command = "left") + intersection_list[3].add_vehicle() + third_setting = intersection_list[3].export_settings() + + write_intersection_settings("third_intersection_setting",third_setting) + new_third_setting = read_intersection_settings('third_intersection_setting') + + intersection_list[2].import_settings(new_third_setting) + + + + + IntersectionBackend(env,intersection_list,allow_collision = False) + finally: + time.sleep(10) + env.destroy_actors() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/legacy/gui_interface/backend/intersection_definition.py b/legacy/gui_interface/backend/intersection_definition.py new file mode 100644 index 0000000..a683cb7 --- /dev/null +++ b/legacy/gui_interface/backend/intersection_definition.py @@ -0,0 +1,1450 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Sun Jul 5 10:20:35 2020 + +@author: shijiliu +""" +import sys +sys.path.append("..") + +import carla +from backend.carla_env import CARLA_ENV +import math +import time +import numpy as np +from configobj import ConfigObj +from backend.generate_path_omit_regulation import generate_path +from scipy.interpolate import UnivariateSpline +import copy + +from backend.intersection_settings_helper import write_intersection_settings, read_intersection_settings + +DEBUG_INIT = True +DEBUG_TRAJECTORY = True + +# color for debug use +red = carla.Color(255, 0, 0) +green = carla.Color(0, 255, 0) +blue = carla.Color(47, 210, 231) +cyan = carla.Color(0, 255, 255) +yellow = carla.Color(255, 255, 0) +orange = carla.Color(255, 162, 0) +white = carla.Color(255, 255, 255) + + +# distance of lane points from traffic light +''' +END1 = -6.5#-5.5 +END2 = -9.0 +START1 = -12.5 +START2 = -15.5#-15.5#-16.0 +''' + +# right shift from the center of the lane when spawning +RIGHT_SHIFT = 0.0#1.6 # 0.0 if requirements changed to spawn in the middle of the lane# + +# API Helper? +def get_traffic_lights(actor_list): + # get all the available traffic lights + traffic_light_list = [] + for actor in actor_list: + if 'traffic_light' in actor.type_id: + traffic_light_list.append(actor) + return traffic_light_list + +# Vehicle/Controller Class +def smooth_trajectory(trajectory): + ''' + + + Parameters + ---------- + trajectory : np.array([(float,float),...,(float,float)]) + 2d trajectory. + + Returns + ------- + smoothed_trajectory : np.array([(float,float),...,(float,float)]) + the smoother trajectory + + ''' + + smoothed_trajectory = [] + smoothed_trajectory.append(trajectory[0]) + + num = 3 + + for ii in range(num - 1,len(trajectory)): + avg_pt = (trajectory[ii - 2] + trajectory[ii - 1] + trajectory[ii]) / num + smoothed_trajectory.append(avg_pt) + + smoothed_trajectory.append(trajectory[-1]) + return np.array(smoothed_trajectory) + +# Vehicle/Controller class +def get_trajectory(way_points): + ''' + + + Parameters + ---------- + way_points : list + A list of (way_point, reference_speed) tuple, + where way_points is a tuple of floats (x,y), the first point must be the **current point** of the vehicle + reference speed is the desired speed for the vehicle after this way point and before the next way point + e.g. [((0.0,0.0),10.0),((0.0,10.0),1.0)] + + Returns + ------- + trajectory : numpy 2d array + the interpolated trajectory. + ref_speed_list : list + the speed correspoding to the interpolated trajectory + + ''' + points, speed = zip(*way_points) + points = np.array([[pt[0], pt[1]] for pt in points]) + + # apply average smoothing of the points + points = smooth_trajectory(points) + + # linear length along the line (reference: https://stackoverflow.com/questions/52014197/how-to-interpolate-a-2d-curve-in-python) + distance = np.cumsum( np.sqrt(np.sum( np.diff(points,axis=0)**2, axis = 1))) + distance = np.insert(distance, 0, 0)/distance[-1] + + # Build a list of the spline function, one for each dimension: + splines = [UnivariateSpline(distance, coords, k=3, s=.2) for coords in points.T] + + alpha = np.linspace(0,1.0, 2 * len(distance)) + trajectory = np.vstack( [spl(alpha) for spl in splines] ).T + + + nearest_index = [] + for pt in points: + nearest_distance = np.inf + index = 0 + count = 0 + for trajectory_pt in trajectory: + dist_2 = sum((trajectory_pt - pt)**2) + if dist_2 < nearest_distance: + nearest_distance = dist_2 + index = count + count += 1 + nearest_index.append(index) + + ref_speed_list = np.zeros(len(trajectory)) + for ii in range(1,len(nearest_index)): + ref_speed_list[nearest_index[ii - 1]:nearest_index[ii]] = speed[ii - 1] + + return trajectory, ref_speed_list + +# Keep intersection class +class Intersection(): + def __init__(self, env, world_pos, traffic_light_list, distance = 75.0, yaw = 0.0, start_sim_distance = 40, navigation_speed = 10.0): + ''' + + + Parameters + ---------- + env: CARLA_ENV + the simulation environment + world_pos : (float,float) + the (rough) central point of the intersection. + traffic_light_list : list + list of all available traffic lights. + distance : float, optional + width and height of the intersection. The default is 75.0 (m). + yaw : float, optional + define the direction the ego vehicle will pass through the intersection. The default is 0. + navigation_speed : float + the navigation speed of the vehicle + Returns + ------- + None. + + ''' + + self.env = env + self.distance = distance + self.yaw = yaw % 360 + self._get_local_traffic_lights(world_pos,traffic_light_list) # get the traffic light at this intersection + self._get_lane_points() # get the in/out point of lane + #self._yaw2vector() + self._split_lane_points() # split in/out point of lane into subject/left/right/ahead + self._get_spawn_reference() # find a reference point for spawning for each of the subject/left/right/ahead lane + self._split_traffic_lights() # split the traffic lights into subject/left/right/ahead + + self.subject_vehicle = [] + self.left_vehicle = [] + self.right_vehicle = [] + self.ahead_vehicle = [] + + self.start_sim_distance = start_sim_distance + self.start_sim = False # whether the simulation at this intersection should start or not + + self.DEBUG_TRAJECTORY = True + + self.navigation_speed = navigation_speed + + # What is the point of this function? + def start_simulation(self, full_path_vehicle_name): + ''' + check whether the first full path vehicle is within this intersection + + Parameters + ---------- + full_path_vehicle_name : string + uniquename of the first full path vehicle (i.e. lead if lead exists, otherwise ego) + + Returns + ------- + None. + + ''' + full_path_vehicle_transform = self.env.get_transform_2d(full_path_vehicle_name) + full_path_vehicle_location = full_path_vehicle_transform[0] # 2d location of the vehicle + ref_waypoint = self.subject_lane_ref + ref_location = ref_waypoint.transform.location + distance = math.sqrt((ref_location.x - full_path_vehicle_location[0])**2 + (ref_location.y - full_path_vehicle_location[1])**2 ) + + # start simulation if distance between the vehicle and the reference point is within + # the pre-set start_sim_distance + if distance < self.start_sim_distance: + self.start_sim = True + + + # Intersection Class + def _get_local_traffic_lights(self, world_pos,traffic_light_list): + ''' + + + Parameters + ---------- + world_pos : (float,float) + the (rough) central point of the intersection. + traffic_light_list : list + list of all available traffic lights. + + Returns + ------- + None. + + ''' + self.local_traffic_lights = [] + for light in traffic_light_list: + location = light.get_location() + distance = math.sqrt((location.x - world_pos[0])**2 + (location.y - world_pos[1]) ** 2) # get the 2d Euclidean distance + if distance < self.distance / 2: + self.local_traffic_lights.append(light) + + assert(len(self.local_traffic_lights) == 4) # should contain and only contain 4 lights + + x = 0 + y = 0 + for light in self.local_traffic_lights: + x += light.get_location().x + y += light.get_location().y + + self.world_pos = (x / len(self.local_traffic_lights),y / len(self.local_traffic_lights)) + + + if DEBUG_INIT: + print(self.world_pos) + for light in self.local_traffic_lights: + print(light.get_location()) + self.env.world.debug.draw_point(light.get_location(),size = 0.1, color = blue, life_time=0.0, persistent_lines=True) + + # Not sure what's going on here either + def _get_lane_points(self): + # get the into/out lane points of this intersection + self.carla_map = self.env.world.get_map() + self.out_lane_points = [] + self.into_lane_points = [] + + for ii in range(len(self.local_traffic_lights)): + light_location = self.local_traffic_lights[ii].get_location() + vector = self.local_traffic_lights[ii].get_transform().get_forward_vector() + for jj in range(len(self.local_traffic_lights)): + if jj != ii: + # calculate the angle between the light's forward vector and + # the vector from this light to the other light + another_light_location = self.local_traffic_lights[jj].get_location() + vec1_2 = np.array([another_light_location.x - light_location.x, another_light_location.y - light_location.y]) + forward_vector_2d = np.array([-vector.x, -vector.y]) # the reverse direction of forward vector is what we need + norm_vec1_2 = vec1_2 / np.linalg.norm(vec1_2) + norm_forward_vector_2d = forward_vector_2d / np.linalg.norm(forward_vector_2d) + dot_product = np.dot(norm_vec1_2,norm_forward_vector_2d) + angle = np.arccos(dot_product) + + + if angle < np.pi / 12: # angle within 15 degrees + other_light_location = another_light_location + break + + + distance = math.sqrt((light_location.x - other_light_location.x)**2 + (light_location.y - other_light_location.y)**2) + + + if distance < 25: # 4 lanes inside + END1 = -6.5#-5.5 + END2 = -9.0 + START1 = -12.5 + START2 = -15.5#-15.5#-16.0 + elif distance >= 25 and distance < 27: + # 6 lane road + END1 = -8.0#-5.5 + END2 = -12.0 + START1 = -16.0 + START2 = -18.0#-15.5#-16.0 + else: + # 6 lane road, wider + END1 = -9.0#-5.5 + END2 = -12.0 + START1 = -16.0 + START2 = -20.0#-15.5#-16.0 + + + end_1 = carla.Location(x = light_location.x + vector.x * END1,y = light_location.y + vector.y * END1, z = light_location.z + vector.z * END1) + end_2 = carla.Location(x = light_location.x + vector.x * END2,y = light_location.y + vector.y * END2, z = light_location.z + vector.z * END2) + start_1 = carla.Location(x = light_location.x + vector.x * START1,y = light_location.y + vector.y * START1, z = light_location.z + vector.z * START1) + start_2 = carla.Location(x = light_location.x + vector.x * START2,y = light_location.y + vector.y * START2, z = light_location.z + vector.z * START2) + into_1 = self.carla_map.get_waypoint(end_1) + into_2 = self.carla_map.get_waypoint(end_2) + out_1 = self.carla_map.get_waypoint(start_1) + out_2 = self.carla_map.get_waypoint(start_2) + self.out_lane_points.append(out_1) + self.out_lane_points.append(out_2) + self.into_lane_points.append(into_1) + self.into_lane_points.append(into_2) + + # General helper function + def _yaw2vector(self): + # get the direction vector of this intersection + yaw_rad = math.radians(self.yaw) + self.direction_vector = [math.cos(yaw_rad),math.sin(yaw_rad)] + + def _debug_lane_point(self,pt,color): + if DEBUG_INIT: + self.env.world.debug.draw_point(pt.transform.location,size = 0.1, color = color, life_time=0.0, persistent_lines=True) + forward_vector = pt.transform.get_forward_vector() + start = pt.transform.location + end = carla.Location(x = start.x + forward_vector.x, y = start.y + forward_vector.y, z = start.z + forward_vector.z) + self.env.world.debug.draw_arrow(start,end,thickness=0.1, arrow_size=0.2, color = color, life_time=0.0, persistent_lines=True) + + def _split_lane_points(self): + # split the lane points into + # subject/left/right/ahead lane + + self.subject_out = [] + self.left_out = [] + self.right_out = [] + self.ahead_out = [] + + self.subject_in = [] + self.left_in = [] + self.right_in = [] + self.ahead_in = [] + + max_angle_dev = 10 + + for pt in self.out_lane_points: + pt_yaw = pt.transform.rotation.yaw % 360 + relative_yaw = (pt_yaw - self.yaw) % 360 + + if abs(relative_yaw - 0) < max_angle_dev or abs(relative_yaw - 360) < max_angle_dev: + self.subject_out.append(pt) + self._debug_lane_point(pt,green) + + elif abs(relative_yaw - 90) < max_angle_dev: + self.left_out.append(pt) + self._debug_lane_point(pt,blue) + + elif abs(relative_yaw - 180) < max_angle_dev: + self.ahead_out.append(pt) + self._debug_lane_point(pt,yellow) + + elif abs(relative_yaw - 270) < max_angle_dev: + self.right_out.append(pt) + self._debug_lane_point(pt,orange) + + for pt in self.into_lane_points: + pt_yaw = pt.transform.rotation.yaw % 360 + relative_yaw = (pt_yaw - self.yaw) % 360 + if abs(relative_yaw - 0) < max_angle_dev or abs(relative_yaw - 360) < max_angle_dev: + self.ahead_in.append(pt) + self._debug_lane_point(pt,green) + + elif abs(relative_yaw - 90) < max_angle_dev : + self.right_in.append(pt) + self._debug_lane_point(pt,blue) + + elif abs(relative_yaw - 180) < max_angle_dev: + self.subject_in.append(pt) + self._debug_lane_point(pt,yellow) + + elif abs(relative_yaw - 270) < max_angle_dev: + self.left_in.append(pt) + self._debug_lane_point(pt,orange) + + def _split_traffic_lights(self): + # split the traffic lights into + # subject/left/right/ahead lane + + # get the 4 direction vector + forward_direction = self.subject_lane_ref.transform.get_forward_vector() + forward_direction_2d = [forward_direction.x,forward_direction.y] + forward_direction_2d = forward_direction_2d / np.linalg.norm(forward_direction_2d) + + left_direction = self.left_lane_ref.transform.get_forward_vector() + left_direction_2d = [left_direction.x,left_direction.y] + left_direction_2d = left_direction_2d / np.linalg.norm(left_direction_2d) + + right_direction = self.right_lane_ref.transform.get_forward_vector() + right_direction_2d = [right_direction.x,right_direction.y] + right_direction_2d = right_direction_2d / np.linalg.norm(right_direction_2d) + + ahead_direction = self.ahead_lane_ref.transform.get_forward_vector() + ahead_direction_2d = [ahead_direction.x,ahead_direction.y] + ahead_direction_2d = ahead_direction_2d / np.linalg.norm(ahead_direction_2d) + + self.subject_light = None + self.left_light = None + self.right_light = None + self.ahead_light = None + + for traffic_light in self.local_traffic_lights: + light_vector = traffic_light.get_transform().get_forward_vector() + left_vector = self._get_left_vector(light_vector) + if abs(np.dot(left_vector,forward_direction_2d) - 1.0) < 0.1: + self.subject_light = traffic_light + elif abs(np.dot(left_vector,left_direction_2d) - 1.0) < 0.1: + self.left_light = traffic_light + elif abs(np.dot(left_vector,right_direction_2d) - 1.0) < 0.1: + self.right_light = traffic_light + elif abs(np.dot(left_vector,ahead_direction_2d) - 1.0) < 0.1: + self.ahead_light = traffic_light + + self.light_config = ConfigObj() + self.light_config["subject"] = None + self.light_config["subject_time"] = None + self.light_config["left"] = None + self.light_config["left_time"] = None + self.light_config["right"] = None + self.light_config["right_time"] = None + self.light_config["ahead"] = None + self.light_config["ahead_time"] = None + + # initialize counter for traffic light color setting + self.local_time_count = 0 + + + if DEBUG_INIT: + self.env.world.debug.draw_point(self.subject_light.get_transform().location,size = 0.2, color = green, life_time=0.0, persistent_lines=True) + self.env.world.debug.draw_point(self.left_light.get_transform().location,size = 0.2, color = yellow, life_time=0.0, persistent_lines=True) + self.env.world.debug.draw_point(self.right_light.get_transform().location,size = 0.2, color = blue, life_time=0.0, persistent_lines=True) + self.env.world.debug.draw_point(self.ahead_light.get_transform().location,size = 0.2, color = red, life_time=0.0, persistent_lines=True) + + def _get_left_vector(self,vector): + # return the left vector of the input 2d vector + # vector : carla.Vector3D + left_x = vector.y + left_y = - vector.x + return [left_x,left_y] + + + def _vec_angle(self,vec1,vec2): + vec1 = vec1 / np.linalg.norm(vec1) + vec2 = vec2 / np.linalg.norm(vec2) + dot_product = np.dot(vec1,vec2) + angle = np.arccos(dot_product) + return angle + + + def _get_lane_spawn_reference(self,lane_out_pts): + # function: return the reference point for spawning in this lane + + # requirements: lane_out_pts should have and only have 2 points + # in theory, the second point should be more "left" + + + return lane_out_pts[1] + + def _get_spawn_reference(self): + # get the reference way point for each lane + self.subject_lane_ref = self._get_lane_spawn_reference(self.subject_out) + self.left_lane_ref = self._get_lane_spawn_reference(self.left_out) + self.right_lane_ref = self._get_lane_spawn_reference(self.right_out) + self.ahead_lane_ref = self._get_lane_spawn_reference(self.ahead_out) + + if DEBUG_INIT: + self.env.world.debug.draw_point(self.subject_lane_ref.transform.location,size = 0.2, color = green, life_time=0.0, persistent_lines=True) + self.env.world.debug.draw_point(self.left_lane_ref.transform.location,size = 0.2, color = yellow, life_time=0.0, persistent_lines=True) + self.env.world.debug.draw_point(self.right_lane_ref.transform.location,size = 0.2, color = blue, life_time=0.0, persistent_lines=True) + self.env.world.debug.draw_point(self.ahead_lane_ref.transform.location,size = 0.2, color = red, life_time=0.0, persistent_lines=True) + + def add_vehicle(self,gap = 10.0,model_name = "vehicle.tesla.model3",choice = "subject", command = "straight", stop_choice = "normal", penetrate_distance = None,obey_traffic_lights = True, run = True, safety_distance = 15.0, vehicle_color = None): + ''' + + + Parameters + ---------- + gap : float,optional + the distance between a vehicle and its previous one + model_name : string, optional + vehicle type. The default is "vehicle.tesla.model3". + choice : string, optional + the lane this vehicle will be added, valid values: "subject", "left", "right", "ahead". The default is "subject". + command : string, optional + the turning command, valid values: "straight", "right", "left" + stop_choice : string, optional + how will the vehicle stop when at yellow or red light. valid values: "normal", "abrupt", "penetrate" + penetrate_distance : float, unit: meter + to what extent the vehicle will penetrate the traffic lane. This parameter will only be use when stop_choice is "penetrate" + obey_traffic_light : bool, optional + whether the vehicle will obey traffic light. Default is True + run : bool, optional + whether the vehicle is running. Default is True + safety_distance : float, optional + smallest distance between this vehicle and vehicle ahead + vehicle_color : string + the RGB representation of the vehicle color. e.g. '255,255,255' + + + Returns + ------- + uniquename : the uniquename of the vehicle + + ''' + + right_shift_value = RIGHT_SHIFT + + vehicle = ConfigObj() + vehicle["model"] = model_name + vehicle["gap"] = gap + vehicle["command"] = command + vehicle["obey_traffic_lights"] = obey_traffic_lights + vehicle["run"] = run + vehicle["safety_distance"] = safety_distance + vehicle["choice"] = choice + + #print(choice) + + if choice == "subject": + ref_waypoint = self.subject_lane_ref + vehicle_set = self.subject_vehicle + vehicle["traffic_light"] = self.subject_light + + elif choice == "left": + ref_waypoint = self.left_lane_ref + vehicle_set = self.left_vehicle + vehicle["traffic_light"] = self.left_light + + elif choice == "ahead": + ref_waypoint = self.ahead_lane_ref + vehicle_set = self.ahead_vehicle + vehicle["traffic_light"] = self.ahead_light + + elif choice == "right": + ref_waypoint = self.right_lane_ref + vehicle_set = self.right_vehicle + vehicle["traffic_light"] = self.right_light + + if len(vehicle_set) != 0: + ref_waypoint = vehicle_set[-1]["ref_waypoint"] + #previous_uniquename = vehicle_set[-1]["uniquename"] + #bb = self.env.get_vehicle_bounding_box(previous_uniquename) + bb = vehicle_set[-1]["bounding_box"] + + right_shift_value = right_shift_value #- bb.y / 2 + + curr_length = self.env.get_vehicle_model_length(model_name) + + gap += bb.x / 2 + curr_length / 2 + + ''' + else: + if gap < 10.0: + gap = 10.0 # add a constraint to the gap between the first vehicle and the lane + # reference point. Add a vehicle too close to reference point + # will lead to vehicle not detecting the traffic light + ''' + + # use the original reference point to get the new reference point + # reference point is in the middle of the lane + # function same as self._get_next_waypoint + forward_vector = ref_waypoint.transform.get_forward_vector() + + location = ref_waypoint.transform.location + raw_spawn_point = carla.Location(x = location.x - gap * forward_vector.x , y = location.y - gap * forward_vector.y , z = location.z + 1.0) + + new_ref_waypoint = self.carla_map.get_waypoint(raw_spawn_point) + + # right shift the spawn point + # right is with respect to the direction of vehicle navigation + ref_yaw = new_ref_waypoint.transform.rotation.yaw + + right_vector = self._get_unit_right_vector(ref_yaw) + + new_location = new_ref_waypoint.transform.location + + spawn_location = carla.Location(x = new_location.x - right_shift_value * right_vector[0], y = new_location.y - right_shift_value * right_vector[1], z = new_location.z + 0.2) + spawn_rotation = new_ref_waypoint.transform.rotation + + uniquename = self.env.spawn_vehicle(model_name = model_name,spawn_point = carla.Transform(spawn_location,spawn_rotation), color = vehicle_color) + vehicle["uniquename"] = uniquename + vehicle["ref_waypoint"] = new_ref_waypoint + vehicle["location"] = spawn_location + vehicle["rotation"] = spawn_rotation + + #print(vehicle_color) + + if vehicle_color == None: + vehicle["vehicle_color"] = vehicle_color + else: + vehicle["vehicle_color"] = vehicle_color.replace(',',';') # replace , by ; to avoid error when importing from file + + + trajectory, ref_speed_list = self._generate_path(choice, command, new_ref_waypoint) + vehicle["trajectory"] = trajectory + vehicle["ref_speed_list"] = ref_speed_list + + # get the bounding box of the new vehicle + + new_bb = self.env.get_vehicle_bounding_box(uniquename) + vehicle["bounding_box"] = new_bb + vehicle["vehicle_type"] = "other" + + # vehicle stop type + vehicle["stop_choice"] = stop_choice + vehicle["penetrate_distance"] = penetrate_distance + if stop_choice == "normal": + stop_point = self._get_next_waypoint(ref_waypoint,distance = -3.0) # 3 meters after the reference point + vehicle["stop_ref_point"] = stop_point.transform + elif stop_choice == "penetrate": + stop_point = self._get_next_waypoint(ref_waypoint,distance = penetrate_distance) + vehicle["stop_ref_point"] = stop_point.transform + else: + vehicle["stop_ref_point"] = ref_waypoint.transform + + + + + vehicle_set.append(vehicle) + + return uniquename + + def _shift_vehicles(self, length, choice = "subject", index = 0): + ''' + shift the location of a list of vehicles + + **note: for ego/lead/follow type, the path is not generated** + + Parameters + ---------- + length : float + the length we want to shift all the vehicles + choice : string, optional + the lane this vehicle will be added, valid values: "subject", "left", "right", "ahead". The default is "subject". + index : int, optional + the index of the vehicle that shifting. The default is 0. + + Returns + ------- + None. + + ''' + right_shift_value = RIGHT_SHIFT + + if choice == "subject": + #ref_waypoint = self.subject_lane_ref + vehicle_set = self.subject_vehicle + elif choice == "left": + #ref_waypoint = self.left_lane_ref + vehicle_set = self.left_vehicle + elif choice == "ahead": + #ref_waypoint = self.ahead_lane_ref + vehicle_set = self.ahead_vehicle + elif choice == "right": + #ref_waypoint = self.right_lane_ref + vehicle_set = self.right_vehicle + + #if index != 0: + # ref_waypoint = vehicle_set[index - 1]["ref_waypoint"] + + # shifting the vehicles in reverse order + for ii in range(len(vehicle_set) - 1,index - 1,-1): + vehicle = vehicle_set[ii] + new_ref_waypoint = self._get_next_waypoint(vehicle["ref_waypoint"],distance = length) + + ref_yaw = new_ref_waypoint.transform.rotation.yaw + + right_vector = self._get_unit_right_vector(ref_yaw) + + new_location = new_ref_waypoint.transform.location + + spawn_location = carla.Location(x = new_location.x - right_shift_value * right_vector[0], y = new_location.y - right_shift_value * right_vector[1], z = new_location.z + 0.1) + spawn_rotation = new_ref_waypoint.transform.rotation + + # move the vehicle location + self.env.move_vehicle_location(vehicle["uniquename"],carla.Transform(spawn_location,spawn_rotation)) + vehicle["ref_waypoint"] = new_ref_waypoint + vehicle["location"] = spawn_location + vehicle["rotation"] = spawn_rotation + + if vehicle["vehicle_type"] == "other": + command = vehicle["command"] + trajectory, ref_speed_list = self._generate_path(choice, command, new_ref_waypoint) # generate new trajectory + vehicle["trajectory"] = trajectory + vehicle["ref_speed_list"] = ref_speed_list + + + + + + + def _get_unit_right_vector(self,yaw): + # get the right vector + right_yaw = (yaw + 270) % 360 + rad_yaw = math.radians(right_yaw) + right_vector = [math.cos(rad_yaw),math.sin(rad_yaw)] + right_vector = right_vector / np.linalg.norm(right_vector) + return right_vector + + + def _generate_path(self, choice, command, start_waypoint): + ''' + + + Parameters + ---------- + choice : string + the lane choice, valid values: "subject","left","right","ahead" + command : string + the command of navigation. valid command: "straight","left","right" + + Returns + ------- + smoothed_full_trajectory : list of 2d points + smoothed and interpolated trajectory + + ref_speed_list : list + the speed correspoding to the interpolated trajectory + ''' + color = green + + if choice == "subject": + first_waypoint = self.subject_lane_ref + straight_waypoint = self.ahead_in[0] # can also be [1], choosing the left lane + left_waypoint = self.left_in[0] + right_waypoint = self.right_in[0] + + + elif choice == "left": + first_waypoint = self.left_lane_ref + straight_waypoint = self.right_in[0] # can also be [1], choosing the left lane + left_waypoint = self.ahead_in[0] + right_waypoint = self.subject_in[0] + + elif choice == "ahead": + first_waypoint = self.ahead_lane_ref + straight_waypoint = self.subject_in[0] # can also be [1], choosing the left lane + left_waypoint = self.right_in[0] + right_waypoint = self.left_in[0] + + elif choice == "right": + first_waypoint = self.right_lane_ref + straight_waypoint = self.left_in[0] # can also be [1], choosing the left lane + left_waypoint = self.subject_in[0] + right_waypoint = self.ahead_in[0] + + #self.env.world.debug.draw_point(start_waypoint.transform.location,size = 0.5, color = red, life_time=0.0, persistent_lines=True) + + if command == "straight": + second_waypoint = straight_waypoint + elif command == "left": + #first_waypoint = self._get_next_waypoint(first_waypoint,3) + second_waypoint = left_waypoint + color = yellow + elif command == "right": + second_waypoint = right_waypoint + color = blue + + third_waypoint = self._get_next_waypoint(second_waypoint,20) + trajectory1 = generate_path(self.env, start_waypoint, first_waypoint, waypoint_separation = 4) + trajectory2 = generate_path(self.env, first_waypoint, second_waypoint,waypoint_separation = 4) + trajectory3 = generate_path(self.env, second_waypoint, third_waypoint) + full_trajectory = trajectory1 + trajectory2[1:] + trajectory3[1:] # append the full trajectory + + trajectory = [((pt[0],pt[1]),self.navigation_speed) for pt in full_trajectory] + + smoothed_full_trajectory, ref_speed_list = get_trajectory(trajectory) + + if self.DEBUG_TRAJECTORY: + for ii in range(1,len(smoothed_full_trajectory)): + loc1 = carla.Location(x = smoothed_full_trajectory[ii - 1][0], y = smoothed_full_trajectory[ii - 1][1], z = 0.0) + loc2 = carla.Location(x = smoothed_full_trajectory[ii][0], y = smoothed_full_trajectory[ii][1], z = 0.0) + self.env.world.debug.draw_arrow(loc1, loc2, thickness = 0.05, arrow_size = 0.1, color = color, life_time=0.0, persistent_lines=True) + return smoothed_full_trajectory, ref_speed_list + + def _get_next_waypoint(self,curr_waypoint,distance = 10): + ''' + + + Parameters + ---------- + curr_waypoint : carla.Waypoint + current waypoint. + distance : float, optional + "distance" between current waypoint and target waypoint . The default is 10. + + Returns + ------- + next_waypoint : carla.Waypoint + next waypoint, "distance" away from curr_waypoint, in the direction of the current way point + ''' + forward_vector = curr_waypoint.transform.get_forward_vector() + + location = curr_waypoint.transform.location + raw_spawn_point = carla.Location(x = location.x + distance * forward_vector.x , y = location.y + distance * forward_vector.y , z = location.z + 0.1) + + next_waypoint = self.carla_map.get_waypoint(raw_spawn_point) + return next_waypoint + + def get_subject_waypoints(self): + first_waypoint = self.subject_lane_ref + second_waypoint = self.ahead_in[0] + third_waypoint = self._get_next_waypoint(second_waypoint,20) + return [first_waypoint,second_waypoint,third_waypoint] + + + def get_subject_traffic_light(self): + return self.subject_light + + # Intersection class (clean up this garbage + def remove_vehicle(self,uniquename): + ''' + remove a specific vehicle from the intersection + + Parameters + ---------- + uniquename : TYPE + DESCRIPTION. + + Returns + ------- + removed : Bool, + whether a vehicle is removed + + ''' + for ii in range(len(self.subject_vehicle)): + if self.subject_vehicle[ii]["uniquename"] == uniquename: # check whether the vehicle is the one we want to remove + # remove vehicle from environment + self.env.destroy_vehicle(uniquename) + self.subject_vehicle.pop(ii) + return True + + for ii in range(len(self.left_vehicle)): + if self.left_vehicle[ii]["uniquename"] == uniquename: # check whether the vehicle is the one we want to remove + # remove vehicle from environment + self.env.destroy_vehicle(uniquename) + self.left_vehicle.pop(ii) + return True + + for ii in range(len(self.right_vehicle)): + if self.right_vehicle[ii]["uniquename"] == uniquename: # check whether the vehicle is the one we want to remove + # remove vehicle from environment + self.env.destroy_vehicle(uniquename) + self.right_vehicle.pop(ii) + return True + + for ii in range(len(self.ahead_vehicle)): + if self.ahead_vehicle[ii]["uniquename"] == uniquename: # check whether the vehicle is the one we want to remove + # remove vehicle from environment + self.env.destroy_vehicle(uniquename) + self.ahead_vehicle.pop(ii) + return True + + return False + + # Maybe move to GUI if all info is communicated at once + def edit_vehicle_settings(self, uniquename, choice, gap = 10.0,model_name = "vehicle.tesla.model3", command = "straight", stop_choice = "normal", penetrate_distance = None,obey_traffic_lights = True, run = True, safety_distance = 15.0, vehicle_color = None ): + ''' + allow user to edit the vehicle settings + Note: the original vehicle will be destroyed, and a new vehicle will be added + + Parameters + ---------- + uniquename : string + original uniquename of the vehicle. + choice : string + the lane choice, valid values: "subject","left","right","ahead" + gap : float,optional + the distance between a vehicle and its previous one + model_name : string, optional + vehicle type. The default is "vehicle.tesla.model3". + command : string, optional + the turning command, valid values: "straight", "right", "left" + stop_choice : string, optional + how will the vehicle stop when at yellow or red light. valid values: "normal", "abrupt", "penetrate" + penetrate_distance : float, unit: meter + to what extent the vehicle will penetrate the traffic lane. This parameter will only be use when stop_choice is "penetrate" + obey_traffic_light : bool, optional + whether the vehicle will obey traffic light. Default is True + run : bool, optional + whether the vehicle is running. Default is True + safety_distance : float, optional + smallest distance between this vehicle and vehicle ahead + vehicle_color : string + the RGB representation of the vehicle color. e.g. '255,255,255' + Returns + ------- + new_uniquename : string + new uniquename of the vehicle + + ''' + + + + + # get the given lane + if choice == "subject": + vehicle_set = self.subject_vehicle + elif choice == "left": + vehicle_set = self.left_vehicle + elif choice == "ahead": + vehicle_set = self.ahead_vehicle + elif choice == "right": + vehicle_set = self.right_vehicle + + + # get vehicle index in the given lane + index = 0 + original_gap = None + for vehicle in vehicle_set: + if vehicle["uniquename"] == uniquename: + original_gap = vehicle["gap"] + break + index += 1 + + + + + # shift the vehicle + if original_gap != None: + shift_distance = original_gap - gap + self._shift_vehicles(shift_distance, choice = choice, index = index) + else: + print("return None in edit vehicle") + return None + + # remove the current vehicle, + # note that after removing the vehicle, index is pointing at the vehicle after the current one + removed = self.remove_vehicle(uniquename) + if not removed: + print("vehicle not found") + return None + + + # get the given lane + if choice == "subject": + # store the vehicles after the current one + vehicles_after_current = self.subject_vehicle[index :] + self.subject_vehicle = self.subject_vehicle[:index] + + elif choice == "left": + vehicles_after_current = self.left_vehicle[index :] + self.left_vehicle = self.left_vehicle[:index] + + elif choice == "ahead": + vehicles_after_current = self.ahead_vehicle[index :] + self.ahead_vehicle = self.ahead_vehicle[:index] + + elif choice == "right": + vehicles_after_current = self.right_vehicle[index :] + self.right_vehicle = self.right_vehicle[:index] + + + + # add a new vehicle with new settings + new_uniquename = self.add_vehicle(gap = gap, model_name = model_name, choice = choice, command = command, stop_choice = stop_choice, penetrate_distance = penetrate_distance, obey_traffic_lights = obey_traffic_lights, run = run, safety_distance = safety_distance, vehicle_color = vehicle_color) + + + if choice == "subject": + self.subject_vehicle += vehicles_after_current + elif choice == "left": + self.left_vehicle += vehicles_after_current + elif choice == "ahead": + self.ahead_vehicle += vehicles_after_current + elif choice == "right": + self.right_vehicle += vehicles_after_current + + + return new_uniquename + + # Intersection Class + def edit_traffic_light(self,light, red_start = 0.0,red_end = 10.0,yellow_start = 10.0,yellow_end = 15.0,green_start = 15.0,green_end = 25.0): + ''' + edit the start and end time for traffic colors + the traffic color timeline will not loop + i.e. after it reaches the end of timeline, the traffic state will be + frozen at that state + + Requirements: there exists and only exists one start time at 0 + otherwise, a red color will be used as placeholder + until the first start time + + Parameters + ---------- + light : string + light choice. valid values: ahead,left,right,subject + + + Returns + ------- + None. + ''' + red_start = red_start / self.env.delta_seconds + red_end = red_end / self.env.delta_seconds + yellow_start = yellow_start / self.env.delta_seconds + yellow_end = yellow_end / self.env.delta_seconds + green_start = green_start / self.env.delta_seconds + green_end = green_end / self.env.delta_seconds + + + # get the end of timeline + max_time = max(red_end,yellow_end,green_end) + + color_timeline = [] + + for ii in range(int(max_time)): + if ii >= red_start and ii < red_end: + color_timeline.append('red') + elif ii >= yellow_start and ii < yellow_end: + color_timeline.append('yellow') + elif ii >= green_start and ii < green_end: + color_timeline.append('green') + else: + color_timeline.append('red') + + self.light_config[light] = color_timeline + self.light_config[light + '_time'] = {} + self.light_config[light + '_time']['red'] = red_end - red_start + self.light_config[light + '_time']['yellow'] = yellow_end - yellow_start + self.light_config[light + '_time']['green'] = green_end - green_start + + # Intersection class (clean up this garbage code) + def set_intersection_traffic_lights(self): + # if any traffic light has been set, use the traffic light setting + # otherwise, do nothing and exit + # call this function each time after a world.tick() + + if self.light_config["subject"] != None: + if len(self.light_config["subject"]) > self.local_time_count: + setting = self.light_config["subject"][self.local_time_count] + #print(setting) + light = self.subject_light + light_state = light.get_state() + if setting == 'red': + if light_state != carla.TrafficLightState.Red: # only set color when the current state is not what we want + light.set_state(carla.TrafficLightState.Red) + light.set_red_time(self.light_config['subject_time']['red']) + light.freeze(True) + elif setting == 'yellow': + if light_state != carla.TrafficLightState.Yellow: + light.set_state(carla.TrafficLightState.Yellow) + light.set_yellow_time(self.light_config['subject_time']['yellow']) + light.freeze(True) + else: + if light_state != carla.TrafficLightState.Green: + light.set_state(carla.TrafficLightState.Green) + light.set_yellow_time(self.light_config['subject_time']['green']) + light.freeze(True) + + if self.light_config["left"] != None: + if len(self.light_config["left"]) > self.local_time_count: + setting = self.light_config["left"][self.local_time_count] + light = self.left_light + light_state = light.get_state() + if setting == 'red': + if light_state != carla.TrafficLightState.Red: # only set color when the current state is not what we want + light.set_state(carla.TrafficLightState.Red) + light.set_red_time(self.light_config['left_time']['red']) + light.freeze(True) + elif setting == 'yellow': + if light_state != carla.TrafficLightState.Yellow: + light.set_state(carla.TrafficLightState.Yellow) + light.set_yellow_time(self.light_config['left_time']['yellow']) + light.freeze(True) + else: + if light_state != carla.TrafficLightState.Green: + light.set_state(carla.TrafficLightState.Green) + light.set_yellow_time(self.light_config['left_time']['green']) + light.freeze(True) + + if self.light_config["right"] != None: + if len(self.light_config["right"]) > self.local_time_count: + setting = self.light_config["right"][self.local_time_count] + light = self.right_light + light_state = light.get_state() + if setting == 'red': + if light_state != carla.TrafficLightState.Red: # only set color when the current state is not what we want + light.set_state(carla.TrafficLightState.Red) + light.set_red_time(self.light_config['right_time']['red']) + light.freeze(True) + elif setting == 'yellow': + if light_state != carla.TrafficLightState.Yellow: + light.set_state(carla.TrafficLightState.Yellow) + light.set_yellow_time(self.light_config['right_time']['yellow']) + light.freeze(True) + else: + if light_state != carla.TrafficLightState.Green: + light.set_state(carla.TrafficLightState.Green) + light.set_yellow_time(self.light_config['right_time']['green']) + light.freeze(True) + + if self.light_config["ahead"] != None: + if len(self.light_config["ahead"]) > self.local_time_count: + setting = self.light_config["ahead"][self.local_time_count] + light = self.ahead_light + light_state = light.get_state() + if setting == 'red': + if light_state != carla.TrafficLightState.Red: # only set color when the current state is not what we want + light.set_state(carla.TrafficLightState.Red) + light.set_red_time(self.light_config['ahead_time']['red']) + light.freeze(True) + elif setting == 'yellow': + if light_state != carla.TrafficLightState.Yellow: + light.set_state(carla.TrafficLightState.Yellow) + light.set_yellow_time(self.light_config['ahead_time']['yellow']) + light.freeze(True) + else: + if light_state != carla.TrafficLightState.Green: + light.set_state(carla.TrafficLightState.Green) + light.set_yellow_time(self.light_config['ahead_time']['green']) + light.freeze(True) + + # update the time count + self.local_time_count += 1 + + # Intersection Class + def export_settings(self): + ''' + export all settings for a specific intersection + + Returns + ------- + intersection_settings : ConfigObj + settings of the intersection + + ''' + intersection_settings = ConfigObj() + + # general setting + intersection_settings["navigation_speed"] = self.navigation_speed + + # vehicles + intersection_settings["subject_vehicle"] = [] + intersection_settings["left_vehicle"] = [] + intersection_settings["right_vehicle"] = [] + intersection_settings["ahead_vehicle"] = [] + + for vehicle in self.subject_vehicle: + # deep copy the vehicle settings + new_vehicle = self._copy_vehicle_settings(vehicle) + intersection_settings["subject_vehicle"].append(new_vehicle) + + for vehicle in self.left_vehicle: + # deep copy the vehicle settings + new_vehicle = self._copy_vehicle_settings(vehicle) + intersection_settings["left_vehicle"].append(new_vehicle) + + for vehicle in self.right_vehicle: + # deep copy the vehicle settings + new_vehicle = self._copy_vehicle_settings(vehicle) + intersection_settings["right_vehicle"].append(new_vehicle) + + for vehicle in self.ahead_vehicle: + # deep copy the vehicle settings + new_vehicle = self._copy_vehicle_settings(vehicle) + intersection_settings["ahead_vehicle"].append(new_vehicle) + + # lights + intersection_settings["subject_light"] = copy.copy(self.light_config["subject"]) + intersection_settings["subject_light_time"] = copy.copy(self.light_config["subject_time"]) + + intersection_settings["left_light"] = copy.copy(self.light_config["left"]) + intersection_settings["left_light_time"] = copy.copy(self.light_config["left_time"]) + + intersection_settings["right_light"] = copy.copy(self.light_config["right"]) + intersection_settings["right_light_time"] = copy.copy(self.light_config["right_time"]) + + intersection_settings["ahead_light"] = copy.copy(self.light_config["ahead"]) + intersection_settings["ahead_light_time"] = copy.copy(self.light_config["ahead_time"]) + + return intersection_settings + + # Intersection Class + def import_settings(self,intersection_settings): + ''' + + + Parameters + ---------- + intersection_settings : ConfigObj + the intersection settings we want to import + + Returns + ------- + new_intersection_setting : ConfigObj + settings of the intersection + this will be generated by call self.export_settings() after finishing import + output these settings are for the purpose of creating the front-end gui + ''' + + # remove all vehicle in this intersection + # if any vehicle has been added + for ii in range(len(self.subject_vehicle) - 1, -1, -1): # go through the array in reverse order + uniquename = self.subject_vehicle[ii]['uniquename'] + self.remove_vehicle(uniquename) + + for ii in range(len(self.left_vehicle) - 1, -1, -1): # go through the array in reverse order + uniquename = self.left_vehicle[ii]['uniquename'] + self.remove_vehicle(uniquename) + + for ii in range(len(self.right_vehicle) - 1, -1, -1): # go through the array in reverse order + uniquename = self.right_vehicle[ii]['uniquename'] + self.remove_vehicle(uniquename) + + for ii in range(len(self.ahead_vehicle) - 1, -1, -1): # go through the array in reverse order + uniquename = self.ahead_vehicle[ii]['uniquename'] + self.remove_vehicle(uniquename) + + # import all settings + + # general settings + self.navigation_speed = intersection_settings["navigation_speed"] + + # vehicles + + for vehicle_config in intersection_settings["subject_vehicle"]: + # add vehicles according to imported settings + if vehicle_config['vehicle_color'] != None: + vehicle_config['vehicle_color'] = vehicle_config['vehicle_color'].replace(';',',') + + self.add_vehicle(gap = vehicle_config["gap"], + model_name = vehicle_config["model"], + choice = vehicle_config['choice'], + command = vehicle_config['command'], + stop_choice = vehicle_config['stop_choice'], + penetrate_distance = vehicle_config['penetrate_distance'], + obey_traffic_lights = vehicle_config['obey_traffic_lights'], + run = vehicle_config['run'], + safety_distance = vehicle_config['safety_distance'], + vehicle_color = vehicle_config['vehicle_color']) + + for vehicle_config in intersection_settings["left_vehicle"]: + # add vehicles according to imported settings + if vehicle_config['vehicle_color'] != None: + vehicle_config['vehicle_color'] = vehicle_config['vehicle_color'].replace(';',',') + + self.add_vehicle(gap = vehicle_config["gap"], + model_name = vehicle_config["model"], + choice = vehicle_config['choice'], + command = vehicle_config['command'], + stop_choice = vehicle_config['stop_choice'], + penetrate_distance = vehicle_config['penetrate_distance'], + obey_traffic_lights = vehicle_config['obey_traffic_lights'], + run = vehicle_config['run'], + safety_distance = vehicle_config['safety_distance'], + vehicle_color = vehicle_config['vehicle_color']) + + for vehicle_config in intersection_settings["right_vehicle"]: + # add vehicles according to imported settings + if vehicle_config['vehicle_color'] != None: + vehicle_config['vehicle_color'] = vehicle_config['vehicle_color'].replace(';',',') + + self.add_vehicle(gap = vehicle_config["gap"], + model_name = vehicle_config["model"], + choice = vehicle_config['choice'], + command = vehicle_config['command'], + stop_choice = vehicle_config['stop_choice'], + penetrate_distance = vehicle_config['penetrate_distance'], + obey_traffic_lights = vehicle_config['obey_traffic_lights'], + run = vehicle_config['run'], + safety_distance = vehicle_config['safety_distance'], + vehicle_color = vehicle_config['vehicle_color']) + + for vehicle_config in intersection_settings["ahead_vehicle"]: + # add vehicles according to imported settings + if vehicle_config['vehicle_color'] != None: + vehicle_config['vehicle_color'] = vehicle_config['vehicle_color'].replace(';',',') + + self.add_vehicle(gap = vehicle_config["gap"], + model_name = vehicle_config["model"], + choice = vehicle_config['choice'], + command = vehicle_config['command'], + stop_choice = vehicle_config['stop_choice'], + penetrate_distance = vehicle_config['penetrate_distance'], + obey_traffic_lights = vehicle_config['obey_traffic_lights'], + run = vehicle_config['run'], + safety_distance = vehicle_config['safety_distance'], + vehicle_color = vehicle_config['vehicle_color']) + + self.light_config['subject'] = copy.copy(intersection_settings['subject_light']) + self.light_config['subject_time'] = copy.copy(intersection_settings['subject_light_time']) + + self.light_config['left'] = copy.copy(intersection_settings['left_light']) + self.light_config['left_time'] = copy.copy(intersection_settings['left_light_time']) + + self.light_config['right'] = copy.copy(intersection_settings['right_light']) + self.light_config['right_time'] = copy.copy(intersection_settings['right_light_time']) + + self.light_config['ahead'] = copy.copy(intersection_settings['ahead_light']) + self.light_config['ahead_time'] = copy.copy(intersection_settings['ahead_light_time']) + + new_intersection_setting = self.export_settings() + return new_intersection_setting + + # Vehicle Class + def get_vehicle_bounding_box(self, uniquename): + ''' + get the bounding box of the vehicle by uniquename + + Parameters + ---------- + uniquename : string + the uniquename of the vehicle. + + Returns + ------- + new_bb : carla.Vector3D + the bounding box of the vehicle, new_bb.x is the length, new_bb.y is the width, new_bb.z is the height + + ''' + new_bb = self.env.get_vehicle_bounding_box(uniquename) + return new_bb + # Vehicle class + def get_vehicle_settings(self, uniquename): + ''' + Get the settings entered for a specific vehicle in this intersection based on uniquename + + Parameters + ---------- + uniquename : string + the uniquename of the vehicle.. + + Returns + ------- + vehicle_settings : ConfigObj + the settings of the vehicle + + ''' + out_vehicle = None + + for vehicle in self.subject_vehicle: + # deep copy the vehicle settings + if uniquename == vehicle["uniquename"]: + out_vehicle = self._copy_vehicle_settings(vehicle) + + for vehicle in self.left_vehicle: + # deep copy the vehicle settings + if uniquename == vehicle["uniquename"]: + out_vehicle = self._copy_vehicle_settings(vehicle) + + for vehicle in self.right_vehicle: + # deep copy the vehicle settings + if uniquename == vehicle["uniquename"]: + out_vehicle = self._copy_vehicle_settings(vehicle) + + for vehicle in self.ahead_vehicle: + # deep copy the vehicle settings + if uniquename == vehicle["uniquename"]: + out_vehicle = self._copy_vehicle_settings(vehicle) + + if out_vehicle == None: + print("Invalid uniquename entered for getting vehicle setting") + return None + + return out_vehicle + + # Vehicle Class (if necessary at all) + def _copy_vehicle_settings(self,vehicle_config): + new_vehicle = copy.copy(vehicle_config) + + new_vehicle["ref_waypoint"] = None + new_vehicle["location"] = None + new_vehicle["rotation"] = None + new_vehicle["trajectory"] = None + new_vehicle["ref_speed_list"] = None + new_vehicle["stop_ref_point"] = None + new_vehicle["bounding_box"] = None + + return new_vehicle + + + +def main(): + try: + client = carla.Client("localhost",2000) + client.set_timeout(10.0) + world = client.load_world('Town05') + + # set the weather + weather = carla.WeatherParameters( + cloudiness=10.0, + precipitation=0.0, + sun_altitude_angle=90.0) + world.set_weather(weather) + + # set the spectator position for demo purpose + spectator = world.get_spectator() + spectator.set_transform(carla.Transform(carla.Location(x=-133.0, y=1.29, z=11.0), carla.Rotation(pitch=-31.0, yaw= -4.23, roll=1.595))) # plain ground + + env = CARLA_ENV(world) + time.sleep(2) # sleep for 2 seconds, wait the initialization to finish + + world_pos = (-133.0,0.0) + traffic_light_list = get_traffic_lights(world.get_actors()) + intersection1 = Intersection(env, world_pos, traffic_light_list) + + name4 = intersection1.add_vehicle(gap = 5,choice = "left", vehicle_color='0,0,0') + name5 = intersection1.add_vehicle(gap = 5, choice = "left",command = "left", vehicle_color='128,128,128') + name6 = intersection1.add_vehicle(gap = 5,choice = "left",command = "right", vehicle_color='255,255,255') + + intersection1.edit_vehicle_settings(name4,choice = "left", vehicle_color = '0,0,0') + intersection1.edit_vehicle_settings(name5,choice = "left", vehicle_color = '0,0,0') + intersection1.edit_vehicle_settings(name6,choice = "left", vehicle_color = '0,0,0') + + time.sleep(2) + + # traffic light + intersection1.edit_traffic_light("subject",red_start = 20.0,red_end = 40.0,yellow_start=0.0,yellow_end=20.0,green_start=40.0,green_end = 60.0) + intersection1.edit_traffic_light("left",red_start = 20.0,red_end = 40.0,yellow_start=0.0,yellow_end=20.0,green_start=40.0,green_end = 60.0) + intersection1.edit_traffic_light("right",red_start = 20.0,red_end = 40.0,yellow_start=0.0,yellow_end=20.0,green_start=40.0,green_end = 60.0) + intersection1.edit_traffic_light("ahead",red_start = 20.0,red_end = 40.0,yellow_start=0.0,yellow_end=20.0,green_start=40.0,green_end = 60.0) + + # check the import/export method + setting = intersection1.export_settings() + + write_intersection_settings(name = 'demo_setting2', settings = setting) + + new_setting = read_intersection_settings('demo_setting2') + + intersection1.import_settings(new_setting) + + print("successfully imported settings") + + for ii in range(int(60 / env.delta_seconds)): + env.world.tick() + intersection1.set_intersection_traffic_lights() + + + finally: + time.sleep(10) + env.destroy_actors() + +if __name__ == '__main__': + main() + + diff --git a/legacy/gui_interface/backend/intersection_settings_helper.py b/legacy/gui_interface/backend/intersection_settings_helper.py new file mode 100644 index 0000000..5a95870 --- /dev/null +++ b/legacy/gui_interface/backend/intersection_settings_helper.py @@ -0,0 +1,197 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Tue Jul 14 16:16:43 2020 + +@author: shijiliu +""" +from configobj import ConfigObj +import copy + +def read_intersection_settings(config_filename): + ''' + read the intersection configuration file + and transform it into the config file that we can import + + Parameters + ---------- + config_filename : TYPE + DESCRIPTION. + + Returns + ------- + intersection_settings : ConfigObj + the valid intersection setting that can be imported by + intersections + + ''' + # read in raw config file + intersection_config_raw = ConfigObj(config_filename) + + # create the return config + intersection_settings = ConfigObj() + + # general + intersection_settings["navigation_speed"] = float(intersection_config_raw["navigation_speed"]) + + # vehicles + intersection_settings["subject_vehicle"] = [] + intersection_settings["left_vehicle"] = [] + intersection_settings["right_vehicle"] = [] + intersection_settings["ahead_vehicle"] = [] + + # lights + intersection_settings["subject_light"] = None + intersection_settings["subject_light_time"] = {} + + intersection_settings["left_light"] = None + intersection_settings["left_light_time"] = {} + + intersection_settings["right_light"] = None + intersection_settings["right_light_time"] = {} + + intersection_settings["ahead_light"] = None + intersection_settings["ahead_light_time"] = {} + + # transform vehicle settings + for vehicle_config_str in intersection_config_raw['subject_vehicle']: + intersection_settings["subject_vehicle"].append(transform_vehicle_config(vehicle_config_str)) + + for vehicle_config_str in intersection_config_raw['left_vehicle']: + intersection_settings["left_vehicle"].append(transform_vehicle_config(vehicle_config_str)) + + for vehicle_config_str in intersection_config_raw['right_vehicle']: + intersection_settings["right_vehicle"].append(transform_vehicle_config(vehicle_config_str)) + + for vehicle_config_str in intersection_config_raw['ahead_vehicle']: + intersection_settings["ahead_vehicle"].append(transform_vehicle_config(vehicle_config_str)) + + # transform light settings + + if intersection_config_raw["subject_light"] != 'None': + intersection_settings["subject_light"] = copy.copy(intersection_config_raw["subject_light"]) + + if intersection_config_raw["left_light"] != 'None': + intersection_settings["left_light"] = copy.copy(intersection_config_raw["left_light"]) + + if intersection_config_raw["right_light"] != 'None': + intersection_settings["right_light"] = copy.copy(intersection_config_raw["right_light"]) + + if intersection_config_raw["ahead_light"] != 'None': + intersection_settings["ahead_light"] = copy.copy(intersection_config_raw["ahead_light"]) + + # transform light time settings + if intersection_config_raw["subject_light_time"] != 'None': + for key in intersection_config_raw["subject_light_time"]: + intersection_settings["subject_light_time"][key] = float(intersection_config_raw["subject_light_time"][key]) + else: + intersection_settings["subject_light_time"] = None + + if intersection_config_raw["left_light_time"] != 'None': + for key in intersection_config_raw["left_light_time"]: + intersection_settings["left_light_time"][key] = float(intersection_config_raw["left_light_time"][key]) + else: + intersection_settings["left_light_time"] = None + + if intersection_config_raw["right_light_time"] != 'None': + for key in intersection_config_raw["right_light_time"]: + intersection_settings["right_light_time"][key] = float(intersection_config_raw["right_light_time"][key]) + else: + intersection_settings["right_light_time"] = None + + if intersection_config_raw["ahead_light_time"] != 'None': + for key in intersection_config_raw["ahead_light_time"]: + intersection_settings["ahead_light_time"][key] = float(intersection_config_raw["ahead_light_time"][key]) + else: + intersection_settings["ahead_light_time"] = None + + + return intersection_settings + +''' +def transform_light_time_settings(light_time_str): + light_time_config = ConfigObj() + key_val_pairs = light_time_str.split(",") + for key_val_pair in key_val_pairs: + temp = key_val_pair.split(": ") + key = temp[0].split('\'')[1] + val = temp[1].split('}')[0] + light_time_config[key] = float(val) + + return light_time_config +''' + +def transform_vehicle_config(vehicle_config_str): + ''' + transform the vehicle config in string form + to configuration file + + Parameters + ---------- + vehicle_config_str : str + vehicle config in string form. + + Returns + ------- + vehicle_config : ConfigObj + vehicle configuration file + + ''' + vehicle_config = ConfigObj() + key_val_pairs = vehicle_config_str.split(",") + for key_val_pair in key_val_pairs: + temp = key_val_pair.split(": ") + key = temp[0].split('\'')[1] + val = temp[1].split('}')[0] + if key == 'model': + vehicle_config[key] = val.split("\'")[1] + elif key == 'gap': + vehicle_config[key] = float(val) + elif key == 'command': + vehicle_config[key] = val.split("\'")[1] + elif key == 'obey_traffic_lights': + vehicle_config[key] = bool(val) + elif key == 'run': + vehicle_config[key] = bool(val) + elif key == 'safety_distance': + vehicle_config[key] = float(val) + elif key == 'choice': + vehicle_config[key] = val.split("\'")[1] + elif key == 'uniquename': + vehicle_config[key] = val.split("\'")[1] + elif key == 'ref_waypoint': + vehicle_config[key] = None + elif key == 'location': + vehicle_config[key] = None + elif key == 'rotation': + vehicle_config[key] = None + elif key == 'trajectory': + vehicle_config[key] = None + elif key == 'ref_speed_list': + vehicle_config[key] = None + elif key == 'bounding_box': + vehicle_config[key] = None + elif key == 'vehicle_type': + vehicle_config[key] = val.split("\'")[1] + elif key == 'stop_choice': + vehicle_config[key] = val.split("\'")[1] + elif key == 'penetrate_distance': + if vehicle_config['stop_choice'] == 'penetrate': + vehicle_config[key] = float(val) + else: + vehicle_config[key] = None + elif key == 'stop_ref_point': + vehicle_config[key] = None + elif key == 'vehicle_color': + if val == 'None': + vehicle_config[key] = None + else: + vehicle_config[key] = val.replace(';',',').split("\'")[1] + #print(vehicle_config[key]) + + + return vehicle_config + +def write_intersection_settings(name, settings): + settings.filename = name + settings.write() \ No newline at end of file diff --git a/legacy/gui_interface/backend/multiple_vehicle_control.py b/legacy/gui_interface/backend/multiple_vehicle_control.py new file mode 100644 index 0000000..7cd3b3a --- /dev/null +++ b/legacy/gui_interface/backend/multiple_vehicle_control.py @@ -0,0 +1,629 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Mon Jul 6 09:54:39 2020 + +@author: shijiliu +""" + + +import carla +import matplotlib.pyplot as plt +import numpy as np +from collections import deque +import time +import math + +import control # the python-control package, install first + +from backend.intersection_definition import Intersection, get_traffic_lights +from backend.carla_env import CARLA_ENV # self-written class that provides help functions, should be in the same folder +from configobj import ConfigObj + +# color for debug use +red = carla.Color(255, 0, 0) +green = carla.Color(0, 255, 0) +blue = carla.Color(47, 210, 231) +cyan = carla.Color(0, 255, 255) +yellow = carla.Color(255, 255, 0) +orange = carla.Color(255, 162, 0) +white = carla.Color(255, 255, 255) + +# Make this some sort of base Vehicle Controller class +class VehicleControl(object): + def __init__(self,env,vehicle_config, delta_seconds, allow_collision = True): + ''' + + + Parameters + ---------- + env : CARLA_ENV + the self-written environment for simulation + + vehicle_config : ConfigObj + the configuration file for this vehicle. The file is created inside the Intersection's add_vehicle function. + containing the trajectory and speed reference for the vehicle. The configuration file should also indicate whether + this vehicle should go or not. + + delta_seconds : float + time between two adjacent simulation step. Used for creating discrete controller + + Returns + ------- + None. + + ''' + + self.env = env + self.vehicle_config = vehicle_config + self.model_uniquename = self.vehicle_config["uniquename"] + self.command = self.vehicle_config["command"] + self.trajectory = self.vehicle_config["trajectory"] + self.ref_speed_list = self.vehicle_config["ref_speed_list"] + self.obey_traffic_lights = self.vehicle_config["obey_traffic_lights"] + self.run = self.vehicle_config["run"] + self.safety_distance = self.vehicle_config["safety_distance"] + self.stop_choice = self.vehicle_config["stop_choice"] + self.penetrate_distance = self.vehicle_config["penetrate_distance"] + self.stop_ref_point = self.vehicle_config["stop_ref_point"] + + # discrete step time + self.delta_seconds = delta_seconds + + # PI controller constants + self.KI = 0.02 + self.KP = 0.5 + + # pure-pursuit model constants + self.k = 0.1 # coefficient for look ahead + self.Lfc = 4.0 # look ahead distance + self.L = self.vehicle_config["bounding_box"].x#2.88 # wheelbase + + # essential storages for the controller to work + self.init_values = deque(maxlen = 2) # the state space values of the system. For a control system to be fully functional + # we need to give initial value + self.ref_speeds = deque(maxlen = 2) # the reference / target speed + self.curr_speeds = deque(maxlen = 2) # the measured speed of the vehicle + + # storage for the visualize the reference speed, throttle and measured speed. + self.speed = [] + self.throttles = [] + self.reference_speed = [] + + # give initial values to storage, assume the car is released at rest, with no initial speed or acceleration + self.init_values.append(0) + self.ref_speeds.append(0) + self.curr_speeds.append(0) + + self.current_ref_speed = 0 + self.index = 0 + + + + + # get the PI controller for the vehicle + self._get_PI_controller() + + + self.debug_vehicle = True # enable drawing vehicle trajectory + self.vehicle_pose = deque(maxlen = 2) + + # indication of whether the vehicle stops at traffic light + self.blocked_by_light = False + + # allow collision or not + # if collision is not allowed, vehicle will abruptly stop when too close to another vehicle + self.allow_collision = allow_collision + + # Vehicle Class + def get_vehicle_transform(self): + transform = self.env.get_transform_3d(self.model_uniquename) + return transform + + # Part of a Controller Class + def _get_PI_controller(self): + ''' + Effects: create a discrete state-space PI controller + ''' + num_pi = [self.KP, self.KI] # numerator of the PI transfer function (KP*s + KI) + den_pi = [1.0, 0.01*self.KI/self.KP] # denominator of PI transfer function (s + 0.01*KI/KP) + + sys = control.tf(num_pi,den_pi) # get transfer function for PI controller (since the denominator has a small term 0.01*KI/KP, it is actually a lag-compensator) + sys = control.sample_system(sys, self.delta_seconds) # discretize the transfer function (from s-domain which is continuous to z-domain) + #since our simulation is discrete + sys = control.tf2ss(sys) # transform transfer function into state space. + self.sys = sys # the system is created for this vehicle + + # Determines the current speed delta and derives a throttle control + # Controller class + def speed_control(self): + ''' + Effects: get the reference speed, current (measured) speed and initial values + Use the difference + e = ref_speeds - curr_speeds + as the input for the PI controller, derive the new throttle + + Parameters + ---------- + self.sys : control.ss + state space controller + self.ref_speeds : list of float + the desired speed we need + self.curr_speeds : list of float + the current speed + self.init_values : the initial_values of the system + DESCRIPTION. + + Returns + ------- + throttle : float type + DESCRIPTION. + + ''' + + + U0 = np.array(self.ref_speeds) - np.array(self.curr_speeds) + #print(U0) + _,y0,x0 = control.forced_response(self.sys,U = U0,X0 = self.init_values[0]) # y0 is the next values, x0 is the state evolution + # see https://python-control.readthedocs.io/en/0.8.3/generated/control.forced_response.html#control.forced_response + #self.init_values.append(x0[-1]) + throttle = y0[-1] + return throttle + + # Figures out what is at the end of the vehicle's current trajectory + # Controller class + def get_target_index(self,location_2d, current_forward_speed, trajectory): + ''' + Get the target for the vehicle to navigate to + + Parameters + ---------- + location_2d : (x,y) + current location of the vehicle. + current_forward_speed : float + current speed of the vehicle. + trajectory : numpy 2d array + interpolated waypoints. + + Returns + ------- + ind : TYPE + DESCRIPTION. + end_trajectory : TYPE + DESCRIPTION. + + ''' + + + distance = np.sum((trajectory - location_2d)**2,axis = 1)**0.5 + ind = np.argmin(distance) + l0 = 0.0 + + Lf = self.k * current_forward_speed + self.Lfc + + while Lf > l0 and (ind + 1) < len(trajectory): + delta_d = sum((trajectory[ind + 1] - trajectory[ind])**2)**0.5 + l0 += delta_d + ind += 1 + + if ind >= len(trajectory) - 1: + end_trajectory = True + else: + end_trajectory = False + + return ind, end_trajectory + + # Controller class + def pure_pursuit_control(self,vehicle_pos_2d, current_forward_speed, trajectory, ref_speed_list, prev_index): + ''' + + + Parameters + ---------- + vehicle_pos_2d : (location_2d,yaw) + tuple of vehicle location and heading in 2d. + location_2d : (x,y), both x and y are in meter + yaw : heading angle **Note** yaw is in degree + current_forward_speed : float + the current velocity of the vehicle. + trajectory : numpy 2d array + interpolated waypoints. + ref_speed_list : list + the reference speed corresponding to each way point + prev_index : int + the previous index + Returns + ------- + delta : float + steer angle of the vehicle. + current_ref_speed : the reference speed + DESCRIPTION. + index : int + the index of the target. + end_trajectory : boolean + whether we have reached clos enough to the destination. + + ''' + + + + location_2d, yaw = vehicle_pos_2d + yaw = np.deg2rad(yaw) # change the unit the radius + index, end_trajectory = self.get_target_index(location_2d, current_forward_speed, trajectory) + + if prev_index >= index: + index = prev_index + + if index < len(trajectory): + tx = trajectory[index][0] + ty = trajectory[index][1] + else: + tx = trajectory[-1][0] + ty = trajectory[-1][1] + + alpha = math.atan2(ty - location_2d[1],tx - location_2d[0]) - yaw + + if current_forward_speed < 0: #back, should not happen in our case + alpha = math.pi - alpha + + Lf = self.k * current_forward_speed + self.Lfc + + delta = math.atan2(2.0 * self.L * math.sin(alpha) / Lf, 1.0) + + current_ref_speed = ref_speed_list[index] + + return delta, current_ref_speed, index, end_trajectory + + # Applies pure_pursuit control to the vehicle (not sure why this is needed) + # Controller class + def pure_pursuit_control_wrapper(self): + ''' + Apply one step control to the vehicle, store essential information for further use + + Returns + ------- + end_trajectory : bool + whether this vehicle reaches its end + + ''' + + curr_speed = self.env.get_forward_speed(self.model_uniquename) + vehicle_pos_2d = self.env.get_transform_2d(self.model_uniquename) # the (x,y) location and yaw angle of the vehicle + self.speed.append(curr_speed) + self.curr_speeds.append(curr_speed) + + # draw real trajectory if debug is enabled + if self.debug_vehicle: + self.vehicle_pose.append(vehicle_pos_2d[0]) + if len(self.vehicle_pose) == 2: + self.env.draw_real_trajectory(self.vehicle_pose) + + self._display_vehicle_type() + + # use pure-pursuit model to get the steer angle (in radius) + delta, current_ref_speed, index, end_trajectory = self.pure_pursuit_control(vehicle_pos_2d, curr_speed, self.trajectory, self.ref_speed_list, self.index) + self.index = index + steer = np.clip(delta,-1.0,1.0) + + + # If vehicle has safety distance set, check whether a vehicle is in the front + current_ref_speed = self._obey_safety_distance(current_ref_speed) + + # If vehicle obey traffic lights and is going straight / turning left, check the traffic light state + current_ref_speed = self._obey_traffic_light(current_ref_speed) + + #if self.debug_vehicle: + # print("--------") + # print("current_ref_speed == ",current_ref_speed) + # print("current_speed ==",curr_speed) + + self.ref_speeds.append(current_ref_speed) + self.reference_speed.append(current_ref_speed) + + # get throttle to get the next reference speed + throttle = self.speed_control() # get the throttle control based on reference and current speed + throttle = np.clip(throttle,0,1) # throttle value is [0,1] + self.throttles.append(throttle) # for visualization + + # check whether we are reaching the destination or not + if end_trajectory: + vehicle_control = carla.VehicleControl(throttle = 0.0,steer=steer,brake = 1.0) # immediately stop the car + self.env.apply_vehicle_control(self.model_uniquename, vehicle_control) # apply control to vehicle + self.run = False + self._destroy_vehicle() + return end_trajectory + + # apply throttle-steer-brake control + if curr_speed <= current_ref_speed: + vehicle_control = carla.VehicleControl(throttle = throttle,steer=steer) + else: + vehicle_control = carla.VehicleControl(throttle = throttle,steer=steer,brake = 0.5) + + self.env.apply_vehicle_control(self.model_uniquename, vehicle_control) # apply control to vehicle + return end_trajectory + + # Updates the internal state of the vehicle according to pursuit control but doesn't update the vehicle at all + def fake_pure_pursuit_control_wrapper(self): + ''' + Get one step control to the vehicle, store essential information for further use + the control command is not applied to the vehicle + + Returns + ------- + end_trajectory : bool + whether this vehicle reaches its end + + ''' + + curr_speed = self.env.get_forward_speed(self.model_uniquename) + vehicle_pos_2d = self.env.get_transform_2d(self.model_uniquename) # the (x,y) location and yaw angle of the vehicle + self.speed.append(curr_speed) + self.curr_speeds.append(curr_speed) + + # draw real trajectory if debug is enabled + if self.debug_vehicle: + self.vehicle_pose.append(vehicle_pos_2d[0]) + if len(self.vehicle_pose) == 2: + self.env.draw_real_trajectory(self.vehicle_pose) + + self._display_vehicle_type() + + # use pure-pursuit model to get the steer angle (in radius) + delta, current_ref_speed, index, end_trajectory = self.pure_pursuit_control(vehicle_pos_2d, curr_speed, self.trajectory, self.ref_speed_list, self.index) + self.index = index + steer = np.clip(delta,-1.0,1.0) + + + # If vehicle has safety distance set, check whether a vehicle is in the front + current_ref_speed = self._obey_safety_distance(current_ref_speed) + + # If vehicle obey traffic lights and is going straight / turning left, check the traffic light state + current_ref_speed = self._obey_traffic_light(current_ref_speed) + + #if self.debug_vehicle: + # print("--------") + # print("current_ref_speed == ",current_ref_speed) + # print("current_speed ==",curr_speed) + + self.ref_speeds.append(current_ref_speed) + self.reference_speed.append(current_ref_speed) + + # get throttle to get the next reference speed + throttle = self.speed_control() # get the throttle control based on reference and current speed + throttle = np.clip(throttle,0,1) # throttle value is [0,1] + self.throttles.append(throttle) # for visualization + + # check whether we are reaching the destination or not + if end_trajectory: + #vehicle_control = carla.VehicleControl(throttle = 0.0,steer=steer,brake = 1.0) # immediately stop the car + #self.env.apply_vehicle_control(self.model_uniquename, vehicle_control) # apply control to vehicle + self.run = False + self._destroy_vehicle() + return end_trajectory + + # apply throttle-steer-brake control + ''' + if curr_speed <= current_ref_speed: + vehicle_control = carla.VehicleControl(throttle = throttle,steer=steer) + else: + vehicle_control = carla.VehicleControl(throttle = throttle,steer=steer,brake = 0.5) + ''' + #self.env.apply_vehicle_control(self.model_uniquename, vehicle_control) # apply control to vehicle + return end_trajectory + + + # Controller class + def _obey_traffic_light(self, current_ref_speed): + # the vehicle should take traffic lights into account when it is required + # to obey lights and is going straight or turning left + self.blocked_by_light = False + + if not self.obey_traffic_lights: + return current_ref_speed + if self.command == "right": + return current_ref_speed + + # get the location of the vehicle + curr_transform = self.env.get_transform_2d(self.model_uniquename) + curr_location = curr_transform[0] + + abrupt_stop_vel = carla.Vector3D(x = 0,y = 0,z = 0) + + if self.vehicle_config["vehicle_type"] == "other": + target_location = self.stop_ref_point.location + target_vector = self.stop_ref_point.get_forward_vector() + target_vector_2d = np.array([target_vector.x,target_vector.y]) + target_vector_2d = target_vector_2d / np.linalg.norm(target_vector_2d) + vec_tar_curr = np.array([curr_location[0] - target_location.x,curr_location[1] - target_location.y]) + vec_tar_curr = vec_tar_curr / np.linalg.norm(vec_tar_curr) + coef = np.dot(vec_tar_curr,target_vector_2d) + + + smallest_distance = math.sqrt((curr_location[0] - target_location.x)**2 + (curr_location[1] - target_location.y)**2) * coef + light = self.vehicle_config["traffic_light"] + else: + smallest_distance = np.inf + real_target = None + for ii in range(len(self.stop_ref_point)): + target_location = self.stop_ref_point[ii].location + distance = math.sqrt((curr_location[0] - target_location.x)**2 + (curr_location[1] - target_location.y)**2) + if distance < smallest_distance: + smallest_distance = distance + light = self.vehicle_config["traffic_light"][ii] + real_target = self.stop_ref_point[ii] + target_vector = real_target.get_forward_vector() + target_vector_2d = np.array([target_vector.x,target_vector.y]) + target_vector_2d = target_vector_2d / np.linalg.norm(target_vector_2d) + vec_tar_curr = np.array([curr_location[0] - real_target.location.x,curr_location[1] - real_target.location.y]) + vec_tar_curr = vec_tar_curr / np.linalg.norm(vec_tar_curr) + coef = np.dot(vec_tar_curr,target_vector_2d) + smallest_distance *= coef + + if smallest_distance < 0.5 and smallest_distance > -10: + state = light.get_state() + if state == carla.TrafficLightState.Red or state == carla.TrafficLightState.Yellow: + self.blocked_by_light = True # indicate the car is blocked by light + if self.stop_choice == "abrupt": + self.env.set_vehicle_velocity(self.model_uniquename , abrupt_stop_vel) # immediately stop vehicle + return 0.0 # abrupt stop + else: + if abs(smallest_distance) < 1.0: # normal or penetrate, note smallest_distance is most likely negative + self.env.set_vehicle_velocity(self.model_uniquename , abrupt_stop_vel) + return 0.0 + else: + return current_ref_speed + else: + return current_ref_speed + + return current_ref_speed + + + # Controller Class + def _obey_safety_distance(self, current_ref_speed): + + has_vehicle_in_front, distance = self.env.check_vehicle_in_front(self.model_uniquename, self.safety_distance) + + if has_vehicle_in_front and abs(distance) < self.L / 2 + 1.0 and not self.allow_collision: + # if vehicle is about to collide with other vehicle and collision is not allowed + # set the vehicle velocity to 0 + abrupt_stop_vel = carla.Vector3D(x = 0,y = 0,z = 0) + self.env.set_vehicle_velocity(self.model_uniquename, abrupt_stop_vel) # set the velocity of vehicle + + + if has_vehicle_in_front: + return 0.0 + + return current_ref_speed + + # Vehicle Class + def _destroy_vehicle(self): + self.env.destroy_vehicle(self.model_uniquename) + + # Vehicle Class + def _display_vehicle_type(self): + vehicle_type = self.vehicle_config["vehicle_type"] + vehicle_transform = self.get_vehicle_transform() + vehicle_location = vehicle_transform.location + debug_location = carla.Location(x = vehicle_location.x, y = vehicle_location.y, z = vehicle_location.z + 2 * self.vehicle_config["bounding_box"].z) + + + if vehicle_type == "other": + self.env.world.debug.draw_string(debug_location,text = "other", color = green, life_time = self.env.delta_seconds) + elif vehicle_type == "lead": + self.env.world.debug.draw_string(debug_location,text = "lead", color = yellow, life_time = self.env.delta_seconds) + elif vehicle_type == "ego": + self.env.world.debug.draw_string(debug_location,text = "ego", color = red, life_time = self.env.delta_seconds) + elif vehicle_type == "follow": + self.env.world.debug.draw_string(debug_location,text = "follow", color = blue, life_time = self.env.delta_seconds) + + # Main simulation loop + # In a base Experiment class +def multiple_vehicle_control(env,intersection_list): + vehicle_list = [] + while True: + env.world.tick() + + # update the distance between vehicles after each tick + env.update_vehicle_distance() + + for ii in range(len(intersection_list)-1,-1,-1): + if intersection_list[ii].start_sim: + for vehicle_config in intersection_list[ii].subject_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + for vehicle_config in intersection_list[ii].left_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + for vehicle_config in intersection_list[ii].right_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + for vehicle_config in intersection_list[ii].ahead_vehicle: + vehicle = VehicleControl(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + intersection_list.pop(ii) + + if len(vehicle_list) == 0: + break + + ''' + Temp function. Start vehicle that's not running + ''' + has_run = False + for vehicle in vehicle_list: + if vehicle.run: + has_run = True + break + + if not has_run: + for vehicle in vehicle_list: + vehicle.run = True + + for jj in range(len(vehicle_list) -1, -1, -1): + vehicle = vehicle_list[jj] + if vehicle.run: + end_trajectory = vehicle.pure_pursuit_control_wrapper() + if end_trajectory: + vehicle_list.pop(jj) + +def main(): + try: + client = carla.Client("localhost",2000) + client.set_timeout(10.0) + world = client.load_world('Town05') + + # set the weather + weather = carla.WeatherParameters( + cloudiness=10.0, + precipitation=0.0, + sun_altitude_angle=90.0) + world.set_weather(weather) + + # set the spectator position for demo purpose + spectator = world.get_spectator() + spectator.set_transform(carla.Transform(carla.Location(x=-133, y=1.29, z=75.0), carla.Rotation(pitch=-88.0, yaw= -1.85, roll=1.595))) # top view of intersection + + env = CARLA_ENV(world) + time.sleep(2) # sleep for 2 seconds, wait the initialization to finish + + traffic_light_list = get_traffic_lights(world.get_actors()) + + intersection_list = [] + + # intersection 1 + world_pos = (-133.0,0.0)#(25.4,0.0) + + intersection1 = Intersection(env, world_pos, traffic_light_list) + intersection1.add_vehicle(obey_traffic_lights = False) + + intersection1.add_vehicle(command = "left", obey_traffic_lights = False) + intersection1.add_vehicle(command = "right", obey_traffic_lights = False) + + intersection1.add_vehicle(gap = 5,choice = "left", obey_traffic_lights = False) + intersection1.add_vehicle(gap = 5, choice = "left",command = "right", obey_traffic_lights = False) + intersection1.add_vehicle(gap = 5,choice = "left",command = "left", obey_traffic_lights = False) + intersection1.add_vehicle(choice = "right", obey_traffic_lights = False) + intersection1.add_vehicle(choice = "right",command = "left", obey_traffic_lights = False) + intersection1.add_vehicle(gap = 10.0,choice = "right",command = "right", obey_traffic_lights = False) + intersection1.add_vehicle(choice = "ahead", obey_traffic_lights = False) + intersection1.add_vehicle(choice = "ahead",command = "right", obey_traffic_lights = False) + intersection1.add_vehicle(choice = "ahead",command = "left", obey_traffic_lights = False) + + intersection1._shift_vehicles(-10,index = 1) + + + intersection1.start_sim = True + + intersection_list.append(intersection1) + + multiple_vehicle_control(env,intersection_list) + + finally: + time.sleep(10) + env.destroy_actors() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/legacy/gui_interface/backend/multiple_vehicle_control_debug.py b/legacy/gui_interface/backend/multiple_vehicle_control_debug.py new file mode 100644 index 0000000..cd18935 --- /dev/null +++ b/legacy/gui_interface/backend/multiple_vehicle_control_debug.py @@ -0,0 +1,590 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Mon Jul 6 09:54:39 2020 + +@author: shijiliu +""" + +## THIS WHOLE FILE IS JUST COPY PASTED FROM THE ONE BEFORE, remove this + +import sys +sys.path.append("..") + +import carla +import matplotlib.pyplot as plt +import numpy as np +from collections import deque +import time +import math + +import control # the python-control package, install first + +from backend.intersection_definition import Intersection, get_traffic_lights +from backend.carla_env import CARLA_ENV # self-written class that provides help functions, should be in the same folder +from configobj import ConfigObj + +# color for debug use +red = carla.Color(255, 0, 0) +green = carla.Color(0, 255, 0) +blue = carla.Color(47, 210, 231) +cyan = carla.Color(0, 255, 255) +yellow = carla.Color(255, 255, 0) +orange = carla.Color(255, 162, 0) +white = carla.Color(255, 255, 255) + +class VehicleControl_debug(object): + def __init__(self,env,vehicle_config, delta_seconds): + ''' + + + Parameters + ---------- + env : CARLA_ENV + the self-written environment for simulation + + vehicle_config : ConfigObj + the configuration file for this vehicle. The file is created inside the Intersection's add_vehicle function. + containing the trajectory and speed reference for the vehicle. The configuration file should also indicate whether + this vehicle should go or not. + + delta_seconds : float + time between two adjacent simulation step. Used for creating discrete controller + + Returns + ------- + None. + + ''' + + self.env = env + self.vehicle_config = vehicle_config + self.model_uniquename = self.vehicle_config["uniquename"] + self.command = self.vehicle_config["command"] + self.trajectory = self.vehicle_config["trajectory"] + self.ref_speed_list = self.vehicle_config["ref_speed_list"] + self.obey_traffic_lights = self.vehicle_config["obey_traffic_lights"] + self.run = self.vehicle_config["run"] + self.safety_distance = self.vehicle_config["safety_distance"] + self.stop_choice = self.vehicle_config["stop_choice"] + self.penetrate_distance = self.vehicle_config["penetrate_distance"] + self.stop_ref_point = self.vehicle_config["stop_ref_point"] + + # discrete step time + self.delta_seconds = delta_seconds + + # PI controller constants + self.KI = 0.02 + self.KP = 0.5 + + # pure-pursuit model constants + self.k = 0.1 # coefficient for look ahead + self.Lfc = 4.0 # look ahead distance + self.L = self.vehicle_config["bounding_box"].x#2.88 # wheelbase + + # essential storages for the controller to work + self.init_values = deque(maxlen = 2) # the state space values of the system. For a control system to be fully functional + # we need to give initial value + self.ref_speeds = deque(maxlen = 2) # the reference / target speed + self.curr_speeds = deque(maxlen = 2) # the measured speed of the vehicle + + # storage for the visualize the reference speed, throttle and measured speed. + self.speed = [] + self.throttles = [] + self.reference_speed = [] + + # give initial values to storage, assume the car is released at rest, with no initial speed or acceleration + self.init_values.append(0) + self.ref_speeds.append(0) + self.curr_speeds.append(0) + + self.current_ref_speed = 0 + self.index = 0 + + + + + # get the PI controller for the vehicle + self._get_PI_controller() + + + self.debug_vehicle = True # enable drawing vehicle trajectory + self.vehicle_pose = deque(maxlen = 2) + + # indication of whether the vehicle stops at traffic light + self.blocked_by_light = False + + + def get_vehicle_transform(self): + transform = self.env.get_transform_3d(self.model_uniquename) + return transform + + def _get_PI_controller(self): + ''' + Effects: create a discrete state-space PI controller + ''' + num_pi = [self.KP, self.KI] # numerator of the PI transfer function (KP*s + KI) + den_pi = [1.0, 0.01*self.KI/self.KP] # denominator of PI transfer function (s + 0.01*KI/KP) + + sys = control.tf(num_pi,den_pi) # get transfer function for PI controller (since the denominator has a small term 0.01*KI/KP, it is actually a lag-compensator) + sys = control.sample_system(sys, self.delta_seconds) # discretize the transfer function (from s-domain which is continuous to z-domain) + #since our simulation is discrete + sys = control.tf2ss(sys) # transform transfer function into state space. + self.sys = sys # the system is created for this vehicle + + def speed_control(self): + ''' + Effects: get the reference speed, current (measured) speed and initial values + Use the difference + e = ref_speeds - curr_speeds + as the input for the PI controller, derive the new throttle + + Parameters + ---------- + self.sys : control.ss + state space controller + self.ref_speeds : list of float + the desired speed we need + self.curr_speeds : list of float + the current speed + self.init_values : the initial_values of the system + DESCRIPTION. + + Returns + ------- + throttle : float type + DESCRIPTION. + + ''' + + + U0 = np.array(self.ref_speeds) - np.array(self.curr_speeds) + #print(U0) + _,y0,x0 = control.forced_response(self.sys,U = U0,X0 = self.init_values[0], return_x=True) # y0 is the next values, x0 is the state evolution + # see https://python-control.readthedocs.io/en/0.8.3/generated/control.forced_response.html#control.forced_response + self.init_values.append(x0[-1]) + throttle = y0[-1] + return throttle + + def get_target_index(self,location_2d, current_forward_speed, trajectory): + ''' + Get the target for the vehicle to navigate to + + Parameters + ---------- + location_2d : (x,y) + current location of the vehicle. + current_forward_speed : float + current speed of the vehicle. + trajectory : numpy 2d array + interpolated waypoints. + + Returns + ------- + ind : TYPE + DESCRIPTION. + end_trajectory : TYPE + DESCRIPTION. + + ''' + + + distance = np.sum((trajectory - location_2d)**2,axis = 1)**0.5 + ind = np.argmin(distance) + l0 = 0.0 + + Lf = self.k * current_forward_speed + self.Lfc + + while Lf > l0 and (ind + 1) < len(trajectory): + delta_d = sum((trajectory[ind + 1] - trajectory[ind])**2)**0.5 + l0 += delta_d + ind += 1 + + if ind >= len(trajectory) - 1: + end_trajectory = True + else: + end_trajectory = False + + return ind, end_trajectory + + def pure_pursuit_control(self,vehicle_pos_2d, current_forward_speed, trajectory, ref_speed_list, prev_index): + ''' + + + Parameters + ---------- + vehicle_pos_2d : (location_2d,yaw) + tuple of vehicle location and heading in 2d. + location_2d : (x,y), both x and y are in meter + yaw : heading angle **Note** yaw is in degree + current_forward_speed : float + the current velocity of the vehicle. + trajectory : numpy 2d array + interpolated waypoints. + ref_speed_list : list + the reference speed corresponding to each way point + prev_index : int + the previous index + Returns + ------- + delta : float + steer angle of the vehicle. + current_ref_speed : the reference speed + DESCRIPTION. + index : int + the index of the target. + end_trajectory : boolean + whether we have reached clos enough to the destination. + + ''' + + + + location_2d, yaw = vehicle_pos_2d + yaw = np.deg2rad(yaw) # change the unit the radius + index, end_trajectory = self.get_target_index(location_2d, current_forward_speed, trajectory) + + if prev_index >= index: + index = prev_index + + if index < len(trajectory): + tx = trajectory[index][0] + ty = trajectory[index][1] + else: + tx = trajectory[-1][0] + ty = trajectory[-1][1] + + alpha = math.atan2(ty - location_2d[1],tx - location_2d[0]) - yaw + + if current_forward_speed < 0: #back, should not happen in our case + alpha = math.pi - alpha + + Lf = self.k * current_forward_speed + self.Lfc + + delta = math.atan2(2.0 * self.L * math.sin(alpha) / Lf, 1.0) + #print("delta == ", delta, "yaw == ", yaw) + + current_ref_speed = ref_speed_list[index] + + return delta, current_ref_speed, index, end_trajectory + + def pure_pursuit_control_wrapper(self): + ''' + Apply one step control to the vehicle, store essential information for further use + + Returns + ------- + end_trajectory : bool + whether this vehicle reaches its end + + ''' + + curr_speed = self.env.get_forward_speed(self.model_uniquename) + vehicle_pos_2d = self.env.get_transform_2d(self.model_uniquename) # the (x,y) location and yaw angle of the vehicle + self.speed.append(curr_speed) + self.curr_speeds.append(curr_speed) + + # draw real trajectory if debug is enabled + if self.debug_vehicle: + self.vehicle_pose.append(vehicle_pos_2d[0]) + if len(self.vehicle_pose) == 2: + self.env.draw_real_trajectory(self.vehicle_pose) + + self._display_vehicle_type() + + # use pure-pursuit model to get the steer angle (in radius) + delta, current_ref_speed, index, end_trajectory = self.pure_pursuit_control(vehicle_pos_2d, curr_speed, self.trajectory, self.ref_speed_list, self.index) + self.index = index + steer = np.clip(delta,-0.5,0.5) + + + # If vehicle has safety distance set, check whether a vehicle is in the front + current_ref_speed = self._obey_safety_distance(current_ref_speed) + + # If vehicle obey traffic lights and is going straight / turning left, check the traffic light state + current_ref_speed = self._obey_traffic_light(current_ref_speed) + + if self.debug_vehicle: + print("--------") + print("current_ref_speed == ",current_ref_speed) + print("current_speed ==",curr_speed) + + self.ref_speeds.append(current_ref_speed) + self.reference_speed.append(current_ref_speed) + + # get throttle to get the next reference speed + throttle = self.speed_control() # get the throttle control based on reference and current speed + throttle = np.clip(throttle,0,1) # throttle value is [0,1] + self.throttles.append(throttle) # for visualization + + # check whether we are reaching the destination or not + if end_trajectory: + vehicle_control = carla.VehicleControl(throttle = 0.0,steer=steer,brake = 1.0) # immediately stop the car + self.env.apply_vehicle_control(self.model_uniquename, vehicle_control) # apply control to vehicle + self.run = False + self._destroy_vehicle() + return end_trajectory + + # apply throttle-steer-brake control + if curr_speed <= current_ref_speed: + vehicle_control = carla.VehicleControl(throttle = throttle,steer=steer) + else: + vehicle_control = carla.VehicleControl(throttle = throttle,steer=steer,brake = 0.5) + + self.env.apply_vehicle_control(self.model_uniquename, vehicle_control) # apply control to vehicle + return end_trajectory + + + def _obey_traffic_light(self, current_ref_speed): + # the vehicle should take traffic lights into account when it is required + # to obey lights and is going straight or turning left + self.blocked_by_light = False + + + + if not self.obey_traffic_lights: + return current_ref_speed + if self.command == "right": + return current_ref_speed + + + + # get the location of the vehicle + curr_transform = self.env.get_transform_2d(self.model_uniquename) + curr_location = curr_transform[0] + + abrupt_stop_vel = carla.Vector3D(x = 0,y = 0,z = 0) + + if self.vehicle_config["vehicle_type"] == "other": + target_location = self.stop_ref_point.location + target_vector = self.stop_ref_point.get_forward_vector() + target_vector_2d = np.array([target_vector.x,target_vector.y]) + target_vector_2d = target_vector_2d / np.linalg.norm(target_vector_2d) + vec_tar_curr = np.array([curr_location[0] - target_location.x,curr_location[1] - target_location.y]) + vec_tar_curr = vec_tar_curr / np.linalg.norm(vec_tar_curr) + coef = np.dot(vec_tar_curr,target_vector_2d) + + + smallest_distance = math.sqrt((curr_location[0] - target_location.x)**2 + (curr_location[1] - target_location.y)**2) * coef + light = self.vehicle_config["traffic_light"] + else: + smallest_distance = np.inf + real_target = None + for ii in range(len(self.stop_ref_point)): + target_location = self.stop_ref_point[ii].location + distance = math.sqrt((curr_location[0] - target_location.x)**2 + (curr_location[1] - target_location.y)**2) + if distance < smallest_distance: + smallest_distance = distance + light = self.vehicle_config["traffic_light"][ii] + real_target = self.stop_ref_point[ii] + target_vector = real_target.get_forward_vector() + target_vector_2d = np.array([target_vector.x,target_vector.y]) + target_vector_2d = target_vector_2d / np.linalg.norm(target_vector_2d) + vec_tar_curr = np.array([curr_location[0] - real_target.location.x,curr_location[1] - real_target.location.y]) + vec_tar_curr = vec_tar_curr / np.linalg.norm(vec_tar_curr) + coef = np.dot(vec_tar_curr,target_vector_2d) + smallest_distance *= coef + + if smallest_distance < 0.5 and smallest_distance > -10: + state = light.get_state() + if state == carla.TrafficLightState.Red or state == carla.TrafficLightState.Yellow: + if self.stop_choice == "abrupt": + self.env.set_vehicle_velocity(self.model_uniquename , abrupt_stop_vel) # immediately stop vehicle + return 0.0 # abrupt stop + else: + if abs(smallest_distance) < 1.0: # normal or penetrate, note smallest_distance is most likely negative + self.env.set_vehicle_velocity(self.model_uniquename , abrupt_stop_vel) + return 0.0 + else: + return current_ref_speed + else: + return current_ref_speed + + return current_ref_speed + + ''' + # check light state + #state = self.env.get_traffic_light_state(self.model_uniquename) + + if self.vehicle_config['vehicle_type'] == 'lead': + print(state) + + if state == carla.TrafficLightState.Red or state == carla.TrafficLightState.Yellow: + # add an indication that the vehicle is blocked by the traffic light + self.blocked_by_light = True + abrupt_stop_vel = carla.Vector3D(x = 0,y = 0,z = 0) + + if self.stop_choice == "abrupt": + self.env.set_vehicle_velocity(self.model_uniquename , abrupt_stop_vel) # immediately stop vehicle + return 0.0 # abrupt stop + else: + curr_transform = self.env.get_transform_2d(self.model_uniquename) + curr_location = curr_transform[0] + + if self.vehicle_config["vehicle_type"] == "other": + + target_location = self.stop_ref_point + distance = math.sqrt((curr_location[0] - target_location.x)**2 + (curr_location[1] - target_location.y)**2) + #print(distance) + if distance < 1.0: # close to target enough + self.env.set_vehicle_velocity(self.model_uniquename , abrupt_stop_vel) # stop vehicle when close enough to reference + return 0.0 + else: + return current_ref_speed + else: # full path vehicle + smallest_distance = np.inf + + # get the smallest distance from the vehicle to target stop point + for target_location in self.stop_ref_point: + distance = math.sqrt((curr_location[0] - target_location.x)**2 + (curr_location[1] - target_location.y)**2) + if distance < smallest_distance: + smallest_distance = distance + + if smallest_distance < 1.0: # close to target enough + self.env.set_vehicle_velocity(self.model_uniquename , abrupt_stop_vel) # stop vehicle when close enough to reference + return 0.0 + else: + return current_ref_speed + + return 0.0 # stop the car immediately + + return current_ref_speed # obey light and light is green + ''' + + def _obey_safety_distance(self, current_ref_speed): + + has_vehicle_in_front, distance = self.env.check_vehicle_in_front(self.model_uniquename, self.safety_distance) + if has_vehicle_in_front: + return 0.0 + + return current_ref_speed + + def _destroy_vehicle(self): + self.env.destroy_vehicle(self.model_uniquename) + + def _display_vehicle_type(self): + vehicle_type = self.vehicle_config["vehicle_type"] + vehicle_transform = self.get_vehicle_transform() + vehicle_location = vehicle_transform.location + debug_location = carla.Location(x = vehicle_location.x, y = vehicle_location.y, z = vehicle_location.z + 2 * self.vehicle_config["bounding_box"].z) + + + if vehicle_type == "other": + self.env.world.debug.draw_string(debug_location,text = "other", color = green, life_time = self.env.delta_seconds) + elif vehicle_type == "lead": + self.env.world.debug.draw_string(debug_location,text = "lead", color = yellow, life_time = self.env.delta_seconds) + elif vehicle_type == "ego": + self.env.world.debug.draw_string(debug_location,text = "ego", color = red, life_time = self.env.delta_seconds) + elif vehicle_type == "follow": + self.env.world.debug.draw_string(debug_location,text = "follow", color = blue, life_time = self.env.delta_seconds) + + +def multiple_vehicle_control(env,intersection_list): + vehicle_list = [] + while True: + env.world.tick() + + # update the distance between vehicles after each tick + env.update_vehicle_distance() + + for ii in range(len(intersection_list)-1,-1,-1): + if intersection_list[ii].start_sim: + for vehicle_config in intersection_list[ii].subject_vehicle: + vehicle = VehicleControl_debug(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + for vehicle_config in intersection_list[ii].left_vehicle: + vehicle = VehicleControl_debug(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + for vehicle_config in intersection_list[ii].right_vehicle: + vehicle = VehicleControl_debug(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + for vehicle_config in intersection_list[ii].ahead_vehicle: + vehicle = VehicleControl_debug(env, vehicle_config, env.delta_seconds) + vehicle_list.append(vehicle) + + intersection_list.pop(ii) + + if len(vehicle_list) == 0: + break + + ''' + Temp function. Start vehicle that's not running + ''' + has_run = False + for vehicle in vehicle_list: + if vehicle.run: + has_run = True + break + + if not has_run: + for vehicle in vehicle_list: + vehicle.run = True + + for jj in range(len(vehicle_list) -1, -1, -1): + vehicle = vehicle_list[jj] + if vehicle.run: + end_trajectory = vehicle.pure_pursuit_control_wrapper() + if end_trajectory: + vehicle_list.pop(jj) + +def main(): + try: + client = carla.Client("localhost",2000) + client.set_timeout(10.0) + world = client.load_world('Town05') + + # set the weather + weather = carla.WeatherParameters( + cloudiness=10.0, + precipitation=0.0, + sun_altitude_angle=90.0) + world.set_weather(weather) + + # set the spectator position for demo purpose + spectator = world.get_spectator() + spectator.set_transform(carla.Transform(carla.Location(x=-133, y=1.29, z=75.0), carla.Rotation(pitch=-88.0, yaw= -1.85, roll=1.595))) # top view of intersection + + env = CARLA_ENV(world) + time.sleep(2) # sleep for 2 seconds, wait the initialization to finish + + traffic_light_list = get_traffic_lights(world.get_actors()) + + intersection_list = [] + + # intersection 1 + world_pos = (-133.0,0.0)#(25.4,0.0) + + intersection1 = Intersection(env, world_pos, traffic_light_list) + intersection1.add_vehicle(obey_traffic_lights = False) + + intersection1.add_vehicle(command = "left", obey_traffic_lights = False) + intersection1.add_vehicle(command = "right", obey_traffic_lights = False) + + intersection1.add_vehicle(gap = 5,choice = "left", obey_traffic_lights = False) + intersection1.add_vehicle(gap = 5, choice = "left",command = "right", obey_traffic_lights = False) + intersection1.add_vehicle(gap = 5,choice = "left",command = "left", obey_traffic_lights = False) + intersection1.add_vehicle(choice = "right", obey_traffic_lights = False) + intersection1.add_vehicle(choice = "right",command = "left", obey_traffic_lights = False) + intersection1.add_vehicle(gap = 10.0,choice = "right",command = "right", obey_traffic_lights = False) + intersection1.add_vehicle(choice = "ahead", obey_traffic_lights = False) + intersection1.add_vehicle(choice = "ahead",command = "right", obey_traffic_lights = False) + intersection1.add_vehicle(choice = "ahead",command = "left", obey_traffic_lights = False) + + intersection1._shift_vehicles(-10,index = 1) + + + intersection1.start_sim = True + + intersection_list.append(intersection1) + + multiple_vehicle_control(env,intersection_list) + + finally: + time.sleep(10) + env.destroy_actors() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/legacy/gui_interface/backend/pure_pursuit_with_auto_trajectory_generation.py b/legacy/gui_interface/backend/pure_pursuit_with_auto_trajectory_generation.py new file mode 100644 index 0000000..73b7248 --- /dev/null +++ b/legacy/gui_interface/backend/pure_pursuit_with_auto_trajectory_generation.py @@ -0,0 +1,498 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Mon Jun 15 20:50:23 2020 + +@author: shijiliu +""" + +## THIS WHOLE FILE IS JUST COPY PASTED FROM THE MULTIPLE_VEHICLE_CONTROL.py, remove this + + +import carla +import matplotlib.pyplot as plt +import numpy as np +from collections import deque +import time +from scipy.interpolate import UnivariateSpline +import math + +import control # the python-control package, install first +from backend.carla_env import CARLA_ENV # self-written class that provides help functions, should be in the same folder + +from backend.generate_path_omit_regulation import generate_path # function for auto-path generation +# PI controller constants +KI = 0.02 +KP = 0.5 + +# pure-pursuit model constants +k = 0.1 # coefficient for look ahead +Lfc = 4.0 # look ahead distance +L = 2.88 # wheelbase + +# distance between two near way points +waypoint_separation = 4.0 +# enable/disable debug mode to draw trajectory points in carla +DEBUG = True + +# Repeat of Multiple_Vehicle_Control::get_Pi_Controller +def get_PI_controller(delta_seconds): + ''' + Effects: create a discrete state-space PI controller + ''' + num_pi = [KP, KI] # numerator of the PI transfer function (KP*s + KI) + den_pi = [1.0, 0.01*KI/KP] # denominator of PI transfer function (s + 0.01*KI/KP) + + sys = control.tf(num_pi,den_pi) # get transfer function for PI controller (since the denominator has a small term 0.01*KI/KP, it is actually a lag-compensator) + sys = control.sample_system(sys, delta_seconds) # discretize the transfer function (from s-domain which is continuous to z-domain) + #since our simulation is discrete + sys = control.tf2ss(sys) # transform transfer function into state space. + return sys + +def speed_control(sys, ref_speeds, curr_speeds, init_values): + ''' + Effects: get the reference speed, current (measured) speed and initial values + Use the difference + e = ref_speeds - curr_speeds + as the input for the PI controller, derive the new throttle + + Parameters + ---------- + sys : control.ss + state space controller + ref_speeds : list of float + the desired speed we need + curr_speeds : list of float + the current speed + init_values : the initial_values of the system + DESCRIPTION. + + Returns + ------- + throttle : float type + DESCRIPTION. + + ''' + U0 = np.array(ref_speeds) - np.array(curr_speeds) + #print(U0) + _,y0,x0 = control.forced_response(sys,U = U0,X0 = init_values[0]) # y0 is the next values, x0 is the state evolution + # see https://python-control.readthedocs.io/en/0.8.3/generated/control.forced_response.html#control.forced_response + init_values.append(x0[-1]) + throttle = y0[-1] + return throttle, init_values + +def get_trajectory(way_points): + ''' + + + Parameters + ---------- + way_points : list + A list of (way_point, reference_speed) tuple, + where way_points is a tuple of floats (x,y), the first point must be the **current point** of the vehicle + reference speed is the desired speed for the vehicle after this way point and before the next way point + e.g. [((0.0,0.0),10.0),((0.0,10.0),1.0)] + + Returns + ------- + trajectory : numpy 2d array + the interpolated trajectory. + ref_speed_list : list + the speed correspoding to the interpolated trajectory + + ''' + points, speed = zip(*way_points) + points = np.array([[pt[0], pt[1]] for pt in points]) + + # apply average smoothing of the points + points = smooth_trajectory(points) + + # linear length along the line (reference: https://stackoverflow.com/questions/52014197/how-to-interpolate-a-2d-curve-in-python) + distance = np.cumsum( np.sqrt(np.sum( np.diff(points,axis=0)**2, axis = 1))) + distance = np.insert(distance, 0, 0)/distance[-1] + + ''' + # define interpolation method + interpolation_method = 'slinear' #'quadratic' + + alpha = np.linspace(0,1, 2 * len(distance)) + + interpolator = interp1d(distance, points, kind = interpolation_method, axis = 0) + trajectory = interpolator(alpha) + ''' + + # Build a list of the spline function, one for each dimension: + splines = [UnivariateSpline(distance, coords, k=1, s=.2) for coords in points.T] + + alpha = np.linspace(0,1.02, 2 * len(distance)) + trajectory = np.vstack( [spl(alpha) for spl in splines] ).T + + + nearest_index = [] + for pt in points: + nearest_distance = np.inf + index = 0 + count = 0 + for trajectory_pt in trajectory: + dist_2 = sum((trajectory_pt - pt)**2) + if dist_2 < nearest_distance: + nearest_distance = dist_2 + index = count + count += 1 + nearest_index.append(index) + + ref_speed_list = np.zeros(len(trajectory)) + for ii in range(1,len(nearest_index)): + ref_speed_list[nearest_index[ii - 1]:nearest_index[ii]] = speed[ii - 1] + + #plt.plot(trajectory[:,0],trajectory[:,1],'.') + #print(ref_speed_list) + + return trajectory, ref_speed_list + +def get_target_index(location_2d, current_forward_speed, trajectory): + ''' + Get the target for the vehicle to navigate to + + Parameters + ---------- + location_2d : (x,y) + current location of the vehicle. + current_forward_speed : float + current speed of the vehicle. + trajectory : numpy 2d array + interpolated waypoints. + + Returns + ------- + ind : TYPE + DESCRIPTION. + end_trajectory : TYPE + DESCRIPTION. + + ''' + + + distance = np.sum((trajectory - location_2d)**2,axis = 1)**0.5 + ind = np.argmin(distance) + l0 = 0.0 + + Lf = k * current_forward_speed + Lfc + + while Lf > l0 and (ind + 1) < len(trajectory): + delta_d = sum((trajectory[ind + 1] - trajectory[ind])**2)**0.5 + l0 += delta_d + ind += 1 + + if ind >= len(trajectory) - 1: + end_trajectory = True + else: + end_trajectory = False + + return ind, end_trajectory + + +def smooth_trajectory(trajectory): + ''' + + + Parameters + ---------- + trajectory : [(float,float),...,(float,float)] + 2d trajectory. + + Returns + ------- + smoothed_trajectory : [(float,float),...,(float,float)] + the smoother trajectory + + ''' + smoothed_trajectory = [] + smoothed_trajectory.append(trajectory[0]) + + num = 3 + + for ii in range(num - 1,len(trajectory)): + avg_pt = (trajectory[ii - 2] + trajectory[ii - 1] + trajectory[ii]) / num + smoothed_trajectory.append(avg_pt) + + smoothed_trajectory.append(trajectory[-1]) + return np.array(smoothed_trajectory) + + +def pure_pursuit_control(vehicle_pos_2d, current_forward_speed, trajectory, ref_speed_list, prev_index): + ''' + + + Parameters + ---------- + vehicle_pos_2d : (location_2d,yaw) + tuple of vehicle location and heading in 2d. + location_2d : (x,y), both x and y are in meter + yaw : heading angle **Note** yaw is in degree + current_forward_speed : float + the current velocity of the vehicle. + trajectory : numpy 2d array + interpolated waypoints. + ref_speed_list : list + the reference speed corresponding to each way point + prev_index : int + the previous index + Returns + ------- + delta : float + steer angle of the vehicle. + current_ref_speed : the reference speed + DESCRIPTION. + index : int + the index of the target. + end_trajectory : boolean + whether we have reached clos enough to the destination. + + ''' + + + + location_2d, yaw = vehicle_pos_2d + yaw = np.deg2rad(yaw) # change the unit the radius + index, end_trajectory = get_target_index(location_2d, current_forward_speed, trajectory) + + if prev_index >= index: + index = prev_index + + if index < len(trajectory): + tx = trajectory[index][0] + ty = trajectory[index][1] + else: + tx = trajectory[-1][0] + ty = trajectory[-1][1] + + alpha = math.atan2(ty - location_2d[1],tx - location_2d[0]) - yaw + + if current_forward_speed < 0: #back, should not happen in our case + alpha = math.pi - alpha + + Lf = k * current_forward_speed + Lfc + + delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0) + #print("delta == ", delta, "yaw == ", yaw) + + current_ref_speed = ref_speed_list[index] + + return delta, current_ref_speed, index, end_trajectory + +def generate_waypoints(env, start, end, constant_speed): + ''' + + + Parameters + ---------- + env : CARLA_ENV + The simulation environment + + start : (float,float) + 2d position of the start point + + end : (float,float) + 2d position of destination + + Returns + ------- + way_points : list + A list of (way_point, reference_speed) tuple, + where way_points is a tuple of floats (x,y), the first point must be the **current point** of the vehicle + reference speed is the desired speed for the vehicle after this way point and before the next way point + e.g. [((0.0,0.0),10.0),((0.0,10.0),1.0)] + + ''' + + # get the waypoint from carla map + carla_map = env.world.get_map() + + start_raw = carla.Location(x = start[0], y = start[1], z = 0.0) + end_raw = carla.Location(x = end[0], y = end[1], z = 0.0) + + start_waypoint = carla_map.get_waypoint(start_raw) + end_waypoint = carla_map.get_waypoint(end_raw) + + trajectory = generate_path(env,start_waypoint, end_waypoint, waypoint_separation = waypoint_separation) + + # generate way_points + way_points = [] + + for ii in range(len(trajectory) - 1): + way_points.append((trajectory[ii],constant_speed)) + + way_points.append((trajectory[-1], 0.0)) # set the speed at destination to be 0 + + return way_points + + + + + +def pure_pursuit_control_wrapper(env,start,end,model_uniquename, constant_speed): + ''' + + + Parameters + ---------- + env : CARLA_ENV + The simulation environment + + start : (float,float) + 2d position of the start point + + end : (float,float) + 2d position of destination + + model_uniquename : str + the name of the vehicle we want to control. + + constant_speed : float + the speed of the vehicle, assume constant + Returns + ------- + None. + + ''' + + # create control system + sys = get_PI_controller(env.delta_seconds) + + # essential storages for the controller to work + init_values = deque(maxlen = 2) # the state space values of the system. For a control system to be fully functional + # we need to give initial value + ref_speeds = deque(maxlen = 2) # the reference / target speed + curr_speeds = deque(maxlen = 2) # the measured speed of the vehicle + + if DEBUG: + vehicle_pose = deque(maxlen = 2) + + + # storage for the visualize the reference speed, throttle and measured speed. + speed = [] + throttles = [] + reference_speed = [] + + # give initial values to storage, assume the car is released at rest, with no initial speed or acceleration + init_values.append(0) + ref_speeds.append(0) + curr_speeds.append(0) + + current_ref_speed = 0 + index = 0 + + # get waypoints from auto-trajectory generation, with a constant navigation speed + way_points = generate_waypoints(env, start, end, constant_speed) + + + # interpolate a trajectory based on way_points for the vehicle to follow + trajectory, ref_speed_list = get_trajectory(way_points) + + if DEBUG: + points_debug, _ = zip(*way_points) + env.draw_waypoints(trajectory, points_debug) # draw waypoints and expected trajectory + + # main control loop + while True: #loop for applying control + env.world.tick() + + # get vehicle's speed, location and orientation + curr_speed = env.get_forward_speed(model_uniquename) + vehicle_pos_2d = env.get_transform_2d(model_uniquename) # the (x,y) location and yaw angle of the vehicle + speed.append(curr_speed) + curr_speeds.append(curr_speed) + + # draw real trajectory if debug is enabled + if DEBUG: + vehicle_pose.append(vehicle_pos_2d[0]) + if len(vehicle_pose) == 2: + env.draw_real_trajectory(vehicle_pose) + + # use pure-pursuit model to get the steer angle (in radius) + delta, current_ref_speed, index, end_trajectory = pure_pursuit_control(vehicle_pos_2d, curr_speed, trajectory, ref_speed_list, index) + steer = np.clip(delta,-1.0,1.0) + ref_speeds.append(current_ref_speed) + reference_speed.append(current_ref_speed) + + # get throttle to get the next reference speed + throttle, init_values = speed_control(sys, ref_speeds, curr_speeds, init_values) # get the throttle control based on reference and current speed + throttle = np.clip(throttle,0,1) # throttle value is [0,1] + throttles.append(throttle) # for visualization + + # check whether we are reaching the destination or not + if end_trajectory: + vehicle_control = carla.VehicleControl(throttle = 0.0,steer=steer,brake = 1.0) # immediately stop the car + env.apply_vehicle_control(model_uniquename, vehicle_control) # apply control to vehicle + break + + + # apply throttle-steer-brake control + if curr_speed <= current_ref_speed: + vehicle_control = carla.VehicleControl(throttle = throttle,steer=steer) + else: + vehicle_control = carla.VehicleControl(throttle = throttle,steer=steer,brake = 0.5) + env.apply_vehicle_control(model_uniquename, vehicle_control) # apply control to vehicle + + + + return throttles, speed, reference_speed + +client = carla.Client("localhost",2000) +client.set_timeout(10.0) +world = client.load_world('Town05') + +# set the weather +world = client.get_world() +weather = carla.WeatherParameters( + cloudiness=10.0, + precipitation=0.0, + sun_altitude_angle=90.0) +world.set_weather(weather) + +# set the spectator position for demo purpose +spectator = world.get_spectator() +spectator.set_transform(carla.Transform(carla.Location(x=0.0, y=0.0, z=20.0), carla.Rotation(pitch=-31.07, yaw= -90.868, roll=1.595))) # plain ground + +env = CARLA_ENV(world) +time.sleep(2) # sleep for 2 seconds, wait the initialization to finish + +# spawn a vehicle, here I choose a Tesla model +spawn_point = carla.Transform(carla.Location(x=14.8, y=5.8, z=5.0), carla.Rotation(pitch=0.000000, yaw= 0, roll=0.000000)) +model_name = "vehicle.tesla.model3" +model_uniquename = env.spawn_vehicle(model_name,spawn_point) # spawn the model and get the uniquename, the CARLA_ENV class will store the vehicle into vehicle actor list + +time.sleep(5) + +''' +way_points = [((-277.08,-15.39),20), + ((-30.08,-15.39),10), + ((-12.0,-12.0),10), + (( -9, 0.0),20), + (( -9, 50),0) + ] +''' + +start = (14.8,5.8) +end = (31.49375, -12.65625) +constant_speed = 10 + +try: + + throttles, speed, reference_speed = pure_pursuit_control_wrapper(env,start,end,model_uniquename, constant_speed) + + fig,a = plt.subplots(3,1) + + #plt.subplot(3,1,1) + a[0].plot(reference_speed) + a[0].set_title('reference speed') + #plt.subplot(3,1,2) + a[1].plot(throttles) + a[1].set_title('throttle applied') + a[2].plot(speed) + a[2].set_title('measured speed') + + +finally: + env.destroy_actors() \ No newline at end of file diff --git a/legacy/gui_interface/backend/section_definition.py b/legacy/gui_interface/backend/section_definition.py new file mode 100644 index 0000000..42433fe --- /dev/null +++ b/legacy/gui_interface/backend/section_definition.py @@ -0,0 +1,369 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Wed Jul 22 09:58:37 2020 + +@author: shijiliu +""" + + +import sys +sys.path.append("..") + +import carla +from backend.carla_env import CARLA_ENV +import math +import time +import numpy as np +from configobj import ConfigObj +from backend.generate_path_omit_regulation import generate_path +from backend.intersection_definition import smooth_trajectory, get_trajectory +from scipy.interpolate import UnivariateSpline +import copy + +# color for debug use +red = carla.Color(255, 0, 0) +green = carla.Color(0, 255, 0) +blue = carla.Color(47, 210, 231) +cyan = carla.Color(0, 255, 255) +yellow = carla.Color(255, 255, 0) +orange = carla.Color(255, 162, 0) +white = carla.Color(255, 255, 255) + +# the definition of the normal section +# Likely make a Section Class +class Section(object): + def __init__(self, env, world_waypoint): + ''' + + + Parameters + ---------- + env : CARLA_ENV + the simulation environment + world_waypoint : carla.Waypoint + the subject point of the section. + + Returns + ------- + None. + + ''' + # store the environment + self.env = env + + + + # get the world map + self.carla_map = self.env.world.get_map() + + # get the central point of the Section, which will be the reference for subject waypoint + self.subject_waypoint = world_waypoint + + self.section_location = self.subject_waypoint.transform.location + + # get the reference points of the Section, which will be used to form trajectory + self._get_section_trajectory_points() + + # variable for storing the vehicles in subject lane and left lane + self.subject_lead_vehicle = [] # for normal section, adding vehicle is currently not allowed + self.subject_follow_vehicle = [] + self.left_lead_vehicle = [] # vehicles will be loaded in from init sections + self.left_follow_vehicle = [] + self.ego_vehicle = None + + # variable for storing the trajectory for subject lane and left lane + self.subject_trajectory = None # these two variables are currently not put into use + self.left_trajectory = None # keep these variables in case adding section only vehicles is allowed in the future + + + # variable to count the time elapsed after simulation starts in this section + self.time_count = 0 + + # Put this in a Controller class + def _get_next_waypoint(self,curr_waypoint,distance = 4): + ''' + + + Parameters + ---------- + curr_waypoint : carla.Waypoint + current waypoint. + distance : float, optional + "distance" between current waypoint and target waypoint . The default is 10. + + Returns + ------- + next_waypoint : carla.Waypoint + next waypoint, "distance" away from curr_waypoint, in the direction of the current way point + ''' + forward_vector = curr_waypoint.transform.get_forward_vector() + + location = curr_waypoint.transform.location + raw_spawn_point = carla.Location(x = location.x + distance * forward_vector.x , y = location.y + distance * forward_vector.y , z = location.z + 0.1) + + next_waypoint = self.carla_map.get_waypoint(raw_spawn_point) + return next_waypoint + + # Section Class + def _get_section_trajectory_points(self): + ''' + get the reference points for trajectory + + Returns + ------- + None. + + ''' + + reference_way_points = [] + curr_waypoint = self.subject_waypoint + + # points after the subject, for adding follow vehicles + for ii in range(8): + distance = -4 + next_waypoint = self._get_next_waypoint(curr_waypoint, distance = distance) + reference_way_points.append(next_waypoint) + curr_waypoint = next_waypoint + + # points before the subject, for adding lead vehicles and navigation + + reference_way_points.reverse() + reference_way_points.append(self.subject_waypoint) + curr_waypoint = self.subject_waypoint + + for ii in range(90): + distance = 4 + next_waypoint = self._get_next_waypoint(curr_waypoint, distance = distance) + reference_way_points.append(next_waypoint) + curr_waypoint = next_waypoint + + self.reference_way_points = reference_way_points + + # Put some of this in the GUI and the rest in the Section class + def _add_full_path_vehicle_normal(self, uniquename, vehicle_type, choice, command = "speed", command_start_time = 0.0): + ''' + Create a setting place holder of the vehicle that has been added to the initial section + The command and command start time will be kept as default + + Front end user should not use this function + + Parameters + ---------- + uniquename : string + the name of the vehicle + vehicle_type : string, + the vehicle type, valid values : "lead", "follow". + choice : string, + the lane choice, valid values are "subject", "left". + command : string, optional + the command the vehicle is going to execute in this section. Valid values: "speed", "lane", "distance". The default is "speed". + command_start_time : string, optional + the time at which the command should be executed. The default is 0.0. + + Returns + ------- + None. + + ''' + if vehicle_type == "lead" and choice == "subject": + vehicle_set = self.subject_lead_vehicle + + elif vehicle_type == "lead" and choice == "left": + vehicle_set = self.left_lead_vehicle + + elif vehicle_type == "follow" and choice == "subject": + vehicle_set = self.subject_follow_vehicle + + elif vehicle_type == "follow" and choice == "left": + vehicle_set = self.left_follow_vehicle + + vehicle_local_config = ConfigObj() + vehicle_local_config["uniquename"] = uniquename + vehicle_local_config["command"] = command + vehicle_local_config["command_start_time"] = command_start_time + + vehicle_set.append(vehicle_local_config) + + # Same as above + def _remove_full_path_vehicle_normal(self, vehicle_type, choice, index): + ''' + remove a full path vehicle that does not exists anymore + + Parameters + ---------- + vehicle_type : string, + the vehicle type, valid values : "lead", "follow". + choice : string, + the lane choice, valid values are "subject", "left". + index : int + the index of the vehicle inside a specific lane. + + Returns + ------- + None. + + ''' + if vehicle_type == "lead" and choice == "subject": + vehicle_set = self.subject_lead_vehicle + + elif vehicle_type == "lead" and choice == "left": + vehicle_set = self.left_lead_vehicle + + elif vehicle_type == "follow" and choice == "subject": + vehicle_set = self.subject_follow_vehicle + + elif vehicle_type == "follow" and choice == "left": + vehicle_set = self.left_follow_vehicle + + if len(vehicle_set) <= index: + print("Invalid index") + return + + vehicle_set.pop(index) # remove the setting from the list + + # Likely can get rid of this + def _update_vehicle_uniquename(self, vehicle_type, choice, index, uniquename): + ''' + Private function for updating the uniquename of the vehicle in case uniquename is changed + + Parameters + ---------- + vehicle_type : string, + the vehicle type, valid values : "lead", "follow". + choice : string, + the lane choice, valid values are "subject", "left". + index : int + the index of the vehicle inside a specific lane. + uniquename : string + the name of the vehicle + + Returns + ------- + None. + + ''' + if vehicle_type == "lead" and choice == "subject": + vehicle_set = self.subject_lead_vehicle + + elif vehicle_type == "lead" and choice == "left": + vehicle_set = self.left_lead_vehicle + + elif vehicle_type == "follow" and choice == "subject": + vehicle_set = self.subject_follow_vehicle + + elif vehicle_type == "follow" and choice == "left": + vehicle_set = self.left_follow_vehicle + + if len(vehicle_set) <= index: + print("Invalid index") + return + + vehicle_local_config = vehicle_set[index] + vehicle_local_config["uniquename"] = uniquename + + # Add this to the vehicle class + def edit_full_path_vehicle_local_setting(self, vehicle_type, choice, index , command = "speed", command_start_time = 0.0): + ''' + API for the users to edit settings of a given vehicle + + Parameters + ---------- + vehicle_type : string, + the vehicle type, valid values : "lead", "follow". + choice : string, + the lane choice, valid values are "subject", "left". + index : int + the index of the vehicle inside a specific lane. + command : string, optional + the command the vehicle is going to execute in this section. Valid values: "speed", "lane", "distance". The default is "speed". + command_start_time : string, optional + the time at which the command should be executed. The default is 0.0. + + Returns + ------- + None. + + ''' + if vehicle_type == "lead" and choice == "subject": + vehicle_set = self.subject_lead_vehicle + + elif vehicle_type == "lead" and choice == "left": + vehicle_set = self.left_lead_vehicle + + elif vehicle_type == "follow" and choice == "subject": + vehicle_set = self.subject_follow_vehicle + + elif vehicle_type == "follow" and choice == "left": + vehicle_set = self.left_follow_vehicle + + if len(vehicle_set) <= index: + print("Invalid index") + return + + vehicle_local_config = vehicle_set[index] + vehicle_local_config["command"] = command + vehicle_local_config["command_start_time"] = command_start_time + + # Vehicle class also + def get_full_path_vehicle_local_setting(self, vehicle_type, choice, index): + # get the settings of the vehicle based on lane and index + # return the command and corresponding start time + if vehicle_type == "lead" and choice == "subject": + vehicle_set = self.subject_lead_vehicle + + elif vehicle_type == "lead" and choice == "left": + vehicle_set = self.left_lead_vehicle + + elif vehicle_type == "follow" and choice == "subject": + vehicle_set = self.subject_follow_vehicle + + elif vehicle_type == "follow" and choice == "left": + vehicle_set = self.left_follow_vehicle + + if len(vehicle_set) <= index: + print("Invalid index") + return None, None + + vehicle_local_config = vehicle_set[index] + command = vehicle_local_config["command"] + command_start_time = vehicle_local_config["command_start_time"] + return command, command_start_time + + # Section class + def get_section_trajectory_points(self): + # get the reference points of this section for use outside + return self.reference_way_points + + # Move this into a main simulation loop (likely in the Experiment class) + def tick(self): + # increment the time_count by one step + # return the elapsed time in terms of seconds + ret_val = self.time_count * self.env.delta_seconds + self.time_count += 1 + return ret_val + + def section_start(self, ego_transform): + ''' + function deciding whether this section should start + + Parameters + ---------- + ego_transform : carla.Transform + the transform of the ego vehicle. + + Returns + ------- + None. + + ''' + ego_location = ego_transform.location + + # get the distance between the ego vehicle and center of section + distance_2d = math.sqrt( (ego_location.x - self.section_location.x) ** 2 + (ego_location.y - self.section_location.y) ** 2) + + if distance_2d < 3.0: # the ego vehicle is close enough to the section reference point + return True + else: + return False + \ No newline at end of file diff --git a/legacy/gui_interface/backend/section_environment.py b/legacy/gui_interface/backend/section_environment.py new file mode 100644 index 0000000..e1666ca --- /dev/null +++ b/legacy/gui_interface/backend/section_environment.py @@ -0,0 +1,898 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Wed Jul 22 11:20:28 2020 + +@author: shijiliu & zhiychen +""" + +# define a class for creating the simulation environment for the section + +import sys +sys.path.append("..") + +import carla +from backend.carla_env import CARLA_ENV +import math +import time +import numpy as np +from configobj import ConfigObj +import pandas as pd +import copy +from openpyxl import load_workbook +from openpyxl import Workbook + +from backend.section_definition import Section +from backend.section_init_definition import InitSection +from backend.generate_path_omit_regulation import generate_path +from backend.intersection_definition import smooth_trajectory, get_trajectory +from backend.multiple_vehicle_control import VehicleControl +from backend.multiple_vehicle_control_debug import VehicleControl_debug +from backend.initial_intersection import get_ego_spectator, get_ego_left_spectator, get_ego_driving_spectator +from backend.section_vehicle_control import VehicleControlFreeway, FullPathVehicleControl, LeadFollowVehicleControl +from backend.human_ego_control import HumanEgoControlServer +import pandas as pd + + +# color for debug use +red = carla.Color(255, 0, 0) +green = carla.Color(0, 255, 0) +blue = carla.Color(47, 210, 231) +cyan = carla.Color(0, 255, 255) +yellow = carla.Color(255, 255, 0) +orange = carla.Color(255, 162, 0) +white = carla.Color(255, 255, 255) + +# Likely move most of this into a base FreewayExperiment class (all specific freeway experiments should derive from this) +class FreewayEnv(object): + # this class holds all methods related to create a simulation environment for freeway + def __init__(self, env, number_of_sections, max_speed = 30.0, min_speed = 15.0): + ''' + initialize the freeway simulation environment + + Parameters + ---------- + number_of_sections : int + number of sections for the freeway environment. + + max_speed : float + the max speed of vehicle. Default to be 30.0. + + min_speed : float + the min speed of vehicle. Default to be 15.0. Vehicle will go under the average of max_speed and min_speed under speed mode + + Returns + ------- + None + + ''' + # debug + self.DEBUG_TRAJECTORY = False#True + self.DEBUG_SUBJECTPOINTS = True + self.DEBUG_SECTION = True + + # Enable/Disable Data Collection + self.RECORD_ENABLE = False + + # store the number of sections + self.number_of_sections = number_of_sections + + self.env = env + + self.spectator = self.env.world.get_spectator() + + + + self.carla_map = self.env.world.get_map() + + # get the subject waypoint of each section + self.subject_point_list = self._get_section_subject_points((-80.0,9.46)) + + # store the reference speed + self.max_speed = max_speed + self.min_speed = min_speed + self.navigation_speed = (self.max_speed + self.min_speed) * 0.5 + + # create the simulation environment and generate the trajectory + self._create_simulation_env() + + # give the trajectory and reference speed list to the initial intersection + self.section_list[0].get_full_path_trajectory(self.smoothed_full_trajectory, self.ref_speed_list, self.left_smoothed_full_trajectory, self.left_ref_speed_list, self.navigation_speed, self.max_speed, self.min_speed) + + + + + #def __del__(self): + # self.env.destroy_actors() + + # public methods + def get_section_list(self): + return self.section_list + + # Vehicle class + def get_vehicle_bounding_box(self,uniquename): + ''' + get the bounding box of the vehicle by uniquename + Parameters + ---------- + uniquename : string + the uniquename of the vehicle. + Returns + ------- + new_bb : carla.Vector3D + the bounding box of the vehicle, new_bb.x is the length, new_bb.y is the width, new_bb.z is the height + ''' + new_bb = self.env.get_vehicle_bounding_box(uniquename) + return new_bb + + # Section class + def add_ego_vehicle(self, model_name = "vehicle.tesla.model3", safety_distance = 15.0, vehicle_color = None): + # wrapper for add_ego_vehicle function of the init_section + + ''' + add ego vehicle to the initial intersection + according to the user case, the ego vehicle will follow the + subject lane all the way with constant speed + + Parameters + ---------- + model_name : string, optional + vehicle type. The default is "vehicle.tesla.model3". + safety_distance : float, optional + smallest distance between this vehicle and vehicle ahead + vehicle_color : string + the RGB representation of the vehicle color. e.g. '255,255,255' + + Returns + ------- + uniquename : string + the name of the vehicle + + ''' + + + uniquename = self.section_list[0].add_ego_vehicle(model_name = model_name, safety_distance = safety_distance, vehicle_color = vehicle_color) + return uniquename + + # Section class + def edit_ego_vehicle(self, model_name = "vehicle.tesla.model3", safety_distance = 15.0, vehicle_color = None): + # wrapper for edit_ego_vehicle function of the init_section + ''' + edit the ego vheicle setting by delete the original ego vehicle and add a new one + + Parameters + ---------- + model_name : string, optional + vehicle type. The default is "vehicle.tesla.model3". + safety_distance : float, optional + smallest distance between this vehicle and vehicle ahead + vehicle_color : string + the RGB representation of the vehicle color. e.g. '255,255,255' + + Returns + ------- + uniquename : string + the name of the vehicle + + ''' + uniquename = self.section_list[0].edit_ego_vehicle(model_name = model_name, safety_distance = safety_distance, vehicle_color = vehicle_color) + return uniquename + + # Section class + def add_full_path_vehicle(self, model_name = "vehicle.tesla.model3", vehicle_type ="lead", choice = "subject", command = "speed", command_start_time = 0.0, gap = 10.0, safety_distance = 15.0, lead_follow_distance = 20.0, vehicle_color = None): + # wrapper for add_full_path_vehicle of the init_section + ''' + add full path vehicle + + Parameters + ---------- + model_name : string, optional + vehicle type. The default is "vehicle.tesla.model3". + vehicle_type : string, optional + the vehicle type, valid values : "lead", "follow". The default is "lead". + choice : string, optional + the lane choice, valid values are "subject", "left". The default is "subject". + command : string, optional + the command the vehicle is going to execute in this section. Valid values: "speed", "lane", "distance". The default is "speed". + command_start_time : string, optional + the time at which the command should be executed. The default is 0.0. + gap : float, optional + the gap between the vehicle and the one in the front of it when adding. The default is 10.0, unit: meter + safety_distance : float, optional + smallest distance between 2 vehicles when simulation is going. The default is 15.0, unit: meter + vehicle_color : string + the RGB representation of the vehicle color. e.g. '255,255,255' + + Returns + ------- + uniquename : string + the name of the vehicle + + + ''' + # add the full path vehicle to the init section + uniquename = self.section_list[0].add_full_path_vehicle(model_name = model_name, vehicle_type = vehicle_type, choice = choice, command = command, command_start_time = command_start_time, gap = gap, safety_distance = safety_distance, lead_follow_distance = lead_follow_distance, vehicle_color = vehicle_color ) + + + # create default vehicle setting for normal sections + for ii in range(1,len(self.section_list)): + self.section_list[ii]._add_full_path_vehicle_normal(uniquename, vehicle_type, choice) + + return uniquename + + # Section class + def remove_full_path_vehicle(self, uniquename): + ''' + remove a full path vehicle based on its uniquename + this includes the vehicle setting in init section and + settings in other normal sections + + Parameters + ---------- + uniquename : string + name of the vehicle. + + Returns + ------- + removed : Bool + whether the given vehicle is found and removed + + ''' + removed, vehicle_type, choice, index = self.section_list[0].remove_full_path_vehicle(uniquename) + if removed: + for ii in range(1, len(self.section_list)): + self.section_list[ii]._remove_full_path_vehicle_normal(vehicle_type, choice, index) # remove the setting from normal + # sections + + return removed + + # Vehicle class + def edit_full_path_vehicle_init_setting(self, uniquename, vehicle_type, choice, model_name = "vehicle.tesla.model3", command = "speed", command_start_time = 0.0, gap = 10.0, safety_distance = 15.0, lead_follow_distance = 20.0, vehicle_color = None): + # wrapper for the edit_full_path_vehicle function of the init section + ''' + edit full path vehicle settings by deleting the original vehicle and then add a new one + + Parameters + ---------- + uniquename : string + the name of the vehicle + vehicle_type : string, + the vehicle type, valid values : "lead", "follow". + choice : string, optional + the lane choice, valid values are "subject", "left". + model_name : string, optional + vehicle type. The default is "vehicle.tesla.model3". + command : string, optional + the command the vehicle is going to execute in this section. Valid values: "speed", "lane", "distance". The default is "speed". + command_start_time : string, optional + the time at which the command should be executed. The default is 0.0. + gap : float, optional + the gap between the vehicle and the one in the front of it when adding. The default is 10.0, unit: meter + safety_distance : float, optional + smallest distance between 2 vehicles when simulation is going. The default is 15.0, unit: meter + vehicle_color : string + the RGB representation of the vehicle color. e.g. '255,255,255' + + Returns + ------- + new_uniquename : string + the new_name of the vehicle + + + ''' + + new_uniquename, index = self.section_list[0].edit_full_path_vehicle(uniquename = uniquename, + vehicle_type = vehicle_type, + choice = choice, + model_name = model_name, + command = command, + command_start_time = command_start_time, + gap = gap, + safety_distance = safety_distance, + lead_follow_distance = lead_follow_distance, + vehicle_color = vehicle_color) + + + + for ii in range(1,len(self.section_list)): + self.section_list[ii]._update_vehicle_uniquename(vehicle_type = vehicle_type, choice = choice,index = index,uniquename = new_uniquename) + + return new_uniquename + + # Vehicle class + def edit_normal_section_setting(self, section_id, vehicle_type, choice, vehicle_index, command = "speed", command_start_time = 0.0): + ''' + function for editing vehicle settings for normal section + + Parameters + ---------- + section_id : TYPE + the section id. e.g., the first normal section is the second section, so enter section_id = 2 + vehicle_type : TYPE + DESCRIPTION. + choice : TYPE + DESCRIPTION. + vehicle_index : int + the index of the vehicle + command : TYPE, optional + DESCRIPTION. The default is "speed". + command_start_time : TYPE, optional + DESCRIPTION. The default is 0.0. + + Returns + ------- + None. + + ''' + if section_id > len(self.section_list): + print("invalid section_id") + return + + section = self.section_list[section_id - 1] + + # edit the section settings + section.edit_full_path_vehicle_local_setting(vehicle_type, choice, vehicle_index, command = command, command_start_time = command_start_time) + + # Simulation loop, move this into the base experiment class + def SectionBackend(self, spectator_mode = None, allow_collision = True, enable_human_control = False): + ''' + back end function for the freeway + + Parameters + ---------- + spectator_mode : string, optional + the spectator mode, valid value is "first_person". The default is None. + + allow_collision : bool, optional + whether collision is allowed during simulation + + enable_human_control : bool, optional + whether ego vehicle is controlled by human + + Returns + ------- + None. + + ''' + + init_section = self.section_list[0] + ego_vehicle = VehicleControlFreeway(self.env, init_section.ego_vehicle, self.env.delta_seconds, allow_collision) + ego_uniquename = init_section.ego_vehicle["uniquename"] + ego_bb = init_section.ego_vehicle["bounding_box"] + left_follow_vehicle = [] + subject_follow_vehicle = [] + left_lead_vehicle = [] + subject_lead_vehicle = [] + + # check whether ego vehicle comes to destination + ego_end = False + + # create vehicle control object + for vehicle_config in init_section.left_follow_vehicle: + left_follow_vehicle.append(LeadFollowVehicleControl(self.env, vehicle_config, self.env.delta_seconds,allow_collision)) + + for vehicle_config in init_section.subject_follow_vehicle: + subject_follow_vehicle.append(LeadFollowVehicleControl(self.env, vehicle_config, self.env.delta_seconds,allow_collision)) + + for vehicle_config in init_section.left_lead_vehicle: + left_lead_vehicle.append(LeadFollowVehicleControl(self.env, vehicle_config, self.env.delta_seconds,allow_collision)) + + for vehicle_config in init_section.subject_lead_vehicle: + subject_lead_vehicle.append(LeadFollowVehicleControl(self.env, vehicle_config, self.env.delta_seconds,allow_collision)) + + + # store the current section that is functioning + curr_section = init_section + self.section_list.pop(0) + + # if enable_human_control, get the ego vehicle from the environment + if enable_human_control: + human_control_server = HumanEgoControlServer() # create the server for receiving the human command + spectator_mode = "human_driving" + + if self.RECORD_ENABLE: + # record 1: data on an plain text file + timestr = time.strftime("%Y%m%d-%H%M%S") + file = open("../data_collection/FrW" + timestr + ".txt", "w+" ) + + filename="../data_collection/FrW" + timestr + ".xlsx" + print("start freeway recordings: ") + world_snapshot = self.env.world.get_snapshot() + tm = world_snapshot.timestamp + file.write("the experiment starts from " + str(round(tm.elapsed_seconds, 3)) + "(seconds)\n") + + + # record 2: data on an excel file + header_row = ['timestamp(sec)'] + for key in self.env.vehicle_dict.keys(): + key = key[8:] + header_row += [key+ '-lane_id', key+'-location_x(m)', key+'-location_y(m)', key+'location_z(m)', key+'-roll(degrees)', key+'-pitch(degrees)', key+'yaw(degrees)', + key+'-velocity_x(m/s)', key+'-velocity_y(m/s)', key+'velocity_z(m/s)', key+'-acceleration_x(m/s2)', key+'-acceleration_y(m/s2)', key+'acceleration_z(m/s2)'] + try: + wb = load_workbook(filename) + ws = wb.worksheets[0] + except FileNotFoundError: + wb = Workbook() + ws = wb.active + ws.append(header_row) + wb.save(filename) + + # main loop for control + while True: + if self.RECORD_ENABLE: + world_snapshot = self.env.world.get_snapshot() + ego_id = (int)(ego_uniquename.split("_")[1]) + ego_actor = world_snapshot.find(ego_id) + + map = self.env.world.get_map() + tm = world_snapshot.timestamp + file.write("time: " + str(round(tm.elapsed_seconds, 3))+"(seconds)\n") + + data = [str(round(tm.elapsed_seconds, 3))] + for key in self.env.vehicle_dict.keys(): + vehicle_data = [] + id = (int)(key.split("_")[1]) + actor = world_snapshot.find(id) + if(id == ego_id): + file.write("ego id: " + key + "\n") + else: + file.write("vehicle id: " + key + "\n") + + actor_location = actor.get_transform().location + lane = map.get_waypoint(actor_location).lane_id + file.write("lane: " + str(lane) + "\n") + vehicle_data+=[lane] + + actor_transform = actor.get_transform() + x = round(actor_transform.location.x, 2) + y = round(actor_transform.location.y, 2) + z = round(actor_transform.location.z, 2) + vehicle_data+=[x, y, z] + file.write("location: x=" + str(x) + " y=" + str(y) + " z=" +str(z) + "(meters)\n" ) + + x = round(actor_transform.rotation.roll, 2) + y = round(actor_transform.rotation.pitch, 2) + z = round(actor_transform.rotation.yaw, 2) + vehicle_data+=[x, y, z] + file.write("Rotation: roll=" + str(x) + " pitch=" + str(y) + " yaw=" +str(z) + "(degrees)\n") + + actor_velocity = actor.get_velocity() + x = round(actor_velocity.x, 2) + y = round(actor_velocity.y, 2) + z = round(actor_velocity.z, 2) + vehicle_data+=[x, y, z] + file.write("Velocity: x=" + str(x) + " y=" + str(y) + " z=" +str(z) + "(m/s)\n") + + actor_acceleration = actor.get_acceleration() + x = round(actor_acceleration.x, 2) + y = round(actor_acceleration.y, 2) + z = round(actor_acceleration.z, 2) + vehicle_data+=[x, y, z] + file.write("Acceleration: x=" + str(x) + " y=" + str(y) + " z=" +str(z) + "(m/s2)\n") + data+=vehicle_data + + ws.append(data) + wb.save(filename) + + + + self.env.world.tick() + # update the distance between vehicles after each tick + self.env.update_vehicle_distance() + # change spectator view + + if self.env.vehicle_available(ego_uniquename) and spectator_mode == "first_person" : + spectator_vehicle_transform = self.env.get_transform_3d(ego_uniquename) + spectator_transform = get_ego_spectator(spectator_vehicle_transform,distance = -40) + self.spectator.set_transform(spectator_transform) + + if self.env.vehicle_available(ego_uniquename) and spectator_mode == "human_driving" : + spectator_vehicle_transform = self.env.get_transform_3d(ego_uniquename) + spectator_transform = get_ego_driving_spectator(spectator_vehicle_transform, ego_bb) + self.spectator.set_transform(spectator_transform) + + # section based control + + # get ego vehicle transform + if self.env.vehicle_available(ego_uniquename): + ego_transform = self.env.get_transform_3d(ego_uniquename) + + + # update the distance with ego vehicle and + # update the elapsed time of the current section + local_time = curr_section.tick() + + for vehicle in left_follow_vehicle: + vehicle.update_distance_with_ego(ego_transform) # update the distance with ego vehicle + vehicle.update_local_time(local_time) # update the local time, change lane or keep distance accordingly + + for vehicle in subject_follow_vehicle: + vehicle.update_distance_with_ego(ego_transform) # update the distance with ego vehicle + vehicle.update_local_time(local_time) # update the local time, change lane or keep distance accordingly + + for vehicle in left_lead_vehicle: + vehicle.update_distance_with_ego(ego_transform) # update the distance with ego vehicle + vehicle.update_local_time(local_time) # update the local time, change lane or keep distance accordingly + + for vehicle in subject_lead_vehicle: + vehicle.update_distance_with_ego(ego_transform) # update the distance with ego vehicle + vehicle.update_local_time(local_time) # update the local time, change lane or keep distance accordingly + + + + + + # apply control to vehicles + if not ego_end: + if not enable_human_control: + ego_end = ego_vehicle.pure_pursuit_control_wrapper() + else: + # get control from human + human_command = human_control_server.get_human_command() + # format the command into carla.VehicleControl + ego_vehicle_control = carla.VehicleControl(throttle = human_command[0] ,steer=human_command[1],brake = human_command[2]) + # apply control to the vehicle + self.env.apply_vehicle_control(ego_uniquename , ego_vehicle_control) + + ego_end = ego_vehicle.fake_pure_pursuit_control_wrapper() + + ''' + print("--------") + print("len(left_follow_vehicle) == ", len(left_follow_vehicle)) + print("len(subject_follow_vehicle) == ", len(subject_follow_vehicle)) + print("len(left_lead_vehicle) == ", len(left_lead_vehicle)) + print("len(subject_lead_vehicle) == ", len(subject_lead_vehicle)) + ''' + + for jj in range(len(left_follow_vehicle) - 1,-1,-1): + vehicle = left_follow_vehicle[jj] + end_trajectory = vehicle.pure_pursuit_control_wrapper() + if end_trajectory: + left_follow_vehicle.pop(jj) + + for jj in range(len(subject_follow_vehicle) - 1,-1,-1): + vehicle = subject_follow_vehicle[jj] + end_trajectory = vehicle.pure_pursuit_control_wrapper() + if end_trajectory: + subject_follow_vehicle.pop(jj) + + for jj in range(len(left_lead_vehicle) - 1,-1,-1): + vehicle = left_lead_vehicle[jj] + end_trajectory = vehicle.pure_pursuit_control_wrapper() + if end_trajectory: + left_lead_vehicle.pop(jj) + + for jj in range(len(subject_lead_vehicle) - 1,-1,-1): + vehicle = subject_lead_vehicle[jj] + end_trajectory = vehicle.pure_pursuit_control_wrapper() + if end_trajectory: + subject_lead_vehicle.pop(jj) + + if ego_end and len(left_follow_vehicle) == 0 and len(subject_follow_vehicle) == 0 and len(left_lead_vehicle) == 0 and len(subject_lead_vehicle) == 0: + # exit simulation when all vehicles have arrived at their destination + break + + + # check whether the subject vehicle is in the next section, if that's the case, change the curr_section + # and apply section based commands to each vehicle + if len(self.section_list) > 0: # there still exists unvisited section(s) + # get the new ego transform + if self.env.vehicle_available(ego_uniquename): + ego_transform = self.env.get_transform_3d(ego_uniquename) + if self.section_list[0].section_start(ego_transform): + curr_section = self.section_list.pop(0) # use the new section; remove this section from wait list + + # apply section based vehicle settings + for ii in range(len(left_follow_vehicle)): + vehicle = left_follow_vehicle[ii] + command, command_start_time = curr_section.get_full_path_vehicle_local_setting("follow","left",ii) + if command != None: + vehicle.command = command + vehicle.command_start_time = command_start_time + + for ii in range(len(subject_follow_vehicle)): + vehicle = subject_follow_vehicle[ii] + command, command_start_time = curr_section.get_full_path_vehicle_local_setting("follow","subject",ii) + if command != None: + vehicle.command = command + vehicle.command_start_time = command_start_time + + for ii in range(len(left_lead_vehicle)): + vehicle = left_lead_vehicle[ii] + command, command_start_time = curr_section.get_full_path_vehicle_local_setting("lead","left",ii) + if command != None: + vehicle.command = command + vehicle.command_start_time = command_start_time + + for ii in range(len(subject_lead_vehicle)): + vehicle = subject_lead_vehicle[ii] + command, command_start_time = curr_section.get_full_path_vehicle_local_setting("lead","subject",ii) + if command != None: + vehicle.command = command + vehicle.command_start_time = command_start_time + + file.close() + + + # private methods + def _create_simulation_env(self): + # create the freeway environment + # this function is the counterpart of the "create_intersections" + # function for intersection backend + + self.section_list = [] # list for all sections + self.trajectory_ref_point_list = [] # list for full path trajectory waypoints + + initial_section = InitSection(self.env, self.subject_point_list[0]) # create the initial intersection + self.section_list.append(initial_section) + self.trajectory_ref_point_list += initial_section.get_section_trajectory_points() + + if self.DEBUG_SECTION: + trajectory_pt = initial_section.get_section_trajectory_points() + for pt in trajectory_pt: + loc1 = pt.transform.location + self.env.world.debug.draw_point(loc1, size = 0.05, color = red, life_time=0.0, persistent_lines=True) + + + for ii in range(1, self.number_of_sections): + normal_section = Section(self.env, self.subject_point_list[ii]) + self.section_list.append(normal_section) + self.trajectory_ref_point_list += normal_section.get_section_trajectory_points() + if self.DEBUG_SECTION: + trajectory_pt = normal_section.get_section_trajectory_points() + for pt in trajectory_pt: + loc1 = pt.transform.location + self.env.world.debug.draw_point(loc1, size = 0.05, color = red, life_time=0.0, persistent_lines=True) + + + if self.DEBUG_SUBJECTPOINTS: + color = green + for ii in range(len(self.subject_point_list)): + loc1 = self.subject_point_list[ii].transform.location + self.env.world.debug.draw_point(loc1, size = 0.1, color = color, life_time=0.0, persistent_lines=True) + + + # connect all reference points for all sections + full_trajectory = generate_path(self.env, self.trajectory_ref_point_list[0] , self.trajectory_ref_point_list[1], waypoint_separation = 4) + for ii in range(1, len(self.trajectory_ref_point_list) - 1): + trajectory = generate_path(self.env, self.trajectory_ref_point_list[ii] , self.trajectory_ref_point_list[ii + 1], waypoint_separation = 4) + full_trajectory += trajectory[1:] + + self.section_subject_trajectory = full_trajectory + ''' + full_trajectory = [] + for pt in self.trajectory_ref_point_list: + location = pt.transform.location + full_trajectory.append((location.x,location.y)) + ''' + + # create smoothed trajectory and reference speed list + whole_trajectory = [((pt[0],pt[1]),self.navigation_speed) for pt in full_trajectory] + + smoothed_full_trajectory, ref_speed_list = get_trajectory(whole_trajectory) + self.smoothed_full_trajectory = smoothed_full_trajectory + self.ref_speed_list = ref_speed_list + + if self.DEBUG_TRAJECTORY: + color = cyan + for ii in range(1,len(smoothed_full_trajectory)): + loc1 = carla.Location(x = smoothed_full_trajectory[ii - 1][0], y = smoothed_full_trajectory[ii - 1][1], z = 10.0) + loc2 = carla.Location(x = smoothed_full_trajectory[ii][0], y = smoothed_full_trajectory[ii][1], z = 10.0) + self.env.world.debug.draw_arrow(loc1, loc2, thickness = 0.05, arrow_size = 0.1, color = color, life_time=0.0, persistent_lines=True) + + + # get the trajectory for the left lane + self.left_lane_waypoints_list = [] + for waypoint in self.trajectory_ref_point_list: + left_waypoint = self._get_left_waypoint(waypoint) + self.left_lane_waypoints_list.append(left_waypoint) + + if self.DEBUG_SECTION: + for ii in range(1,len(self.left_lane_waypoints_list)): + loc1 = self.left_lane_waypoints_list[ii - 1].transform.location + loc2 = self.left_lane_waypoints_list[ii].transform.location + self.env.world.debug.draw_arrow(loc1, loc2, thickness = 0.1, arrow_size = 0.2, color = blue, life_time=0.0, persistent_lines=True) + + + left_full_trajectory = [] + for pt in self.left_lane_waypoints_list: + location = pt.transform.location + left_full_trajectory.append((location.x,location.y)) + + + left_whole_trajectory = [((pt[0],pt[1]),self.navigation_speed) for pt in left_full_trajectory] + + left_smoothed_full_trajectory, left_ref_speed_list = get_trajectory(left_whole_trajectory) + self.left_smoothed_full_trajectory = left_smoothed_full_trajectory + self.left_ref_speed_list = left_ref_speed_list + + if self.DEBUG_TRAJECTORY: + color = blue + for ii in range(1,len(left_smoothed_full_trajectory)): + loc1 = carla.Location(x = left_smoothed_full_trajectory[ii - 1][0], y = left_smoothed_full_trajectory[ii - 1][1], z = 10.0) + loc2 = carla.Location(x = left_smoothed_full_trajectory[ii][0], y = left_smoothed_full_trajectory[ii][1], z = 10.0) + self.env.world.debug.draw_arrow(loc1, loc2, thickness = 0.05, arrow_size = 0.1, color = color, life_time=0.0, persistent_lines=True) + + + + def _get_unit_left_vector(self,yaw): + # get the right vector + right_yaw = (yaw + 90) % 360 + rad_yaw = math.radians(right_yaw) + right_vector = [math.cos(rad_yaw),math.sin(rad_yaw)] + right_vector = right_vector / np.linalg.norm(right_vector) + return right_vector + + + def _get_left_waypoint(self, curr_waypoint): + # get the point to the left of the current one + left_shift = 3.0 + + curr_location = curr_waypoint.transform.location + ref_yaw = curr_waypoint.transform.rotation.yaw + left_vector = self._get_unit_left_vector(ref_yaw) + + new_location = carla.Location(x = curr_location.x - left_shift * left_vector[0], y = curr_location.y - left_shift * left_vector[1], z = curr_location.z) + + left_waypoint = self.carla_map.get_waypoint(new_location) + return left_waypoint + + def _get_section_subject_points(self, initial_point): + # get the section subject points + + world_pose_list = [] + + initial_point_raw = carla.Location(initial_point[0], initial_point[1], z = 10.0) + initial_way_point = self.carla_map.get_waypoint(initial_point_raw) + + world_pose_list.append(initial_way_point) + + curr_waypoint = initial_way_point + + for ii in range(self.number_of_sections - 1): + + for jj in range(100): + next_waypoint = self._get_next_waypoint(curr_waypoint, distance = 4) + curr_waypoint = next_waypoint + world_pose_list.append(curr_waypoint) + + return world_pose_list + + def _get_next_waypoint(self,curr_waypoint,distance = 4): + ''' + + + Parameters + ---------- + curr_waypoint : carla.Waypoint + current waypoint. + distance : float, optional + "distance" between current waypoint and target waypoint . The default is 10. + + Returns + ------- + next_waypoint : carla.Waypoint + next waypoint, "distance" away from curr_waypoint, in the direction of the current way point + ''' + forward_vector = curr_waypoint.transform.get_forward_vector() + + location = curr_waypoint.transform.location + raw_spawn_point = carla.Location(x = location.x + distance * forward_vector.x , y = location.y + distance * forward_vector.y , z = location.z + 0.1) + + next_waypoint = self.carla_map.get_waypoint(raw_spawn_point) + return next_waypoint + +def main(): + try: + + # create the CARLA_ENV helper + client = carla.Client("localhost",2000) + client.set_timeout(10.0) + world = client.load_world('Town04') # use Town 04 which contains a freeway + + # default the weather to be a fine day + weather = carla.WeatherParameters( + cloudiness=10.0, + precipitation=0.0, + sun_altitude_angle=90.0) + world.set_weather(weather) + + spectator = world.get_spectator() + spectator.set_transform(carla.Transform(carla.Location(x=-170, y=-151, z=116.5), carla.Rotation(pitch=-33, yaw= 56.9, roll=0.0))) + + # create the environment + env = CARLA_ENV(world) + time.sleep(2) # sleep for 2 seconds, wait the initialization to finish + + # create a 14 section environment (support up to 7) + freewayenv = FreewayEnv(env,7) + + # add ego vehicle + freewayenv.add_ego_vehicle() + freewayenv.edit_ego_vehicle(vehicle_color = '255,255,255') + + # add 2 lead vehicle and 2 follow vehicle + ''' + name0 = freewayenv.add_full_path_vehicle(gap = 20.0, vehicle_type = "lead", choice = "subject", command = "lane", command_start_time = 2.0) + time.sleep(1.0) + name0 = freewayenv.add_full_path_vehicle(gap = 20.0, vehicle_type = "lead", choice = "subject", command = "lane", command_start_time = 2.0) + time.sleep(1.0) + name1 = freewayenv.add_full_path_vehicle(gap = 20.0, vehicle_type = "lead", choice = "left", command = "lane", command_start_time = 2.0) + time.sleep(1.0) + name1 = freewayenv.add_full_path_vehicle(gap = 20.0, vehicle_type = "lead", choice = "left", command = "lane", command_start_time = 2.0) + time.sleep(1.0) + name2 = freewayenv.add_full_path_vehicle(vehicle_type = "follow", choice = "subject", command = "lane", command_start_time = 3.0) + time.sleep(1.0) + name2 = freewayenv.add_full_path_vehicle(vehicle_type = "follow", choice = "subject", command = "lane", command_start_time = 3.0) + time.sleep(1.0) + name3 = freewayenv.add_full_path_vehicle(vehicle_type = "follow", choice = "left", command = "lane", command_start_time = 3.0) + time.sleep(1.0) + name3 = freewayenv.add_full_path_vehicle(vehicle_type = "follow", choice = "left", command = "lane", command_start_time = 3.0) + time.sleep(1.0) + name4 = freewayenv.add_full_path_vehicle(gap = 20.0, vehicle_type = "lead", choice = "subject", command = "lane", command_start_time = 3.0) + time.sleep(1.0) + name5 = freewayenv.add_full_path_vehicle(vehicle_type = "follow", choice = "subject", command = "lane", command_start_time = 0.0) + time.sleep(1.0) + + freewayenv.remove_full_path_vehicle(name4) + freewayenv.remove_full_path_vehicle(name5) + + + + + ''' + name0 = freewayenv.add_full_path_vehicle(gap = 20.0, vehicle_type = "lead", choice = "subject") + name1 = freewayenv.add_full_path_vehicle(gap = 20.0, vehicle_type = "lead", choice = "left") + name2 = freewayenv.add_full_path_vehicle(vehicle_type = "follow", choice = "subject") + name3 = freewayenv.add_full_path_vehicle(vehicle_type = "follow", choice = "left") + name4 = freewayenv.add_full_path_vehicle(gap = 20.0, vehicle_type = "lead", choice = "subject") + name5 = freewayenv.add_full_path_vehicle(vehicle_type = "follow", choice = "subject") + + # get bounding box + bb = freewayenv.get_vehicle_bounding_box(name1) + print("bb.x = %f, bb.y = %f, bb.z = %f" % (bb.x,bb.y,bb.z)) + + + # adjust the lead and follow vehicle settings in the third section + freewayenv.edit_normal_section_setting(section_id = 3, vehicle_type = "lead", choice = "subject", vehicle_index = 0,command = "distance") + freewayenv.edit_normal_section_setting(section_id = 3, vehicle_type = "lead", choice = "left", vehicle_index = 0,command = "distance") + freewayenv.edit_normal_section_setting(section_id = 3, vehicle_type = "follow", choice = "subject", vehicle_index = 0,command = "distance") + freewayenv.edit_normal_section_setting(section_id = 3, vehicle_type = "follow", choice = "left", vehicle_index = 0,command = "distance") + freewayenv.edit_normal_section_setting(section_id = 3, vehicle_type = "lead", choice = "subject", vehicle_index = 1,command = "distance") + freewayenv.edit_normal_section_setting(section_id = 3, vehicle_type = "follow", choice = "subject", vehicle_index = 1,command = "distance") + + # adjust the lead and follow vehicle settings in the fourth section + freewayenv.edit_normal_section_setting(section_id = 4, vehicle_type = "lead", choice = "subject", vehicle_index = 0,command = "speed", command_start_time = 0.0) + freewayenv.edit_normal_section_setting(section_id = 4, vehicle_type = "lead", choice = "left", vehicle_index = 0,command = "speed", command_start_time = 0.0) + freewayenv.edit_normal_section_setting(section_id = 4, vehicle_type = "follow", choice = "subject", vehicle_index = 0,command = "speed", command_start_time = 0.0) + freewayenv.edit_normal_section_setting(section_id = 4, vehicle_type = "follow", choice = "left", vehicle_index = 0,command = "speed", command_start_time = 0.0) + freewayenv.edit_normal_section_setting(section_id = 4, vehicle_type = "lead", choice = "subject", vehicle_index = 1,command = "lane") + freewayenv.edit_normal_section_setting(section_id = 4, vehicle_type = "follow", choice = "subject", vehicle_index = 1,command = "lane") + + # adjust the lead and follow vehicle settings in the sixth section + freewayenv.edit_normal_section_setting(section_id = 6, vehicle_type = "lead", choice = "subject", vehicle_index = 0,command = "lane") + freewayenv.edit_normal_section_setting(section_id = 6, vehicle_type = "lead", choice = "left", vehicle_index = 0,command = "lane") + freewayenv.edit_normal_section_setting(section_id = 6, vehicle_type = "follow", choice = "subject", vehicle_index = 0,command = "lane") + freewayenv.edit_normal_section_setting(section_id = 6, vehicle_type = "follow", choice = "left", vehicle_index = 0,command = "lane") + freewayenv.edit_normal_section_setting(section_id = 6, vehicle_type = "lead", choice = "subject", vehicle_index = 1,command = "lane") + freewayenv.edit_normal_section_setting(section_id = 6, vehicle_type = "follow", choice = "subject", vehicle_index = 1,command = "lane") + # test remove vehicle + #freewayenv.remove_full_path_vehicle(name4) + + + + # test editing vehicle settings + freewayenv.edit_full_path_vehicle_init_setting(name0, gap = 25.0, vehicle_type = "lead", choice = "subject", vehicle_color = '0,0,0') + freewayenv.edit_full_path_vehicle_init_setting(name1, gap = 25.0, vehicle_type = "lead", choice = "left", vehicle_color = '255,255,255') + #freewayenv.edit_full_path_vehicle_init_setting(name2, gap = 25.0, vehicle_type = "follow", choice = "subject", vehicle_color = '0,0,0') + #freewayenv.edit_full_path_vehicle_init_setting(name3, gap = 25.0, vehicle_type = "follow", choice = "left", vehicle_color = '255,255,255') + + + freewayenv.SectionBackend(spectator_mode = "first_person",allow_collision = False) + finally: + time.sleep(10) + env.destroy_actors() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/legacy/gui_interface/backend/section_init_definition.py b/legacy/gui_interface/backend/section_init_definition.py new file mode 100644 index 0000000..78c52e4 --- /dev/null +++ b/legacy/gui_interface/backend/section_init_definition.py @@ -0,0 +1,594 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Wed Jul 22 11:24:10 2020 + +@author: shijiliu +""" + +## MAJORITY OF THIS IS JUST REPEATED FROM THE PREVIOUS FILE + +import sys +sys.path.append("..") + +import carla +from backend.carla_env import CARLA_ENV +import math +import time +import numpy as np +from configobj import ConfigObj +from backend.generate_path_omit_regulation import generate_path +from backend.intersection_definition import smooth_trajectory, get_trajectory +from scipy.interpolate import UnivariateSpline +import copy + +from backend.section_definition import Section + + + +# define a class for defining the initial section +# add vehicles is allowed in this section +class InitSection(Section): + def __init__(self, env, world_waypoint): + super().__init__(env, world_waypoint) + self.left_ref_waypoint = self._get_left_waypoint(self.subject_waypoint) + + def get_full_path_trajectory(self, subject_trajectory, subject_ref_speed, left_trajectory, left_ref_speed, ref_speed, max_speed,min_speed): + ''' + + + Parameters + ---------- + subject_trajectory : list, [(float, float), ... ] + the subject trajectory. + subject_ref_speed : list + reference speed for subject trajectory + left_trajectory : list, [(float, float), ... ] + the left trajectory. + left_ref_speed : list + reference speed for left trajectory + ref_speed : float + the average of the max and min speed + max_speed : float + the max speed of the vehicle + min_speed : float + the minimum speed of the vehicle + + Returns + ------- + None. + + ''' + self.subject_trajectory = subject_trajectory + self.subject_ref_speed = subject_ref_speed + self.subject_max_speed_list = subject_ref_speed / ref_speed * max_speed + self.subject_min_speed_list = subject_ref_speed / ref_speed * min_speed + self.left_trajectory = left_trajectory + self.left_ref_speed = left_ref_speed + self.left_max_speed_list = left_ref_speed / ref_speed * max_speed + self.left_min_speed_list = left_ref_speed / ref_speed * min_speed + + def add_ego_vehicle(self, model_name = "vehicle.tesla.model3", safety_distance = 15.0, vehicle_color = None): + ''' + add ego vehicle to the initial intersection + according to the user case, the ego vehicle will follow the + subject lane all the way with constant speed + + Parameters + ---------- + model_name : string, optional + vehicle type. The default is "vehicle.tesla.model3". + safety_distance : float, optional + smallest distance between this vehicle and vehicle ahead + vehicle_color : string + the RGB representation of the vehicle color. e.g. '255,255,255' + + Returns + ------- + uniquename : string + the name of the vehicle + + ''' + vehicle = ConfigObj() + vehicle["model"] = model_name + vehicle["safety_distance"] = safety_distance + + new_ref_waypoint = self.subject_waypoint + spawn_transform = new_ref_waypoint.transform + spawn_location = spawn_transform.location + spawn_rotation = spawn_transform.rotation + + spawn_location = carla.Location(x = spawn_location.x, y = spawn_location.y, z = spawn_location.z + 0.2) + + uniquename = self.env.spawn_vehicle(model_name = model_name,spawn_point = carla.Transform(spawn_location,spawn_rotation), color = vehicle_color) + vehicle["uniquename"] = uniquename + vehicle["ref_waypoint"] = new_ref_waypoint + vehicle["location"] = spawn_transform.location + vehicle["rotation"] = spawn_transform.rotation + + if vehicle_color == None: + vehicle["vehicle_color"] = vehicle_color + else: + vehicle["vehicle_color"] = vehicle_color.replace(',',';') # replace , by ; to avoid error when importing from file + + vehicle["trajectory"] = self.subject_trajectory + vehicle["ref_speed_list"] = self.subject_ref_speed + + new_bb = self.env.get_vehicle_bounding_box(uniquename) + vehicle["bounding_box"] = new_bb + vehicle["vehicle_type"] = "ego" + + # additional settings that's necessary for using the VehicleControl class + vehicle["stop_choice"] = None + vehicle["penetrate_distance"] = None + vehicle["stop_ref_point"] = None + + vehicle["gap"] = None + vehicle["command"] = None + vehicle["obey_traffic_lights"] = False + vehicle["traffic_light"] = False + vehicle["run"] = True + vehicle["choice"] = None + + self.ego_vehicle = vehicle + return uniquename + + + + + + def edit_ego_vehicle(self, model_name = "vehicle.tesla.model3", safety_distance = 15.0, vehicle_color = None): + ''' + edit the ego vheicle setting by delete the original ego vehicle and add a new one + + Parameters + ---------- + model_name : string, optional + vehicle type. The default is "vehicle.tesla.model3". + safety_distance : float, optional + smallest distance between this vehicle and vehicle ahead + vehicle_color : string + the RGB representation of the vehicle color. e.g. '255,255,255' + + Returns + ------- + uniquename : string + the name of the vehicle + + ''' + # get the original uniquename + original_uniquename = self.ego_vehicle["uniquename"] + + # remove the ego vehicle + self.env.destroy_vehicle(original_uniquename) + self.ego_vehicle = None + + # add the new ego vehicle + uniquename = self.add_ego_vehicle(model_name = model_name, safety_distance = safety_distance, vehicle_color = vehicle_color) + return uniquename + + def remove_full_path_vehicle(self, uniquename): + ''' + remove a full path vehicle based on its uniquename + + Parameters + ---------- + uniquename : string + name of the vehicle. + + Returns + ------- + Bool : whether the given vehicle is found and removed + + vehicle_type : string + the vehicle type, valid values are "lead", "follow" + + choice : string + the lane choice, valid values are "subject", "left" + + index : int + the index of the vehicle in a specific lane + + ''' + vehicle_set = self.subject_lead_vehicle + for jj in range(len(vehicle_set) - 1,-1,-1): + vehicle = vehicle_set[jj] + if vehicle["uniquename"] == uniquename: + self.env.destroy_vehicle(uniquename) + vehicle_set.pop(jj) + return True, "lead", "subject", jj + + vehicle_set = self.subject_follow_vehicle + for jj in range(len(vehicle_set) - 1,-1,-1): + vehicle = vehicle_set[jj] + if vehicle["uniquename"] == uniquename: + self.env.destroy_vehicle(uniquename) + vehicle_set.pop(jj) + return True, "follow", "subject", jj + + vehicle_set = self.left_lead_vehicle + for jj in range(len(vehicle_set) - 1,-1,-1): + vehicle = vehicle_set[jj] + if vehicle["uniquename"] == uniquename: + self.env.destroy_vehicle(uniquename) + vehicle_set.pop(jj) + return True, "lead", "left", jj + + vehicle_set = self.left_follow_vehicle + for jj in range(len(vehicle_set) - 1,-1,-1): + vehicle = vehicle_set[jj] + if vehicle["uniquename"] == uniquename: + self.env.destroy_vehicle(uniquename) + vehicle_set.pop(jj) + return True, "follow", "left", jj + + return False, None, None, None + + + def add_full_path_vehicle(self, model_name = "vehicle.tesla.model3", vehicle_type ="lead", choice = "subject", command = "speed", command_start_time = 0.0, gap = 10.0, safety_distance = 15.0, lead_follow_distance = 20.0, vehicle_color = None): + ''' + add full path vehicle + + Parameters + ---------- + model_name : string, optional + vehicle type. The default is "vehicle.tesla.model3". + vehicle_type : string, optional + the vehicle type, valid values : "lead", "follow". The default is "lead". + choice : string, optional + the lane choice, valid values are "subject", "left". The default is "subject". + command : string, optional + the command the vehicle is going to execute in this section. Valid values: "speed", "lane", "distance". The default is "speed". + command_start_time : string, optional + the time at which the command should be executed. The default is 0.0. + gap : float, optional + the gap between the vehicle and the one in the front of it when adding. The default is 10.0, unit: meter + safety_distance : float, optional + smallest distance between 2 vehicles when simulation is going. The default is 15.0, unit: meter + vehicle_color : string + the RGB representation of the vehicle color. e.g. '255,255,255' + + Returns + ------- + uniquename : string + the name of the vehicle + + + ''' + # create configuration file for vehicle + vehicle = ConfigObj() + vehicle["model"] = model_name + vehicle["safety_distance"] = safety_distance + vehicle["gap"] = gap + vehicle["command"] = command + vehicle["command_start_time"] = command_start_time + vehicle["run"] = True + vehicle["choice"] = choice + vehicle["current_lane"] = choice # which lane the vehicle is currently in + vehicle["vehicle_type"] = vehicle_type + vehicle["lead_follow_distance"] = lead_follow_distance + + vehicle["stop_choice"] = None + vehicle["penetrate_distance"] = None + vehicle["stop_ref_point"] = None + vehicle["obey_traffic_lights"] = False + vehicle["traffic_light"] = False + + vehicle_set = None + ref_waypoint = None + vehicle["trajectory"] = None + vehicle["ref_speed_list"] = None + + # get the vehicle set by input parameters, so as to create add the vehicle + if vehicle_type == "lead" and choice == "subject": + vehicle_set = self.subject_lead_vehicle + ref_waypoint = self.subject_waypoint + lane_direction = 1 # positive direction for lead + vehicle["trajectory"] = self.subject_trajectory + vehicle["ref_speed_list"] = self.subject_ref_speed + + elif vehicle_type == "lead" and choice == "left": + vehicle_set = self.left_lead_vehicle + ref_waypoint = self.left_ref_waypoint + lane_direction = 1 # positive direction for lead + vehicle["trajectory"] = self.left_trajectory + vehicle["ref_speed_list"] = self.left_ref_speed + + elif vehicle_type == "follow" and choice == "subject": + vehicle_set = self.subject_follow_vehicle + ref_waypoint = self.subject_waypoint + lane_direction = -1 # negative direction for follow + vehicle["trajectory"] = self.subject_trajectory + vehicle["ref_speed_list"] = self.subject_ref_speed + + elif vehicle_type == "follow" and choice == "left": + vehicle_set = self.left_follow_vehicle + ref_waypoint = self.left_ref_waypoint + lane_direction = -1 # negative direction for follow + vehicle["trajectory"] = self.left_trajectory + vehicle["ref_speed_list"] = self.left_ref_speed + + + + + # get the spawn location + if len(vehicle_set) != 0: + ref_waypoint = vehicle_set[-1]["ref_waypoint"] + bb = vehicle_set[-1]["bounding_box"] + + curr_length = self.env.get_vehicle_model_length(model_name) + + gap += bb.x / 2 + curr_length / 2 + + else: + bb = self.ego_vehicle["bounding_box"] + curr_length = self.env.get_vehicle_model_length(model_name) + gap += bb.x / 2 + curr_length / 2 + #print(gap) + + + forward_vector = ref_waypoint.transform.get_forward_vector() + + location = ref_waypoint.transform.location + raw_spawn_point = carla.Location(x = location.x + lane_direction * gap * forward_vector.x , y = location.y + lane_direction * gap * forward_vector.y , z = location.z + 0.1) + + new_ref_waypoint = self.carla_map.get_waypoint(raw_spawn_point) + + spawn_transform = new_ref_waypoint.transform + spawn_location = spawn_transform.location + spawn_rotation = spawn_transform.rotation + + spawn_location = carla.Location(x = spawn_location.x, y = spawn_location.y, z = spawn_location.z + 0.2) + + uniquename = self.env.spawn_vehicle(model_name = model_name,spawn_point = carla.Transform(spawn_location,spawn_rotation) , color = vehicle_color) + + vehicle["uniquename"] = uniquename + vehicle["ref_waypoint"] = new_ref_waypoint + vehicle["location"] = new_ref_waypoint.transform.location + vehicle["rotation"] = new_ref_waypoint.transform.rotation + + if vehicle_color == None: + vehicle["vehicle_color"] = vehicle_color + else: + vehicle["vehicle_color"] = vehicle_color.replace(',',';') # replace , by ; to avoid error when importing from file + + vehicle["subject_trajectory"] = self.subject_trajectory + vehicle["subject_ref_speed_list"] = self.subject_ref_speed + vehicle["subject_max_speed_list"] = self.subject_max_speed_list + vehicle["subject_min_speed_list"] = self.subject_min_speed_list + vehicle["left_trajectory"] = self.left_trajectory + vehicle["left_ref_speed_list"] = self.left_ref_speed + vehicle["left_max_speed_list"] = self.left_max_speed_list + vehicle["left_min_speed_list"] = self.left_min_speed_list + + new_bb = self.env.get_vehicle_bounding_box(uniquename) + vehicle["bounding_box"] = new_bb + + vehicle_set.append(vehicle) + + return uniquename + + def edit_full_path_vehicle(self, uniquename, vehicle_type, choice, model_name = "vehicle.tesla.model3", command = "speed", command_start_time = 0.0, gap = 10.0, safety_distance = 25.0, lead_follow_distance = 30.0, vehicle_color = None): + ''' + edit full path vehicle settings by deleting the original vehicle and then add a new one + + Parameters + ---------- + uniquename : string + the name of the vehicle + vehicle_type : string, + the vehicle type, valid values : "lead", "follow". + choice : string, optional + the lane choice, valid values are "subject", "left". + model_name : string, optional + vehicle type. The default is "vehicle.tesla.model3". + command : string, optional + the command the vehicle is going to execute in this section. Valid values: "speed", "lane", "distance". The default is "speed". + command_start_time : string, optional + the time at which the command should be executed. The default is 0.0. + gap : float, optional + the gap between the vehicle and the one in the front of it when adding. The default is 10.0, unit: meter + safety_distance : float, optional + smallest distance between 2 vehicles when simulation is going. The default is 15.0, unit: meter + vehicle_color : string + the RGB representation of the vehicle color. e.g. '255,255,255' + + Returns + ------- + new_uniquename : string + the new_name of the vehicle + + index: int + the index of the vehicle to be changed + + ''' + if vehicle_type == "lead" and choice == "subject": + vehicle_set = self.subject_lead_vehicle + lane_direction = 1 # positive direction for lead + + + elif vehicle_type == "lead" and choice == "left": + vehicle_set = self.left_lead_vehicle + lane_direction = 1 # positive direction for lead + + elif vehicle_type == "follow" and choice == "subject": + vehicle_set = self.subject_follow_vehicle + lane_direction = -1 # negative direction for follow + + elif vehicle_type == "follow" and choice == "left": + vehicle_set = self.left_follow_vehicle + lane_direction = -1 # negative direction for follow + + # get vehicle index in the given lane + index = 0 + original_gap = None + for vehicle in vehicle_set: + if vehicle["uniquename"] == uniquename: + original_gap = vehicle["gap"] + break + index += 1 + + # shift the vehicle + if original_gap != None: + shift_distance = original_gap - gap + self._shift_vehicles(shift_distance, vehicle_type = vehicle_type , choice = choice, index = index) + else: + print("return None in edit vehicle") + return None + + # remove the current vehicle, + # note that after removing the vehicle, index is pointing at the vehicle after the current one + removed, _, _, _ = self.remove_full_path_vehicle(uniquename) + if not removed: + print("vehicle not found") + return None + + # split the vehicle set + if vehicle_type == "lead" and choice == "subject": + vehicles_after_current = self.subject_lead_vehicle[index:] + self.subject_lead_vehicle = self.subject_lead_vehicle[:index] + + + elif vehicle_type == "lead" and choice == "left": + vehicles_after_current = self.left_lead_vehicle[index:] + self.left_lead_vehicle = self.left_lead_vehicle[:index] + + + elif vehicle_type == "follow" and choice == "subject": + vehicles_after_current = self.subject_follow_vehicle[index:] + self.subject_follow_vehicle = self.subject_follow_vehicle[:index] + + + elif vehicle_type == "follow" and choice == "left": + vehicles_after_current = self.left_follow_vehicle[index:] + self.left_follow_vehicle = self.left_follow_vehicle[:index] + + + # add the new vehicle + new_uniquename = self.add_full_path_vehicle(model_name = model_name, + vehicle_type = vehicle_type, + choice = choice, + command = command, + command_start_time = command_start_time, + gap = gap, safety_distance = safety_distance, + lead_follow_distance = lead_follow_distance, + vehicle_color = vehicle_color) + + # put back the vehicles after the current one + if vehicle_type == "lead" and choice == "subject": + self.subject_lead_vehicle += vehicles_after_current + + + elif vehicle_type == "lead" and choice == "left": + self.left_lead_vehicle += vehicles_after_current + + + elif vehicle_type == "follow" and choice == "subject": + self.subject_follow_vehicle += vehicles_after_current + + + elif vehicle_type == "follow" and choice == "left": + self.left_follow_vehicle += vehicles_after_current + + return new_uniquename, index + + + def _shift_vehicles(self, length, vehicle_type, choice, index = 0): + ''' + shift the vehicle starting at "index" in a specific lane specified by the + vehicle_type and choice + + Parameters + ---------- + length : float + the length these vehicles will be moved. Positive value moves vehicles away from the ego + vehicle_type : string, + the vehicle type, valid values : "lead", "follow". + choice : string, + the lane choice, valid values are "subject", "left". + index : int + the index of the vehicle inside a specific lane. default is 0 + + Returns + ------- + None. + + ''' + if vehicle_type == "lead" and choice == "subject": + vehicle_set = self.subject_lead_vehicle + lane_direction = 1 # positive direction for lead + + + elif vehicle_type == "lead" and choice == "left": + vehicle_set = self.left_lead_vehicle + lane_direction = 1 # positive direction for lead + + elif vehicle_type == "follow" and choice == "subject": + vehicle_set = self.subject_follow_vehicle + lane_direction = -1 # negative direction for follow + + elif vehicle_type == "follow" and choice == "left": + vehicle_set = self.left_follow_vehicle + lane_direction = -1 # negative direction for follow + + for ii in range(len(vehicle_set) - 1,index - 1,-1): + vehicle = vehicle_set[ii] + new_ref_waypoint = self._get_next_waypoint(vehicle["ref_waypoint"],distance = length * lane_direction) # get the new + # reference point + new_ref_location = new_ref_waypoint.transform.location + + spawn_location = carla.Location(x = new_ref_location.x, y = new_ref_location.y, z = new_ref_location.z + 0.1) + spawn_rotation = new_ref_waypoint.transform.rotation + + self.env.move_vehicle_location(vehicle["uniquename"],carla.Transform(spawn_location,spawn_rotation)) + vehicle["ref_waypoint"] = new_ref_waypoint + vehicle["location"] = spawn_location + vehicle["rotation"] = spawn_rotation + + + + def _get_unit_left_vector(self,yaw): + # get the right vector + right_yaw = (yaw + 90) % 360 + rad_yaw = math.radians(right_yaw) + right_vector = [math.cos(rad_yaw),math.sin(rad_yaw)] + right_vector = right_vector / np.linalg.norm(right_vector) + return right_vector + + + def _get_left_waypoint(self, curr_waypoint): + # get the point to the left of the current one + left_shift = 3.0 + + curr_location = curr_waypoint.transform.location + ref_yaw = curr_waypoint.transform.rotation.yaw + left_vector = self._get_unit_left_vector(ref_yaw) + + new_location = carla.Location(x = curr_location.x - left_shift * left_vector[0], y = curr_location.y - left_shift * left_vector[1], z = curr_location.z) + + left_waypoint = self.carla_map.get_waypoint(new_location) + return left_waypoint + + def _get_next_waypoint(self,curr_waypoint,distance = 10): + ''' + + + Parameters + ---------- + curr_waypoint : carla.Waypoint + current waypoint. + distance : float, optional + "distance" between current waypoint and target waypoint . The default is 10. + + Returns + ------- + next_waypoint : carla.Waypoint + next waypoint, "distance" away from curr_waypoint, in the direction of the current way point + ''' + forward_vector = curr_waypoint.transform.get_forward_vector() + + location = curr_waypoint.transform.location + raw_spawn_point = carla.Location(x = location.x + distance * forward_vector.x , y = location.y + distance * forward_vector.y , z = location.z + 0.1) + + next_waypoint = self.carla_map.get_waypoint(raw_spawn_point) + return next_waypoint \ No newline at end of file diff --git a/legacy/gui_interface/backend/section_vehicle_control.py b/legacy/gui_interface/backend/section_vehicle_control.py new file mode 100644 index 0000000..1641c2e --- /dev/null +++ b/legacy/gui_interface/backend/section_vehicle_control.py @@ -0,0 +1,329 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Thu Jul 23 10:15:47 2020 + +@author: shijiliu +""" + +import sys +sys.path.append("..") + +import carla +import matplotlib.pyplot as plt +import numpy as np +from collections import deque +import time +import math + +import control # the python-control package, install first + +from backend.intersection_definition import Intersection, get_traffic_lights +from backend.carla_env import CARLA_ENV # self-written class that provides help functions, should be in the same folder +from configobj import ConfigObj +from backend.multiple_vehicle_control import VehicleControl + +import copy + + +# color for debug use +red = carla.Color(255, 0, 0) +green = carla.Color(0, 255, 0) +blue = carla.Color(47, 210, 231) +cyan = carla.Color(0, 255, 255) +yellow = carla.Color(255, 255, 0) +orange = carla.Color(255, 162, 0) +white = carla.Color(255, 255, 255) + + +class VehicleControlFreeway(VehicleControl): + def __init__(self, env, vehicle_config, delta_seconds, allow_collision = True): + super().__init__(env, vehicle_config, delta_seconds, allow_collision = True) + + def _obey_safety_distance(self, current_ref_speed): + # override the _obey_safety_distance method + has_vehicle_in_front, distance = self.env.check_vehicle_in_front_freeway(self.model_uniquename, self.safety_distance) #self.env.check_vehicle_in_front_freeway(self.model_uniquename, self.safety_distance) + + if has_vehicle_in_front and abs(distance) < self.L / 2 + 1.0 and not self.allow_collision: + # if vehicle is about to collide with other vehicle and collision is not allowed + # set the vehicle velocity to 0 + abrupt_stop_vel = carla.Vector3D(x = 0,y = 0,z = 0) + self.env.set_vehicle_velocity(self.model_uniquename, abrupt_stop_vel) # set the velocity of vehicle + + + if has_vehicle_in_front: + #print("---") + #print("safety_distance: ", self.safety_distance) + #print("distance with previous vehicle ", distance) + return 0.0 + + return current_ref_speed + + + + + +class FullPathVehicleControl(VehicleControlFreeway): + def __init__(self, env, vehicle_config, delta_seconds, allow_collision = True): + super().__init__(env,vehicle_config,delta_seconds, allow_collision = True) + + # store the subject trajectory and left trajectory + self.subject_trajectory = copy.copy(self.vehicle_config["subject_trajectory"]) + self.subject_ref_speed = copy.copy(self.vehicle_config["subject_ref_speed_list"]) + self.subject_max_ref_speed = copy.copy(self.vehicle_config["subject_max_speed_list"]) + self.subject_min_ref_speed = copy.copy(self.vehicle_config["subject_min_speed_list"]) + + self.left_trajectory = copy.copy(self.vehicle_config["left_trajectory"]) + self.left_ref_speed = copy.copy(self.vehicle_config["left_ref_speed_list"]) + self.left_max_ref_speed = copy.copy(self.vehicle_config["left_max_speed_list"]) + self.left_min_ref_speed = copy.copy(self.vehicle_config["left_min_speed_list"]) + + # store the current lane of the vehicle + self.current_lane = copy.copy(self.vehicle_config["current_lane"]) + + # store the local time of the section the vehicle is in + self.local_time = 0.0 + + # store the time the vehicle is going to take command, (command should be "lane") + self.command_start_time = self.vehicle_config["command_start_time"] + + # store time steps after executing the change lane command + self.lane_change_step = 0 + + # store variable for deciding whether the vehicle should change lane + self.lane_change_available = False + + def _change_lane(self): + ''' + order the vehicle to change its lane + + Returns + ------- + None. + + ''' + # check whether the vehicle is safe to change lane + if not self.lane_change_available: + if self.current_lane == "subject": + has_vehicle_in_left, distance = self.env.check_vehicle_in_left(self.model_uniquename, safety_distance = 20) + + print("--------") + print("has_vehicle_in_left : ", has_vehicle_in_left) + print("distance: ", distance) + if has_vehicle_in_left: + if distance < 0: + self.ref_speed_list = copy.copy(self.subject_max_ref_speed) # accelerate to max speed if the close vehicle is behind + # the current vehicle + else: + self.ref_speed_list = copy.copy(self.subject_min_ref_speed) # deccelerate to min speed if the close vehicle is in + # front of the current vehicle + return # only change speed, don't change lane + else: + self.lane_change_available = True # no close vehicle in left lane, enable lane change + else: + # vehicle currently in right lane + has_vehicle_in_right, distance = self.env.check_vehicle_in_right(self.model_uniquename, safety_distance = 20) + + print("--------") + print("has_vehicle_in_right : ", has_vehicle_in_right) + print("distance: ", distance) + + if has_vehicle_in_right: + if distance < 0: + self.ref_speed_list = copy.copy(self.left_max_ref_speed) # accelerate to max speed if the close vehicle is behind + # the current vehicle + else: + self.ref_speed_list = copy.copy(self.left_min_ref_speed) # deccelerate to min speed if the close vehicle is in + # front of the current vehicle + return # only change speed, don't change lane + else: + self.lane_change_available = True # no close vehicle in left lane, enable lane change + + + + if self.lane_change_step == 0: + if self.current_lane == "subject": + self.trajectory = copy.copy(self.left_trajectory) + self.ref_speed_list = copy.copy(self.left_ref_speed) + self.current_lane = "left" + else: + self.trajectory = copy.copy(self.subject_trajectory) + self.ref_speed_list = copy.copy(self.subject_ref_speed) + self.current_lane = "subject" + self.lane_change_step += 1 + self.Lfc = 15.0 # set a large look ahead distance when changing the lane + elif self.lane_change_step > 0 and self.lane_change_step < 15: + self.lane_change_step += 1 + else: + self.command = "speed" # have changed the lane, change the mode to speed + self.lane_change_step = 0 + self.Lfc = 4.0 # change the look ahead distance back to its original value + self.lane_change_available = False + + def update_local_time(self, local_time): + # update the local time of the section the vehicle is in + # the time is used to decide whether the vehicle is going to + # change direction + self.local_time = local_time + if self.command == "lane" and self.command_start_time <= self.local_time: + self._change_lane() + + elif self.command == "distance" and self.command_start_time <= self.local_time: + self._keep_distance() + + else: + self._keep_speed() + + def _keep_speed(self): + if self.current_lane == "subject": + self.ref_speed_list = copy.copy(self.subject_ref_speed) # accelerate to the max speed + elif self.current_lane == "left": + self.ref_speed_list = copy.copy(self.left_ref_speed) # accelerate to the max speed + + def _keep_distance(self): + pass + +class LeadFollowVehicleControl(FullPathVehicleControl): + def __init__(self, env, vehicle_config, delta_seconds, allow_collision = True): + super().__init__(env, vehicle_config, delta_seconds, allow_collision = True) + + # store the vehicle type + self.vehicle_type = self.vehicle_config["vehicle_type"] + + + # store the constant distance + self.lead_follow_distance = self.vehicle_config["lead_follow_distance"] + if self.vehicle_type == "lead": + self.lead_follow_distance = - self.lead_follow_distance # if the vehicle is of "lead" type, the lead distance is negative + self.lead_follow_far_limit = 1.05 * self.lead_follow_distance + self.lead_follow_close_limit = 0.95 * self.lead_follow_distance + + + # store variable indicating whether the current vehicle and the ego vehicle are within desired range + self.within_desired_range = False + + def update_distance_with_ego(self, ego_transform): + ego_location = ego_transform.location + + # get local location + local_transform = self.env.get_transform_3d(self.model_uniquename) + local_location = local_transform.location + forward_vector = local_transform.get_forward_vector() + forward_vector_2d = np.array([forward_vector.x,forward_vector.y]) + unit_forward_vector_2d = forward_vector_2d / np.linalg.norm(forward_vector_2d) + + vec_loc_target = np.array([ego_location.x - local_location.x,ego_location.y - local_location.y]) + distance = np.dot(vec_loc_target,unit_forward_vector_2d) + + self.distance_with_ego = distance + + def _keep_distance(self): + # keep constant distance between the current vehicle and the ego vehicle + if self.vehicle_type == "lead": + # for lead vehicle, check if there exists vehicle in the back + has_vehicle_in_back, distance = self.env.check_vehicle_in_back_freeway(self.model_uniquename,abs(self.lead_follow_distance) * 4 + self.safety_distance * 4) # use a large distance + #print("---lead---") + #print("distance with back vehicle ", distance) + if has_vehicle_in_back: + if abs(self.distance_with_ego) > abs(distance) + 1.0: # there exists at least one vehicle between current vehicle and lead vehicle + self.command = "speed" # give up keeping constant distance, keep constant speed + print("give up distance mode") + if self.current_lane == "subject": + self.ref_speed_list = copy.copy(self.subject_ref_speed) # keep constant speed + elif self.current_lane == "left": + self.ref_speed_list = copy.copy(self.left_ref_speed) # keep constant speed + + # set the vehicle speed + local_transform = self.env.get_transform_3d(self.model_uniquename) + forward_vector = local_transform.get_forward_vector() + vehicle_velocity = carla.Vector3D(x = forward_vector.x * self.subject_ref_speed[0], y = forward_vector.y * self.subject_ref_speed[0], z = forward_vector.z * self.subject_ref_speed[0]) + self.env.set_vehicle_velocity(self.model_uniquename , vehicle_velocity) # set the velocity + return + + if self.distance_with_ego <= self.lead_follow_far_limit: # the vehicle is leading the ego vehicle and far away + # deccelerate + if self.current_lane == "subject": + self.ref_speed_list = copy.copy(self.subject_min_ref_speed) # decelerate to the min speed + elif self.current_lane == "left": + self.ref_speed_list = copy.copy(self.left_min_ref_speed) # decelerate to the min speed + + self.within_desired_range = False + + elif self.distance_with_ego > self.lead_follow_close_limit and self.distance_with_ego < 0: # the vehicle is leading + # and close to ego + # accelerate + if self.current_lane == "subject": + self.ref_speed_list = copy.copy(self.subject_max_ref_speed) # accelerate to the max speed + elif self.current_lane == "left": + self.ref_speed_list = copy.copy(self.left_max_ref_speed) # accelerate to the max speed + + self.within_desired_range = False + + else: + # vehicle within expected range or lead vehicle behind the ego vehicle, which could happen due to change lane + # keep normal navigation speed + if self.current_lane == "subject": + self.ref_speed_list = copy.copy(self.subject_ref_speed) # keep constant speed + elif self.current_lane == "left": + self.ref_speed_list = copy.copy(self.left_ref_speed) # keep constant speed + + if self.within_desired_range == False: # just come into the desired range, set the speed + local_transform = self.env.get_transform_3d(self.model_uniquename) + forward_vector = local_transform.get_forward_vector() + vehicle_velocity = carla.Vector3D(x = forward_vector.x * self.subject_ref_speed[0], y = forward_vector.y * self.subject_ref_speed[0], z = forward_vector.z * self.subject_ref_speed[0]) + self.env.set_vehicle_velocity(self.model_uniquename , vehicle_velocity) # set the velocity + + self.within_desired_range = True + + + + + else: # "follow" vehicle + # for follow vehicle, check if there exists vehicle in the front + has_vehicle_in_front, distance = self.env.check_vehicle_in_front_freeway(self.model_uniquename, abs(self.lead_follow_distance)) + if has_vehicle_in_front: # has vehicle in front that is not the ego vehicle + if abs(self.distance_with_ego) > abs(distance) + 1.0: # there exists at least one vehicle between current vehicle and lead vehicle + self.command = "speed" # give up keeping constant distance, keep constant speed + + if self.current_lane == "subject": + self.ref_speed_list = copy.copy(self.subject_ref_speed) # keep constant speed + elif self.current_lane == "left": + self.ref_speed_list = copy.copy(self.left_ref_speed) # keep constant speed + return + + if self.distance_with_ego >= self.lead_follow_far_limit: # the vehicle is following the ego vehicle and far away + # accelerate + if self.current_lane == "subject": + self.ref_speed_list = copy.copy(self.subject_max_ref_speed) # accelerate to the max speed + elif self.current_lane == "left": + self.ref_speed_list = copy.copy(self.left_max_ref_speed) # accelerate to the max speed + self.within_desired_range = False + + elif self.distance_with_ego < self.lead_follow_close_limit and self.distance_with_ego > 0: # the vehicle is following + # and close to ego + # deccelerate + if self.current_lane == "subject": + self.ref_speed_list = copy.copy(self.subject_min_ref_speed) # decelerate to the min speed + elif self.current_lane == "left": + self.ref_speed_list = copy.copy(self.left_min_ref_speed) # decelerate to the min speed + self.within_desired_range = False + + else: + # vehicle within expected range or follow vehicle before the ego vehicle, which could happen due to change lane + # keep normal navigation speed + if self.current_lane == "subject": + self.ref_speed_list = copy.copy(self.subject_ref_speed) # keep constant speed + elif self.current_lane == "left": + self.ref_speed_list = copy.copy(self.left_ref_speed) # keep constant speed + + if self.within_desired_range == False: # just come into the desired range, set the speed + local_transform = self.env.get_transform_3d(self.model_uniquename) + forward_vector = local_transform.get_forward_vector() + vehicle_velocity = carla.Vector3D(x = forward_vector.x * self.subject_ref_speed[0], y = forward_vector.y * self.subject_ref_speed[0], z = forward_vector.z * self.subject_ref_speed[0]) + self.env.set_vehicle_velocity(self.model_uniquename , vehicle_velocity) # set the velocity + + self.within_desired_range = True + + + + #print("distance with ego == ", self.distance_with_ego) \ No newline at end of file diff --git a/legacy/gui_interface/backend/third_intersection_setting b/legacy/gui_interface/backend/third_intersection_setting new file mode 100644 index 0000000..cd179d6 --- /dev/null +++ b/legacy/gui_interface/backend/third_intersection_setting @@ -0,0 +1,12 @@ +subject_vehicle = "{'model': 'vehicle.tesla.model3', 'gap': 10.0, 'command': 'left', 'obey_traffic_lights': True, 'run': True, 'safety_distance': 15.0, 'choice': 'subject', 'traffic_light': , 'uniquename': 'vehicle.tesla.model3_482', 'ref_waypoint': None, 'location': None, 'rotation': None, 'trajectory': None, 'ref_speed_list': None, 'bounding_box': None, 'vehicle_type': 'other', 'stop_choice': 'normal', 'penetrate_distance': None, 'stop_ref_point': None}", "{'model': 'vehicle.tesla.model3', 'gap': 10.0, 'command': 'straight', 'obey_traffic_lights': True, 'run': True, 'safety_distance': 15.0, 'choice': 'subject', 'traffic_light': , 'uniquename': 'vehicle.tesla.model3_483', 'ref_waypoint': None, 'location': None, 'rotation': None, 'trajectory': None, 'ref_speed_list': None, 'bounding_box': None, 'vehicle_type': 'other', 'stop_choice': 'normal', 'penetrate_distance': None, 'stop_ref_point': None}" +left_vehicle = "{'model': 'vehicle.tesla.model3', 'gap': 10.0, 'command': 'straight', 'obey_traffic_lights': True, 'run': True, 'safety_distance': 15.0, 'choice': 'left', 'traffic_light': , 'uniquename': 'vehicle.tesla.model3_478', 'ref_waypoint': None, 'location': None, 'rotation': None, 'trajectory': None, 'ref_speed_list': None, 'bounding_box': None, 'vehicle_type': 'other', 'stop_choice': 'abrupt', 'penetrate_distance': None, 'stop_ref_point': None}", +right_vehicle = "{'model': 'vehicle.tesla.model3', 'gap': 10.0, 'command': 'left', 'obey_traffic_lights': True, 'run': True, 'safety_distance': 15.0, 'choice': 'right', 'traffic_light': , 'uniquename': 'vehicle.tesla.model3_479', 'ref_waypoint': None, 'location': None, 'rotation': None, 'trajectory': None, 'ref_speed_list': None, 'bounding_box': None, 'vehicle_type': 'other', 'stop_choice': 'normal', 'penetrate_distance': None, 'stop_ref_point': None}", +ahead_vehicle = "{'model': 'vehicle.tesla.model3', 'gap': 10.0, 'command': 'left', 'obey_traffic_lights': True, 'run': True, 'safety_distance': 15.0, 'choice': 'ahead', 'traffic_light': , 'uniquename': 'vehicle.tesla.model3_480', 'ref_waypoint': None, 'location': None, 'rotation': None, 'trajectory': None, 'ref_speed_list': None, 'bounding_box': None, 'vehicle_type': 'other', 'stop_choice': 'normal', 'penetrate_distance': None, 'stop_ref_point': None}", "{'model': 'vehicle.tesla.model3', 'gap': 10.0, 'command': 'right', 'obey_traffic_lights': True, 'run': True, 'safety_distance': 15.0, 'choice': 'ahead', 'traffic_light': , 'uniquename': 'vehicle.tesla.model3_481', 'ref_waypoint': None, 'location': None, 'rotation': None, 'trajectory': None, 'ref_speed_list': None, 'bounding_box': None, 'vehicle_type': 'other', 'stop_choice': 'normal', 'penetrate_distance': None, 'stop_ref_point': None}" +subject_light = None +subject_light_time = None +left_light = None +left_light_time = None +right_light = None +right_light_time = None +ahead_light = None +ahead_light_time = None diff --git a/legacy/gui_interface/backend/vehicle_length_config.txt b/legacy/gui_interface/backend/vehicle_length_config.txt new file mode 100644 index 0000000..11906bf --- /dev/null +++ b/legacy/gui_interface/backend/vehicle_length_config.txt @@ -0,0 +1,27 @@ +vehicle.audi.a2 = 1.8593107461929321 +vehicle.audi.tt = 2.073613166809082 +vehicle.mercedes-benz.coupe = 2.521892547607422 +vehicle.bmw.grandtourer = 2.3195438385009766 +vehicle.yamaha.yzf = 1.0953842401504517 +vehicle.audi.etron = 2.446002721786499 +vehicle.nissan.micra = 1.8344237804412842 +vehicle.bh.crossbike = 0.7546613812446594 +vehicle.lincoln.mkz2017 = 2.452350616455078 +vehicle.gazelle.omafiets = 0.9217206835746765 +vehicle.tesla.cybertruck = 3.1821160316467285 +vehicle.dodge_charger.police = 2.505488157272339 +vehicle.tesla.model3 = 2.4044108390808105 +vehicle.harley-davidson.low_rider = 1.175087809562683 +vehicle.toyota.prius = 2.2691667079925537 +vehicle.seat.leon = 2.1039369106292725 +vehicle.kawasaki.ninja = 1.021842122077942 +vehicle.nissan.patrol = 2.259999990463257 +vehicle.mini.cooperst = 1.9010963439941406 +vehicle.jeep.wrangler_rubicon = 1.9340534210205078 +vehicle.mustang.mustang = 2.452350616455078 +vehicle.volkswagen.t2 = 2.2368555068969727 +vehicle.chevrolet.impala = 2.684596538543701 +vehicle.citroen.c3 = 1.9876738786697388 +vehicle.diamondback.century = 0.8281218409538269 +vehicle.bmw.isetta = 1.1012483835220337 +vehicle.carlamotors.carlacola = 2.5996146202087402 diff --git a/scripts/freeway_test.py b/scripts/freeway_test.py index 6b6cf6d..38f81c2 100644 --- a/scripts/freeway_test.py +++ b/scripts/freeway_test.py @@ -13,30 +13,95 @@ # the value of spawn_point corresponds with the spawn_point numbers found in the MapExplorationExperiment, # spawn_offset shifts the spawn point forward or backward by x meters configuration_dictionary = { - "debug": True, - "number_of_vehicles": 2, + "debug": False, + "number_of_vehicles": 7, # Ego vehicle that simply goes straight through each Freeway section 0: { "type": VehicleType.EGO_FULL_MANUAL, + "vehicle": "vehicle.audi.tt", "spawn_point": 13, "spawn_offset": -10.0, "initial_lane_index": 1, "sections": { 0: 'straight' } - }, - + }, + # Initial lead vehicle that turns right at the second intersection 1: { "type": VehicleType.LEAD, - "spawn_point": 13, - "spawn_offset": 0.0, + "vehicle": "vehicle.lincoln.mkz_2017", + "spawn_point": 14, #lane 1 + "spawn_offset": 10.0, + "initial_lane_index": 1, + "sections": { + 0: 'straight' + } + }, + + # Initial lead vehicle that turns right at the second intersection + 2: { + "type": VehicleType.LEAD, + "vehicle": "vehicle.lincoln.mkz_2017", + "spawn_point": 13, #lane 2 + "spawn_offset": 7.0, + "initial_lane_index": 1, + "sections": { + 0: 'straight' + } + }, + + + # Initial lead vehicle that turns right at the second intersection + 3: { + "type": VehicleType.LEAD, + "vehicle": "vehicle.lincoln.mkz_2017", + "spawn_point": 10, #lane 3 + "spawn_offset": 52.0, + "initial_lane_index": 1, + "sections": { + 0: 'straight' + } + }, + + # Initial lead vehicle that turns right at the second intersection + 4: { + "type": VehicleType.LEAD, + "vehicle": "vehicle.lincoln.mkz_2017", + "spawn_point": 14, #lane 1 + "spawn_offset": -20.0, + "initial_lane_index": 1, + "sections": { + 0: 'straight' + } + }, + + # Initial lead vehicle that turns right at the second intersection + 5: { + "type": VehicleType.LEAD, + "vehicle": "vehicle.lincoln.mkz_2017", + "spawn_point": 13, #lane 2 + "spawn_offset": -17.0, + "initial_lane_index": 1, + "sections": { + 0: 'straight' + } + }, + + + # Initial lead vehicle that turns right at the second intersection + 6: { + "type": VehicleType.LEAD, + "vehicle": "vehicle.lincoln.mkz_2017", + "spawn_point": 10, #lane 3 + "spawn_offset": 22.0, "initial_lane_index": 1, "sections": { 0: 'straight' } } + } diff --git a/umich_sim/sim_backend/experiments/experiment.py b/umich_sim/sim_backend/experiments/experiment.py index 7015aa9..6ecb3f4 100644 --- a/umich_sim/sim_backend/experiments/experiment.py +++ b/umich_sim/sim_backend/experiments/experiment.py @@ -78,7 +78,12 @@ def init(self) -> None: pygame.font.init() try: client = carla.Client(config.server_addr, config.carla_port) - client.set_timeout(2.0) + + client.set_timeout(5.0) + self.tm = client.get_trafficmanager() # create a TM object + self.tm.global_percentage_speed_difference(10.0) # set the global speed limitation + self.tm_port = self.tm.get_port() # get the port of tm. we need add vehicle to tm by this port + hud = HUD(*config.client_resolution) world: World = World(client, hud, config.car_filter, self.MAP) @@ -193,7 +198,19 @@ def run_experiment(self) -> None: world: World = World.get_instance() hud: HUD = HUD.get_instance() world.restart() - + + # Update the speed of vehicle in traffic manager + for vehicle in self.vehicle_list: + vehicle.carla_vehicle.set_autopilot(True,self.tm_port) + self.tm.ignore_lights_percentage(vehicle.carla_vehicle, 0) + #self.tm.distance_to_leading_vehicle(vehicle.carla_vehicle, 20) + #self.tm.vehicle_percentage_speed_difference(vehicle.carla_vehicle, -20) + + physics_control = vehicle.carla_vehicle.get_physics_control() + physics_control.mass = 100000 + vehicle.carla_vehicle.apply_physics_control(physics_control) + self.tm.set_global_distance_to_leading_vehicle(50) + self.tm.global_percentage_speed_difference(-200) try: # Loop continuously clock = pygame.time.Clock() @@ -215,6 +232,10 @@ def run_experiment(self) -> None: for vehicle in self.vehicle_list + [self.ego_vehicle]: vehicle.update_other_vehicle_locations(self.vehicle_list) + + + + # Apply control to the Ego Vehicle if self.ego_vehicle is not None: # Lambda used to avoid passing all the arguments into the update_control function @@ -224,7 +245,7 @@ def run_experiment(self) -> None: # Apply control to every other Vehicle for vehicle in self.vehicle_list: self.update_control(vehicle) - + # Update the UI elements hud.tick(clock) world.render(display) @@ -242,6 +263,7 @@ def update_control(self, vehicle: Vehicle) -> None: update control based on specific experiment type :param vehicle: the vehicle to update control """ + #vehicle.apply_control(throttle = .4, brake = 0) pass def clean_up_experiment(self) -> None: @@ -336,7 +358,8 @@ def add_vehicles_from_configuration(self, configuration: Dict[int, VehicleType.EGO_FULL_MANUAL, VehicleType.EGO_MANUAL_STEER) vehicle = self.add_vehicle(spawn_point, ego=is_ego, - type_id=vehicle_configuration["type"]) + type_id=vehicle_configuration["type"], + blueprint_id = vehicle_configuration["vehicle"]) if is_ego: from umich_sim.sim_backend.carla_modules import EgoVehicle EgoVehicle.get_instance().set_vehicle(vehicle.carla_vehicle) diff --git a/umich_sim/sim_backend/helpers.py b/umich_sim/sim_backend/helpers.py index 0679e81..0e8d4dc 100644 --- a/umich_sim/sim_backend/helpers.py +++ b/umich_sim/sim_backend/helpers.py @@ -171,11 +171,11 @@ def project_forward(transform: carla.Transform, :return: a new carla.Transform that represents the transform projected forward """ - x_component = math.cos(transform.rotation.yaw * (math.pi / 180)) + x_component= math.cos(transform.rotation.yaw * (math.pi / 180)) y_component = math.sin(transform.rotation.yaw * (math.pi / 180)) projected_location = carla.Location( - x=transform.location.x + distance * x_component, + x=transform.location.x + distance * x_component , y=transform.location.y + distance * y_component, z=transform.location.z)