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 set_arrived_free(self):
"""Set the active controller of vehicles arriving to ACC to dissolve platoons"""
vehicles = self._members
leader = vehicles[0]
if traci.vehicle.getLaneID(leader).split("_")[1] == "0":
for vehicle in vehicles:
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
def set_arrived_free(ARR_EDGES):
for edge in ARR_EDGES:
lane_of_interest = edge +"_0"
vehicles = traci.lane.getLastStepVehicleIDs(lane_of_interest)
for vehicle in vehicles:
if traci.vehicle.getTypeID(vehicle)=="vtypeauto":
set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
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 set_arrived_free(self):
"""Set arriving vehicle active controllers to ACC"""
vehicles = self._members
leader = vehicles[0]
if traci.vehicle.getLaneID(leader).split("_")[1] == "0":
for vehicle in vehicles:
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME, 0.2)
#utils.set_par(vehicle, cc.PAR_FIXED_ACCELERATION, 1)
print("Vehicles set free : {}".format(vehicles))
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, 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 get_in_position(jid, fid, topology):
"""
Makes the joining vehicle get close to the join position. This is done by
changing the topology and setting the leader and the front vehicle for
the joiner. In addition, we increase the cruising speed and we switch to
the "fake" CACC, which uses a given GPS distance instead of the radar
distance to compute the control action
:param jid: id of the joiner
:param fid: id of the vehicle that will become the predecessor of the joiner
:param topology: the current platoon topology
:return: the modified topology
"""
topology[jid] = {"leader": LEADER, "front": fid}
set_par(jid, cc.PAR_CC_DESIRED_SPEED, SPEED + 15)
set_par(jid, cc.PAR_ACTIVE_CONTROLLER, cc.FAKED_CACC)
return topology
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 update_desired_speed_and_lane(self):
"""
The ACC and CACC of Plexe SUMO seem to limit the desired speed
automatically to 13.9m/s, so this function should be called at every
time step to ensure that the maximum speed is updated after every edge
change. This function also changes the desired lane when a platoon
composed by one single vehicle has performed a manual lane change
"""
for vehicle in self._members:
# getAllowedSpeed() also includes the speed factor
lane_max_speed = traci.vehicle.getAllowedSpeed(vehicle)
vehicle_max_speed = traci.vehicle.getMaxSpeed(vehicle)
desired_speed = min(lane_max_speed, vehicle_max_speed)
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED, desired_speed)
if len(self._members) == 1:
self._desired_lane = traci.vehicle.getLaneIndex(self._members[0])
self._desired_lane_id = traci.vehicle.getLaneID(self._members[0])
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, 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 open_gap(vid, jid, topology, n):
"""
Makes the vehicle that will be behind the joiner open a gap to let the
joiner in. This is done by creating a temporary platoon, i.e., setting
the leader of all vehicles behind to the one that opens the gap and then
setting the front vehicle of the latter to be the joiner. To properly
open the gap, the vehicle leaving space switches to the "fake" CACC,
to consider the GPS distance to the joiner
:param vid: vehicle that should open the gap
:param jid: id of the joiner
:param topology: the current platoon topology
:param n: total number of vehicles currently in the platoon
:return: the modified topology
"""
index = int(vid.split(".")[1])
for i in range(index + 1, n):
# temporarily change the leader
topology["v.%d" % i]["leader"] = vid
# the front vehicle if the vehicle opening the gap is the joiner
topology[vid]["front"] = jid
set_par(vid, cc.PAR_ACTIVE_CONTROLLER, cc.FAKED_CACC)
set_par(vid, cc.PAR_CACC_SPACING, JOIN_DISTANCE)
return topology
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 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 pla_speed_spacing(self, topology):
"""
This function is designed to control primary, same route secondary and different route
secondary platoons.
"""
vehicles = self._members
lane = self._ID
first_of_lane = traci.lane.getLastStepVehicleIDs(lane)[::-1][0]
lane_num = traci.vehicle.getLaneID(first_of_lane).split("_")[1]
if vehicles[0] == first_of_lane:
for vehicle in vehicles:
# Vehicles controlled here are lane leaders
if vehicle == vehicles[0]:
if lane_num == "0":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED)
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.ACC)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME, 1)
if lane_num == "1":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED + 4)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME, 1)
if lane_num == "2":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED + 7)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME, 1)
if lane_num == "3":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED + 10)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME, 1)
elif vehicle in self._children_vehicles:
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER, cc.CACC)
utils.set_par(vehicle, cc.PAR_CACC_SPACING, self.DISTANCE)
else:
# Vehicles controlled here are lane followers of the first platoon in the lane
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER, cc.CACC)
utils.set_par(vehicle, cc.PAR_CACC_SPACING, self.DISTANCE)
topology = topology
return topology
### Secondary Platoon Control
if vehicles[0] != traci.lane.getLastStepVehicleIDs(lane)[::-1][0]:
sec_leader_index = traci.lane.getLastStepVehicleIDs(
lane)[::-1].index(vehicles[0])
last_veh_plat_ahead = traci.lane.getLastStepVehicleIDs(lane)[::-1][
sec_leader_index - 1]
lane_num = traci.vehicle.getLaneID(vehicles[0]).split("_")[1]
for vehicle in vehicles:
if vehicle == vehicles[0]: #
# Vehicles controlled here are secondary platoons with same route as lane leader
if traci.vehicle.getRouteID(
vehicle) == traci.vehicle.getRouteID(
last_veh_plat_ahead) and utils.get_distance(
vehicle, last_veh_plat_ahead) < 100:
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.CACC)
utils.set_par(vehicle, cc.PAR_CACC_SPACING,
self.DISTANCE)
else:
# Vehicles controlled here are temporary secondary platoon leaders in same lane but diff routes as lane leader
# or Distance between platoons is greater than 100m
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.ACC)
if lane_num == "0":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED)
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.ACC)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME, 2)
if lane_num == "1":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED + 4)
if lane_num == "2":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED + 7)
if lane_num == "3":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED + 10)
else:
# All secondary followers control
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER, cc.CACC)
utils.set_par(vehicle, cc.PAR_CACC_SPACING, self.DISTANCE)
return topology
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 pla_speed_spacing(self, topology, states):
'''
This funnction is designed to control platoon speed and spacing
This method is invoked every time step to ensure platoons have the
most current speed and spacing commands. It can control primary and
secondary platoon where a given lane contains more than one platoon.
It also controls the platoon in the presence of obstructions by adjusting
the speed of the platoon to avoid collisions with the obstructing vehicle
'''
vehicles = self._members
lane = self._ID
first_of_lane = traci.lane.getLastStepVehicleIDs(lane)[::-1][0]
## Check if vehicle is obstructed
try:
vehicles_0_states = states[vehicles[0]][-1]
except IndexError:
print("{} has no avoidance states entering no prior state control".
format(vehicles[0]))
if vehicles[
0] == first_of_lane: ## There is no preceeding obstructing vehicle on the lane
for vehicle in vehicles:
lane_num = traci.vehicle.getLaneID(vehicle).split("_")[1]
if traci.vehicle.getRouteID(vehicle).startswith(':'):
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.CACC)
utils.set_par(vehicle, cc.PAR_CACC_SPACING,
self.DISTANCE)
continue
# Vehicles controlled here are lane leaders
if vehicle == vehicles[0]:
veh_route_edges = traci.vehicle.getRoute(vehicle)
veh_rou_index = traci.vehicle.getRouteIndex(vehicle)
next_veh_edge = veh_route_edges[veh_rou_index + 1]
next_veh_lane = next_veh_edge.split(
"_")[0] + "_" + lane_num
if traci.lane.getLastStepVehicleNumber(
next_veh_lane
) == 0: # There is no vehicle on the nexl edge lane
if lane_num == "0":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED)
utils.set_par(vehicle,
cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME,
0.5)
print("{} under full primary control".format(
vehicle))
if lane_num == "1":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME,
0.5)
print("{} under full primary control".format(
vehicle))
if lane_num == "2":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED + 5)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME,
0.5)
print("{} under full primary control".format(
vehicle))
if lane_num == "3":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED + 10)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME,
0.5)
print("{} under full primary control".format(
vehicle))
elif traci.lane.getLastStepVehicleNumber(
next_veh_lane
) != 0: # and states[self._father_vehicle][-1] not in ["transit0","transit1", "transit2"]: # There is a vehicle on the next lane edge
last_veh_next_lane = traci.lane.getLastStepVehicleIDs(
next_veh_lane)[::-1][-1]
if utils.get_distance(
vehicle, last_veh_next_lane
) > 500: # If far enough control platoon as normal
if lane_num == "0":
#utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED, self.PSPEED) # check impact of setting speed in this case
utils.set_par(vehicle,
cc.PAR_ACTIVE_CONTROLLER,
cc.ACC)
utils.set_par(vehicle,
cc.PAR_ACC_HEADWAY_TIME, 0.8)
print("{} under primary observing control".
format(vehicle))
if lane_num == "1":
utils.set_par(vehicle,
cc.PAR_ACC_HEADWAY_TIME, 0.8)
print("{} under primary observing control".
format(vehicle))
if lane_num == "2":
utils.set_par(vehicle,
cc.PAR_ACC_HEADWAY_TIME, 0.8)
print("{} under primary observing control".
format(vehicle))
if lane_num == "3":
utils.set_par(vehicle,
cc.PAR_ACC_HEADWAY_TIME, 0.8)
print("{} under primary observing control".
format(vehicle))
else: # set the platoon leaders speed to speed of veh ahead
utils.set_par(
vehicle, cc.PAR_ACC_HEADWAY_TIME, 0.3
) # abrupt change causes collisions change to average or headway
print(
"{} under primary speed synch control with {}"
.format(vehicle, last_veh_next_lane))
else: ## Vehicle is a follower and CACC controlled
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.CACC)
utils.set_par(vehicle, cc.PAR_CACC_SPACING,
self.DISTANCE)
topology = topology
return topology
### Secondary Platoon Control
if vehicles[0] != traci.lane.getLastStepVehicleIDs(lane)[::-1][
0]: ## There is a preceeding vehicle or platoon
index = traci.lane.getLastStepVehicleIDs(lane)[::-1].index(
vehicles[0])
last_veh_plat_ahead = traci.lane.getLastStepVehicleIDs(
lane)[::-1][index - 1]
for vehicle in vehicles:
lane_num = traci.vehicle.getLaneID(vehicle).split("_")[1]
if vehicle == vehicles[0]: #
# Vehicles controlled here are secondary platoons with same route as lane leader and are close enough
if traci.vehicle.getRouteID(vehicle) == traci.vehicle.getRouteID(last_veh_plat_ahead) \
and traci.vehicle.getTypeID(last_veh_plat_ahead)== "vtypeauto":
if utils.get_distance(vehicle,
last_veh_plat_ahead) < 200:
utils.set_par(vehicle,
cc.PAR_ACTIVE_CONTROLLER,
cc.CACC)
utils.set_par(vehicle, cc.PAR_CACC_SPACING,
self.DISTANCE)
print(
"{} under Sec same route plat control gap less than 100"
.format(vehicle))
elif utils.get_distance(vehicle,
last_veh_plat_ahead) > 200:
utils.set_par(vehicle,
cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME,
0.5)
print(
"{} under Sec same route plat control gap greater than 100"
.format(vehicle))
elif traci.vehicle.getTypeID(
last_veh_plat_ahead
) != "vtypeauto": #Check for redundancy
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.ACC)
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME,
0.5)
print("{} under Sec diff route plat control elif".
format(vehicle))
else:
# Vehicles controlled here are temporary secondary platoon leaders in same lane but diff routes as lane leader
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.ACC)
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME,
0.5)
print("{} under Sec diff route plat control else".
format(vehicle))
else:
# All secondary followers control
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.CACC)
utils.set_par(vehicle, cc.PAR_CACC_SPACING,
self.DISTANCE)
return topology
else: # The platoon is not obstructed by a preceeding vehicle
# Primary platoon control
if vehicles[0] == first_of_lane and states[
vehicles[0]][-1] not in [
"transit0", "transit1", "transit2"
]: ## There is no preceeding vehicle on the lane
print(
"{} has prior states and not in transit 0 1 or 2 in primary, state is {}"
.format(vehicles[0], states[vehicles[0]][-1]))
for vehicle in vehicles:
lane_num = traci.vehicle.getLaneID(vehicle).split("_")[1]
if traci.vehicle.getRouteID(vehicle).startswith(':'):
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.CACC)
utils.set_par(vehicle, cc.PAR_CACC_SPACING,
self.DISTANCE)
continue
# Vehicles controlled here are lane leaders
if vehicle == vehicles[0]:
veh_route_edges = traci.vehicle.getRoute(vehicle)
veh_rou_index = traci.vehicle.getRouteIndex(vehicle)
next_veh_edge = veh_route_edges[veh_rou_index + 1]
next_veh_lane = next_veh_edge.split(
"_")[0] + "_" + lane_num
if traci.lane.getLastStepVehicleNumber(
next_veh_lane
) == 0: # There is no vehicle on the nexl edge lane
if lane_num == "0":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED)
utils.set_par(vehicle,
cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME,
0.5)
print("{} under full primary control".format(
vehicle))
if lane_num == "1":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME,
0.5)
print("{} under full primary control".format(
vehicle))
if lane_num == "2":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED + 5)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME,
0.5)
print("{} under full primary control".format(
vehicle))
if lane_num == "3":
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED + 10)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME,
0.5)
print("{} under full primary control".format(
vehicle))
elif traci.lane.getLastStepVehicleNumber(
next_veh_lane) != 0:
last_veh_next_lane = traci.lane.getLastStepVehicleIDs(
next_veh_lane)[::-1][-1]
if utils.get_distance(vehicle,
last_veh_next_lane) > 500:
if lane_num == "0":
utils.set_par(vehicle,
cc.PAR_ACTIVE_CONTROLLER,
cc.ACC)
utils.set_par(vehicle,
cc.PAR_ACC_HEADWAY_TIME, 0.8)
print("{} under primary observing control".
format(vehicle))
if lane_num == "1":
utils.set_par(vehicle,
cc.PAR_ACC_HEADWAY_TIME, 0.8)
print("{} under primary observing control".
format(vehicle))
if lane_num == "2":
utils.set_par(vehicle,
cc.PAR_ACC_HEADWAY_TIME, 0.8)
print("{} under primary observing control".
format(vehicle))
if lane_num == "3":
utils.set_par(vehicle,
cc.PAR_ACC_HEADWAY_TIME, 0.8)
print("{} under primary observing control".
format(vehicle))
else: # set the platoon leaders speed to speed of veh ahead
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME,
0.3) #
print(
"{} under primary speed synch control with {}"
.format(vehicle, last_veh_next_lane))
else: ## Vehicle is a follower and CACC controlled
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.CACC)
utils.set_par(vehicle, cc.PAR_CACC_SPACING,
self.DISTANCE)
topology = topology
return topology
### Secondary Platoon Control
if vehicles[0] != traci.lane.getLastStepVehicleIDs(
lane)[::-1][0] and states[self._members[0]][-1] not in [
"transit0", "transit1", "transit2"
]: ## There is a preceeding vehicle or platoon
index = traci.lane.getLastStepVehicleIDs(lane)[::-1].index(
vehicles[0])
last_veh_plat_ahead = traci.lane.getLastStepVehicleIDs(
lane)[::-1][index - 1]
for vehicle in vehicles:
lane_num = traci.vehicle.getLaneID(vehicle).split("_")[1]
if vehicle == vehicles[0]: #
# Vehicles controlled here are secondary platoons with same route as lane leader and are close enough
if traci.vehicle.getRouteID(vehicle) == traci.vehicle.getRouteID(last_veh_plat_ahead) \
and traci.vehicle.getTypeID(last_veh_plat_ahead)== "vtypeauto":
if utils.get_distance(vehicle,
last_veh_plat_ahead) < 200:
utils.set_par(vehicle,
cc.PAR_ACTIVE_CONTROLLER,
cc.CACC)
utils.set_par(vehicle, cc.PAR_CACC_SPACING,
self.DISTANCE)
elif utils.get_distance(vehicle,
last_veh_plat_ahead) > 200:
utils.set_par(vehicle,
cc.PAR_ACTIVE_CONTROLLER, cc.ACC)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME,
0.5)
print(
"{} under Sec same route plat control gap greater than 100"
.format(vehicle))
elif traci.vehicle.getTypeID(
last_veh_plat_ahead) != "vtypeauto":
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.ACC)
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME,
0.5)
print("{} under Sec diff route plat control elif".
format(vehicle))
else:
# Vehicles controlled here are temporary secondary platoon leaders in same lane but diff routes as lane leader
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.ACC)
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME,
0.5)
print(
"{} under Sec diff route plat control".format(
vehicle))
else:
# All secondary followers control
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.CACC)
utils.set_par(vehicle, cc.PAR_CACC_SPACING,
self.DISTANCE)
return topology
# Transitory control
if states[self._members[0]][
-1] == "obstructed": #Vehicles on lane 3 may be exlcuded
print(
"{} has prior states and in transit 0 1 or 2 in transitory, state is {}"
.format(vehicles[0], states[vehicles[0]][-1]))
for vehicle in vehicles:
if vehicle == vehicles[0]:
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.ACC)
utils.set_par(vehicle, cc.PAR_CC_DESIRED_SPEED,
self.PSPEED)
utils.set_par(vehicle, cc.PAR_ACC_HEADWAY_TIME, 0.5)
print(
"{} under transitory control and checking avoidance"
.format(vehicle))
else:
# All secondary followers control
utils.set_par(vehicle, cc.PAR_ACTIVE_CONTROLLER,
cc.CACC)
utils.set_par(vehicle, cc.PAR_CACC_SPACING,
self.DISTANCE)
topology = topology
return topology
