Costmap displaying all future lanes

gym_carla/envs/carla_env.py

@@ -21,11 +21,9 @@
 from gym import spaces
 from gym.utils import seeding
 import carla
-
 from gym_carla.envs.render import BirdeyeRender
 from gym_carla.envs.route_planner import RoutePlanner
 
-
 class CarlaEnv(gym.Env):
   """An OpenAI gym wrapper for CARLA simulator."""
 
@@ -73,7 +71,8 @@ def __init__(self, params):
       'vh_clas': spaces.Box(low=0, high=1, shape=(self.pixor_size, self.pixor_size, 1), dtype=np.float32),
       'vh_regr': spaces.Box(low=-5, high=5, shape=(self.pixor_size, self.pixor_size, 6), dtype=np.float32),
       'state': spaces.Box(np.array([-2, -1, -5, 0]), np.array([2, 1, 30, 1]), dtype=np.float32),
-      'pixor_state': spaces.Box(np.array([-1000, -1000, -1, -1, -5]), np.array([1000, 1000, 1, 1, 20]), dtype=np.float32)})
+      'pixor_state': spaces.Box(np.array([-1000, -1000, -1, -1, -5]), np.array([1000, 1000, 1, 1, 20]), dtype=np.float32),
+      'costmap': spaces.Box(low=0, high=255, shape=(self.obs_size, self.obs_size, 1), dtype=np.uint8)}) #costmap should be a 2d array
 
     # Connect to carla server and get world object
     print('connecting to Carla server...')
@@ -191,7 +190,11 @@ def reset(self):
         self.reset()
 
       if self.task_mode == 'random':
-        transform = random.choice(self.vehicle_spawn_points)
+        #transform = random.choice(self.vehicle_spawn_points)
+        transform = self.vehicle_spawn_points[0]
+        #transform.rotation.yaw = 0
+        tup = (transform.location.x, transform.location.y, transform.rotation.yaw)
+        print("Transform: " + str(tup))
       if self.task_mode == 'roundabout':
         self.start=[52.1+np.random.uniform(-5,5),-4.2, 178.66] # random
         # self.start=[52.1,-4.2, 178.66] # static
@@ -290,7 +293,6 @@ def step(self, action):
     # Update timesteps
     self.time_step += 1
     self.total_step += 1
-
     return (self._get_obs(), self._get_reward(), self._terminal(), copy.deepcopy(info))
 
   def seed(self, seed=None):
