Merge pull request ucla-mobility#42 from ucla-mobility/feature/adapti…

…ve_delta_t

Feature/adaptive delta t

opencda/core/application/platooning/platoon_behavior_agent.py

@@ -224,20 +224,45 @@ def platooning_following_manager(self, inter_gap):
  frontal_vehicle_manager, _ = self.v2x_manager.get_platoon_front_rear()
  frontal_front_vehicle_manger, _ = frontal_vehicle_manager.v2x_manager.get_platoon_front_rear()
 
- if len(self._local_planner.get_trajetory()) > 7:
+ if len(self._local_planner.get_trajetory()) > self.get_local_planner().trajectory_update_freq - 2:
  return self._local_planner.run_step([], [], [], following=True)
  else:
+ # this agent is a behavior agent 
  frontal_trajectory = frontal_vehicle_manager.agent.get_local_planner().get_trajetory()
 
+ # get front speed
+ frontal_speed = frontal_vehicle_manager.agent._ego_speed
+
  ego_trajetory = deque(maxlen=30)
  ego_loc_x, ego_loc_y, ego_loc_z = self._ego_pos.location.x, self._ego_pos.location.y, \
  self._ego_pos.location.z
 
+ # get ego speed
+ ego_speed = self._ego_speed
+
+ # compare speed with frontal veh
+ frontal_speedd_diff = ego_speed - frontal_speed
+
  tracked_length = len(frontal_trajectory) - 1 if not frontal_front_vehicle_manger \
  else len(frontal_trajectory)
+
  # todo: current not working well on curve
  for i in range(tracked_length):
  delta_t = self.get_local_planner().dt
+ # if leader is slowing down(leader target speed is smaller than current speed), use a bigger dt.
+ # spd diff max at 15. If diff greater than 8, increase dt
+ if frontal_speedd_diff > 3.0:
+ '''
+ # only increase dt when V_ego > V_front (avoid collision)
+ # if V_ego < V_front (diff < 0), stick with small dt 
+ # todo: change delta_t to a function: 
+ # --> 1. {V_ego > V_front}: decrease dt to increase gap, help avoid collision
+ # --> 2. more difference, more dt adjustment 
+ # --> 3. {V_ego < V_front}: will not collide, keep default dt to keep gap
+ # --> 4. {V_ego ~ V_front}: keep default dt to keep gap 
+ '''
+ delta_t = delta_t + frontal_speedd_diff * 0.0125
+
  # 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) / \
@@ -264,7 +289,7 @@ def platooning_following_manager(self, inter_gap):
 
  if not ego_trajetory:
  wpt = self._map.get_waypoint(self._ego_pos.location)
- next_wpt = wpt.next(max(2, self._ego_speed / 3.6 * 1))[0]
+ next_wpt = wpt.next(max(2, int(self._ego_speed / 3.6 * 1)))[0]
  ego_trajetory.append((next_wpt.transform,
  self._ego_speed))
 
@@ -497,6 +522,7 @@ def run_step_back_joining(self):
  # 2. check if there is any other vehicle blocking between ego and platooning
  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._ego_pos,
@@ -508,16 +534,16 @@ def dist(v):
  # and it is close enough, then we regard the back joining finished
  if frontal_lane == ego_vehicle_lane \
  and not vehicle_blocking_status \
- and distance < 1.0 * self._ego_speed / 3.6 :
+ and distance < 1.0 * self._ego_speed / 3.6:
  print('joining finished !')
  return (*self.run_step_maintaining(), FSM.JOINING_FINISHED)
 
  # 4. If vehicle is not blocked, make ego back to the frontal vehicle's lane
  if not vehicle_blocking_status:
  print('no vehicle is blocking!!!')
  if frontal_lane != ego_vehicle_lane:
- left_wpt = ego_wpt.next(max(1.2 * self._ego_speed / 3.6 , 5))[0].get_left_lane()
- right_wpt = ego_wpt.next(max(1.2 * self._ego_speed / 3.6 , 5))[0].get_right_lane()
+ left_wpt = ego_wpt.next(max(1.2 * self._ego_speed / 3.6, 5))[0].get_left_lane()
+ right_wpt = ego_wpt.next(max(1.2 * self._ego_speed / 3.6, 5))[0].get_right_lane()
 
  if not left_wpt and not right_wpt:
  pass

