def initialiseControlledCars(num_cars, controllers, accel_range,
yaw_rate_range, debug):
"""Initialises the specified number of cars with the indicated controllers. Any cars with unspecified controllers
get random action controllers by default. First car initialised (with the first provided controller) is the ego
vehicle by default.
num_cars: [int] number of cars to be initialised
controllers: [list] list of linear_controller_classes objects to be assigned to the cars
accel_range: [list] list indicating the lower and upper range for the linear acceleration
yaw_rate_range: [list] list inidicating the lower and upper range for the angular acceleration
debug: [bool] indicates whether or not debug print statements should be printed"""
cars = []
controllers += [
lcc.DrivingController(controller="random",
accel_range=accel_range,
yaw_rate_range=yaw_rate_range)
for _ in range(num_cars - len(controllers))
]
is_ego = True
for i in range(num_cars):
cars.append(vehicle_classes.Car(controllers[i], is_ego, i, debug))
is_ego = False
return cars
def runExperiment(experiment_order):
#debug mode
debug = False
exp_start_time = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
#########################################################################
#Setting Up Simulation
#Vehicle Dimensions
lane_width = 5
veh_length = 4.6
veh_width = 2
#Dynamics Parameters
accel_jerk = 3
yaw_rate_jerk = 10
dt = .1
speed_limit = 5.5
participant_accel_range = [-3,3]
accel_range = [-3,3]
yaw_rate_range = [-10,10] # degree per second^2
#Define the car(s)
#Human Controlled Car (this is the car being modelled)
lane_changer = vehicle_classes.Car(None,True,1,timestep=dt,car_params={"length":veh_length,"width":veh_width},debug=debug)
manual_controller = lcc.DrivingController(controller="manual",ego=lane_changer,speed_limit=speed_limit,yaw_rate_range=yaw_rate_range,accel_range=participant_accel_range,accel_jerk=accel_jerk,yaw_rate_jerk=yaw_rate_jerk)
lane_changer.addControllers({"default":manual_controller})
lane_changer.setController(tag="default")
#Other Car
lane_keeper = vehicle_classes.Car(None,False,2,timestep=dt,car_params={"length":veh_length,"width":veh_width},debug=debug)
#################################################################################
#Initialise Simulator here becayse need state definition
w,h = 1200,800
graphic_position = (0,0)
graphic_dimensions = (w,h)
sim = initialiseSimulator([lane_changer,lane_keeper],speed_limit,init_speeds=[5,5],lane_width=lane_width,dt=dt,graphic_position=graphic_position,graphic_dimensions=graphic_dimensions,debug=debug)
lane_keeper.heading = (lane_keeper.heading+180)%360
lane_keeper.initialisation_params["heading"] = lane_keeper.heading
lane_keeper.sense()
##################################################################################
#Write Instructions
pygame.init()
g_sim = sim.g_sim
screen = sim.g_sim.screen #This is messy, but the best way to get this I think
font_size = 25
space_size = 10
instructions = ["-Press and hold UP arrow to accelerate","-Press and hold DOWN arrow to decelerate","-Press and hold the LEFT arrow to turn anti-clockwise","-Press and hold RIGHT arrow to turn clockwise","-Press SPACE to pause/unpause simulation"]
write_instructions = writeText(screen,instructions,(0,int(h/5)),font_size,space_size)
###########################################################################################
#Setting up Controllers for Lane Keeping Vehicle
#Needs constant velocity controller AND IDM controller
#IDM controller
aggressive_idm_params = {"headway":0.0,"s0":0,"b":50} #aggressive
aggressive_idm_controller = lcc.DrivingController(controller="idm",ego=lane_keeper,other=lane_changer,timestep=dt,speed_limit=speed_limit,accel_range=accel_range,accel_jerk=accel_jerk,yaw_rate_range=yaw_rate_range,yaw_rate_jerk=yaw_rate_jerk,**aggressive_idm_params)
passive_idm_params = {"headway":1.6,"s0":2,"b":3} #passive
passive_idm_controller = lcc.DrivingController(controller="idm",ego=lane_keeper,other=lane_changer,timestep=dt,speed_limit=speed_limit,accel_range=accel_range,accel_jerk=accel_jerk,yaw_rate_range=yaw_rate_range,yaw_rate_jerk=yaw_rate_jerk,**passive_idm_params)
#Constant velocity controller
constant_controller = lcc.DrivingController(controller="constant",ego=lane_keeper,other=lane_changer,timestep=dt,speed_limit=speed_limit,accel_range=accel_range,accel_jerk=accel_jerk,yaw_rate_range=yaw_rate_range,yaw_rate_jerk=yaw_rate_jerk)
lane_keeper.addControllers({"default":constant_controller})
lane_keeper.setController(tag="default",controller=None)
lane_trigger = onLaneTrigger(lane_keeper,lane_changer)
consequent = changeController(lane_keeper,"idm")
triggers = {andTrigger([lane_trigger,relativeXRadiusTrigger(lane_keeper,lane_changer,0,'<')]):consequent}
lane_keeper.addTriggers(triggers)
#To end the simulation
heading_radius = 2
lane_changer_heading_trigger = headingTrigger(lane_changer,heading_radius)
y_dist_trigger = relativeYRadiusTrigger(lane_changer,lane_keeper,lane_width/4,'<')
sim_triggers = {andTrigger([lane_trigger,lane_changer_heading_trigger,y_dist_trigger]):sim.endSimulation,\
distanceTravelledTrigger(lane_keeper,105):sim.endSimulation,\
distanceTravelledTrigger(lane_changer,105):sim.endSimulation}
sim.addTriggers(sim_triggers)
#########################################################################################
#Run Experiments
for i,(lane_changer_type,lane_keeper_type) in enumerate(experiment_order):
if lane_keeper_type == "aggressive":
lane_keeper.addControllers({"idm":aggressive_idm_controller})
else:
lane_keeper.addControllers({"idm":passive_idm_controller})
######################################################################
#Set Graphic Simulator triggers
iteration_count = "Round: {}/{}".format(i+1,len(experiment_order))
if lane_changer_type == "aggressive":
#directive = "Get to the end of the lane as quickly as possible"
directive = "Drive as if in a rush and change lanes"
else:
#directive = "Stay in lane as safely as possible"
directive = "Drive cautiously and change lanes"
write_task = writeText(screen,[iteration_count,directive],(int(w/2),int(h/5)),font_size,space_size)
triggers = {trueFunc:write_instructions,trueFunc2:write_task}
g_sim.triggers = {}
g_sim.addTriggers(triggers)
######################################################################
#Run simulation
sim.reinitialise()
sim.runComplete() #will start from paused
######################################################################
#Extract log of behaviours from controller
num_cars = 2
lane_keeper_log_list = lane_keeper.controller.getLog()
lane_changer_log_list = manual_controller.getLog()
#Behaviour being modelled/learnt put in first
lane_keeper_state_list = [(x[0]["position"][0],x[0]["position"][1],x[0]["velocity"],math.radians(x[0]["heading"])) for x in lane_keeper_log_list]
lane_keeper_act_list = [(x[1][0],math.radians(x[1][1])) for x in lane_keeper_log_list]
#Behaviour of all other cars goes after
lane_changer_state_list = [(x[0]["position"][0],x[0]["position"][1],x[0]["velocity"],math.radians(x[0]["heading"])) for x in lane_changer_log_list]
lane_changer_act_list = [(x[1][0],math.radians(x[1][1])) for x in lane_changer_log_list]
results = open("{}/lane_change_results-{}-{}.txt".format(DATA_ADDRESS,exp_start_time,i),"w")
results.write("num_cars: {}\n".format(num_cars))
results.write("lane_width: {}\n".format(lane_width))
results.write("veh_length: {}\n".format(veh_length))
results.write("veh_width: {}\n".format(veh_width))
results.write("dt: {}\n".format(dt))
results.write("speed_limit: {}\n".format(speed_limit))
#Behaviour being modelled/learnt put in first
results.write("\nEgo\n")
results.write("Type: {}\n".format(lane_changer_type))
results.write("On Road: {}\n".format(int(lane_changer.on_road)))
results.write("Crash: {}\n".format(int(lane_changer.crashed)))
results.write("States: {}\n".format(lane_changer_state_list))
results.write("Actions: {}\n".format(lane_changer_act_list))
#Behaviour of all other cars goes after
results.write("\nOther\n")
results.write("Type: {}\n".format(lane_keeper_type))
results.write("On Road: {}\n".format(int(lane_keeper.on_road)))
results.write("Crash: {}\n".format(int(lane_keeper.crashed)))
results.write("States: {}\n".format(lane_keeper_state_list))
results.write("Actions: {}\n".format(lane_keeper_act_list))
results.close()
#Shut down the graphic screen
sim.wrapUp()
def runExperiment():
#debug mode
debug = False
#########################################################################
#Setting Up Simulation
#Vehicle Dimensions
lane_width = 5
veh_length = 4.6
veh_width = 2
#Dynamics Parameters
accel_jerk = 3
yaw_rate_jerk = 10
dt = .1
speed_limit = 5.5
participant_accel_range = [-6, 3]
accel_range = [-3, 3]
yaw_rate_range = [-10, 10] # degree per second^2
#Define the car(s)
#Human Controlled Car (this is the car being modelled)
lane_changer = vehicle_classes.Car(None,
True,
1,
timestep=dt,
car_params={
"length": veh_length,
"width": veh_width
},
debug=debug)
manual_controller = lcc.DrivingController(
controller="manual",
ego=lane_changer,
speed_limit=speed_limit,
yaw_rate_range=yaw_rate_range,
accel_range=participant_accel_range,
accel_jerk=accel_jerk,
yaw_rate_jerk=yaw_rate_jerk)
lane_changer.addControllers({"default": manual_controller})
lane_changer.setController(tag="default")
#Other Car
lane_keeper = vehicle_classes.Car(None,
False,
2,
timestep=dt,
car_params={
"length": veh_length,
"width": veh_width
},
debug=debug)
#################################################################################
#Initialise Simulator here becayse need state definition
w, h = 1280, 800
graphic_position = (0, 0)
graphic_dimensions = (w, h)
sim = initialiseSimulator([lane_changer, lane_keeper],
speed_limit,
init_speeds=[5, 0],
lane_width=lane_width,
dt=dt,
graphic_position=graphic_position,
graphic_dimensions=graphic_dimensions,
debug=debug)
lane_keeper.heading = (lane_keeper.heading + 180) % 360
lane_keeper.initialisation_params["heading"] = lane_keeper.heading
lane_keeper.sense()
##################################################################################
#Write Instructions
pygame.init()
g_sim = sim.g_sim
screen = sim.g_sim.screen #This is messy, but the best way to get this I think
font_size = 25
space_size = 15
instructions = [
"-Press and hold UP arrow to accelerate",
"-Press and hold DOWN arrow to decelerate",
"-Press and hold the LEFT arrow to turn anti-clockwise",
"-Press and hold RIGHT arrow to turn clockwise",
"-Press SPACE to pause/unpause simulation"
]
write_instructions = writeText(screen, instructions, (0, int(h / 5)),
font_size, space_size)
###########################################################################################
#Setting up Controllers for Lane Keeping Vehicle
#Constant velocity controller
constant_controller = lcc.DrivingController(controller="constant",
ego=lane_keeper,
other=lane_changer,
timestep=dt,
speed_limit=speed_limit,
accel_range=accel_range,
accel_jerk=accel_jerk,
yaw_rate_range=yaw_rate_range,
yaw_rate_jerk=yaw_rate_jerk)
lane_keeper.addControllers({"default": constant_controller})
lane_keeper.setController(tag="default", controller=None)
#########################################################################################
#Run Experiments
while True:
######################################################################
#Set Graphic Simulator triggers
triggers = {trueFunc: write_instructions}
g_sim.triggers = {}
g_sim.addTriggers(triggers)
######################################################################
#Run simulation
sim.reinitialise()
sim.runComplete() #will start from paused
#Shut down the graphic screen
sim.wrapUp()
def runExperiment(experiment_order):
#debug mode
debug = False
exp_start_time = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
##########################################################################################
#Setting Up Simulation
#Vehicle Dimensions
lane_width = 5
veh_length = 4.6
veh_width = 2
axle_length = 2.72 #default value
#Dynamics Parameters
accel_jerk = 3
yaw_rate_jerk = 10
dt = .1
init_speed = 5
speed_limit = 5.5
participant_accel_range = [-3,3]
accel_range = [-3,3]
yaw_rate_range = [-10,10] # degree per second^2
#Define the car(s)
#Human Controlled Car (this is the car being modelled)
lane_keeper = vehicle_classes.Car(None,True,1,timestep=dt,car_params={"length":veh_length,"width":veh_width},debug=debug)
manual_controller = lcc.DrivingController(controller="manual",ego=lane_keeper,speed_limit=speed_limit,yaw_rate_range=yaw_rate_range,accel_range=participant_accel_range,accel_jerk=accel_jerk,yaw_rate_jerk=yaw_rate_jerk)
lane_keeper.addControllers({"default":manual_controller})
lane_keeper.setController(tag="default")
#Other Car
lane_changer = vehicle_classes.Car(None,False,2,timestep=dt,car_params={"length":veh_length,"width":veh_width},debug=debug)
###########################################################################################
#Initialise Simulator here because need state definition
sim_position = (0,0)
w,h = 1200,800
sim_dimensions = (w,h)
sim = initialiseSimulator([lane_changer,lane_keeper],speed_limit,init_speeds=[init_speed,init_speed],lane_width=lane_width,dt=dt,debug=debug,sim_position=sim_position,sim_dimensions=sim_dimensions)
lane_keeper.heading = (lane_keeper.heading+180)%360
lane_keeper.initialisation_params["heading"] = lane_keeper.heading
lane_keeper.sense()
###########################################################################################
#Write Instructions
pygame.init()
g_sim = sim.g_sim
screen = sim.g_sim.screen #This is messy, but the best way to get this I think
font_size = 25
space_size = 10
instructions = ["-Press and hold UP arrow to accelerate","-Press and hold DOWN arrow to decelerate","-Press and hold the LEFT arrow to turn anti-clockwise","-Press and hold RIGHT arrow to turn clockwise","-Press SPACE to pause/unpause simulation"]
write_instructions = writeText(screen,instructions,(0,int(h/5)),font_size,space_size)
###########################################################################################
#Setting up Controllers for Lane Changing Vehicle
#First Lane Change Controller: Try lane change
right_lane_state = lane_keeper.state.copy() #lane keeper is initially centred in the right lane
right_lane_state["velocity"] = speed_limit
lcT = 15
lc_controller = lcc.DrivingController(controller="NA",ego=lane_changer,other=None,timestep=dt,speed_limit=speed_limit,accel_range=accel_range,accel_jerk=accel_jerk)
lc_controller.controller = LaneChangeController(ego=lane_changer,other=None,timestep=dt,speed_limit=speed_limit,accel_range=accel_range,dest_state=right_lane_state,axle_length=axle_length,T=lcT)
lane_changer.addControllers({"default":lc_controller})
lane_changer.setController(tag="default",controller=None)
#Second Lane Change Controller: Lane Change Successful (shorten up the remaining trajectory)
slcT = 5
slc_controller = lcc.DrivingController(controller="NA",ego=lane_changer,other=None,timestep=dt,speed_limit=speed_limit,accel_range=accel_range,accel_jerk=accel_jerk)
slc_controller.controller = LaneChangeController(ego=lane_changer,other=None,timestep=dt,speed_limit=speed_limit,accel_range=accel_range,dest_state=right_lane_state,axle_length=axle_length,T=slcT)
lane_changer.addControllers({"short_lane_change":slc_controller})
short_lane_change_trigger = onLaneTrigger(lane_changer,lane_keeper) and relativeXRadiusTrigger(lane_changer,lane_keeper,veh_length,'>')
short_lane_change_consequent = changeController(lane_changer,"short_lane_change")
#Third Lane Change Controller: Lane Change Rejected
left_lane_state = lane_changer.state.copy()
left_lane_state["velocity"] = 4 #2*init_speed - speed_limit
rlcT = 5
rlc_controller = lcc.DrivingController(controller="NA",ego=lane_changer,other=None,timestep=dt,speed_limit=speed_limit,accel_range=accel_range,accel_jerk=accel_jerk)
rlc_controller.controller = LaneChangeController(ego=lane_changer,other=None,timestep=dt,speed_limit=speed_limit,accel_range=accel_range,dest_state=left_lane_state,axle_length=axle_length,T=rlcT)
lane_changer.addControllers({"reverse_lane_change":rlc_controller})
#Fourth Lane Change Controller: Lane Change Behind
lcbT = 5
slow_right_lane_state = lane_keeper.state.copy() #lane keeper is initially centred in the right lane
behind_controller = lcc.DrivingController(controller="NA",ego=lane_changer,other=None,timestep=dt,speed_limit=speed_limit,accel_range=accel_range,accel_jerk=accel_jerk)
behind_controller.controller = LaneChangeController(ego=lane_changer,other=None,timestep=dt,speed_limit=speed_limit,accel_range=accel_range,dest_state=slow_right_lane_state,axle_length=axle_length,T=lcbT)
lane_changer.addControllers({"lane_change_behind":behind_controller})
lane_change_behind_trigger = relativeXRadiusTrigger(lane_keeper,lane_changer,veh_length,rel='>')
lane_change_behind_consequent = changeController(lane_changer,"lane_change_behind")
heading_fix_trigger = headingFixTrigger(lane_changer)
heading_fix_consequent = fixHeading(lane_changer)
#To end the simulation
heading_radius = 2
lane_changer_heading_trigger = headingTrigger(lane_changer,heading_radius)
y_dist_trigger = relativeYRadiusTrigger(lane_changer,lane_keeper,lane_width/4,'<')
sim_triggers = {andTrigger([distanceTravelledTrigger(lane_keeper,60),onLaneTrigger(lane_changer,lane_keeper),lane_changer_heading_trigger,y_dist_trigger]):sim.endSimulation,\
distanceTravelledTrigger(lane_keeper,105):sim.endSimulation,\
distanceTravelledTrigger(lane_changer,105):sim.endSimulation}
sim.addTriggers(sim_triggers)
##################################################################################################################
#Run Experiments
for i,(lane_keeper_type,lane_changer_type) in enumerate(experiment_order):
######################################################################
#Set other car triggers
lane_changer.triggers = {} #erase all previous triggers
if lane_changer_type == "aggressive":
risk_radius = veh_width
reverse_lane_change_trigger = andTrigger([radiusTrigger(lane_changer,lane_keeper,risk_radius), relativeXRadiusTrigger(lane_changer,lane_keeper,veh_length,'<')]) #aggressive
else:
reverse_lane_change_trigger = andTrigger([onLaneTrigger(lane_changer,lane_keeper), relativeXRadiusTrigger(lane_changer,lane_keeper,veh_length,'<')]) #passive
reverse_lane_change_consequent = changeController(lane_changer,"reverse_lane_change")
triggers = {short_lane_change_trigger:short_lane_change_consequent, reverse_lane_change_trigger:reverse_lane_change_consequent,lane_change_behind_trigger:lane_change_behind_consequent,\
heading_fix_trigger:heading_fix_consequent}
lane_changer.addTriggers(triggers)
######################################################################
#Set Graphic Simulator triggers
iteration_count = "Round: {}/{}".format(i+1,len(experiment_order))
if lane_keeper_type == "aggressive":
#idirective = "Get to the end of the lane as quickly as possible"
directive = "Drive as if in a rush and stay in your lane"
else:
directive = "Drive cautiously and stay in your lane"
write_task = writeText(screen,[iteration_count,directive],(int(w/2),int(h/5)),font_size,space_size)
triggers = {trueFunc:write_instructions,trueFunc2:write_task}
g_sim.triggers = {}
g_sim.addTriggers(triggers)
######################################################################
#Run simulation
sim.reinitialise()
#Controllers have to be individually reinitialised because they are bespoke
lc_controller.controller = LaneChangeController(ego=lane_changer,other=None,timestep=dt,speed_limit=speed_limit,accel_range=accel_range,dest_state=right_lane_state,axle_length=axle_length,T=lcT)
slc_controller.controller = LaneChangeController(ego=lane_changer,other=None,timestep=dt,speed_limit=speed_limit,accel_range=accel_range,dest_state=right_lane_state,axle_length=axle_length,T=slcT)
rlc_controller.controller = LaneChangeController(ego=lane_changer,other=None,timestep=dt,speed_limit=speed_limit,accel_range=accel_range,dest_state=left_lane_state,axle_length=axle_length,T=rlcT)
behind_controller.controller = LaneChangeController(ego=lane_changer,other=None,timestep=dt,speed_limit=speed_limit,accel_range=accel_range,dest_state=slow_right_lane_state,axle_length=axle_length,T=lcbT)
sim.runComplete() #will start from paused
######################################################################
#Extract log of behaviours from controller
num_cars = 2
lane_changer_log_list = lane_changer.controller.getLog()
lane_keeper_log_list = manual_controller.getLog()
#Behaviour being modelled/learnt put in first
lane_changer_state_list = [(x[0]["position"][0],x[0]["position"][1],x[0]["velocity"],math.radians(x[0]["heading"])) for x in lane_changer_log_list]
lane_changer_act_list = [(x[1][0],math.radians(x[1][1])) for x in lane_changer_log_list]
#Behaviour of all other cars goes after
lane_keeper_state_list = [(x[0]["position"][0],x[0]["position"][1],x[0]["velocity"],math.radians(x[0]["heading"])) for x in lane_keeper_log_list]
lane_keeper_act_list = [(x[1][0],math.radians(x[1][1])) for x in lane_keeper_log_list]
results = open("{}/lane_keeping_results-{}-{}.txt".format(DATA_ADDRESS,exp_start_time,i),"w")
results.write("num_cars: {}\n".format(num_cars))
results.write("lane_width: {}\n".format(lane_width))
results.write("veh_length: {}\n".format(veh_length))
results.write("veh_width: {}\n".format(veh_width))
results.write("dt: {}\n".format(dt))
results.write("speed_limit: {}\n".format(speed_limit))
#Behaviour being modelled/learnt put in first
results.write("\nEgo\n")
results.write("Type: {}\n".format(lane_keeper_type))
results.write("On Road: {}\n".format(int(lane_keeper.on_road)))
results.write("Crash: {}\n".format(int(lane_keeper.crashed)))
results.write("States: {}\n".format(lane_keeper_state_list))
results.write("Actions: {}\n".format(lane_keeper_act_list))
#Behaviour of all other cars goes after
results.write("\nOthers\n")
results.write("Type: {}\n".format(lane_changer_type))
results.write("On Road: {}\n".format(int(lane_changer.on_road)))
results.write("Crash: {}\n".format(int(lane_changer.crashed)))
results.write("States: {}\n".format(lane_changer_state_list))
results.write("Actions: {}\n".format(lane_changer_act_list))
results.close()
#Shut down the graphic screen
sim.wrapUp()
time_len = 3
traj_classes = TrajectoryClasses(time_len=time_len,
lane_width=lane_width,
accel_range=accel_range,
jerk=jerk)
traj_classes_2 = TrajectoryClasses(time_len=1.2 * time_len,
lane_width=lane_width,
accel_range=accel_range,
jerk=jerk)
traj_types = traj_classes.traj_types.keys()
#veh0 = vehicle_classes.Car(controller=None,is_ego=False,debug=debug,label="Lane-Lead-grey",timestep=dt)
veh1 = vehicle_classes.Car(controller=None,
is_ego=False,
debug=debug,
label="Lane-Follow",
timestep=dt)
#veh1 = vehicle_classes.Car(controller=None,is_ego=False,debug=debug,label="Non-Ego",timestep=dt)
veh2 = vehicle_classes.Car(controller=None,
is_ego=True,
debug=debug,
label="Ego",
timestep=dt)
sim = initialiseSimulator([veh1, veh2], speed_limit, graphics,
[speed_limit / 2, speed_limit / 2], lane_width,
False)
#sim = initialiseSimulator([veh1],speed_limit,True,[speed_limit/2],lane_width,False)
#veh2.heading = (veh2.heading+180)%360
#veh2.y_com = veh1.y_com