1# Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo 2# Copyright (C) 2017-2019 German Aerospace Center (DLR) and others. 3# This program and the accompanying materials 4# are made available under the terms of the Eclipse Public License v2.0 5# which accompanies this distribution, and is available at 6# http://www.eclipse.org/legal/epl-v20.html 7# SPDX-License-Identifier: EPL-2.0 8 9# @file _platoon.py 10# @author Leonhard Luecken 11# @date 2017-04-09 12# @version $Id$ 13 14import os 15import sys 16 17if 'SUMO_HOME' in os.environ: 18 tools = os.path.join(os.environ['SUMO_HOME'], 'tools') 19 sys.path.append(tools) 20else: 21 sys.exit("please declare environment variable 'SUMO_HOME'") 22 23from simpla._platoonmode import PlatoonMode # noqa 24import simpla._reporting as rp # noqa 25 26warn = rp.Warner("Platoon") 27report = rp.Reporter("Platoon") 28 29 30class Platoon(object): 31 ''' 32 ''' 33 34 # static platoon ID counter 35 _nextID = 0 36 37 def __init__(self, vehicles, controlInterval, registerVehicles=True): 38 '''Platoon(list(PVehicle), float, bool) -> Platoon 39 40 Create a Platoon object that holds an ordered list of its members, which is inititialized with 'vehicles'. 41 Creator is responsible for setting the platoon mode of the vehicles. If registerVehicles is set, the vehicle's 42 platoon reference veh._platoon is set to the newly created platoon. 'deltaT' is the control interval provided 43 to give the platoon a sense of time (used for decreasing active speed factor when trying to switch modes 44 unsuccessfully). 45 ''' 46 self._ID = Platoon._nextID 47 Platoon._nextID += 1 48 self._vehicles = vehicles 49 if registerVehicles: 50 self.registerVehicles() 51 52 self._controlInterval = controlInterval 53 54 def registerVehicles(self): 55 ''' registerVehicles() -> void 56 Sets reference to this platoon at member-vehicles' side 57 ''' 58 for veh in self._vehicles: 59 veh.setPlatoon(self) 60 61 def getLeader(self): 62 '''getLeader() -> PVehicle 63 64 Returns the current platoon leader 65 ''' 66 return self._vehicles[0] 67 68 def removeVehicles(self, vehs): 69 '''removeVehicles(PVehicle) 70 71 Removes the vehicles from the platoon 72 ''' 73 for veh in vehs: 74 self._vehicles.remove(veh) 75 76 if self.size() == 0: 77 return 78 try: 79 if self.size() == 1: 80 self.setMode(PlatoonMode.NONE) 81 return 82 83 if self.getMode() == PlatoonMode.CATCHUP: 84 self.setMode(PlatoonMode.CATCHUP) 85 else: 86 # remains the regular platoon situation 87 self.setMode(PlatoonMode.LEADER) 88 except: 89 warn("Ignoring error (probably due to platoon.setMode() operation on non-existing vehicle).") 90 91 def getID(self): 92 '''getID() -> int 93 94 Returns the platoon's id 95 ''' 96 return self._ID 97 98 def getVehicles(self): 99 '''getVehicles() -> list(PVehicle) 100 101 Returns the platoon members as an ordered list. The leader is at index 0. 102 ''' 103 return self._vehicles 104 105 def setVehicles(self, vehs): 106 '''setVehicles(list(PVehicle)) 107 108 Sets the platoon members. Used for reordering, e.g.. 109 ''' 110 self._vehicles = vehs 111 112 def size(self): 113 '''size() -> int 114 115 Returns number of vehicles currently in the platoon 116 ''' 117 return len(self._vehicles) 118 119 def __getitem__(self, i): 120 return self._vehicles[i] 121 122 def setModeWithImpatience(self, mode, waitingTimeIncrement): 123 '''setModeWithImpatience(PlatoonMode, float) -> bool 124 125 The same as setMode(), except acting on the platoon leaders switch waiting time as follows. 126 If the mode switch was successful, all waiting times are reset to 0., if not, the mode specific 127 waiting time is increased. 128 ''' 129 success = self.setMode(mode) 130 leader = self._vehicles[0] 131 if success: 132 leader.resetSwitchWaitingTime() 133 else: 134 leader.addSwitchWaitingTime(mode, waitingTimeIncrement) 135 return success 136 137 def setMode(self, mode): 138 '''setMode(PlatoonMode) -> bool 139 140 Returns whether the change to the requested platoon mode could be performed safely. 141 Only checks for safety with regard to leaders. 142 Note: safety assumptions of previous versions are dropped, now 143 ''' 144 old_mode = self.getMode() 145 success = False 146 if mode == PlatoonMode.NONE: 147 if self.size() == 1: 148 if (self._vehicles[0].isSwitchSafe(mode)): 149 self._vehicles[0].setPlatoonMode(mode) 150 success = True 151 else: 152 success = False 153 else: 154 # PlatoonMode.NONE is only admissible for solitons 155 success = False 156 157 elif mode == PlatoonMode.LEADER: 158 if self._vehicles[0].isSwitchSafe(mode): 159 self._vehicles[0].setPlatoonMode(mode) 160 for veh in self._vehicles[1:]: 161 if veh.isSwitchSafe(PlatoonMode.FOLLOWER): 162 veh.setPlatoonMode(PlatoonMode.FOLLOWER) 163 success = True 164 else: 165 success = False 166 167 elif mode == PlatoonMode.CATCHUP: 168 if self._vehicles[0].isSwitchSafe(mode): 169 self._vehicles[0].setPlatoonMode(mode) 170 for veh in self._vehicles[1:]: 171 if veh.isSwitchSafe(PlatoonMode.CATCHUP_FOLLOWER): 172 veh.setPlatoonMode(PlatoonMode.CATCHUP_FOLLOWER) 173 success = True 174 else: 175 success = False 176 177 elif mode == PlatoonMode.CATCHUP_FOLLOWER or mode == PlatoonMode.FOLLOWER: 178 if self._vehicles[0].isSwitchSafe(mode): 179 self._vehicles[0].setPlatoonMode(mode) 180 for veh in self._vehicles[1:]: 181 if veh.isSwitchSafe(mode): 182 veh.setPlatoonMode(mode) 183 success = True 184 else: 185 success = False 186 187 else: 188 raise ValueError("Unknown PlatoonMode %s" % str(mode)) 189 190 if rp.VERBOSITY >= 3 and success and not old_mode == mode: 191 report("Activated mode {mode} for platoon '{pltnID}' ({pltn_members})".format( 192 mode=mode, pltnID=self.getID(), pltn_members=str([veh.getID() for veh in self.getVehicles()]))) 193 194 return success 195 196 def adviseMemberModes(self): 197 ''' adviseMemberModes() -> void 198 Advise all member vehicles to adopt the adequate platoon mode if safely possible. 199 ''' 200 mode = self.getMode() 201 vehs = self._vehicles 202 203 # impose mode for leader 204 if vehs[0].isSwitchSafe(mode): 205 vehs[0].setPlatoonMode(mode) 206 else: 207 # TODO: increase switch impatience/(waiting time) here, too? 208 pass 209 210 if mode == PlatoonMode.LEADER: 211 # use follower mode for followers if platoon is in normal mode 212 mode = PlatoonMode.FOLLOWER 213 214 if mode == PlatoonMode.CATCHUP: 215 # use catchupfollower mode for followers if platoon is in catchup mode 216 mode = PlatoonMode.CATCHUP_FOLLOWER 217 218 # impose mode for followers 219 for veh in vehs[1:]: 220 if veh.isSwitchSafe(mode): 221 veh.setPlatoonMode(mode) 222 else: 223 # TODO: increase switch impatience/(waiting time) here, too? 224 pass 225 226 def split(self, index): 227 '''split(int) -> Platoon 228 229 Splits off a subplatoon from the end of the platoon at the given index. 230 The given index must correspond to the new leader (of the splitoff subplatoon) 231 Returns the splitoff platoon. 232 ''' 233 if index <= 0 or index > self.size(): 234 raise ValueError( 235 "Platoon.split(index) expected and index in [1,%d]. Given value: %s" % (self.size(), index)) 236 237 mode = PlatoonMode.LEADER if (index < self.size() - 1) else PlatoonMode.NONE 238 # splitImpatience = 1. - math.exp(min([0., self._vehicles[index]._timeUntilSplit])) 239 pltn = Platoon(self._vehicles[index:], self._controlInterval, False) 240 241 if not pltn.setModeWithImpatience(mode, self._controlInterval): 242 # could not split off platoon safely 243 return None 244 245 # split can be taken out safely -> reduce vehicles in this platoon 246 self._vehicles = self._vehicles[:index] 247 # set reference to new platoon in splitted vehicles 248 pltn.registerVehicles() 249 250 if len(self._vehicles) == 1: 251 # only one vehicle remains, turn off its platoon-specific behavior 252 self.setModeWithImpatience(PlatoonMode.NONE, self._controlInterval) 253 254 return pltn 255 256 def join(self, pltn): 257 '''join(Platoon) 258 259 Tries to add the given platoon to the end of this. Returns True if this could safely be executed. 260 ''' 261 vehs = pltn.getVehicles() 262 if self.getMode() == PlatoonMode.CATCHUP: 263 if pltn.setModeWithImpatience(PlatoonMode.CATCHUP, self._controlInterval): 264 for v in vehs: 265 v.setPlatoon(self) 266 self._vehicles.extend(vehs) 267 return True 268 else: 269 return False 270 271 if self.getMode() == PlatoonMode.NONE: 272 # this implies that size == 1, i.e. the platoon is a solitary vehicle 273 if not self.getLeader().isSwitchSafe(PlatoonMode.LEADER): 274 # vehicle cant safely switch to LEADER mode -> don't join 275 return False 276 277 # At this point either this has PlatoonMode.LEADER or PlatoonMode.NONE (size==1), and switch to leader was safe 278 if pltn.setModeWithImpatience(PlatoonMode.FOLLOWER, self._controlInterval): 279 for v in vehs: 280 v.setPlatoon(self) 281 self._vehicles.extend(vehs) 282 self.getLeader().setPlatoonMode(PlatoonMode.LEADER) 283 return True 284 else: 285 return False 286 287 def getMode(self): 288 '''getMode() -> PlatoonMode 289 290 Returns the platoon leader's desired PlatoonMode (may return LEADER if current mode is FOLLOWER). 291 ''' 292 mode = self._vehicles[0].getCurrentPlatoonMode() 293 if mode == PlatoonMode.FOLLOWER: 294 # Leader was kept in FOLLOW mode due to safety constraints 295 mode = PlatoonMode.LEADER 296 elif mode == PlatoonMode.CATCHUP_FOLLOWER: 297 # Leader was kept in CATCHUP_FOLLOW mode due to safety constraints 298 mode = PlatoonMode.CATCHUP 299 return mode 300