opencda/core/application/platooning/platooning_manager.py

@@ -230,9 +230,6 @@ def evaluate(self):
 
  figure.legend(label, loc='upper right')
 
- plt_manager = plt.get_current_fig_manager()
- plt_manager.resize(*plt_manager.window.maxsize())
-
  return figure, perform_txt
 
  def destroy(self):

opencda/core/sensing/perception/perception_manager.py

@@ -277,8 +277,9 @@ def deactivate_mode(self, objects):
  vehicle_list = [v for v in vehicle_list if self.dist(v) < 50 and
  v.id != self.vehicle.id]
 
- # convert carla.Vehicle to opencda.ObstacleVehicle
- vehicle_list = [ObstacleVehicle(None, None, v, self.lidar.sensor) for v in vehicle_list]
+ # convert carla.Vehicle to opencda.ObstacleVehicle if lidar visualization is required.
+ if self.lidar:
+ vehicle_list = [ObstacleVehicle(None, None, v, self.lidar.sensor) for v in vehicle_list]
  objects.update({'vehicles': vehicle_list})
 
  if self.camera_visualize:

opencda/scenario_testing/config_yaml/platoon_stability_2lanefree_carla.yaml

@@ -0,0 +1,143 @@
+description: |-
+ Copyright 2021 <UCLA Mobility Lab>
+ Author: Runsheng Xu <[email protected]>
+ Content: This is the scenario testing configuration file for platooning joining and cooperative merge
+ at the customized 2lanefree simple version.
+
+# define carla simulation setting
+world:
+ sync_mode: true
+ client_port: 2000
+ fixed_delta_seconds: &delta 0.05
+
+# First define the basic parameters of the vehicles
+vehicle_base: &vehicle_base
+ sensing: &base_sensing
+ perception: &base_perception
+ activate: false # when not activated, objects positions will be retrieved from server directly
+ camera_visualize: 0 # whether to visualize rgb camera image using cv2.imshow
+ camera_num: 0 # how many cameras are mounted on the vehicle. Maximum 3(frontal, left and right cameras)
+ lidar_visualize: false # whether to visualize lidar points using open3d
+ lidar: # lidar sensor configuration, check CARLA sensor reference for more details
+ channels: 32
+ range: 50
+ points_per_second: 200000
+ rotation_frequency: 20 # the simulation is 20 fps
+ upper_fov: 10.0
+ lower_fov: -30.0
+ dropoff_general_rate: 0.0
+ dropoff_intensity_limit: 1.0
+ dropoff_zero_intensity: 0.0
+ noise_stddev: 0.0
+ localization: &base_localize
+ activate: true # when not activated, ego position will be retrieved from server directly
+ dt: *delta # used for kalman filter
+ gnss: # gnss sensor configuration
+ noise_alt_stddev: 0.005
+ noise_lat_stddev: 2e-6
+ noise_lon_stddev: 2e-6
+ heading_direction_stddev: 0.1 # degree
+ speed_stddev: 0.2
+ debug_helper: &loc_debug_helper
+ show_animation: false # whether to show real-time trajectory plotting
+ show_plotting: false # whether to show loc data plotting after simulation
+ x_scale: 10.0 # used to multiply with the x coordinate to make the error on x axis clearer
+ y_scale: 10.0 # used to multiply with the y coordinate to make the error on y axis clearer
+ behavior: &base_behavior
+ max_speed: 80 # maximum speed, km/h
+ tailgate_speed: 105 # when a vehicles needs to be close to another vehicle asap
+ speed_lim_dist: 3 # max_speed - speed_lim_dist = target speed
+ speed_decrease: 15 # used in car following mode to decrease speed for distance keeping
+ safety_time: 4 # ttc safety thresholding for decreasing speed
+ emergency_param: 0.4 # used to identify whether a emergency stop needed
+ 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
+ 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
+ trajectory_dt: 0.20 # for every dt seconds, we sample a trajectory point from the trajectory path as next goal state
+ waypoint_update_freq: 12 # used to control waypoint updating frequency
+ 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
+ controller: &base_controller
+ type: pid_controller # this has to be exactly the same name as the controller py file
+ args: &control_args
+ lat:
+ k_p: 0.75
+ k_d: 0.02
+ k_i: 0.4
+ lon:
+ k_p: 0.37
+ k_d: 0.024
+ k_i: 0.032
+ dynamic: false # whether use dynamic pid setting
+ dt: *delta # this should be equal to your simulation time-step
+ max_brake: 1.0
+ max_throttle: 1.0
+ max_steering: 0.3
+ v2x: &base_v2x # comminuation related
+ enabled: true
+ communication_range: 35
+
+# define the platoon basic characteristics
+platoon_base: &platoon_base
+ max_capacity: 10
+ inter_gap: 0.6 # desired time gap
+ open_gap: 1.2 # open gap
+ warm_up_speed: 55 # required speed before cooperative merging
+ change_leader_speed: true # whether to assign leader multiple speed to follow
+ leader_speeds_profile: [ 85, 95 ] # different speed for leader to follow
+ stage_duration: 10 # how long should the leader keeps in the current velocity stage
+
+# define the background traffic control by carla
+carla_traffic_manager:
+ sync_mode: true # has to be same as the world setting
+ global_distance: 4.0 # the minimum distance in meters that vehicles have to keep with the rest
+ # Sets the difference the vehicle's intended speed and its current speed limit.
+ # Carla default speed is 30 km/h, so -100 represents 60 km/h,
+ # and 20 represents 24 km/h
+ global_speed_perc: -300
+ set_osm_mode: true # Enables or disables the OSM mode.
+ auto_lane_change: false
+ vehicle_list: []
+ # - spawn_position: [-1000.722836, 8.3, 0.3, 0, 0, 0]
+
+
+# define scenario. In this scenario, a 4-vehicle platoon exists.
+# 4 agent speed to different speeds and stay at current speed before changing again.
+scenario:
+ platoon_list:
+ - <<: *platoon_base
+ destination: [1100.372955, 8.3, 0.3]
+ members: # the first one is regarded as leader by default
+ - <<: *vehicle_base
+ spawn_position: [-1000.722836, 8.3, 0.3, 0, 0, 0] # x, y, z, roll, yaw, pitch
+ platoon: # we need to add platoon specific params
+ <<: *platoon_base
+ behavior:
+ <<: *base_behavior
+ local_planner:
+ <<: *base_local_planner
+ debug_trajectory: true
+ debug: false
+ - <<: *vehicle_base
+ spawn_position: [-1010.722836, 8.3, 0.3, 0, 0, 0]
+ platoon: # we need to add platoon specific params
+ <<: *platoon_base
+ - <<: *vehicle_base
+ spawn_position: [-1020.722836, 8.3, 0.3, 0, 0, 0]
+ platoon: # we need to add platoon specific params
+ <<: *platoon_base
+ - <<: *vehicle_base
+ spawn_position: [-1030.722836, 8.3, 0.3, 0, 0, 0]
+ platoon: # we need to add platoon specific params
+ <<: *platoon_base
+ - <<: *vehicle_base
+ spawn_position: [-1040.722836, 8.3, 0.3, 0, 0, 0]
+ platoon: # we need to add platoon specific params
+ <<: *platoon_base
+

