def __init__(self, vehicles, desired_gap, safe_gap, merging=False, lane_change=False):
"""
Constructor of the Platoon class. Every platoon is initialized with a
ordered list of vehicles. The first vehicle is the platoon leader and
uses an ACC controller, while the followers use a CACC controller. If
the vehicles list is composed by just one vehicle then a platoon of one
vehicle is created.
:param vehicles: list of vehicles that compose the platoon
:param desired_gap: distance between vehicles in a platoon
:param safe_gap: safety distance between vehicles for lane changes
:param merging: whether the new platoon is in a maneuver or not
:type vehicles: list[str]
:type desired_gap: float
:type safe_gap: float
:type merging: bool
"""
self._topology = dict()
self._states = [self.IDLE]
if len(vehicles) > 1:
traci.vehicle.setLaneChangeMode(vehicles[0], utils.FIX_LC)
utils.set_par(vehicles[0], cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
traci.vehicle.setSpeedFactor(vehicles[0], 1)
lanes = [traci.vehicle.getLaneIndex(vehicles[0])]
lane_ids = [traci.vehicle.getLaneID(vehicles[0])]
for i in range(1, len(vehicles)):
traci.vehicle.setLaneChangeMode(vehicles[i], utils.FIX_LC)
self._topology[vehicles[i]] = {"leader": vehicles[0], "front": vehicles[i - 1]}
self._states.append(self.WAITING)
utils.set_par(vehicles[i], cc.PAR_ACTIVE_CONTROLLER, cc.CACC)
min_gap = traci.vehicle.getMinGap(vehicles[i])
# The CACC controller tries to keep (desired_gap + min_gap)
# meters as intra-platoon spacing
utils.set_par(vehicles[i], cc.PAR_CACC_SPACING, desired_gap - min_gap)
traci.vehicle.setSpeedFactor(vehicles[i], 1.05)
lanes.append(traci.vehicle.getLaneIndex(vehicles[i]))
lane_ids.append(traci.vehicle.getLaneID(vehicles[i]))
self._members = vehicles
self._desired_gap = desired_gap
self._safe_gap = safe_gap
self._merging = merging
self._splitting = False
self._vehicles_splitting = [False] * len(vehicles)
leader_max_speed = traci.vehicle.getAllowedSpeed(vehicles[0])
lane = traci.vehicle.getLaneID(vehicles[0])
road_max_speed = traci.lane.getMaxSpeed(lane)
# If the platoon is fast enough merge all vehicles to the leftmost
# lane, else merge to the rightmost lane
self._desired_lane = max(lanes) if leader_max_speed > 0.9 * road_max_speed else min(lanes)
self._desired_lane_id = lane_ids[lanes.index(self._desired_lane)]
if lane_change:
for vehicle in self._members:
if traci.vehicle.getLaneIndex != self._desired_lane:
utils.change_lane(vehicle, self._desired_lane)
def add_vehicles(n, real_engine=False):
"""
Adds a platoon of n vehicles to the simulation, plus an additional one
farther away that wants to join the platoon
:param n: number of vehicles of the platoon
:param real_engine: set to true to use the realistic engine model,
false to use a first order lag model
:return: returns the topology of the platoon, i.e., a dictionary which
indicates, for each vehicle, who is its leader and who is its front
vehicle. The topology can the be used by the data exchange logic to
automatically fetch data from leading and front vehicle to feed the CACC
"""
# add a platoon of n vehicles
topology = {}
for i in range(n):
vid = "v.%d" % i
add_vehicle(vid, (n - i + 1) * (DISTANCE + LENGTH) + 50, 0, SPEED,
DISTANCE, real_engine)
change_lane(vid, 0)
if i == 0:
set_par(vid, cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
else:
set_par(vid, cc.PAR_ACTIVE_CONTROLLER, cc.CACC)
if i > 0:
topology[vid] = {"front": "v.%d" % (i - 1), "leader": LEADER}
# add a vehicle that wants to join the platoon
vid = "v.%d" % n
add_vehicle(vid, 10, 1, SPEED, DISTANCE, real_engine)
change_lane(vid, 1)
set_par(vid, cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
set_par(vid, cc.PAR_CACC_SPACING, JOIN_DISTANCE)
return topology
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 split(self):
"""
Executes the split maneuver finite state machine of every vehicle of
the platoon
"""
for i in range(1, len(self._members)):
if self._states[i] is self.LEAVING:
front_distance = gap_between_vehicles(self._members[i],
self._members[i - 1])[0]
if self._safe_gap - 1 < front_distance < self._safe_gap + 1:
self._states[i] = self.CHECK_LANE
else:
utils.set_par(self._members[i], cc.PAR_ACTIVE_CONTROLLER,
cc.FAKED_CACC)
utils.set_par(self._members[i], cc.PAR_CACC_SPACING,
self._safe_gap)
if self._states[i] is self.CHECK_LANE:
desired_lane = self._desired_lane - 1 if self._desired_lane > 1 else 0
desired_lane_id = self._desired_lane_id[:-1] + str(
desired_lane)
if it_is_safe_to_change_lane(self._members[i], desired_lane_id,
self._safe_gap - 1):
utils.change_lane(self._members[i], desired_lane)
utils.set_par(self._members[i], cc.PAR_ACTIVE_CONTROLLER,
cc.ACC)
traci.vehicle.setSpeedFactor(self._members[i], 1.0)
self._states[i] = self.IDLE
if self._states[i] is self.OPENING_GAP:
front_distance = gap_between_vehicles(self._members[i],
self._members[i - 1])[0]
if self._safe_gap - 1 < front_distance < self._safe_gap + 1 \
and self._states[i - 1] is self.CHECK_LANE or self._states[i - 1] is self.IDLE:
self._states[i] = self.WAITING
else:
utils.set_par(self._members[i], cc.PAR_CACC_SPACING,
self._safe_gap)
if self._states[i] is self.WAITING:
front_vehicle_index = self._members.index(
self._topology[self._members[i]]["front"])
if self._states[front_vehicle_index] is self.IDLE:
self._topology[self._members[i]]["front"] = self._members[
front_vehicle_index - 1]
if not self._vehicles_splitting[front_vehicle_index - 1]:
utils.set_par(self._members[i], cc.PAR_CACC_SPACING,
self._desired_gap)
self._states[i] = self.IDLE
if self.all_members_are_idle():
self._splitting = False
remaining_platoon = [
self._members[i] for i in range(len(self._members))
if not self._vehicles_splitting[i]
]
self.__init__(remaining_platoon, self._desired_gap, True)
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 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 add_vehicles():
"""
Adds the vehicles to the simulation
"""
for i in range(len(VEHICLES)):
vid = VEHICLES[i]
add_vehicle(vid, 10, i, 0, 5)
change_lane(vid, i)
set_par(vid, cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
set_par(vid, cc.CC_PAR_VEHICLE_ENGINE_MODEL,
cc.CC_ENGINE_MODEL_REALISTIC)
set_par(vid, cc.CC_PAR_VEHICLES_FILE, "vehicles.xml")
set_par(vid, cc.CC_PAR_VEHICLE_MODEL, vid)
set_par(vid, cc.PAR_FIXED_ACCELERATION, cc.pack(1, 0))
def add_vehicles(n, batch_num, list_of_leaders, platoon_len, fromEdge,
real_engine):
"""
This function adds n number of vehicles as a platoon
to 3 lanes of the source edge
It also assigns the route of each member of a lane platoon
based on the lane it is found on
Param n : Total len of platoon
Param batch num: nth batch of planes inserted so far starting
for the particular lane
Param platoon_len: len of platoon in secondary formation
Param sourceEdge: Which sourcr to add vehicles to
"""
start_from = n * batch_num # start naming vehs from this number
end_at = start_from + platoon_len # stop at this number
index = fromEdge.split("e")[1]
if index == "0":
exitEdges = ['exit0', 'exit1', 'exit2']
elif index == "1":
exitEdges = ['exit1', 'exit2', 'exit3']
elif index == "2":
exitEdges = ['exit2', 'exit3', 'exit1']
else:
exitEdges = ['exit3', 'exit1', 'exit2']
# Add vehicles to lane 1 of the source edge and assign them 1st exit ramp
for i in range(start_from, end_at):
lane = 1
vid = "v.%d" % i
toEdge = exitEdges[0]
route = "route_" + index + "_" + str(lane - 1)
add_vehicle(vid, route, (end_at - i + 1) * (DISTANCE + LENGTH) + 100,
lane, SPEED, DISTANCE, real_engine)
set_par(vid, cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
change_lane(vid, lane)
start_from = start_from + platoon_len # start naming vehs from this num
end_at = start_from + platoon_len # stop here
# Add vehicles to lane 2 of the source edge and assign them 2nd exit ramp
for i in range(start_from, end_at):
lane = 2
vid = "v.%d" % i
toEdge = exitEdges[1]
route = "route_" + index + "_" + str(lane - 1)
add_vehicle(vid, route, (end_at - i + 1) * (DISTANCE + LENGTH) + 100,
lane, SPEED, DISTANCE, real_engine)
set_par(vid, cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
change_lane(vid, lane)
start_from = start_from + platoon_len # start naming from this number
end_at = start_from + platoon_len # stop naming from this
# Add vehicles to lane 3 of the source edge and assign them 3rd exit ramp
for i in range(start_from, end_at):
lane = 3
vid = "v.%d" % i
toEdge = exitEdges[2]
route = route = "route_" + index + "_" + str(lane - 1)
add_vehicle(vid, route, (end_at - i + 1) * (DISTANCE + LENGTH) + 100,
lane, SPEED, DISTANCE, real_engine)
set_par(vid, cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
change_lane(vid, lane)
def add_vehicles(n, batch_num, platoon_len, fromEdge, real_engine):
# route param: for each source edge there are four possible routes
start_from = n*batch_num
end_at = start_from + platoon_len
index = fromEdge.split("e")[1]
if index == "0":
exitEdges =['exit0', 'exit1', 'exit2']
elif index=="1":
exitEdges =['exit1', 'exit2', 'exit3']
elif index == "2":
exitEdges =['exit2', 'exit3', 'exit1']
else: # index = "3"
exitEdges =['exit3', 'exit1', 'exit2']
for i in range(start_from, end_at):
lane = 1
vid = "v.%d" % i
toEdge= exitEdges[0]
route = "route_"+index+"_"+str(lane-1)
add_vehicle(vid, route, (end_at - i + 1) * (DISTANCE + LENGTH) +
100, lane, SPEED, DISTANCE, real_engine)
set_par(vid, cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
change_lane(vid, lane)
start_from = start_from + platoon_len
end_at = start_from + platoon_len
for i in range(start_from, end_at):
lane = 2
vid = "v.%d" % i
toEdge= exitEdges[1]
route = "route_"+index+"_"+str(lane-1)
add_vehicle(vid, route, (end_at - i + 1) * (DISTANCE + LENGTH) +
100, lane, SPEED, DISTANCE, real_engine)
set_par(vid, cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
change_lane(vid, lane)
start_from = start_from + platoon_len
end_at = start_from + platoon_len
for i in range(start_from, end_at):
lane = 3
vid = "v.%d" % i
toEdge= exitEdges[2]
route = route = "route_"+index+"_"+str(lane-1)
add_vehicle(vid, route, (end_at - i + 1) * (DISTANCE + LENGTH) +
100, lane, SPEED, DISTANCE, real_engine)
set_par(vid, cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
change_lane(vid, lane)
def merge(self):
"""
Executes the merge maneuver finite state machine of every vehicle of
the platoon
"""
if it_is_safe_to_change_lane(self._members[0], self._desired_lane_id, self._safe_gap - 1):
utils.change_lane(self._members[0], self._desired_lane)
for i in range(1, len(self._members)):
if self._states[i] is self.WAITING:
# The gap-making stage is done in a sequential manner to avoid
# huge decelerations and possible dangerous situations as is
# done in:
# E. Semsar-kazerooni, J. Ploeg, "Interaction protocols for
# cooperative merging and lane reduction scenarios," IEEE
# Conference on Intelligent Transportation Systems (ITSC), Las
# Palmas, Spain, 2015, pp. 1964-1970
lane = traci.vehicle.getLaneIndex(self._members[i])
lane_front = traci.vehicle.getLaneIndex(self._members[i - 1])
if lane != lane_front:
utils.set_par(self._members[i], cc.PAR_ACTIVE_CONTROLLER, cc.FAKED_CACC)
if self._states[i - 2] is not self.WAITING and self._states[i - 2] is not self.GOING_TO_POSITION \
or i == 1:
self._topology[self._members[i]]["leader"] = self._members[0]
self._states[i] = self.GOING_TO_POSITION
else:
self._topology[self._members[i]]["leader"] = self._members[i - 1]
if self._states[i] is self.GOING_TO_POSITION:
lane = traci.vehicle.getLaneIndex(self._members[i])
lane_front = traci.vehicle.getLaneIndex(self._members[i - 1])
if lane != lane_front:
utils.set_par(self._members[i], cc.PAR_ACTIVE_CONTROLLER, cc.FAKED_CACC)
else:
utils.set_par(self._members[i], cc.PAR_ACTIVE_CONTROLLER, cc.CACC)
front_distance = gap_between_vehicles(self._members[i], self._members[i - 1])[0]
no_need_to_open_gap = already_in_lane(self._members[i], self._desired_lane)
no_need_to_open_gap = no_need_to_open_gap and already_in_lane(self._members[i - 1], self._desired_lane)
if self._safe_gap - 1 < front_distance < self._safe_gap + 1 \
and self._states[i - 1] is not self.GOING_TO_POSITION or no_need_to_open_gap:
self._states[i] = self.CHECK_LANE
else:
utils.set_par(self._members[i], cc.PAR_CACC_SPACING, self._safe_gap)
if self._states[i] is self.CHECK_LANE:
lane = traci.vehicle.getLaneIndex(self._members[i])
lane_front = traci.vehicle.getLaneIndex(self._members[i - 1])
if lane != lane_front:
utils.set_par(self._members[i], cc.PAR_ACTIVE_CONTROLLER, cc.FAKED_CACC)
else:
utils.set_par(self._members[i], cc.PAR_ACTIVE_CONTROLLER, cc.CACC)
if it_is_safe_to_change_lane(self._members[i], self._desired_lane_id, self._safe_gap - 1)\
or already_in_lane(self._members[i], self._desired_lane):
utils.change_lane(self._members[i], self._desired_lane)
utils.set_par(self._members[i], cc.PAR_ACTIVE_CONTROLLER, cc.CACC)
min_gap = traci.vehicle.getMinGap(self._members[i])
# The CACC controller tries to keep (cacc_spacing + min_gap)
# meters as intra-platoon spacing
utils.set_par(self._members[i], cc.PAR_CACC_SPACING, self._desired_gap - min_gap)
self._states[i] = self.CLOSING_GAP
if self._states[i] is self.CLOSING_GAP:
front_distance = gap_between_vehicles(self._members[i], self._members[i - 1])[0]
if self._desired_gap - 1 < front_distance < self._desired_gap + 1:
self._states[i] = self.IDLE
if self.all_members_are_idle():
self._merging = False
def change_lanes(dest_lane):
for i in range(len(dest_lane)):
for vehicle in dest_lane[i]:
change_lane(vehicle, i + 1)