50 initialize with vehicles acting as speed barriers

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 <https://opensource.org/licenses/MIT>.
+
+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.')
+