/
intersection.py
312 lines (262 loc) · 12.4 KB
/
intersection.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
from u import *
from exp import *
from env import *
class Platoon(Entity):
pass
class GridEnv(Env):
def def_sumo(self):
c = self.c
types = [E('vType', id='human', **IDM, **LC2013), E('vType', id='rl', **IDM, **LC2013), E('vType', id='generic', **IDM, **LC2013)]
default_flows = lambda flow_id, route_id, flow_rate: [E('flow', **params) for params in [
FLOW(f'{flow_id}', type='generic', route=route_id, departSpeed=c.depart_speed, vehsPerHour=flow_rate),
] if params.get('vehsPerHour')]
builder = NetBuilder()
xys = np.array(np.ones((c.n_rows + 2, c.n_cols + 2)).nonzero()).T * c.length
nodes = builder.add_nodes(
[Namespace(x=x, y=y, type='priority') for y, x in xys]
).reshape(c.n_rows + 2, c.n_cols + 2)
tl = c.setdefault('tl', False)
if tl:
c.av_frac = 0
c.pop('av_range', None)
c.speed_mode = SPEED_MODE.all_checks
flows = []
c.setdefaults(flow_rate_h=c.flow_rate, flow_rate_v=c.flow_rate)
priority = ['left', 'right'] if c.get('priority', 'vertical') == 'horizontal' else ['up', 'down']
for direction in c.directions:
chains = nodes if direction in ['left', 'right'] else nodes.T
chains = chains if direction in ['up', 'right'] else np.fliplr(chains)
flow_rate = c.flow_rate_h if direction in ['left', 'right'] else c.flow_rate_v
edge_attrs = dict(priority=int(direction in priority))
if c.get('set_edge_speed', True):
edge_attrs['speed'] = c.max_speed
for i, chain in enumerate(chains[1:-1]):
route_id, flow_id = f'r_{direction}_{i}', f'f_{direction}_{i}'
builder.chain(chain, route_id=route_id, edge_attrs=edge_attrs)
flows.extend(default_flows(flow_id, route_id, flow_rate))
tls = []
if tl:
tl = 1000000 if tl == 'MaxPressure' else tl
tl_h, tl_v = tl if isinstance(tl, tuple) else (tl, tl)
tl_offset = c.get('tl_offset', 'auto')
yellow = c.get('yellow', 0.5)
if tl_offset == 'auto':
offsets = c.length * (np.arange(c.n_rows).reshape(-1, 1) + np.arange(c.n_cols).reshape(1, -1)) / 10
elif tl_offset == 'same':
offsets = np.zeros(c.n_rows).reshape(-1, 1) + np.zeros(c.n_cols).reshape(1, -1)
for node, offset in zip(nodes[1:-1, 1:-1].reshape(-1), offsets.reshape(-1)):
node.type = 'traffic_light'
phase_multiple = len(c.directions) // 2
tls.append(E('tlLogic',
E('phase', duration=tl_v, state='Gr' * phase_multiple),
*lif(yellow, E('phase', duration=yellow, state='yr' * phase_multiple)),
E('phase', duration=tl_h, state='rG' * phase_multiple),
*lif(yellow, E('phase', duration=yellow, state='ry' * phase_multiple)),
id=node.id, offset=offset, type='static', programID='1'))
nodes, edges, connections, routes = builder.build()
additional = E('additional', *types, *routes, *flows, *tls)
return super().def_sumo(nodes, edges, connections, additional)
def build_platoon(self):
ts = self.ts
rl_type = ts.types.rl
for route in ts.routes:
vehs = []
route_offset = 0
for edge in route.edges:
for veh in edge.vehicles:
veh.route_position = route_offset + veh.laneposition
vehs.append(veh)
route_offset += edge.length
rl_mask = np.array([veh.type is rl_type for veh in vehs])
if len(rl_mask) > 1 and c.get('merge_consecutive_avs'):
rl_mask[1:] = rl_mask[1:] & ~rl_mask[:-1]
rl_idxs, = rl_mask.nonzero()
split_idxs = 1 + rl_idxs
prev = None
for i, vehs_i in enumerate(np.split(vehs, split_idxs)):
if not len(vehs_i):
continue # Last vehicle is RL, so the last split is empty
platoon = Platoon(id=f'{route}.platoon_{i}', route=route,
vehs=vehs_i, head=vehs_i[-1], tail=vehs_i[0], prev=prev
)
if prev is not None:
prev.next = platoon
prev = platoon
for veh in vehs_i:
veh.platoon = platoon
if prev is not None:
prev.next = None
def reset(self):
c = self.c
self.mp_tlast = 0
while not self.reset_sumo():
pass
ret = super().init_env()
return ret
def step(self, action=[]):
c = self.c
ts = self.ts
max_dist = c.max_dist
max_speed = c.max_speed
rl_type = ts.types.rl
prev_rls = sorted(rl_type.vehicles, key=lambda x: x.id)
for veh, act in zip(prev_rls, action):
if c.act_type == 'accel':
level = (np.clip(act, c.low, 1) - c.low) / (1 - c.low)
else:
level = act / (c.n_actions - 1)
ts.accel(veh, (level * 2 - 1) * (c.max_accel if level > 0.5 else c.max_decel))
if c.tl == 'MaxPressure':
self.mp_tlast += c.sim_step
tmin = c.get('mp_tmin', 0)
if self.mp_tlast >= tmin:
for tl in ts.traffic_lights:
if ts.get_program(tl) == 'off':
break
jun = tl.junction
pressures = [len(p.vehicles) - len(n.vehicles) for p, n in zip(jun.prev_lanes, jun.next_lanes)]
total_pressures = []
for phase in (ph for ph in tl.phases if 'y' not in ph.state):
total_pressures.append(sum(p for p, s in zip(pressures, phase.state) if s == 'G'))
ts.set_phase(tl, np.argmax(total_pressures))
self.mp_tlast = 0
super().step()
self.build_platoon()
obs = {}
veh_default_close = Namespace(speed=max_speed, route_position=np.inf)
veh_default_far = Namespace(speed=0, route_position=-np.inf)
vehs_default = lambda: [veh_default_close] + [veh_default_far] * 2 * c.obs_next_cross_platoons
for veh in rl_type.vehicles:
route, lane, platoon = veh.route, veh.lane, veh.platoon
junction = lane.next_junction
head, tail = veh, platoon.tail
route_vehs = [(route, [head, *lif(c.obs_tail, tail)])]
if junction is ts.sentinel_junction:
route_vehs.extend([(None, vehs_default())] * (len(c.directions) - 1))
else:
for jun_lane in lane.next_junction_lanes:
# Defaults for jun_lane
jun_headtails = vehs_default()
jun_lane_route = nexti(jun_lane.from_routes)
jun_veh, _ = jun_lane.prev_vehicle(0, route=jun_lane_route)
jun_veh = jun_veh if jun_veh and jun_veh.lane.next_junction is junction else None
if jun_veh:
if jun_veh.type is rl_type:
# If jun_veh is RL or jun_veh is human and there's no RL vehicle in front of it
jun_headtails[1: 3] = jun_veh, jun_veh.platoon.tail
platoon = jun_veh.platoon.prev
for i in 1 + 2 * np.arange(1, c.obs_next_cross_platoons):
if platoon is None: break
jun_headtails[i: i + 2] = platoon.head, platoon.tail
platoon = platoon.prev
else:
# If jun_veh is a human vehicle behind some RL vehicle (in another lane)
jun_headtails[0] = jun_veh.platoon.tail
next_cross_platoon = jun_veh.platoon.prev
if next_cross_platoon:
jun_headtails[1: 3] = next_cross_platoon.head, next_cross_platoon.tail
platoon = next_cross_platoon.prev
for i in 1 + 2 * np.arange(1, c.obs_next_cross_platoons):
if platoon is None: break
jun_headtails[i: i + 2] = platoon.head, platoon.tail
platoon = platoon.prev
route_vehs.append((jun_lane_route, jun_headtails))
dist_features, speed_features = [], []
for route, vehs in route_vehs:
j_pos = junction.route_position[route]
dist_features.extend([0 if j_pos == v.route_position else (j_pos - v.route_position) / max_dist for v in vehs])
speed_features.extend([v.speed / max_speed for v in vehs])
obs[veh.id] = np.clip([*dist_features, *speed_features], 0, 1).astype(np.float32) * (1 - c.low) + c.low
sort_id = lambda d: [v for k, v in sorted(d.items())]
ids = sorted(obs)
obs = arrayf(sort_id(obs)).reshape(-1, c._n_obs)
reward = len(ts.new_arrived) - c.collision_coef * len(ts.new_collided)
return Namespace(obs=obs, id=ids, reward=reward)
def append_step_info(self):
super().append_step_info()
self.rollout_info.append(n_veh_network=len(self.ts.vehicles))
@property
def stats(self):
c = self.c
info = self.rollout_info[1 + c.warmup_steps + c.skip_stat_steps:]
mean = lambda L: np.mean(L) if len(L) else np.nan
stats = {**super().stats, **dif('length_range' in c, length=c.length), **dif('av_range' in c, av_frac=c.av_frac)}
stats['backlog_step'] = mean(info['backlog'])
stats['n_total_veh_step'] = mean(info['n_veh_network']) + stats['backlog_step']
stats['flow_horizontal'] = c.flow_rate_h
stats['flow_vertical'] = c.flow_rate_v
return stats
class GridExp(Main):
def create_env(c):
return NormEnv(c, GridEnv(c))
@property
def observation_space(c):
low = np.full(c._n_obs, c.low)
return Box(low, np.ones_like(low))
@property
def action_space(c):
if c.act_type == 'accel':
return Box(low=c.low, high=1, shape=(1,), dtype=np.float32)
else:
return Discrete(c.n_actions)
def on_rollout_end(c, rollout, stats, ii=None, n_ii=None):
log = c.get_log_ii(ii, n_ii)
step_obs_ = rollout.obs
step_obs = step_obs_[:-1]
ret, _ = calc_adv(rollout.reward, c.gamma)
n_veh = np.array([len(o) for o in step_obs])
step_ret = [[r] * nv for r, nv in zip(ret, n_veh)]
rollout.update(obs=step_obs, ret=step_ret)
step_id_ = rollout.pop('id')
id = np.concatenate(step_id_[:-1])
id_unique = np.unique(id)
reward = np.array(rollout.pop('reward'))
raw_reward = np.array(rollout.pop('raw_reward'))
log(**stats)
log(raw_reward_mean=raw_reward.mean(), raw_reward_sum=raw_reward.sum())
log(reward_mean=reward.mean(), reward_sum=reward.sum())
log(n_veh_step_mean=n_veh.mean(), n_veh_step_sum=n_veh.sum(), n_veh_unique=len(id_unique))
return rollout
if __name__ == '__main__':
c = GridExp.from_args(globals(), locals())
c.setdefaults(
n_steps=200,
step_save=5,
depart_speed=0,
max_speed=13,
max_dist=100,
max_accel=1.5,
max_decel=3.5,
sim_step=0.5,
generic_type=True,
n_actions=3,
adv_norm=False,
batch_concat=True,
render=False,
warmup_steps=100,
horizon=2000,
directions=['up', 'right'],
av_frac=0.15,
flow_rate=700,
length=100,
n_rows=2,
n_cols=1,
speed_mode=SPEED_MODE.obey_safe_speed,
act_type='accel_discrete',
low=-1,
alg=PG,
n_gds=1,
lr=1e-3,
gamma=0.99,
collision_coef=5, # If there's a collision, it always involves an even number of vehicles
norm_reward=True,
center_reward=True,
opt='RMSprop',
obs_tail=True,
obs_next_cross_platoons=1,
)
if c.directions == '4way':
c.directions = ['up', 'right', 'down', 'left']
c._n_obs = 2 * (1 + c.obs_tail + (1 + 2 * c.obs_next_cross_platoons) * (len(c.directions) - 1))
assert c.alg == PG, 'Not supporting value functions yet'
c.run()