Merge pull request ucla-mobility#41 from ucla-mobility/feature/code_m…

…inor_optimization

Feature/code minor optimization

opencda/core/application/platooning/platoon_behavior_agent.py

@@ -5,6 +5,7 @@
 # Author: Runsheng Xu <[email protected]>
 # License: MIT
 
+import weakref
 from collections import deque
 
 import carla
@@ -24,7 +25,7 @@ def __init__(self, vehicle, vehicle_manager, v2x_manager, behavior_yaml, platoon
  """
  Construct class
  :param vehicle: carla actor todo:remove this later
- :param vehicle_manager: vehicle manager of this agent. todo: try to avoid such circle reference in future
+ :param vehicle_manager: vehicle manager of this agent.
  :param v2x_manager: communication manager
  :param behavior_yaml: configure yaml file for normal behavior agent
  :param platoon_yaml: configure yaml file for platoon behavior agent
@@ -34,9 +35,9 @@ def __init__(self, vehicle, vehicle_manager, v2x_manager, behavior_yaml, platoon
 
  super(PlatooningBehaviorAgent, self).__init__(vehicle, carla_map, behavior_yaml)
 
- self.vehicle_manager = vehicle_manager
+ self.vehicle_manager = weakref.ref(vehicle_manager)()
  # communication manager
- self.v2x_manager = v2x_manager
+ self.v2x_manager = weakref.ref(v2x_manager)()
 
  # used for gap keeping
  self.inter_gap = platoon_yaml['inter_gap']
@@ -59,7 +60,8 @@ def __init__(self, vehicle, vehicle_manager, v2x_manager, behavior_yaml, platoon
 
  def run_step(self, target_speed=None, collision_detector_enabled=True, lane_change_allowed=True):
  """
- Run a single step for navigation under platooning agent.
+ Run a single step for navigation under platooning agent. Finite state machine is used to switch between
+ different platooning states.
  Args:
  target_speed (float): Target speed in km/h
  collision_detector_enabled (bool): Whether collision detection enabled.
@@ -206,7 +208,7 @@ def platooning_following_manager(self, inter_gap):
  else len(frontal_trajectory)
  # todo: current not working well on curve
  for i in range(tracked_length):
- delta_t = 0.25 # todo: this is harded coded
+ delta_t = self.get_local_planner().dt
  # print('previous x :%f, delta t: %f' % (frontal_trajectory[i][0].location.x, delta_t))
  if i == 0:
  pos_x = (frontal_trajectory[i][0].location.x + inter_gap / delta_t * ego_loc_x) / \
@@ -304,7 +306,6 @@ def run_step_maintaining(self):
 
  def run_step_cut_in_move2point(self):
  """
- TODO: More than 2 lanes not working
  The vehicle is trying to get to the move in point
  :return: target_speed, target_waypoint, next FSM state
  """
@@ -347,7 +348,7 @@ def run_step_cut_in_move2point(self):
  return (*super().run_step(2.0 * get_speed(frontal_vehicle)), FSM.MOVE_TO_POINT)
 
  # if the ego vehicle is too close or exceed the frontal vehicle
- if distance < self._ego_speed / 3.6 * self.inter_gap / 1.5 or angle >= 80:
+ if distance < self._ego_speed / 3.6 * self.inter_gap / 1.5 or angle >= 70:
  print('too close, step back!')
  return (*super().run_step(0.9 * get_speed(frontal_vehicle)), FSM.MOVE_TO_POINT)
 
@@ -474,7 +475,7 @@ def dist(v):
  return v.get_location().distance(ego_vehicle_loc)
  vehicle_blocking_status = False
  for vehicle in self.obstacle_vehicles:
- vehicle_blocking_status = vehicle_blocking_status or self._collision_check.is_in_range(self.vehicle,
+ vehicle_blocking_status = vehicle_blocking_status or self._collision_check.is_in_range(self._ego_pos,
  frontal_vehicle,
  vehicle,
  self._map)
@@ -547,7 +548,7 @@ def dist(v):
 
  vehicle_blocking_status = False
  for vehicle in self.obstacle_vehicles:
- vehicle_blocking_status = vehicle_blocking_status or self._collision_check.is_in_range(self.vehicle,
+ vehicle_blocking_status = vehicle_blocking_status or self._collision_check.is_in_range(self._ego_pos,
  rear_vehicle,
  vehicle,
  self._map)

opencda/core/plan/behavior_agent.py

@@ -60,14 +60,13 @@ def __init__(self, vehicle, carla_map, config_yaml):
  self.hazard_flag = False
 
  # route planner related
- self.look_ahead_steps = 3 # todo: hard coded
  self._global_planner = None
  self.start_waypoint = None
  self.end_waypoint = None
  self._sampling_resolution = config_yaml['sample_resolution']
 
  # intersection agent related
- self.light_state = "Green"
+ self.light_state = "Red"
  self.light_id_to_ignore = -1
 
  # trajectory planner

opencda/core/plan/collision_check.py

@@ -26,16 +26,16 @@ def __init__(self, time_ahead=1.2, circle_radius=1.3, circle_offsets=None):
  self._circle_offsets = [-1.0, 1.0] if circle_offsets is None else circle_offsets
  self._circle_radius = circle_radius
 
- def is_in_range(self, ego_vehicle, target_vehicle, candidate_vehicle, carla_map):
+ def is_in_range(self, ego_pos, target_vehicle, candidate_vehicle, carla_map):
  """
  Check whether there is a obstacle vehicle between target_vehicle and ego_vehicle during back_joining
  :param carla_map: carla map
- :param ego_vehicle: The vehicle trying to join platooning
+ :param ego_pos: Ego vehicle position
  :param target_vehicle: The vehicle that is suppose to be catched
  :param candidate_vehicle: The possible obstacle vehicle blocking the ego vehicle and target vehicle
  :return:
  """
- ego_loc = ego_vehicle.get_location()
+ ego_loc = ego_pos.location
  target_loc = target_vehicle.get_location()
  candidate_loc = candidate_vehicle.get_location()
 

opencda/core/plan/local_planner_behavior.py

@@ -60,8 +60,6 @@ def __init__(self, agent, carla_map, config_yaml):
  # waypoint pop out thresholding
  self._min_distance = config_yaml['min_dist']
  self._buffer_size = config_yaml['buffer_size']
- # TODO: Redudant, remove later
- self._current_waypoint = None
 
  # TODO: pid controller should be outside
  self._pid_controller = None
@@ -76,6 +74,9 @@ def __init__(self, agent, carla_map, config_yaml):
  self._history_buffer = deque(maxlen=3)
  self.trajectory_update_freq = config_yaml['trajectory_update_freq']
 
+ # trajectory sampling rate
+ self.dt = config_yaml['trajectory_dt']
+
  # used to identify whether lane change is operated
  self.lane_change = False
  # In some corner cases, the id is not changed but we regard it as lane change due to large lateral diff
@@ -117,22 +118,6 @@ def update_information(self, ego_pos, ego_speed):
  self._ego_pos = ego_pos
  self._ego_speed = ego_speed
 
- def get_incoming_waypoint_and_direction(self, steps=3):
- """
- Returns direction and waypoint at a distance ahead defined by the user.
-
- :param steps: number of steps to get the incoming waypoint.
- """
- if len(self.waypoints_queue) > steps:
- return self.waypoints_queue[steps]
-
- else:
- try:
- wpt, direction = self.waypoints_queue[-1]
- return wpt, direction
- except IndexError as i:
- print(i)
- return None, RoadOption.VOID
 
  def get_trajetory(self):
  """
@@ -270,8 +255,8 @@ def generate_trajectory(self, rx, ry, rk):
  """
  # unit distance for interpolation points
  ds = 0.1
- # unit time space TODO: Make this dynamic to map a linear relationship with speed
- dt = 0.25
+ # unit sampling resolution
+ dt = self.dt
 
  target_speed = self._target_speed
  current_speed = self._ego_speed
@@ -389,9 +374,6 @@ def run_step(self, rx, ry, rk, target_speed=None, trajectory=None, following=Fal
  elif trajectory:
  self._trajectory_buffer = trajectory.copy()
 
- # Current vehicle waypoint
- self._current_waypoint = self._map.get_waypoint(self._ego_pos.location)
-
  # Target waypoint TODO: dt is never used
  self.target_waypoint, self.target_road_option, self._target_speed, dt = \
  self._trajectory_buffer[min(1, len(self._trajectory_buffer) - 1)]

opencda/scenario_testing/config_yaml/platoon_joining_2lanefree_carla.yaml

@@ -45,13 +45,14 @@ vehicle_base: &vehicle_base
  ignore_traffic_light: true # whether to ignore traffic light
  overtake_allowed: false # whether overtake allowed, typically false for platoon leader
  collision_time_ahead: 1.3 # used for collision checking
- overtake_counter_recover: 35 # used to avoid successive overtaking
  sample_resolution: 4.5 # the unit distance between two adjacent waypoints in meter
+ overtake_counter_recover: 35 # the vehicle can not do another overtake during next certain steps
  local_planner: &base_local_planner # trajectory planning related
  buffer_size: 12 # waypoint buffer size
  trajectory_update_freq: 15 # used to control trajectory points updating frequency
  waypoint_update_freq: 12 # used to control waypoint updating frequency
  min_dist: 3 # used to pop out the waypoints too close to current location
+ trajectory_dt: 0.25 # for every dt seconds, we sample a trajectory point from the trajectory path as next goal state
  debug: false # whether to draw future/history waypoints
  debug_trajectory: false # whether to draw the trajectory points and path
 
@@ -100,15 +101,15 @@ carla_traffic_manager:
  set_osm_mode: true # Enables or disables the OSM mode.
  auto_lane_change: false
  vehicle_list:
- - spawn_position: [-585, 8.3, 0.3, 0, 0, 0]
- - spawn_position: [-610, 8.3, 0.3, 0, 0, 0]
- - spawn_position: [-690, 8.3, 0.3, 0, 0, 0]
- - spawn_position: [-620, 4.8, 0.3, 0, 0, 0]
+ - spawn_position: [-285, 8.3, 0.3, 0, 0, 0]
+ - spawn_position: [-310, 8.3, 0.3, 0, 0, 0]
+ - spawn_position: [-390, 8.3, 0.3, 0, 0, 0]
+ - spawn_position: [-320, 4.8, 0.3, 0, 0, 0]
  vehicle_speed_perc: -200
- - spawn_position: [-635, 4.8, 0.3, 0, 0, 0]
- - spawn_position: [-660, 4.8, 0.3, 0, 0, 0]
- - spawn_position: [-700, 4.8, 0.3, 0, 0, 0]
- - spawn_position: [-710, 4.8, 0.3, 0, 0, 0]
+ - spawn_position: [-335, 4.8, 0.3, 0, 0, 0]
+ - spawn_position: [-360, 4.8, 0.3, 0, 0, 0]
+ - spawn_position: [-400, 4.8, 0.3, 0, 0, 0]
+ - spawn_position: [-410, 4.8, 0.3, 0, 0, 0]
 
 # define scenario. In this scenario, a 4-vehicle platoon already exists.
 scenario:
@@ -117,12 +118,12 @@ scenario:
  destination: [1000.372955, 8.3, 0.3]
  members: # the first one is regarded as leader by default
  - <<: *vehicle_base
- spawn_position: [-650, 8.3, 0.3, 0, 0, 0] # x, y, z, roll, yaw, pitch
+ spawn_position: [-350, 8.3, 0.3, 0, 0, 0] # x, y, z, roll, yaw, pitch
  sensing:
  <<: *base_sensing
  perception:
  <<: *base_perception
- activate: true
+ activate: false
  camera_visualize: false
  lidar_visualize: true
  platoon: # we need to add platoon specific params
@@ -134,7 +135,7 @@ scenario:
  debug_trajectory: false
  debug: false
  - <<: *vehicle_base
- spawn_position: [-660, 8.3, 0.3, 0, 0, 0]
+ spawn_position: [-360, 8.3, 0.3, 0, 0, 0]
  platoon: # we need to add platoon specific params
  <<: *platoon_base
  controller:
@@ -143,33 +144,33 @@ scenario:
  <<: *control_args
  max_throttle: 1.0
  - <<: *vehicle_base
- spawn_position: [-670, 8.3, 0.3, 0, 0, 0]
+ spawn_position: [-370, 8.3, 0.3, 0, 0, 0]
  platoon: # we need to add platoon specific params
  <<: *platoon_base
  args:
  <<: *control_args
  max_throttle: 1.0
  - <<: *vehicle_base
- spawn_position: [-680, 8.3, 0.3, 0, 0, 0]
+ spawn_position: [-380, 8.3, 0.3, 0, 0, 0]
  platoon: # we need to add platoon specific params
  <<: *platoon_base
  args:
  <<: *control_args
  max_throttle: 1.0
  single_cav_list: # this is for merging vehicle or single cav without v2x
  - <<: *vehicle_base
- spawn_position: [-680, 4.8, 0.3, 0, 0, 0]
+ spawn_position: [-380, 4.8, 0.3, 0, 0, 0]
  # when this is defined, the above parameter will be ignored, and a special map function will
  # be used to define the spawn position based on the argument
- spawn_special: [0.345]
+ spawn_special: [0.625]
  destination: [300, 12.0, 0]
  sensing:
  <<: *base_sensing
  perception:
  <<: *base_perception
  activate: false
  camera_visualize: false
- lidar_visualize: false
+ lidar_visualize: true
  localization:
  <<: *base_localize
  debug_helper:

opencda/scenario_testing/config_yaml/platoon_joining_2lanefreecomplete_carla.yaml

@@ -46,13 +46,13 @@ vehicle_base: &vehicle_base
  ignore_traffic_light: true # whether to ignore traffic light
  overtake_allowed: false # whether overtake allowed, typically false for platoon leader
  collision_time_ahead: 1.3 # used for collision checking
- overtake_counter_recover: 35 # used to avoid successive overtaking
- sample_resolution: 4.5 # the unit distance between two adjacent waypoints in meter
+ overtake_counter_recover: 35 # the vehicle can not do another overtake during next certain steps
  local_planner: &base_local_planner # trajectory planning related
  buffer_size: 12 # waypoint buffer size
  trajectory_update_freq: 15 # used to control trajectory points updating frequency
  waypoint_update_freq: 12 # used to control waypoint updating frequency
  min_dist: 3 # used to pop out the waypoints too close to current location
+ trajectory_dt: 0.20 # for every dt seconds, we sample a trajectory point from the trajectory path as next goal state
  debug: false # whether to draw future/history waypoints
  debug_trajectory: false # whether to draw the trajectory points and path
 

opencda/scenario_testing/config_yaml/platoon_joining_town06_carla.yaml

@@ -45,13 +45,14 @@ vehicle_base: &vehicle_base
  ignore_traffic_light: true # whether to ignore traffic light
  overtake_allowed: false # whether overtake allowed, typically false for platoon leader
  collision_time_ahead: 1.1 # used for collision checking
- overtake_counter_recover: 35 # used to avoid successive overtaking
+ overtake_counter_recover: 35 # the vehicle can not do another overtake during next certain steps
  sample_resolution: 4.5 # the unit distance between two adjacent waypoints in meter
  local_planner: &base_local_planner # trajectory planning related
  buffer_size: 12 # waypoint buffer size
  trajectory_update_freq: 15 # used to control trajectory points updating frequency
  waypoint_update_freq: 12 # used to control waypoint updating frequency
  min_dist: 3 # used to pop out the waypoints too close to current location
+ trajectory_dt: 0.20 # for every dt seconds, we sample a trajectory point from the trajectory path as next goal state
  debug: false # whether to draw future/history waypoints
  debug_trajectory: false # whether to draw the trajectory points and path
  controller: &base_controller

opencda/scenario_testing/config_yaml/single_2lanefree_carla.yaml

@@ -45,13 +45,14 @@ vehicle_base: &vehicle_base
  ignore_traffic_light: true # whether to ignore traffic light
  overtake_allowed: true # whether overtake allowed, typically false for platoon leader
  collision_time_ahead: 1.5 # used for collision checking
- overtake_counter_recover: 35 # used to avoid successive overtaking
+ overtake_counter_recover: 35 # the vehicle can not do another overtake during next certain steps
  sample_resolution: 4.5 # the unit distance between two adjacent waypoints in meter
  local_planner: &base_local_planner # trajectory planning related
  buffer_size: 12 # waypoint buffer size
  trajectory_update_freq: 15 # used to control trajectory points updating frequency
  waypoint_update_freq: 12 # used to control waypoint updating frequency
  min_dist: 3 # used to pop out the waypoints too close to current location
+ trajectory_dt: 0.20 # for every dt seconds, we sample a trajectory point from the trajectory path as next goal state
  debug: false # whether to draw future/history waypoints
  debug_trajectory: false # whether to draw the trajectory points and path
  controller: &base_controller

opencda/scenario_testing/config_yaml/single_town06_carla.yaml

@@ -45,12 +45,13 @@ vehicle_base: &vehicle_base
  ignore_traffic_light: true # whether to ignore traffic light
  overtake_allowed: true # whether overtake allowed, typically false for platoon leader
  collision_time_ahead: 1.5 # used for collision checking
- overtake_counter_recover: 35 # used to avoid successive overtaking
+ overtake_counter_recover: 35 # the vehicle can not do another overtake during next certain steps
  sample_resolution: 4.5 # the unit distance between two adjacent waypoints in meter
  local_planner: &base_local_planner # trajectory planning related
  buffer_size: 12 # waypoint buffer size
  trajectory_update_freq: 15 # used to control trajectory points updating frequency
  waypoint_update_freq: 12 # used to control waypoint updating frequency
+ trajectory_dt: 0.20 # for every dt seconds, we sample a trajectory point from the trajectory path as next goal state
  min_dist: 3 # used to pop out the waypoints too close to current location
  debug: false # whether to draw future/history waypoints
  debug_trajectory: false # whether to draw the trajectory points and path
@@ -91,22 +92,22 @@ carla_traffic_manager:
  set_osm_mode: true # Enables or disables the OSM mode.
  auto_lane_change: false
  vehicle_list:
- - spawn_position: [101.7194, 136.51, 0.3, 0, 0, 0]
+ - spawn_position: [101.7194, 139.51, 0.3, 0, 0, 0]
  vehicle_speed_perc: -200
  - spawn_position: [57.7194, 143.51, 0.3, 0, 0, 0]
  - spawn_position: [75.7194, 139.51, 0.3, 0, 0, 0]
  vehicle_speed_perc: 0
  - spawn_position: [161.7194, 139.51, 0.3, 0, 0, 0]
  vehicle_speed_perc: -300
- - spawn_position: [ 141.7194, 139.51, 0.3, 0, 0, 0 ]
- vehicle_speed_perc: -300
+ - spawn_position: [ 141.7194, 136.51, 0.3, 0, 0, 0 ]
+ vehicle_speed_perc: -100
 
 # define scenario. In this scenario, a 4-vehicle platoon already exists.
 scenario:
  single_cav_list: # this is for merging vehicle or single cav without v2x
  - <<: *vehicle_base
  spawn_position: [47.7194, 139.51, 0.3, 0, 0, 0]
- destination: [606.87, 143.39, 0]
+ destination: [630, 145.51, 0.3]
  v2x:
  <<: *base_v2x
  communication_range: 45