VerifCar_template_decision_algorithm.xml
27.8 KB
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
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
<?xml version="1.0" encoding="utf-8"?>
<!DOCTYPE nta PUBLIC '-//Uppaal Team//DTD Flat System 1.1//EN' 'http://www.it.uu.se/research/group/darts/uppaal/flat-1_2.dtd'>
<nta>
<declaration>
// Scale
const int scale = 100;
// Abstraction parameters (defines level of abstraction)
const int S := 10; // sample period, in 1/scale seconds
const int GranA := 100; // granularity of the acceleration expressed in 1/scale meters per second squared
const int NormX := 100; // maximal loss of precision during a second in 1/scale meters (>= (GranA*S/scale)/2)
// Road parameters
const int L := 60000; // length of the road segment, in 1/scale meters
const int R := 1050; // width of the road segment, in 1/scale meters
const int length_vehicle := 500; // length of a vehicle in 1/scale meters
const int width_vehicle := 200; // width of a vehicle in 1/scale meters
const int begin_junction := 20000; // begining of junction lane in 1/scale meters
const int end_junction := 40000; // end of junction lane in 1/scale meters
const int nb_lane := 3; // number of lanes, including junction (>= 2)
const int marks[nb_lane+1] := {0,350,700,R}; // lateral position of markings separating lanes in 1/scale meters
// Vehicles parameters (to change the number of vehicle in the system, change nb_car value, adjust the size of the sets bellow, set the appropriate number of automaton in system definition and add the needed transitions to A0)
const int V_min := 1000; // min value of longitudinal speed, in 1/scale meters per second
const int V_max := 4000; // max value of longitudinal speed, in 1/scale meters per second
const int A_min := -500; // min value of longitudinal acceleration, in 1/scale meters per second squared
const int A_max := 300; // max value of longitudinal acceleration, in 1/scale meters per second squared
const int W := 100; // maximal absolute value of the lateral speed expressed in 1/scale meters per second
const int nb_car := 3; // number of vehicles (>= 2)
const int freq[nb_car] := {10,10,10}; // activation sample of the controler for each vehicle in 1/scale second
const int min_com_delay[nb_car] := {3,3,3}; // min communication delay for each vehicle in 1/scale second, must be smaller than the vehicle's controller sample
const int max_com_delay[nb_car] := {4,4,4}; // max communication delay for each vehicle in 1/scale second, must be smaller than the vehicle's controller sample
const int init_posX[nb_car] := {5000,0,2000}; // initial longitudinal position for each vehicle in 1/scale meters
const int init_posY[nb_car] := {525,525,175}; // initial lateral position for each vehicle in 1/scale meters
const int init_V[nb_car] := {2000,3500,2820}; // initial longitudinal speed for each vehicle in 1/scale meters per second
const int init_A[nb_car] := {3,3,3}; // initial longitudinal acceleration for each vehicle in 1/scale meters per second squared
const int init_clock[nb_car] := {1,4,8}; // initial controller clock value for each vehicle in 1/scale seconds
//Global clocks
clock C1; // in 1/scale seconds
clock C2; // in 1/scale seconds
clock C3; // in 1/scale seconds
clock C4; // in 1/scale seconds
clock C5; // in 1/scale seconds
//Synchronisation channel
broadcast chan k;
// Constants and data types obtained from parameters
const int GranV := GranA*S; // granularity of the longitudinal speed expressed in 1/scale/scale meters per second
const int GranX := NormX*S; // granularity of the longitudinal position expressed in 1/scale/scale meters
const int GranY := W*S; // granularity of the lateral position expressed in 1/scale/scale meters
const int p := 2*NormX*scale/GranV; // used for posX update
const int LengthX := (L*scale)/GranX; // normalized length of the road segment
const int LengthY := (R*scale)/GranY; // normalized width of the road segment
const int min_speed := (V_min*scale)/GranV; // normalized min value of longitudinal speed
const int max_speed := (V_max*scale)/GranV; // normalized max value of longitudinal speed
const int min_acceleration := A_min/GranA; // normalized min value of longitudinal acceleration
const int max_acceleration := A_max/GranA; // normalized max value of longitudinal acceleration
const int C_len := (length_vehicle*scale)/GranX; // normalized length of a vehicle
const int C_wid := (width_vehicle*scale)/GranY; // normalized width of a vehicle
const int marking[nb_lane+1] := {(marks[0]*scale)/GranY,(marks[1]*scale)/GranY,(marks[2]*scale)/GranY,(marks[3]*scale)/GranY}; // normalized lateral position of markings separating lanes
const int J_beg := (begin_junction*scale)/GranX; // used for posX and posY update
const int J_end := (end_junction*scale)/GranX; // used for posX and posY update
const int J_inf := marking[1]-(C_wid/2); // used for posY update
const int J_sup := marking[1]+(C_wid/2); // used for posY update
typedef int[0,LengthX] RangeX; // longitudinal position range
typedef int[1,LengthY] RangeY; // lateral position range
typedef int[min_speed,max_speed] RangeV; // speed range
typedef int[min_acceleration,max_acceleration] RangeA; // acceleration range
typedef int[-1,1] RangeD; // direction range
typedef int[0,nb_car-1] RangeId; // ids range
typedef int[0,nb_lane-1] RangeLane; // lanes range
// Decision related parameters
const int navigation_points := 2; // number of coordinate on a navigation list
const int navigation_list[nb_car][navigation_points][3] := {
{{0,0,2},{LengthX,1,1}},
{{0,0,2},{LengthX,1,1}},
{{J_end,1,2},{LengthX,1,1}}
}; // GPS trajectory for each vehicle in list of {posX,lane(min),lane(max)}, each car MUST have a complete trajectory that goes up to horizon value
const int safety_length := 200; // longitudinal safety distance of a vehicle in 1/scale meters
const int safety_width := 50; // lateral safety distance of a vehicle in 1/scale meters
const int traj_length := 1000; // length of the predicted trajectory in 1/scale seconds
const int delay_step := 100; // delay step in 1/scale seconds
const int max_delay := 500; // maximum delay in 1/scale seconds
// Constants and data types obtained from decision related parameters
const int S_len := (safety_length*scale)/GranX; // normalized safety length of a vehicle
const int S_wid := (safety_width*scale)/GranY; // normalized safety width of a vehicle
const int traj_range := traj_length/S; // range of the predicted trajectory (number of points)
const int LengthDelay := max_delay/delay_step;
typedef int[0,LengthDelay] RangeDelay; // delay range
// Querries memory
int[0,(L*scale)/(S*V_min)] nb_updates;
// Information structure for each vehicle, the parenthesis indicate wich automaton updates the variable
struct{
bool on_the_road; // tells if the vehicle is on or out of the road (environment)
RangeX posX; // longitudinal position of the car (environment)
RangeY posY; // lateral position of the car (environment)
RangeV speed; // longitudinal speed (environment)
RangeA acceleration; // longitudinal acceleration (vehicle)
RangeD direction; // lateral speed (vehicle)
RangeLane goal; // signal to other vehicles the lane this vehicle is trying to reach (vehicle)
RangeDelay delay; // signal to other vehicles how long the vehicle is waiting before applying its intention (vehicle)
}car[nb_car];
struct{
RangeLane goal[nb_car-1]; // keeps other vehicles' goal
RangeDelay delay[nb_car-1]; // keeps other vehicles' delay
}data[nb_car];
void update(){
bool unempty := false;
for(id : int[0,nb_car-1]) if(car[id].on_the_road) unempty := true;
if(unempty) nb_updates++;
for(id : int[0,nb_car-1]){
//initialization (only occurs once)
if(car[id].on_the_road == false){
if(car[id].posX == 0){
car[id].on_the_road := true;
car[id].posX := (init_posX[id]*scale)/GranX;
car[id].posY := (init_posY[id]*scale)/GranY;
car[id].speed := (init_V[id]*scale)/GranV;
car[id].acceleration := (init_A[id]*scale)/GranA;
}
}
else{
//update longitudinal position
if((((2*car[id].speed)+car[id].acceleration)/p)*2 < (((2*car[id].speed)+car[id].acceleration)*2)/p and car[id].posX < LengthX) car[id].posX++; // upper rounded when rest is > 0.5
if(car[id].posX + (((2*car[id].speed)+car[id].acceleration)/p) >= LengthX){
car[id].posX := LengthX;
car[id].on_the_road := false;
}
else car[id].posX += ((2*car[id].speed)+car[id].acceleration)/p;
if(car[id].posX > J_end and car[id].posY < J_sup) car[id].on_the_road := false; // car is out of the road if did not change lane before the end of junction lane
//update speed
if(car[id].speed + car[id].acceleration > max_speed) car[id].speed := max_speed;
else if(car[id].speed + car[id].acceleration < min_speed) car[id].speed := min_speed;
else car[id].speed += car[id].acceleration; // adjust speed regarding the variation choosen by the controler
//update lateral position
if(car[id].direction == -1 and car[id].on_the_road){
if((car[id].posX <= J_end and car[id].posX >= J_beg) or car[id].posY >= J_sup or car[id].posY <= J_inf){ // forbiding to go on junction lane before the junction starts
if(car[id].posY > 1) car[id].posY--;
}
}
if(car[id].direction == 1 and car[id].on_the_road){
if((car[id].posX <= J_end and car[id].posX >= J_beg) or car[id].posY >= J_sup or car[id].posY <= J_inf){ // forbiding to get out of junction lane before the junction starts
if(car[id].posY < LengthY) car[id].posY++;
}
}
if(car[id].posX < J_beg and car[id].posY < J_sup and car[id].posY > J_inf) car[id].on_the_road := false; // car is out of the road if beetween junction lane and highway out of the junction zone
}
}
}
void communicate(RangeId id){
// Send goal and data value to other vehicles
for(n : int[0,nb_car-1]){
if(id<n){
data[n].goal[id] := car[id].goal;
data[n].delay[id] := car[id].delay;
}
if(id>n){
data[n].goal[id-1] := car[id].goal;
data[n].delay[id-1] := car[id].delay;
}
}
}
// Give the goal value that id knows about n
RangeLane read_goal(RangeId id, RangeId n){
if(id>n) return data[id].goal[n];
if(id<n) return data[id].goal[n-1];
return car[id].goal;
}
// Give the delay value that id knows about n
RangeDelay read_delay(RangeId id, RangeId n){
if(id>n) return data[id].delay[n];
if(id<n) return data[id].delay[n-1];
return car[id].delay;
}
// Tells which lane matches with a given lateral position
RangeLane y_to_lane(RangeY y){
for(i : int[1,nb_lane]){
if(y<=marking[i]) return i-1;
}
return nb_lane-1; // for compilation needs
}
// Put the new value of the flag in regard to GPS trajectory
RangeLane navigation(RangeId id){
for(i : int[0,navigation_points-1]){
if(navigation_list[id][i][0]>car[id].posX){
if(navigation_list[id][i][1] > y_to_lane(car[id].posY)) return navigation_list[id][i][1];
if(navigation_list[id][i][2] < y_to_lane(car[id].posY)) return navigation_list[id][i][2];
return y_to_lane(car[id].posY);
}
}
return y_to_lane(car[id].posY);
}
// Tells if vehicle can change lane in regard to GPS trajectory
bool far_point(RangeId id){
for(i : int[0,navigation_points-1]){
if(navigation_list[id][i][0]>car[id].posX){
if(((navigation_list[id][i][0]-car[id].posX)*GranX)/(car[id].speed*GranV)<4) return false;
}
}
return true;
}
// Computes a predicted trajectory based on parameters
void compute_traj(RangeX& traj_X[traj_range], RangeY& traj_Y[traj_range], RangeX posX, RangeY posY, RangeV speed, RangeA acceleration, RangeLane goal, RangeDelay d){
int[-S,max_delay] delay := d*delay_step; //conversion of the delay in 1/scale seconds unit
RangeD direction; //virtual direction
// At each sample, choose a direction, mimic an update of the environment, then store the position in the trajectory data structure
for(i: int[0,traj_range-1]){
//Choosing virtual direction
direction := 0;
if(y_to_lane(posY) < goal) direction := 1;
if(y_to_lane(posY) > goal) direction := -1;
if(direction == 0){
if(posY < (marking[y_to_lane(posY)]+marking[y_to_lane(posY)+1])/2) direction := 1;
if(posY > (marking[y_to_lane(posY)]+marking[y_to_lane(posY)+1])/2) direction := -1;
}
//update longitudinal position
if((((2*speed)+acceleration)/p)*2 < (((2*speed)+acceleration)*2)/p and posX < LengthX) posX++; // upper rounded when rest is > 0.5
if(posX + (((2*speed)+acceleration)/p) >= LengthX) posX := LengthX;
else posX += ((2*speed)+acceleration)/p;
//update speed
if(speed + acceleration > max_speed) speed := max_speed;
else if(speed + acceleration < min_speed) speed := min_speed;
else speed += acceleration; // adjust speed regarding the variation choosen by the controler
//update lateral position
if(delay > 0) delay -= S;
else{
if(direction == -1 and posY > 1) posY--;
if(direction == 1 and posY < LengthY) posY++;
}
//Storing data
if(posX < LengthX){
traj_X[i] := posX;
traj_Y[i] := posY;
}
else traj_X[i] := 0;
}
}
// Check if there is a possible collision beetween two trajectories
bool possible_collision(RangeX& traj_X1[traj_range], RangeY& traj_Y1[traj_range], RangeX& traj_X2[traj_range], RangeY& traj_Y2[traj_range]){
// For each point in trajectory one, check if the point of trajectory two with the same timed indicator is in the neighborhood
for(i : int[0,traj_range-1]){
if(traj_X1[i] != 0 and traj_X2[i] != 0){ // do not check if no value
if(traj_X1[i] < traj_X2[i] + C_len + S_len and traj_X1[i] > traj_X2[i] - C_len - S_len){
if(traj_Y1[i] < traj_Y2[i] + C_wid + S_wid and traj_Y1[i] > traj_Y2[i] - C_wid - S_wid) return true;
}
}
}
return false;
}
// Check wished trajectory against prioritary vehicles wished trajectory
bool wished_behaviour_not_safe(RangeId id, bool prio[nb_car], RangeX long[nb_car][traj_range], RangeY lat[nb_car][traj_range], RangeA acceleration, RangeLane goal, RangeDelay delay){
compute_traj(long[id], lat[id], car[id].posX, car[id].posY, car[id].speed, acceleration, goal, delay);
for(n : int[0,nb_car-1]){
if(id != n and prio[n] and possible_collision(long[id], lat[id], long[n], lat[n])) return true;
}
// check obstacles
for(i : int[0,traj_range-1]){
if(long[id][i] > J_end and lat[id][i] < J_sup) return true; // junction lane after end of zone
if(long[id][i] < J_beg and lat[id][i] < J_sup and lat[id][i] > J_inf) return true; // beetween junction lane and highway out of the junction zone
}
return false;
}
void decision(RangeId id){
RangeD temp_dir;
RangeA temp_acc;
bool prio[nb_car]; // list of prioritary vehicles
RangeX long[nb_car][traj_range];
RangeY lat[nb_car][traj_range];
RangeDelay temp_del;
RangeLane temp_goal;
if(car[id].on_the_road){
// Choosing best possible choices in regard of GPS indication
car[id].goal := navigation(id);
temp_acc := max_acceleration;
temp_del := 0;
temp_goal := car[id].goal;
// Computing the list of other vehicles that have priority
for(n : int[0,nb_car-1]){
if(car[n].on_the_road){
if(car[id].posX < car[n].posX or (car[id].posX == car[n].posX and car[id].posY < car[n].posY)) prio[n] := true;
else prio[n] := false;
}
else prio[n] := false;
}
// Computing other vehicles' trajectory
for(n : int[0,nb_car-1]){
compute_traj(long[n], lat[n], car[n].posX, car[n].posY, car[n].speed, car[n].acceleration, read_goal(id,n), read_delay(id,n));
}
// Finding a suitable behaviour closest to the original intention and respecting prioritary vehicles intention
// variation speed
while(wished_behaviour_not_safe(id,prio,long,lat,temp_acc,temp_goal,temp_del) and temp_acc > min_acceleration){
// checking if possible with delay
while(wished_behaviour_not_safe(id,prio,long,lat,temp_acc,temp_goal,temp_del) and temp_del < LengthDelay and far_point(id)) temp_del++;
//overtaking obstacle (left)
if(wished_behaviour_not_safe(id,prio,long,lat,temp_acc,temp_goal,temp_del) and car[id].goal + 1 <= nb_lane-1 and far_point(id)){
temp_del := 0;
temp_goal := car[id].goal + 1;
// checking if possible with delay
while(wished_behaviour_not_safe(id,prio,long,lat,temp_acc,temp_goal,temp_del) and temp_del < LengthDelay) temp_del++;
}
//overtaking obstacle (right)
if(wished_behaviour_not_safe(id,prio,long,lat,temp_acc,temp_goal,temp_del) and car[id].goal - 1 >= 0 and far_point(id)){
temp_del := 0;
temp_goal := car[id].goal - 1;
// checking if possible with delay
while(wished_behaviour_not_safe(id,prio,long,lat,temp_acc,temp_goal,temp_del) and temp_del < LengthDelay) temp_del++;
}
if(wished_behaviour_not_safe(id,prio,long,lat,temp_acc,temp_goal,temp_del)){
temp_del := 0;
temp_acc--;
temp_goal := car[id].goal;
}
}
// Urgent behaviour
if(wished_behaviour_not_safe(id,prio,long,lat,temp_acc,temp_goal,temp_del)){
temp_goal := y_to_lane(car[id].posY);
temp_del := 0;
}
// Computing new direction
if(temp_del == 0){
if(y_to_lane(car[id].posY) < temp_goal) temp_dir := 1;
if(y_to_lane(car[id].posY) > temp_goal) temp_dir := -1;
if(temp_dir == 0){
if(car[id].posY < (marking[y_to_lane(car[id].posY)]+marking[y_to_lane(car[id].posY)+1])/2) temp_dir := 1;
if(car[id].posY > (marking[y_to_lane(car[id].posY)]+marking[y_to_lane(car[id].posY)+1])/2) temp_dir := -1;
}
}
//Applying decision
car[id].goal := temp_goal;
car[id].delay := temp_del;
car[id].direction := temp_dir;
car[id].acceleration := temp_acc;
}
}
// Time to collision between two vehicles in 1/scale seconds, dont go above 100s
int time_to_collision(RangeId id1, RangeId id2){
// set buffers
int px_a, px_b, vx_a, vx_b, py_a, py_b, vy_a, vy_b, X_in, X_out, Y_in, Y_out;
if(car[id1].posX > car[id2].posX){
px_a := car[id2].posX;
px_b := car[id1].posX;
vx_a := car[id2].speed;
vx_b := car[id1].speed;
}
else{
px_a := car[id1].posX;
px_b := car[id2].posX;
vx_a := car[id1].speed;
vx_b := car[id2].speed;
}
if(car[id1].posY > car[id2].posY){
py_a := car[id2].posY;
py_b := car[id1].posY;
vy_a := car[id2].direction;
vy_b := car[id1].direction;
}
else{
py_a := car[id1].posY;
py_b := car[id2].posY;
vy_a := car[id1].direction;
vy_b := car[id2].direction;
}
// compute X_in and X_out
if(vx_a > vx_b){
// X_in if faster A
if(((px_b-px_a-C_len)*GranX)/((vx_a-vx_b)*GranV) < 100) X_in := ((px_b-px_a-C_len)*GranX*scale)/((vx_a-vx_b)*GranV);
else X_in := 100*scale;
// X_out if faster A
if(((px_b-px_a+C_len)*GranX)/((vx_a-vx_b)*GranV) < 100) X_out := ((px_b-px_a+C_len)*GranX*scale)/((vx_a-vx_b)*GranV);
else X_out := 100*scale;
}
else{
// X_in if faster B or equal speed
if(px_b-px_a-C_len > 0) X_in := 100*scale;
else X_in := 0;
// X_out if faster B
if(vx_a < vx_b){
if(px_b-px_a-C_len > 0){
if(((px_b-px_a-C_len)*GranX)/((vx_b-vx_a)*GranV) < 100) X_out := ((px_b-px_a-C_len)*GranX*scale)/((vx_b-vx_a)*GranV);
else X_out := 100*scale;
}
else{
if(((C_len-(px_b-px_a))*GranX)/((vx_b-vx_a)*GranV) < 100) X_out := ((C_len-(px_b-px_a))*GranX*scale)/((vx_b-vx_a)*GranV);
else X_out := 100*scale;
}
}
// X_out if equal speed
else{
if(px_b-px_a-C_len > 0) X_out := 0;
else X_out := 100*scale;
}
}
// compute Y_in and Y_out
if(vy_a > vy_b){
// Y_in if faster A
if(((py_b-py_a-C_wid)*S)/((vy_a-vy_b)*scale) < 100) Y_in := ((py_b-py_a-C_wid)*S)/(vy_a-vy_b);
else Y_in := 100*scale;
// Y_out if faster A
if(((py_b-py_a+C_wid)*S)/((vy_a-vy_b)*scale) < 100) Y_out := ((py_b-py_a+C_wid)*S)/(vy_a-vy_b);
else Y_out := 100*scale;
}
else{
// Y_in if faster B or equal speed
if(py_b-py_a-C_wid > 0) Y_in := 100*scale;
else Y_in := 0;
// Y_out if faster B
if(vy_a < vy_b){
if(((py_b-py_a-C_wid)*S)/((vy_a-vy_b)*scale) < 100) Y_out := ((py_b-py_a-C_wid)*S)/(vy_a-vy_b);
else Y_out := 100*scale;
}
// Y_out if equal speed
else{
if(py_b-py_a-C_wid > 0) Y_out := 0;
else Y_out := 100*scale;
}
}
// compute TTC
if(X_in <= Y_in and Y_in <= X_out) return Y_in;
if(Y_in <= X_in and X_in <= Y_out) return X_in;
return 100*scale;
}</declaration>
<template>
<name x="5" y="5">A0</name>
<declaration>clock C0; // in 1/scale seconds</declaration>
<location id="id0" x="-161" y="0">
<name x="-178" y="-34">I</name>
<urgent/>
</location>
<location id="id1" x="-17" y="0">
<name x="-42" y="-34">s0</name>
<label kind="invariant" x="-34" y="17">C0<=S</label>
</location>
<init ref="id0"/>
<transition>
<source ref="id1"/>
<target ref="id1"/>
<label kind="guard" x="-229" y="-204">C0<S and C1<freq[0] and C2<freq[1] and C3>=freq[2]</label>
<label kind="synchronisation" x="-25" y="-187">k!</label>
<nail x="-348" y="-187"/>
<nail x="280" y="-187"/>
</transition>
<transition>
<source ref="id1"/>
<target ref="id1"/>
<label kind="guard" x="-170" y="-161">C0<S and C1<freq[0] and C2>=freq[1]</label>
<label kind="synchronisation" x="-25" y="-144">k!</label>
<nail x="-272" y="-144"/>
<nail x="212" y="-144"/>
</transition>
<transition>
<source ref="id1"/>
<target ref="id1"/>
<label kind="guard" x="-110" y="-119">C0<S and C1>=freq[0]</label>
<label kind="synchronisation" x="-25" y="-102">k!</label>
<nail x="-195" y="-102"/>
<nail x="144" y="-102"/>
</transition>
<transition>
<source ref="id1"/>
<target ref="id1"/>
<label kind="guard" x="-42" y="-76">C0>=S</label>
<label kind="synchronisation" x="34" y="-59">k!</label>
<label kind="assignment" x="-76" y="-59">C0=0,update()</label>
<nail x="-119" y="-59"/>
<nail x="76" y="-59"/>
</transition>
<transition>
<source ref="id0"/>
<target ref="id1"/>
<label kind="assignment" x="-119" y="0">update()</label>
</transition>
</template>
<template>
<name>A1</name>
<parameter>int[0,nb_car-1] id</parameter>
<location id="id2" x="-289" y="-85">
<name x="-306" y="-119">I</name>
<urgent/>
</location>
<location id="id3" x="161" y="-85">
<name x="136" y="-110">s1</name>
<label kind="invariant" x="102" y="-68">C1<=max_com_delay[id]</label>
</location>
<location id="id4" x="-102" y="-85">
<name x="-127" y="-110">s0</name>
<label kind="invariant" x="-153" y="-68">C1<=freq[id]</label>
</location>
<init ref="id2"/>
<transition>
<source ref="id2"/>
<target ref="id4"/>
<label kind="assignment" x="-263" y="-85">C1=init_clock[id]</label>
</transition>
<transition>
<source ref="id3"/>
<target ref="id4"/>
<label kind="guard" x="-42" y="-102">C1>=min_com_delay[id]</label>
<label kind="assignment" x="-25" y="-85">communicate(id)</label>
</transition>
<transition>
<source ref="id4"/>
<target ref="id3"/>
<label kind="guard" x="-25" y="-178">C1>=freq[id]</label>
<label kind="synchronisation" x="25" y="-161">k?</label>
<label kind="assignment" x="-34" y="-144">C1=0, decision(id)</label>
<nail x="-102" y="-145"/>
<nail x="161" y="-145"/>
</transition>
</template>
<template>
<name>A2</name>
<parameter>int[0,nb_car-1] id</parameter>
<location id="id5" x="-289" y="-85">
<name x="-299" y="-119">I</name>
<urgent/>
</location>
<location id="id6" x="161" y="-85">
<name x="136" y="-110">s1</name>
<label kind="invariant" x="102" y="-68">C2<=max_com_delay[id]</label>
</location>
<location id="id7" x="-102" y="-85">
<name x="-127" y="-110">s0</name>
<label kind="invariant" x="-153" y="-68">C2<=freq[id]</label>
</location>
<init ref="id5"/>
<transition>
<source ref="id5"/>
<target ref="id7"/>
<label kind="assignment" x="-263" y="-85">C2=init_clock[id]</label>
</transition>
<transition>
<source ref="id6"/>
<target ref="id7"/>
<label kind="guard" x="-42" y="-102">C2>=min_com_delay[id]</label>
<label kind="assignment" x="-25" y="-85">communicate(id)</label>
</transition>
<transition>
<source ref="id7"/>
<target ref="id6"/>
<label kind="guard" x="-25" y="-178">C2>=freq[id]</label>
<label kind="synchronisation" x="25" y="-161">k?</label>
<label kind="assignment" x="-34" y="-144">C2=0, decision(id)</label>
<nail x="-102" y="-145"/>
<nail x="161" y="-145"/>
</transition>
</template>
<template>
<name>A3</name>
<parameter>int[0,nb_car-1] id</parameter>
<location id="id8" x="-289" y="-85">
<name x="-299" y="-119">I</name>
<urgent/>
</location>
<location id="id9" x="161" y="-85">
<name x="136" y="-110">s1</name>
<label kind="invariant" x="102" y="-68">C3<=max_com_delay[id]</label>
</location>
<location id="id10" x="-102" y="-85">
<name x="-127" y="-110">s0</name>
<label kind="invariant" x="-153" y="-68">C3<=freq[id]</label>
</location>
<init ref="id8"/>
<transition>
<source ref="id8"/>
<target ref="id10"/>
<label kind="assignment" x="-263" y="-85">C3=init_clock[id]</label>
</transition>
<transition>
<source ref="id9"/>
<target ref="id10"/>
<label kind="guard" x="-42" y="-102">C3>=min_com_delay[id]</label>
<label kind="assignment" x="-25" y="-85">communicate(id)</label>
</transition>
<transition>
<source ref="id10"/>
<target ref="id9"/>
<label kind="guard" x="-17" y="-178">C3>=freq[id]</label>
<label kind="synchronisation" x="25" y="-161">k?</label>
<label kind="assignment" x="-34" y="-144">C3=0, decision(id)</label>
<nail x="-102" y="-145"/>
<nail x="161" y="-145"/>
</transition>
</template>
<template>
<name>A4</name>
<parameter>int[0,nb_car-1] id</parameter>
<location id="id11" x="-289" y="-85">
<name x="-299" y="-119">I</name>
<urgent/>
</location>
<location id="id12" x="161" y="-85">
<name x="136" y="-110">s1</name>
<label kind="invariant" x="102" y="-68">C4<=max_com_delay[id]</label>
</location>
<location id="id13" x="-102" y="-85">
<name x="-127" y="-110">s0</name>
<label kind="invariant" x="-153" y="-68">C4<=freq[id]</label>
</location>
<init ref="id11"/>
<transition>
<source ref="id11"/>
<target ref="id13"/>
<label kind="assignment" x="-263" y="-85">C4=init_clock[id]</label>
</transition>
<transition>
<source ref="id12"/>
<target ref="id13"/>
<label kind="guard" x="-42" y="-102">C4>=min_com_delay[id]</label>
<label kind="assignment" x="-25" y="-85">communicate(id)</label>
</transition>
<transition>
<source ref="id13"/>
<target ref="id12"/>
<label kind="guard" x="-25" y="-178">C4>=freq[id]</label>
<label kind="synchronisation" x="25" y="-161">k?</label>
<label kind="assignment" x="-34" y="-144">C4=0, decision(id)</label>
<nail x="-102" y="-145"/>
<nail x="161" y="-145"/>
</transition>
</template>
<template>
<name>A5</name>
<parameter>int[0,nb_car-1] id</parameter>
<location id="id14" x="-289" y="-85">
<name x="-299" y="-119">I</name>
<urgent/>
</location>
<location id="id15" x="161" y="-85">
<name x="136" y="-110">s1</name>
<label kind="invariant" x="102" y="-68">C5<=max_com_delay[id]</label>
</location>
<location id="id16" x="-102" y="-85">
<name x="-127" y="-110">s0</name>
<label kind="invariant" x="-153" y="-68">C5<=freq[id]</label>
</location>
<init ref="id14"/>
<transition>
<source ref="id14"/>
<target ref="id16"/>
<label kind="assignment" x="-263" y="-85">C5=init_clock[id]</label>
</transition>
<transition>
<source ref="id15"/>
<target ref="id16"/>
<label kind="guard" x="-42" y="-102">C5>=min_com_delay[id]</label>
<label kind="assignment" x="-25" y="-85">communicate(id)</label>
</transition>
<transition>
<source ref="id16"/>
<target ref="id15"/>
<label kind="guard" x="-17" y="-178">C5>=freq[id]</label>
<label kind="synchronisation" x="25" y="-161">k?</label>
<label kind="assignment" x="-34" y="-144">C5=0, decision(id)</label>
<nail x="-102" y="-145"/>
<nail x="161" y="-145"/>
</transition>
</template>
<system>Environment = A0();
VehicleA = A1(0);
VehicleB = A2(1);
VehicleC = A3(2);
VehicleD = A4(3);
VehicleE = A5(4);
system Environment, VehicleA, VehicleB, VehicleC;
</system>
<queries>
<query>
<formula>A[] time_to_collision(0,1) >=0
</formula>
<comment>
</comment>
</query>
</queries>
</nta>