@@ -325,7 +327,7 @@ def _init_renderer(self):
     """
     pygame.init()
     self.display = pygame.display.set_mode(
-    (self.display_size * 3, self.display_size),
+    (self.display_size * 4, self.display_size),
     pygame.HWSURFACE | pygame.DOUBLEBUF)
 
     pixels_per_meter = self.display_size / self.obs_range
@@ -672,6 +674,192 @@ def get_poly_from_info(info):
     # Pixor state, [x, y, cos(yaw), sin(yaw), speed]
     pixor_state = [ego_x, ego_y, np.cos(ego_yaw), np.sin(ego_yaw), speed]
 
+
+    """
+    Explanation of how my costmap implementation works:
+      First we get a list of all of the waypoints from the current position. We iterate through this list in pairs so that there is a current 
+      waypoint and a previous waypoint. These along with parameter cost are passed into _get_costmap which returns a costmap only relevant to the
+      lane defined by the line between the two points. This costmap is summed with the global costmap. This profess is repeated for the left and right
+      lanes of the current waypoint if they exist and are in the same direction.  
+    """
+    def _get_perp_dis(x1, y1, x2, y2, x3, y3):
+    """ Computes the perpindicular distance between a point (x3,y3) and a line segment defined by points
+        (x1, y1) and (x2, y2). If the point doesn't have a perpincicular line within the line segment, the
+        distance is inf
+    """  
+      x = np.array([x3, y3])
+      p = np.array([x1, y1])
+      q = np.array([x2, y2])
+      lamb = np.dot((x - p), (q - p)) / np.dot((q - p), (q - p)) #lamb checks if point is within line segment
+      if lamb <= 1 and lamb >= 0:
+        s = p + (lamb * (q - p))
+        return np.linalg.norm(x - s)
+      return float('inf')
+
+
+    ego_y = ego_y - 2 #shift the cost map down because originally it was too high 
+
+    def _get_costmap(pWaypoint, cWaypoint, cost):
+    """ Generates a costmap for a current waypoint cWaypoint and its preceding waypoint pWaypoint using cost.
+        I refer a lot to global vs local frame. Global means the xy coordinate in the Carla coordinates
+        Local is the coordinate in the costmap matrix.
+        Also the letters x and y are swapped when referring to the local frame but it works 
+    """
+      single_costmap = np.zeros((self.obs_size, self.obs_size))
+
+      #Definitions for the waypoints' x and y coordinates in the global frame 
+      laneWidth = pWaypoint.lane_width
+      cX = cWaypoint.transform.location.x
+      cY = cWaypoint.transform.location.y
+      pX = pWaypoint.transform.location.x
+      pY = pWaypoint.transform.location.y
+
+      #If we draw a square around the center of the ego vehicle (length is determined by range), this is the bottom left corner in global coords
+      corner_x = ego_x - (self.obs_range / 2)
+      corner_y = ego_y - (self.obs_range / 2)
+
+      #Here we create two matrices with the same dimensions as the costmap. One represents the x coordinate and one represents the y coordinate in the local frame.
+      y_array, x_array = np.meshgrid(np.arange(0, self.obs_size), np.arange(0, self.obs_size))
+      #y_array is [[0 1 2 ... 255] [0 1 2 ... 255] ...] 
+      #x_array is [[0 0 0 .... 0] [1 1 1 .... 1]... [255 255 ... 255]]
+
+      rotated_x_array = (2 * ego_x) - ((x_array * self.lidar_bin) + corner_x)
+      rotated_y_array = (y_array * self.lidar_bin) + corner_y
+      c = np.cos(ego_yaw)
+      s = np.sin(ego_yaw)
+      global_x_array = (c * (rotated_x_array - ego_x)) - (s * (rotated_y_array - ego_y)) + ego_x #for each point in our matrix, we have their global coordinates 
+      global_y_array = (s * (rotated_x_array - ego_x)) + (c * (rotated_y_array - ego_y)) + ego_y
+
+      p = np.array([pX, pY])
+      q = np.array([cX, cY])
+      q_dif = q - p
+      lamb_array= (((global_x_array - pX) * (cX - pX)) + ((global_y_array - pY) * (cY - pY ))) / np.dot((q_dif), (q_dif))
+      sX = pX + (lamb_array * (cX - pX))
+      sY = pY + (lamb_array * (cY - pY))
+
+      distanceMap = np.sqrt(np.square(global_x_array - sX) + np.square(global_y_array - sY))
+      penal = (laneWidth / 2) * (-cost) / abs(cost)      
+      distanceMap = np.where((lamb_array <=1) & (lamb_array >= 0) & (distanceMap <= laneWidth / 2), distanceMap, penal) #will have perpDistance in the spot if its within the lane
+      single_costmap = distanceMap * (abs(cost / (laneWidth / 2))) + cost
+
+      return single_costmap
+
+    #Generate list of waypoints. Previously, we relied on self.routeplanner._actualWaypoints
+    #there is way to save space for this. Instead of recalculating all waypoints, we can reuse most of them.
+
+    #we can do this for each neighboring lane 
+    ego_waypoints = [self.world.get_map().get_waypoint(self.ego.get_location())] #make current ego position into a waypoint
+    current_dir= ego_waypoints[0].lane_id #positive or negative integer depending on which direction the lane is going in 
+    left_lane = ego_waypoints[0].get_left_lane()
+    right_lane = ego_waypoints[0].get_right_lane()
+
+    waypoints_threshold = 5 #this is how many waypoints to keep 
+    sampling_radius = 5 #how many meters away to sample for the next waypoint
+
+    if left_lane and left_lane.lane_type == carla.LaneType.Driving and left_lane.lane_id * current_dir >= 0: #check if neighboring lane exists, is drivable, and in same direction
+      ego_waypoints.append(left_lane)
+
+    if right_lane and right_lane.lane_type == carla.LaneType.Driving and right_lane.lane_id * current_dir >= 0:
+      ego_waypoints.append(right_lane)
+    dir_list = [] #list of lists of waypoints. each list inside represents a possible path
+
+
+    #we loop through ego_waypoints for the center, left, and right lanes 
+    for current_waypoint in ego_waypoints:
+      #current_waypoint = self.world.get_map().get_waypoint(self.ego.get_location())
+      current_waypoints = [current_waypoint]
+      lane_end = False #did we reach the end of the lane
+      ctr = 0 #counter to make sure we don't pass the waypoints threshold
+      next_waypoints = [] #these represents all the other directions we have to go in after the current waypoint. TODO contains a tuple of starting waypoint and list of lists
+      while (not lane_end) and ctr < waypoints_threshold:
+        sample_waypoint = current_waypoints[-1]
+        ctr += 1
+        next_waypoint = sample_waypoint.next(sampling_radius)
+
+        # if (sample_waypoint.is_junction):
+        #   junc = sample_waypoint.get_junction().get_waypoints(carla.LaneType.Driving)
+        #   print("Sample", sample_waypoint)
+        #   print("Junc", junc)
+        #   for tup in junc:
+        #     if tup[0] == sample_waypoint:
+        #       next_waypoints.append([tup[0], tup[1]])
+
+        #TODO so that we don't just stop when there's multiple directions but instead stop when the lane ends 
+        if (len(next_waypoint) != 1): #if there is more than 1 waypoint to go to we stop because we have to explore those directions 
+          lane_end = True
+          #print("Testing if the last means in junction", sample_waypoint.is_junction)
+        else: 
+          current_waypoints.append(next_waypoint[0]) #if there's only one direction just append it to the current array
+
+      last_waypoint = current_waypoints[-1] #this will be the starting waypoint for all the diverging lanes 
+
+      dir_list.append(current_waypoints)
+
+      if lane_end:
+        #this means the lane changes direction so we have to compute the new lanes for the junction
+        #print("last waypoint coming up")
+        next_waypoints = last_waypoint.next(sampling_radius)
+        #print(next_waypoints)
+        for new_direction in next_waypoints: #we append some points
+          new_waypoints = [last_waypoint, new_direction]
+          ctr = 0
+          lane_end = False
+          while not lane_end and ctr < waypoints_threshold + 5:
+            ctr += 1
+            sample_waypoint = new_waypoints[-1]
+
+            next_waypoint = sample_waypoint.next(sampling_radius)
+            if (len(next_waypoint) > 1):
+              lane_end = True
+            else: 
+              new_waypoints.append(next_waypoint[0])
+          #print('ctr', ctr)
+          #print("new_waypoints", new_waypoints)
+          dir_list.append(new_waypoints)
+
+
+    #listofWaypoints = self.routeplanner._actualWaypoints
+
+    cost = -10
+    costmap = np.zeros((self.obs_size, self.obs_size))
+
+    for listofWaypoints in dir_list:
+      if len(listofWaypoints) < 1:
+        print("Not enough waypoints to form costmap")
+
+      else:
+        pWaypoint = listofWaypoints[0]
+        for cWaypoint in listofWaypoints[1:]:
+
+          currentDirection = cWaypoint.lane_id #positive or negative integer depending on which direction the lane is going in 
+          costmap = costmap + _get_costmap(pWaypoint, cWaypoint, cost)
+
+          #The current implementation of left and right lanes is contingent on whether the current lane has a left/right lane AND the previous lane has a left/right lane
+          pleftWaypoint = pWaypoint.get_left_lane()
+          prightWaypoint = pWaypoint.get_right_lane()
+          cleftWaypoint = cWaypoint.get_left_lane()
+          crightWaypoint = cWaypoint.get_right_lane()
+          pWaypoint = cWaypoint
+
+
+
+
+        # if pleftWaypoint and (pleftWaypoint.lane_id * currentDirection >= 0): #check if left waypoint exists for the previous waypoint and it goes in the same direction 
+        #   if cleftWaypoint and (cleftWaypoint.lane_id * currentDirection >= 0): #check if the left waypoint exists for the current waypoint and it goes in the same direction
+        #     costmap = costmap + _get_costmap(pleftWaypoint, cleftWaypoint, cost)
+
+        # if prightWaypoint and (prightWaypoint.lane_id * currentDirection >= 0):
+        #   if crightWaypoint and (crightWaypoint.lane_id * currentDirection >= 0):
+        #     costmap = costmap + _get_costmap(prightWaypoint, crightWaypoint, cost)
+
+
+    #Here we convert the cost map which ranges from -cost to 0 (low cost to high cost) to a displayable costmap that has values from 0 to 255
+    costmap = np.clip(costmap, cost, 0)
+    costmap = (costmap - cost) * 255 / abs(cost)
+
+    costmap_surface = self._rgb_to_display_surface(np.moveaxis(np.array([costmap, costmap, costmap]), 0, -1))
+    self.display.blit(costmap_surface, (self.display_size * 3, 0))
+
     obs = {}
     obs.update({
       'birdeye':birdeye.astype(np.uint8),
@@ -682,6 +870,7 @@ def get_poly_from_info(info):
       'vh_regr':vh_regr.astype(np.float32),
       'state': state,
       'pixor_state': pixor_state,
+      'costmap' : costmap
     })
 
     return obs

gym_carla/envs/route_planner.py

@@ -47,8 +47,8 @@ def __init__(self, vehicle, buffer_size):
 
     self._last_traffic_light = None
     self._proximity_threshold = 15.0
-
     self._compute_next_waypoints(k=200)
+    self._actualWaypoints = None
 
   def _compute_next_waypoints(self, k=1):
     """
@@ -114,6 +114,8 @@ def _get_waypoints(self):
     for i, (waypoint, _) in enumerate(self._waypoint_buffer):
       waypoints.append([waypoint.transform.location.x, waypoint.transform.location.y, waypoint.transform.rotation.yaw])
 
+    self._actualWaypoints = np.array([i[0] for i in self._waypoint_buffer])
+    #self._actualWaypoints = self._waypoint_buffer
     # current vehicle waypoint
     self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location())
     # target waypoint

test.py

@@ -12,7 +12,7 @@
 def main():
   # parameters for the gym_carla environment
   params = {
-    'number_of_vehicles': 100,
+    'number_of_vehicles': 0,
     'number_of_walkers': 0,
     'display_size': 256,  # screen size of bird-eye render
     'max_past_step': 1,  # the number of past steps to draw
@@ -45,9 +45,10 @@ def main():
   while True:
     action = [2.0, 0.0]
     obs,r,done,info = env.step(action)
+    #print(obs)
 
-    if done:
-      obs = env.reset()
+    # if done:
+    #   obs = env.reset()
 
 
 if __name__ == '__main__':