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