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()