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" timestr = time.strftime("%Y%m%d-%H%M%S") file = open("../data_collection/FrW" + timestr + ".txt", "w+" ) print("start freeway recordings: ") world_snapshot = self.env.world.get_snapshot() tm = world_snapshot.timestamp file.write("the experiment starts from " + str(tm.elapsed_seconds) + "(seconds)\n") # main loop for control while True: world_snapshot = self.env.world.get_snapshot() ego_id = (int)(ego_uniquename.split("_")[1]) ego_actor = world_snapshot.find(ego_id) world_snapshot = self.env.world.get_snapshot() tm = world_snapshot.timestamp file.write("time: " + str(tm.elapsed_seconds)+"(seconds)\n") ego_actor_transform = ego_actor.get_transform() file.write("location: " + str(ego_actor_transform.location) + "(meters)\n" ) ego_actor_velocity = ego_actor.get_velocity() file.write("Rotation: " + str(ego_actor_velocity) + "(degrees)\n") ego_actor_angular_velocity = ego_actor.get_angular_velocity() file.write("Angular velocity: " + str(ego_actor.get_angular_velocity()) + "(rad/s)\n") ego_actor_acceleration = ego_actor.get_acceleration() file.write("Acceleration: " + str(ego_actor.get_acceleration()) + "(m/s2)\n") 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()
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) vehicle_list.append(vehicle) ego_uniquename = ego_vehicle_config["uniquename"] timestr = time.strftime("%Y%m%d-%H%M%S") file = open("../data_collection/Urb" + timestr + ".txt", "w+") print("start urban recordings: ") while True: world_snapshot = env.world.get_snapshot() ego_id = (int)(ego_uniquename.split("_")[1]) ego_actor = world_snapshot.find(ego_id) world_snapshot = env.world.get_snapshot() tm = world_snapshot.timestamp file.write("time: " + str(tm.elapsed_seconds) + "(seconds)\n") ego_actor_transform = ego_actor.get_transform() file.write("location: " + str(ego_actor_transform.location) + "(meters)\n") ego_actor_velocity = ego_actor.get_velocity() file.write("Rotation: " + str(ego_actor_velocity) + "(degrees)\n") ego_actor_angular_velocity = ego_actor.get_angular_velocity() file.write("Angular velocity: " + str(ego_actor.get_angular_velocity()) + "(rad/s)\n") ego_actor_acceleration = ego_actor.get_acceleration() file.write("Acceleration: " + str(ego_actor.get_acceleration()) + "(m/s2)\n") 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 SectionBackend(self, spectator_mode=None): ''' back end function for the freeway Parameters ---------- spectator_mode : string, optional the spectator mode, valid value is "first_person". The default is None. Returns ------- None. ''' init_section = self.section_list[0] ego_vehicle = VehicleControlFreeway(self.env, init_section.ego_vehicle, self.env.delta_seconds) ego_uniquename = init_section.ego_vehicle["uniquename"] 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)) for vehicle_config in init_section.subject_follow_vehicle: subject_follow_vehicle.append( LeadFollowVehicleControl(self.env, vehicle_config, self.env.delta_seconds)) for vehicle_config in init_section.left_lead_vehicle: left_lead_vehicle.append( LeadFollowVehicleControl(self.env, vehicle_config, self.env.delta_seconds)) for vehicle_config in init_section.subject_lead_vehicle: subject_lead_vehicle.append( LeadFollowVehicleControl(self.env, vehicle_config, self.env.delta_seconds)) # store the current section that is functioning curr_section = init_section self.section_list.pop(0) # main loop for control while True: 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) # 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: ego_end = ego_vehicle.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
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) # 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: 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 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" # 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: 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()