def createSafeZone(self, foo):
self.geom = loader.loadModel(
'phase_14/models/neighborhoods/cogtropolis.egg')
self.geom.reparentTo(hidden)
for i in range(2):
bldgSectionNode = render.attachNewNode('bldgSection' + str(i))
bldgSectionNode.setPos(self.bldgSectionData[i][0])
bldgSectionNode.setHpr(self.bldgSectionData[i][1])
for point in self.bldgPoints:
bldg = loader.loadModel(
'phase_14/models/props/cogtropolis_big_building_1.egg')
bldg.reparentTo(bldgSectionNode)
bldg.setPos(point)
self.buildingSectionNodes.append(bldgSectionNode)
for data in self.tLightData:
node = render.attachNewNode('tlight-intersection-holder')
node.setPos(data[0])
node.setHpr(data[1])
light = TrafficLight.TrafficLight()
light.reparentTo(node)
light.startFlashing()
light2 = TrafficLight.TrafficLight(1)
light2.reparentTo(node)
light2.startFlashing()
self.trafficLightNodes.append(node)
self.trafficLights.append(light)
self.trafficLights.append(light2)
self.fog = Fog('CogTropolis-fog')
self.fog.setColor(0.3, 0.3, 0.3)
self.fog.setExpDensity(0.0075)
self.geom.flattenMedium()
gsg = base.win.getGsg()
if gsg:
self.geom.prepareScene(gsg)
def _getDistanceNextTrafficLight(vhId): TL = _getNextTrafficLight(vhId) if not TL: return None TL_cord = traci.junction.getPosition(TL[0]) #Note that junction and traffic light is not the same but have identical id's Vh_cord = traci.vehicle.getPosition(vhId) return math.sqrt(((TL_cord[0]-Vh_cord[0])**2) + ((TL_cord[1]-Vh_cord[1])**2)) - TrafficLight.getRadius(TL[0]) #-20
def getRecommentedSpeed(vhId,minDistance, maxtime):
distance = _getDistanceNextTrafficLight(vhId)
spans = _getGreenSpans(vhId, maxtime)
t = traci.simulation.getCurrentTime()
maxSpeed = traci.lane.getMaxSpeed(traci.vehicle.getLaneID(vhId))
#Vehicle is on a connection or there is an induction loop ahead.
if traci.vehicle.getRoadID(vhId).find("Ju_") >= 0 or TrafficLight.inductionLoopAhead(vhId):
return maxSpeed
#If there are no more traffic lights on route or the traffic light is too far away
# -> drive at max speed
if distance == None or distance >= minDistance:
return maxSpeed
#Calculate recommended speed
smax = 0 #Reaching just as light changes to green
smin = 0 #Reaching just before light changes to red
for span in spans:
deltaTbegin = (span[0] - t)
deltaTend = (span[1] - t)
#If slowest speed is larger than max speed
# -> look at next span
smin = distance/(deltaTend/1000)
if smin > maxSpeed:
continue
#If light is green
if(deltaTbegin <= 0):
smax = maxSpeed #Drive as fast possible
else:
smax = distance/(deltaTbegin/1000) #Set speed to reach connection when it changes
#Only drive at max speed
if smax > maxSpeed:
smax = maxSpeed
#Always recommend at least 15 km/h ~ 4.1 m/s
if smax < 4.1:
return 4.1
return smax
#No traffic light ahead
return maxSpeed
def initializeTrafficLight(self):
self.trali = TrafficLight.TrafficLight(self.incoming, self.outgoing)
def main():
args = parseArguments()
with open('config.json') as json_config_file:
data = json.load(json_config_file)
print(
"Simulation process of {name} starts!\nInitializing with configs ...".
format(**data))
rng_seed = data["rng_seed"]
main_scene = Scene(args.total_events, args.simulation_time, rng_seed)
# setting up input for vehicle_generate()
initial_time_stamps, poisson = main_scene.poisson_generate_timestamps()
num_trial = len(initial_time_stamps)
car_type = np.zeros(shape=(num_trial, ))
which_lane = np.zeros(shape=(num_trial, ))
car_direction = np.zeros(shape=(num_trial, ))
car_id = np.arange(0, num_trial)
for i in range(num_trial):
car_type[i] = randint(0, 2)
which_lane[i] = randint(0, 5)
car_direction[i] = randint(0, 2)
global_q = main_scene.vehicle_generate(initial_time_stamps, car_type,
car_direction, which_lane, car_id)
trafficLight = tl.TrafficLight(10, 3, 10) # greenTime,yellowTime,redTime
main_server = sr.server(trafficLight, global_q)
updated_global_q = main_server.run()
all_lane_q = {}
all_lane_q_final = []
all_lane_q_id = {}
all_lane_q_id_final = []
for i in range(len(updated_global_q)):
lane_q = []
lane_q_id = []
for j in range(len(updated_global_q[i])):
lane_q.append(updated_global_q[i][j].arrival_time +
updated_global_q[i][j].waiteTime)
all_lane_q_final.append(updated_global_q[i][j].arrival_time +
updated_global_q[i][j].waiteTime)
lane_q_id.append(updated_global_q[i][j].ID)
all_lane_q_id_final.append(updated_global_q[i][j].ID)
all_lane_q[i] = lane_q
all_lane_q_id[i] = lane_q_id
init_all_lane_q = {}
init_all_lane_q_final = []
init_all_lane_q_id = {}
init_all_lane_q_id_final = []
for i in range(len(global_q)):
lane_q = []
lane_q_id = []
for j in range(len(global_q[i])):
lane_q.append(global_q[i][j].arrival_time)
init_all_lane_q_final.append(global_q[i][j].arrival_time)
lane_q_id.append(global_q[i][j].ID)
init_all_lane_q_id_final.append(global_q[i][j].ID)
init_all_lane_q[i] = lane_q
init_all_lane_q_id[i] = lane_q_id
main_scene.pedestrain_generate()
# save_timestamps_plot(initial_time_stamps, [i for i in range(len(initial_time_stamps))], 'Initial_Timestamps')
# save_poisson_hist_plot(poisson, 'Events_Poisson')
# save_timestamps_plot(all_lane_q[0],all_lane_q_id[0],'Final_lane_0_Timestamps')
# save_timestamps_plot(all_lane_q[1],all_lane_q_id[1], 'Final_lane_1_Timestamps')
# save_timestamps_plot(all_lane_q[2], all_lane_q_id[2],'Final_lane_2_Timestamps')
# save_timestamps_plot(all_lane_q[3], all_lane_q_id[3],'Final_lane_3_Timestamps')
# save_timestamps_plot(all_lane_q[4], all_lane_q_id[4],'Final_lane_4_Timestamps')
# save_timestamps_plot(all_lane_q[5], all_lane_q_id[5],'Final_lane_5_Timestamps')
# save_timestamps_plot(all_lane_q_final,all_lane_q_id_final, 'Final_all_lanes_Timestamps')
# save_timestamps_plot(init_all_lane_q[0],init_all_lane_q_id[0],'Initial_lane_0_Timestamps')
# save_timestamps_plot(init_all_lane_q[1],init_all_lane_q_id[1], 'Initial_lane_1_Timestamps')
# save_timestamps_plot(init_all_lane_q[2], init_all_lane_q_id[2],'Initial_lane_2_Timestamps')
# save_timestamps_plot(init_all_lane_q[3], init_all_lane_q_id[3],'Initial_lane_3_Timestamps')
# save_timestamps_plot(init_all_lane_q[4], init_all_lane_q_id[4],'Initial_lane_4_Timestamps')
# save_timestamps_plot(init_all_lane_q[5], init_all_lane_q_id[5],'Initial_lane_5_Timestamps')
# compare_timestamps_plot(initial_time_stamps, [i for i in range(len(initial_time_stamps))],all_lane_q_final,all_lane_q_id_final, 'Comparison_all_lanes_Timestamps')
# compare_timestamps_plot(init_all_lane_q[0],init_all_lane_q_id[0],all_lane_q[0],all_lane_q_id[0], 'Comparison_lane_0_Timestamps')
# compare_timestamps_plot(init_all_lane_q[1],init_all_lane_q_id[1],all_lane_q[1],all_lane_q_id[1], 'Comparison_lane_1_Timestamps')
# compare_timestamps_plot(init_all_lane_q[2],init_all_lane_q_id[2],all_lane_q[2],all_lane_q_id[2], 'Comparison_lane_2_Timestamps')
# compare_timestamps_plot(init_all_lane_q[3],init_all_lane_q_id[3],all_lane_q[3],all_lane_q_id[3], 'Comparison_lane_3_Timestamps')
# compare_timestamps_plot(init_all_lane_q[4],init_all_lane_q_id[4],all_lane_q[4],all_lane_q_id[4], 'Comparison_lane_4_Timestamps')
# compare_timestamps_plot(init_all_lane_q[5],init_all_lane_q_id[5],all_lane_q[5],all_lane_q_id[5], 'Comparison_lane_5_Timestamps')
return
def test_red():
assert TrafficLight.trafficLight("red") == "Stop"
assert TrafficLight.trafficLight("yellow") == "Stand By"
assert TrafficLight.trafficLight("green") == "Run"
def createLight(self, a, b, c, d, e, f, g, h, C):
self.tLight = TrafficLight(a, b, c, d, e, f, g, h, C)
def _getGreenSpans(vhId, maxtime): TL = _getNextTrafficLight(vhId) if TL: return TrafficLight.getNextGreen(TL[0],TL[1], TL[2], maxtime) return []
#! /usr/bin/python
import time, sys
import RPi.GPIO as GPIO
#lib_path = os.path.abspath('../TrafficLight')
sys.path.append('../traffic_light')
import TrafficLight
led = TrafficLight.Led("yellow", 18, 2, status_blink=True)
while True:
led.turnOnLedAndAutoOff()
while led.ledStatus != 0:
time.sleep(1)
autoModule = p.def2(name="2")
zodoModule.start()
liderModule.start()
autoModule.start()
previous = 0
TrafficLight = False # False일때는 빨간불찾기모드 // True는 파란불찾기 모드 // 처음엔 일단 직전으로시작하므로 일단은 빨간불부터 찾아야한다.
t1 = time.time()
while (True):
if t2 == None:
ret, src = cap.read() # src는 프레임임(원본이미지)
ret2, src2 = cap2.read(
) # piCam is cap2 we gonna use this to trafficLight
src = cv2.resize(src, (640, 360)) # 가로640 세로 360
src2 = cv2.resize(src2, (640, 360)) # 가로640 세로 360
TrafficLight = TL.TrafficLight(ret2, src2, TrafficLight)
if customControlServer.OnOff == True:
if (TrafficLight == True or lider.liderflag == 1):
stopflag = 1
else:
stopflag = 0
t2 = time.time()
if t2 - t1 >= 0.01:
#==============setlocation부==========================================
[lines, lines_img] = AUTO.writeline(src, ret)
[result, ListLowerZero,
ListUpperZero] = AUTO.editline(lines, lines_img)
if result[0] == 1:
t1 = time.time() - 0.2
t2 = time.time()
xt = result[1]
def main():
args = parseArguments()
with open('config.json') as json_config_file:
data = json.load(json_config_file)
print(
"Simulation process of {name} starts!\nInitializing with configs ...".
format(**data))
rng_seed = data["rng_seed"]
main_scene = Scene(args.total_events, args.simulation_time,
args.total_people, rng_seed)
# setting up input for vehicle_generate()
initial_time_stamps, poisson = main_scene.poisson_generate_timestamps()
initial_time_stamps_people, poisson = main_scene.poisson_generate_timestamps_people(
)
np.random.seed(main_scene.rng_seed)
num_trial = len(initial_time_stamps)
car_type = np.zeros(shape=(num_trial, ))
which_lane = np.zeros(shape=(num_trial, ))
car_direction = np.zeros(shape=(num_trial, ))
car_id = np.arange(0, num_trial)
for i in range(num_trial):
car_type[i] = np.random.randint(3, size=1)
which_lane[i] = np.random.randint(6, size=1)
if (which_lane[i] == 4 or which_lane[i] == 3):
car_direction[i] = np.random.randint(2, size=1)
elif (which_lane[i] == 1 or which_lane[i] == 2):
car_direction[i] = 0
elif (which_lane[i] == 0 or which_lane[i] == 5):
dir = np.random.randint(2, size=1)
if (dir == 0):
car_direction[i] = 0
else:
car_direction[i] = dir + 1
# setting up input for pedestrian zss
initial_time_stamps_p = initial_time_stamps_people
# print("car time stamp")
# print(len(initial_time_stamps))
# print(initial_time_stamps)
# print("people time stamp")
# print(len(initial_time_stamps_p))
# print(initial_time_stamps_p)
num_trial_p = len(initial_time_stamps_p)
pedestrian_origin = np.zeros(shape=(num_trial_p, ))
pedestrian_destination = np.zeros(shape=(num_trial_p, ))
pedestrian_id = np.arange(0, num_trial_p)
for i in range(num_trial_p):
pedestrian_id[i] = i
pedestrian_origin[i] = np.random.randint(low=1, high=4, size=1)
pedestrian_destination[i] = np.random.randint(low=1, high=4, size=1)
if (pedestrian_origin[i] == pedestrian_destination[i]):
pedestrian_origin[i] = 5 - pedestrian_origin[i]
# zss
global_p_list = main_scene.pedestrian_generate(initial_time_stamps_p,
pedestrian_id,
pedestrian_origin,
pedestrian_destination)
global_q = main_scene.vehicle_generate(initial_time_stamps, car_type,
car_direction, which_lane, car_id)
trafficLight = tl.TrafficLight([0, 20], [20, 23],
[23, 61]) # greenTime,yellowTime,redTime
main_server = sr.server(trafficLight, global_q, global_p_list,
args.simulation_time)
updated_global_v_q, updated_global_p_q = main_server.run()
# Create Plots
output_plot = True
all_lane_q = {}
all_lane_q_final = []
all_lane_q_id = {}
all_lane_q_id_final = []
for i in range(len(updated_global_v_q)):
lane_q = []
lane_q_id = []
for j in range(len(updated_global_v_q[i])):
lane_q.append(updated_global_v_q[i][j].timeStamp[-1])
all_lane_q_final.append(updated_global_v_q[i][j].timeStamp[-1])
lane_q_id.append(updated_global_v_q[i][j].ID)
all_lane_q_id_final.append(updated_global_v_q[i][j].ID)
all_lane_q[i] = lane_q
all_lane_q_id[i] = lane_q_id
init_all_lane_q = {}
init_all_lane_q_final = []
init_all_lane_q_id = {}
init_all_lane_q_id_final = []
for i in range(len(updated_global_v_q)):
lane_q = []
lane_q_id = []
for j in range(len(updated_global_v_q[i])):
lane_q.append(updated_global_v_q[i][j].timeStamp[0])
init_all_lane_q_final.append(updated_global_v_q[i][j].timeStamp[0])
lane_q_id.append(updated_global_v_q[i][j].ID)
init_all_lane_q_id_final.append(updated_global_v_q[i][j].ID)
init_all_lane_q[i] = lane_q
init_all_lane_q_id[i] = lane_q_id
if output_plot:
save_timestamps_plot(initial_time_stamps,
[i for i in range(len(initial_time_stamps))],
'Initial_Timestamps')
save_poisson_hist_plot(poisson, 'Events_Poisson')
save_timestamps_plot(all_lane_q[0], all_lane_q_id[0],
'Final_lane_0_Timestamps')
save_timestamps_plot(all_lane_q[1], all_lane_q_id[1],
'Final_lane_1_Timestamps')
save_timestamps_plot(all_lane_q[2], all_lane_q_id[2],
'Final_lane_2_Timestamps')
save_timestamps_plot(all_lane_q[3], all_lane_q_id[3],
'Final_lane_3_Timestamps')
save_timestamps_plot(all_lane_q[4], all_lane_q_id[4],
'Final_lane_4_Timestamps')
save_timestamps_plot(all_lane_q[5], all_lane_q_id[5],
'Final_lane_5_Timestamps')
save_timestamps_plot(all_lane_q_final, all_lane_q_id_final,
'Final_all_lanes_Timestamps')
save_timestamps_plot(init_all_lane_q[0], init_all_lane_q_id[0],
'Initial_lane_0_Timestamps')
save_timestamps_plot(init_all_lane_q[1], init_all_lane_q_id[1],
'Initial_lane_1_Timestamps')
save_timestamps_plot(init_all_lane_q[2], init_all_lane_q_id[2],
'Initial_lane_2_Timestamps')
save_timestamps_plot(init_all_lane_q[3], init_all_lane_q_id[3],
'Initial_lane_3_Timestamps')
save_timestamps_plot(init_all_lane_q[4], init_all_lane_q_id[4],
'Initial_lane_4_Timestamps')
save_timestamps_plot(init_all_lane_q[5], init_all_lane_q_id[5],
'Initial_lane_5_Timestamps')
compare_timestamps_plot(initial_time_stamps,
[i for i in range(len(initial_time_stamps))],
all_lane_q_final, all_lane_q_id_final,
'Comparison_all_lanes_Timestamps')
compare_timestamps_plot(init_all_lane_q[0], init_all_lane_q_id[0],
all_lane_q[0], all_lane_q_id[0],
'Comparison_lane_0_Timestamps')
compare_timestamps_plot(init_all_lane_q[1], init_all_lane_q_id[1],
all_lane_q[1], all_lane_q_id[1],
'Comparison_lane_1_Timestamps')
compare_timestamps_plot(init_all_lane_q[2], init_all_lane_q_id[2],
all_lane_q[2], all_lane_q_id[2],
'Comparison_lane_2_Timestamps')
compare_timestamps_plot(init_all_lane_q[3], init_all_lane_q_id[3],
all_lane_q[3], all_lane_q_id[3],
'Comparison_lane_3_Timestamps')
compare_timestamps_plot(init_all_lane_q[4], init_all_lane_q_id[4],
all_lane_q[4], all_lane_q_id[4],
'Comparison_lane_4_Timestamps')
compare_timestamps_plot(init_all_lane_q[5], init_all_lane_q_id[5],
all_lane_q[5], all_lane_q_id[5],
'Comparison_lane_5_Timestamps')