opencda/scenario_testing/platoon_stability_2lanefree_carla.py

@@ -0,0 +1,141 @@
+# -*- coding: utf-8 -*-
+"""
+Scenario testing: merging vehicle joining a platoon in the customized 2-lane freeway simplified map sorely with carla
+"""
+# Author: Runsheng Xu <[email protected]>
+# License: MIT
+
+import sys
+import os
+
+import carla
+
+import opencda.scenario_testing.utils.sim_api as sim_api
+from opencda.core.common.misc import get_speed
+from opencda.scenario_testing.evaluations.evaluate_manager import EvaluationManager
+from opencda.scenario_testing.utils.yaml_utils import load_yaml
+
+
+def run_scenario(opt, config_yaml):
+ try:
+ scenario_params = load_yaml(config_yaml)
+ current_path = os.path.dirname(os.path.realpath(__file__))
+ xodr_path = os.path.join(current_path,
+ '../assets/2lane_freeway_simplified/map_v7.6_12ft_lane.xodr')
+
+ # create simulation world
+ simulation_config = scenario_params['world']
+ client, world, carla_map, origin_settings = sim_api.createSimulationWorld(simulation_config, xodr_path)
+
+ if opt.record:
+ client.start_recorder("platoon_joining_2lanefree_carla.log", True)
+
+ # create background traffic in carla
+ traffic_manager, bg_veh_list = sim_api.createTrafficManager(client, world,
+ scenario_params['carla_traffic_manager'])
+
+ # create platoon members
+ platoon_list, cav_world = sim_api.createPlatoonManagers(world, carla_map, scenario_params,
+ apply_ml=opt.apply_ml)
+
+ if len(platoon_list) > 1:
+ sys.exit("In this scenario testing, only single platoon is allowed.")
+
+ spectator = world.get_spectator()
+ spectator_vehicle = platoon_list[0].vehicle_manager_list[0].vehicle
+
+ eval_manager = EvaluationManager(cav_world)
+
+ # adjusting leader speed 
+ leader_speed_profile = scenario_params['platoon_base']['leader_speeds_profile']
+ stage_duration = scenario_params['platoon_base']['stage_duration']
+
+ test_platoon_manager = platoon_list[0]
+
+ if len(leader_speed_profile) > 1: 
+ platoon_speed_adjusting = True
+ time_counter = 0
+ stage = 0
+ else: 
+ platoon_speed_adjusting = False
+
+ # run steps
+ while True:
+ world.tick()
+ transform = spectator_vehicle.get_transform()
+ spectator.set_transform(carla.Transform(transform.location + carla.Location(z=80),
+ carla.Rotation(pitch=-90)))
+
+ test_platoon_manager.update_information()
+ test_platoon_manager.run_step()
+
+ # leader speed change to test the stability of the platoon
+ if platoon_speed_adjusting:
+ leader_speed = get_speed(spectator_vehicle, True)*3.6
+
+ # stage 0
+ if stage == 0:
+ print('stage 0, accelrate to %f' % leader_speed_profile[0])
+ if leader_speed >= (leader_speed_profile[0]-3):
+ stage = 1
+ else: 
+ test_platoon_manager.origin_leader_target_speed = leader_speed_profile[0]
+
+ # stage 1
+ if stage == 1:
+ print('stage 1, speed has keep for %f second' % (time_counter * 0.05))
+ if time_counter >= stage_duration / 0.05:
+ stage = 2
+ time_counter = 0
+ else:
+ time_counter += 1
+ test_platoon_manager.origin_leader_target_speed = leader_speed_profile[0]
+
+ # stage2: accelerate to next level
+ if stage == 2:
+ print('stage 2, accelerate to %f' % leader_speed_profile[1])
+ if leader_speed >= (leader_speed_profile[1]-3):
+ stage = 3
+ else:
+ test_platoon_manager.origin_leader_target_speed = leader_speed_profile[1]
+
+ # stage3: keep velocity for a fixed duration
+ if stage == 3:
+ print('stage 3, speed has keep for %f second' % (time_counter * 0.05))
+ if time_counter >= stage_duration / 0.05:
+ stage = 4
+ time_counter = 0
+ else:
+ time_counter += 1
+ test_platoon_manager.origin_leader_target_speed = leader_speed_profile[1]
+
+ # stage4: de-accelerate the velocity back to stage1
+ if stage == 4:
+ print('stage 4, de-accelerate to %f' % leader_speed_profile[0])
+ if leader_speed <= (leader_speed_profile[0] + 3):
+ stage = 5
+ else:
+ test_platoon_manager.origin_leader_target_speed = leader_speed_profile[0]
+
+ # stage5: keep the speed for another duration
+ if stage == 5:
+ print('stage5, speed has keep for %f second' % (time_counter * 0.05))
+ if time_counter >= stage_duration / 0.05:
+ break
+ else:
+ time_counter += 1
+ test_platoon_manager.origin_leader_target_speed = leader_speed_profile[0]
+
+ finally:
+ eval_manager.evaluate()
+
+ if opt.record:
+ client.stop_recorder()
+
+ world.apply_settings(origin_settings)
+
+ for platoon in platoon_list:
+ platoon.destroy()
+
+ for v in bg_veh_list:
+ v.destroy()