def communicate(self):
"""
Performs data transfer between vehicles, i.e., fetching data from
leading and front vehicles to feed the CACC algorithm. This function is
an adaptation of the original that can be found in the Michele Segata's
GitHub repository and in the utils.py file
"""
for vid, links in self._topology.iteritems():
# get data about platoon leader
leader_data = utils.get_par(links["leader"], cc.PAR_SPEED_AND_ACCELERATION)
(l_v, l_a, l_u, l_x, l_y, l_t) = cc.unpack(leader_data)
leader_data = cc.pack(l_v, l_u, l_x, l_y, l_t)
# get data about front vehicle
front_data = utils.get_par(links["front"], cc.PAR_SPEED_AND_ACCELERATION)
(f_v, f_a, f_u, f_x, f_y, f_t) = cc.unpack(front_data)
front_data = cc.pack(f_v, f_u, f_x, f_y, f_t)
# pass leader and front vehicle data to CACC
if (l_a < 0 and f_a < 0) or (l_a > 0 and f_a > 0):
utils.set_par(vid, cc.PAR_LEADER_SPEED_AND_ACCELERATION, leader_data)
else:
utils.set_par(vid, cc.PAR_LEADER_SPEED_AND_ACCELERATION, front_data)
utils.set_par(vid, cc.PAR_PRECEDING_SPEED_AND_ACCELERATION, front_data)
# compute GPS distance and pass it to the fake CACC
f_d = gap_between_vehicles(vid, links["front"])[0]
utils.set_par(vid, cc.PAR_LEADER_FAKE_DATA, cc.pack(l_v, l_u))
utils.set_par(vid, cc.PAR_FRONT_FAKE_DATA, cc.pack(f_v, f_u, f_d))
def main(demo_mode, real_engine, setter=None):
# used to randomly color the vehicles
random.seed(1)
start_sumo("cfg/freeway.sumo.cfg", False)
step = 0
state = GOING_TO_POSITION
while running(demo_mode, step, 6000):
# when reaching 60 seconds, reset the simulation when in demo_mode
if demo_mode and step == 6000:
start_sumo("cfg/freeway.sumo.cfg", True)
step = 0
state = GOING_TO_POSITION
random.seed(1)
traci.simulationStep()
if step == 0:
# create vehicles and track the joiner
topology = add_vehicles(N_VEHICLES, real_engine)
traci.gui.trackVehicle("View #0", JOINER)
traci.gui.setZoom("View #0", 20000)
if step % 10 == 1:
# simulate vehicle communication every 100 ms
communicate(topology)
if step == 100:
# at 1 second, let the joiner get closer to the platoon
topology = get_in_position(JOINER, FRONT_JOIN, topology)
if state == GOING_TO_POSITION:
# when the distance of the joiner is small enough, let the others
# open a gap to let the joiner enter the platoon
if get_distance(JOINER, FRONT_JOIN) < JOIN_DISTANCE + 1:
state = OPENING_GAP
topology = open_gap(BEHIND_JOIN, JOINER, topology, N_VEHICLES)
if state == OPENING_GAP:
# when the gap is large enough, complete the maneuver
if get_distance(BEHIND_JOIN, FRONT_JOIN) > 2 * JOIN_DISTANCE + 2:
state = COMPLETED
change_lane(JOINER, 0)
set_par(JOINER, cc.PAR_ACTIVE_CONTROLLER, cc.CACC)
set_par(JOINER, cc.PAR_CACC_SPACING, DISTANCE)
set_par(BEHIND_JOIN, cc.PAR_ACTIVE_CONTROLLER, cc.CACC)
set_par(BEHIND_JOIN, cc.PAR_CACC_SPACING, DISTANCE)
topology = reset_leader(BEHIND_JOIN, topology, N_VEHICLES)
if real_engine and setter is not None:
# if we are running with the dashboard, update its values
tracked_id = traci.gui.getTrackedVehicle("View #0")
if tracked_id != "":
(g, rpm) = cc.unpack(get_par(tracked_id, cc.PAR_ENGINE_DATA))
data = get_par(tracked_id, cc.PAR_SPEED_AND_ACCELERATION)
(v, a, u, x, y, t) = cc.unpack(data)
setter(rpm, g, v, a)
step += 1
traci.close()
def look_for_flags(self, pois, step):
"""
Evaluates whether a plane is less than 70 meters
from or past a flag within which lane change is permisssible
"""
vehicles = self._members
leader = vehicles[0]
flag_found = False
poi_index = "0"
vehicle_data = utils.get_par(leader, cc.PAR_SPEED_AND_ACCELERATION)
(v, a, u, x1, y1, t) = cc.unpack(vehicle_data)
time_to_pass = (((self.DISTANCE + self.VLENGTH) * self.N_VEHICLES) / v)
for poi in pois:
if self.states[-1] == self.FLAG_PASSED:
continue
if utils.get_dist_to_POI(leader, poi) < 70:
poi_index = int(poi.split("_")[2])
flag_found = True
self.time_flags_found[poi_index].append(
traci.simulation.getCurrentTime() / 1000)
if self.time_flags_found[poi_index][-1] - self.time_flags_found[
poi_index][-2] < time_to_pass:
self.states.append(self.FLAG_PASSED)
poi_index = int(poi.split("_")[2])
flag_found = False
flag_n_poi_index = [flag_found, poi_index]
return flag_n_poi_index
def move_to_next_best_lane(self, step, flag_poi_index_rank):
"""
Evaluates whether a plane should change lanes based on its
current location and past states. It prevents the plane from
changing lanes more than once at each lane change station
"""
times_flag_found = self.times_flag_found
flag_found = flag_poi_index_rank[0]
poi_index = flag_poi_index_rank[1]
plane_rank = flag_poi_index_rank[2]
vehicle_data = utils.get_par(self._members[0],
cc.PAR_SPEED_AND_ACCELERATION)
(v, a, u, x1, y1, t) = cc.unpack(vehicle_data)
time_to_pass = (((self.DISTANCE + self.VLENGTH) * self._plength) / v)
## Move to next best lane only if it's first of this flag and don't move within passing time
if traci.simulation.getCurrentTime(
) > times_flag_found['POI_' + str(poi_index)][plane_rank][
-2] + time_to_pass and flag_found == True:
vehicles = self._members
# print("vehs at move to next: {}".format(vehicles))
current_lane_num = traci.vehicle.getLaneIndex(vehicles[0])
next_best_lane = current_lane_num - 1
for vehicle in vehicles:
utils.change_lane(vehicle, next_best_lane)
self.states.append(self.SWITCHED)
else:
self.states.append(self.PASSING_FLAG)
def move_to_next_best_lane(self, step):
"""Change travel lane of platoon toone lane down"""
vehicle_data = utils.get_par(self._members[0],
cc.PAR_SPEED_AND_ACCELERATION)
(v, a, u, x1, y1, t) = cc.unpack(vehicle_data)
time_to_pass = (((self.DISTANCE + self.VLENGTH) * self.N_VEHICLES) / v)
vehicles = self._members
pois = ["exit_POI_0", "exit_POI_1", "exit_POI_2", "exit_POI_3"]
current_lane_num = traci.vehicle.getLaneIndex(vehicles[0])
next_best_lane = current_lane_num - 1
for vehicle in vehicles:
utils.change_lane(vehicle, next_best_lane)
def look_for_flags(self, pois, step):
"""
Evaluates whether a plane is within the ideal range to a flag
to execute a lane change. The Lane change must occur once and once only
subsequent flags found at the same location must be ignored
"""
vehicles = self._members
leader = vehicles[0]
flag_found = False
poi_index = "0"
plane_rank = 0
vehicle_data = utils.get_par(self._members[0],
cc.PAR_SPEED_AND_ACCELERATION)
(v, a, u, x1, y1, t) = cc.unpack(vehicle_data)
time_to_pass = (((self.DISTANCE + self.VLENGTH) * self._plength) / v)
for poi in pois:
if utils.get_dist_to_POI(leader, poi) < 100:
poi_index = int(poi.split("_")[2])
flag_found = True
cur_edge_ind = traci.vehicle.getRouteIndex(leader)
num_edge_2_finish = len(
traci.vehicle.getRoute(leader)[cur_edge_ind:])
if num_edge_2_finish == 3:
plane_rank = 0
self.times_flag_found[
'POI_' + str(poi_index)][plane_rank].append(
traci.simulation.getCurrentTime() / 1000)
elif num_edge_2_finish == 8:
plane_rank = 1
self.times_flag_found[
'POI_' + str(poi_index)][plane_rank].append(
traci.simulation.getCurrentTime() / 1000)
elif num_edge_2_finish == 13:
plane_rank = 2
self.times_flag_found[
'POI_' + str(poi_index)][plane_rank].append(
traci.simulation.getCurrentTime() / 1000)
### The following condition is meant to catch stray planes whose routes may be different from the 3,8,13 len(route) format ###
else:
plane_rank = 4
self.times_flag_found[
'POI_' + str(poi_index)][plane_rank].append(
traci.simulation.getCurrentTime() / 1000)
if self.times_flag_found['POI_' + str(poi_index)][plane_rank][
-1] - self.times_flag_found['POI_' + str(
poi_index)][plane_rank][-2] < time_to_pass:
self.states.append(self.FLAG_PASSED)
poi_index = int(poi.split("_")[2])
flag_found = False
flag_poi_index_rank = [flag_found, poi_index, plane_rank]
return flag_poi_index_rank
def main(demo_mode, real_engine=True, setter=None):
random.seed(1)
start_sumo("cfg/freeway.sumo.cfg", False)
step = 0
while running(demo_mode, step, 4000):
# when reaching 60 seconds, reset the simulation when in demo_mode
if demo_mode and step == 4000:
start_sumo("cfg/freeway.sumo.cfg", True)
step = 0
random.seed(1)
traci.simulationStep()
if step == 0:
# create vehicles and track one of them
add_vehicles()
traci.gui.trackVehicle("View #0", VEHICLES[TRACK])
traci.gui.setZoom("View #0", 20000)
if step == 100:
# at 1 second let them accelerate as much as they can
for vid in VEHICLES:
set_par(vid, cc.PAR_FIXED_ACCELERATION, cc.pack(1, 20))
# in the dashboard, show the engine information about the vehicle
# being tracked
tracked_id = traci.gui.getTrackedVehicle("View #0")
if setter is not None and tracked_id != "":
(g, rpm) = cc.unpack(get_par(tracked_id, cc.PAR_ENGINE_DATA))
data = get_par(tracked_id, cc.PAR_SPEED_AND_ACCELERATION)
(v, a, u, x, y, t) = cc.unpack(data)
setter(rpm, g, v, a)
step += 1
traci.close()
def look_for_flags(leader, pois, flags_at_pois, step):
flag_edges = ["n3", "n8", "n13", "n18"]
flag = False
vehicle = leader
if vehicle in flags_at_pois.keys() and flags_at_pois[vehicle]['state'][-1] == 'completed':# and traci.simulation.getCurrentTime()/1000 - flags_at_pois[leader]['time'][-1] < time_to_pass : # Might lead to false false flags if speed is low and time to pass increases
vehicle_data = get_par(vehicle, cc.PAR_SPEED_AND_ACCELERATION)
(v, a, u, x1, y1, t) = cc.unpack(vehicle_data)
time_to_pass = (((DISTANCE + LENGTH)* N_VEHICLES)/v)
print("time to pass is {} ".format(time_to_pass))
print("veh {} states are {}".format(vehicle, flags_at_pois[vehicle]['state']))
print("veh {} poitypes are {}".format(vehicle, flags_at_pois[vehicle]['poitype']))
print("time to pass is {} ".format(time_to_pass))
print("time since last change is {}".format(traci.simulation.getCurrentTime()/1000 - flags_at_pois[leader]['time'][-1]))
#if traci.simulation.getCurrentTime()/1000 - flags_at_pois[leader]['time'][-1] < time_to_pass :
curr_edge = traci.vehicle.getRoadID(vehicle)
poi_edges = [('exit_POI_0', 'n3'), ('exit_POI_1', 'n8'), ('exit_POI_2', 'n13'), ('exit_POI_3', 'n18')]
actual_poi = [item[0] for item in poi_edges if item[1] == curr_edge]
print("actual poi is {}".format(actual_poi))
print("in flag poi is {}".format(flags_at_pois[vehicle]['poi'][-1]))
if actual_poi[-1] == flags_at_pois[vehicle]['poi'][-1]:
print("veh {} has false flag identified".format(vehicle))
flag = False
flag_data = (flag, flags_at_pois[vehicle]['poitype'][-1])
return flag_data
else: # This entry isn't valid actual flag and dictionary entry mismatch
del flags_at_pois[vehicle]
flag = False
flag_data = (flag, 'dummy')
else:
d = defaultdict(list)
if traci.vehicle.getRoadID(vehicle) in flag_edges:
for poi in pois:
if get_dist_to_POI_new(vehicle,poi) <= 150:
print("Distance to poi new is{}".format(get_dist_to_POI_new(vehicle,poi)))
#print("Distance to poi old is{}".format(get_dist_to_POI_old(vehicle,poi)))
ctime = traci.simulation.getCurrentTime()/1000
poitype = traci.poi.getType(poi)
s = [('poi', poi), ('poitype', poitype),('time', ctime), ('state', 'InZone')]
for k, v in s:
d[k].append(v)
flags_at_pois[vehicle] = dict(d.items())
poi_pos = traci.poi.getPosition(poi)
veh_pos = traci.vehicle.getPosition(vehicle)
if flags_at_pois[vehicle]['poitype'][-1] == 'WE' and traci.vehicle.getRoadID(vehicle) == 'n3':
if flags_at_pois[vehicle]['state'][-1] == 'InZone':
print("Yaaaaaaaaay")
flags_at_pois[vehicle]['state'][-1] = 'changed'
print("Flags at Yaaaaaaaaay is {}".format(flags_at_pois))
flag = True
flag_data = (flag, poitype)
return flag_data
if flags_at_pois[vehicle]['poitype'][-1] == 'NS' and traci.vehicle.getRoadID(vehicle) == 'n8':
if flags_at_pois[vehicle]['state'][-1] == 'InZone':
print("Yaaaaaaaaay")
flags_at_pois[vehicle]['state'][-1] = 'changed'
print("Flags at Yaaaaaaaaay is {}".format(flags_at_pois))
flag = True
flag_data = (flag, poitype)
return flag_data
if flags_at_pois[vehicle]['poitype'][-1] == 'EW' and traci.vehicle.getRoadID(vehicle) == 'n13':
if flags_at_pois[vehicle]['state'][-1] == 'InZone':
print("Yaaaaaaaaay")
flags_at_pois[vehicle]['state'][-1] = 'changed'
print("Flags at Yaaaaaaaaay is {}".format(flags_at_pois))
flag = True
flag_data = (flag, poitype)
return flag_data
if flags_at_pois[vehicle]['poitype'][-1] == 'SN' and traci.vehicle.getRoadID(vehicle) == 'n18':
if flags_at_pois[vehicle]['state'][-1] == 'InZone':
print("Yaaaaaaaaay")
flags_at_pois[vehicle]['state'][-1] = 'changed'
print("Flags at Yaaaaaaaaay is {}".format(flags_at_pois))
flag = True
flag_data = (flag, poitype)
return flag_data
