def IntersectionBackend(env, intersection_list):
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()
# 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)
ego_vehicle = FollowVehicleControl(env, ego_vehicle_config,
env.delta_seconds)
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)
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
follow_vehicle = FollowVehicleControl(env, follow_vehicle_config,
env.delta_seconds)
end_follow = False
follow_vehicle.use_distance_mode(ego_vehicle_config["follow_distance"])
else:
spectator_vehicle = ego_vehicle_config
end_follow = True
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)
vehicle_list.append(vehicle)
for vehicle_config in init_intersection.left_vehicle:
vehicle = VehicleControl(env, vehicle_config, env.delta_seconds)
vehicle_list.append(vehicle)
for vehicle_config in init_intersection.right_vehicle:
vehicle = VehicleControl(env, vehicle_config, env.delta_seconds)
vehicle_list.append(vehicle)
for vehicle_config in init_intersection.ahead_vehicle:
vehicle = VehicleControl(env, vehicle_config, env.delta_seconds)
vehicle_list.append(vehicle)
while True:
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)
spectator_transform = get_ego_left_spectator(spectator_vehicle_transform)
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)
vehicle_list.append(vehicle)
for vehicle_config in intersection_list[ii].left_vehicle:
vehicle = VehicleControl(env, vehicle_config,
env.delta_seconds)
vehicle_list.append(vehicle)
for vehicle_config in intersection_list[ii].right_vehicle:
vehicle = VehicleControl(env, vehicle_config,
env.delta_seconds)
vehicle_list.append(vehicle)
for vehicle_config in intersection_list[ii].ahead_vehicle:
vehicle = VehicleControl(env, vehicle_config,
env.delta_seconds)
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:
end_ego = ego_vehicle.pure_pursuit_control_wrapper()
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 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 IntersectionBackend(env, intersection_list):
vehicle_list = []
started_intersection_list = []
ego_vehicle = intersection_list[
0].ego_vehicle #init_intersection.ego_vehicle
lead_vehicle = intersection_list[0].lead_vehicle
follow_vehicle = intersection_list[0].follow_vehicle
spectator = env.world.get_spectator()
# assign the first full path vehicle, to determine whether
# each intersection should start
if not lead_vehicle == None:
first_full_path_vehicle_name = lead_vehicle["uniquename"]
else:
first_full_path_vehicle_name = ego_vehicle["uniquename"]
# assign the vehicle for the spectator to follow
if follow_vehicle != None:
spectator_vehicle = follow_vehicle
else:
spectator_vehicle = ego_vehicle
# get the init intersection
init_intersection = intersection_list.pop(0)
for vehicle_config in init_intersection.subject_vehicle:
# initialize vehicles by different type (ego,lead,follow,other)
vehicle = VehicleControl(env, vehicle_config, env.delta_seconds)
vehicle_list.append(vehicle)
for vehicle_config in init_intersection.left_vehicle:
vehicle = VehicleControl(env, vehicle_config, env.delta_seconds)
vehicle_list.append(vehicle)
for vehicle_config in init_intersection.right_vehicle:
vehicle = VehicleControl(env, vehicle_config, env.delta_seconds)
vehicle_list.append(vehicle)
for vehicle_config in init_intersection.ahead_vehicle:
vehicle = VehicleControl(env, vehicle_config, env.delta_seconds)
vehicle_list.append(vehicle)
while True:
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)
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)
vehicle_list.append(vehicle)
for vehicle_config in intersection_list[ii].left_vehicle:
vehicle = VehicleControl(env, vehicle_config,
env.delta_seconds)
vehicle_list.append(vehicle)
for vehicle_config in intersection_list[ii].right_vehicle:
vehicle = VehicleControl(env, vehicle_config,
env.delta_seconds)
vehicle_list.append(vehicle)
for vehicle_config in intersection_list[ii].ahead_vehicle:
vehicle = VehicleControl(env, vehicle_config,
env.delta_seconds)
vehicle_list.append(vehicle)
# move the intersection to started intersection list
intersection = intersection_list.pop(ii)
started_intersection_list.append(intersection)
if len(vehicle_list) == 0:
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 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)