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