1 package org.opentrafficsim.imb.kpi;
2
3 import java.util.HashMap;
4 import java.util.HashSet;
5 import java.util.Iterator;
6 import java.util.Map;
7 import java.util.Set;
8
9 import org.djunits.unit.AccelerationUnit;
10 import org.djunits.unit.FrequencyUnit;
11 import org.djunits.unit.LengthUnit;
12 import org.djunits.unit.SpeedUnit;
13 import org.djunits.unit.TimeUnit;
14 import org.djunits.value.vdouble.scalar.Acceleration;
15 import org.djunits.value.vdouble.scalar.Duration;
16 import org.djunits.value.vdouble.scalar.Frequency;
17 import org.djunits.value.vdouble.scalar.Length;
18 import org.djunits.value.vdouble.scalar.Speed;
19 import org.djunits.value.vdouble.scalar.Time;
20 import org.opentrafficsim.imb.IMBException;
21 import org.opentrafficsim.imb.connector.Connector;
22 import org.opentrafficsim.imb.connector.IMBConnector;
23 import org.opentrafficsim.imb.transceiver.Transceiver;
24 import org.opentrafficsim.kpi.interfaces.GtuTypeDataInterface;
25 import org.opentrafficsim.kpi.sampling.KpiGtuDirectionality;
26 import org.opentrafficsim.kpi.sampling.KpiLaneDirection;
27 import org.opentrafficsim.kpi.sampling.Query;
28 import org.opentrafficsim.kpi.sampling.Sampler;
29 import org.opentrafficsim.kpi.sampling.meta.MetaDataGtuType;
30 import org.opentrafficsim.kpi.sampling.meta.MetaDataSet;
31
32 import nl.tno.imb.TByteBuffer;
33 import nl.tno.imb.TEventEntry;
34 import nl.tudelft.simulation.language.d3.CartesianPoint;
35
36
37
38
39
40
41
42
43
44
45
46
47 public class IMBSampler extends Sampler
48 {
49
50 private final IMBConnector imbConnector;
51
52
53 private ImbKpiTransceiver imbKpiTransceiver;
54
55
56 private Time lastTimestamp = Time.ZERO;
57
58
59 private final Map<KpiLaneDirection, Time> startRecordingMap = new HashMap<>();
60
61
62 private final Map<KpiLaneDirection, Time> stopRecordingMap = new HashMap<>();
63
64
65 protected final Map<String, NodeData> nodes = new HashMap<>();
66
67
68 protected final Map<String, LinkData> links = new HashMap<>();
69
70
71 protected final Map<String, LaneData> lanes = new HashMap<>();
72
73
74 protected final Map<String, GtuData> gtus = new HashMap<>();
75
76
77 protected final Map<String, KpiLaneDirection> lastLanes = new HashMap<>();
78
79
80 protected final GtuTypeData defaultGtuType;
81
82
83 protected final RouteData defaultRoute;
84
85
86
87
88
89
90 public static void main(final String[] args) throws IMBException
91 {
92 if (args.length == 0)
93 {
94 new IMBSampler("localhost", 4000, "OTS_IMB_KPI", 1, "OTS_RT");
95 }
96 else
97 {
98 if (args.length != 5)
99 {
100 throw new IMBException("Use as follows: java -jar IMBSampler host port model modelId federation");
101 }
102 String host = args[0];
103 int port = Integer.valueOf(args[1]);
104 String modelName = args[2];
105 int modelId = Integer.valueOf(args[3]);
106 String federation = args[4];
107 new IMBSampler(host, port, modelName, modelId, federation);
108 }
109 }
110
111
112
113
114
115
116
117
118
119
120 public IMBSampler(final String host, final int port, final String modelName, final int modelId, final String federation)
121 throws IMBException
122 {
123 this.imbConnector = new IMBConnector(host, port, modelName, modelId, federation);
124
125
126 this.defaultGtuType = new GtuTypeData("car");
127 NodeData nodeA = new NodeData("NodeA", new CartesianPoint(0, 0, 0));
128 NodeData nodeB = new NodeData("NodeB", new CartesianPoint(1, 1, 0));
129 this.nodes.put(nodeA.getNodeName(), nodeA);
130 this.nodes.put(nodeB.getNodeName(), nodeB);
131 this.defaultRoute = new RouteData("Route A-B", nodeA, nodeB);
132
133 Transceiver nodeTransceiver = new NodeTransceiver(this, this.imbConnector);
134 this.imbConnector.register(nodeTransceiver.getId(), nodeTransceiver);
135
136 Transceiver linkTransceiver = new LinkTransceiver(this, this.imbConnector);
137 this.imbConnector.register(linkTransceiver.getId(), linkTransceiver);
138
139 Transceiver laneTransceiver = new LaneTransceiver(this, this.imbConnector);
140 this.imbConnector.register(laneTransceiver.getId(), laneTransceiver);
141
142 Transceiver gtuTransceiver = new GTUTransceiver(this, this.imbConnector);
143 this.imbConnector.register(gtuTransceiver.getId(), gtuTransceiver);
144
145 }
146
147
148
149
150 private Query getQuery()
151 {
152
153 MetaDataSet metaDataSet = new MetaDataSet();
154 Set<GtuTypeDataInterface> gtuTypes = new HashSet<>();
155 gtuTypes.add(new GtuTypeData("car"));
156 gtuTypes.add(new GtuTypeData("truck"));
157 metaDataSet.put(new MetaDataGtuType("gtuType"), gtuTypes);
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175 String[] eastBound = new String[] { "L1EB", "L2EB", "L4EB", "L6EB", "L7EB", "L10EB", "L11EB", "L13EB", "L14EB", "L16EB",
176 "L17EB", "L19EB", "L20EB", "L22EB", "L23EB", "L25EB", "L26EB", "L28EB", "L29EB", "L31EB", "L32EB", "L33EB",
177 "L35EB", "L36EB" };
178
179
180
181 Query query = new Query(this, "A58 both directions", metaDataSet, new Frequency(2.0, FrequencyUnit.PER_MINUTE));
182 addSpaceTimeRegions(query, eastBound);
183
184
185 try
186 {
187 this.imbKpiTransceiver = new ImbKpiTransceiver(this.imbConnector, Time.ZERO, "A58 network", query, new Duration(30, TimeUnit.SECOND));
188 }
189 catch (IMBException exception)
190 {
191 throw new RuntimeException("Cannot start ImbKpiTransceiver.", exception);
192 }
193
194 return query;
195 }
196
197
198
199
200
201 private void addSpaceTimeRegions(final Query query, final String[] linksIds)
202 {
203 for (String link : linksIds)
204 {
205 query.addSpaceTimeRegionLink(this.links.get(link), KpiGtuDirectionality.DIR_PLUS, new Length(0.0, LengthUnit.SI),
206 this.links.get(link).getLength(), new Time(0.0, TimeUnit.SI), new Time(1.0, TimeUnit.HOUR));
207 }
208 }
209
210
211 @Override
212 public final Time now()
213 {
214 return this.lastTimestamp;
215 }
216
217
218 @Override
219 public final void scheduleStartRecording(final Time time, final KpiLaneDirection kpiLaneDirection)
220 {
221
222 this.startRecordingMap.put(kpiLaneDirection, time);
223 }
224
225
226 @Override
227 public final void scheduleStopRecording(final Time time, final KpiLaneDirection kpiLaneDirection)
228 {
229
230 this.stopRecordingMap.put(kpiLaneDirection, time);
231 }
232
233
234 @Override
235 public final void initRecording(final KpiLaneDirection kpiLaneDirection)
236 {
237
238 }
239
240
241 @Override
242 public final void finalizeRecording(final KpiLaneDirection kpiLaneDirection)
243 {
244
245 }
246
247
248
249
250
251
252
253
254
255
256
257 protected void sample(double timeStamp, String gtuId, String laneId, boolean forward, double longitudinalPosition,
258 double speed, double acceleration)
259 {
260
261 updateClock(timeStamp);
262 if (!this.lanes.containsKey(laneId) || !this.gtus.containsKey(gtuId))
263 {
264
265 return;
266 }
267 KpiLaneDirection kpiLaneDirection = new KpiLaneDirection(this.lanes.get(laneId),
268 forward ? KpiGtuDirectionality.DIR_PLUS : KpiGtuDirectionality.DIR_MINUS);
269 GtuData gtu = this.gtus.get(gtuId);
270 if (this.lastLanes.containsKey(gtuId) && contains(this.lastLanes.get(gtuId))
271 && !this.lastLanes.get(gtuId).equals(kpiLaneDirection))
272 {
273 processGtuRemoveEvent(this.lastLanes.get(gtuId), gtu);
274 }
275 if ((!this.lastLanes.containsKey(gtuId) || !this.lastLanes.get(gtuId).equals(kpiLaneDirection))
276 && contains(kpiLaneDirection))
277 {
278 processGtuAddEvent(kpiLaneDirection, new Length(longitudinalPosition, LengthUnit.SI),
279 new Speed(speed, SpeedUnit.SI), new Acceleration(acceleration, AccelerationUnit.SI), now(), gtu);
280 }
281 else if (contains(kpiLaneDirection))
282 {
283
284 processGtuMoveEvent(kpiLaneDirection, new Length(longitudinalPosition, LengthUnit.SI),
285 new Speed(speed, SpeedUnit.SI), new Acceleration(acceleration, AccelerationUnit.SI), now(), gtu);
286 }
287 this.lastLanes.put(gtuId, kpiLaneDirection);
288 }
289
290
291 private boolean queryObtained = false;
292
293
294
295
296
297 protected void updateClock(double timeStamp)
298 {
299 if (!this.queryObtained)
300 {
301 getQuery();
302 this.queryObtained = true;
303 }
304 if (this.lastTimestamp.si >= timeStamp && this.lastTimestamp.si > 0)
305 {
306 return;
307 }
308 this.imbKpiTransceiver.notifyTime(now());
309 this.lastTimestamp = new Time(timeStamp, TimeUnit.SI);
310 Iterator<KpiLaneDirection> iterator = this.startRecordingMap.keySet().iterator();
311 while (iterator.hasNext())
312 {
313 KpiLaneDirection kpiLaneDirection = iterator.next();
314 if (now().ge(this.startRecordingMap.get(kpiLaneDirection)))
315 {
316 startRecording(kpiLaneDirection);
317 }
318 iterator.remove();
319 }
320 iterator = this.stopRecordingMap.keySet().iterator();
321 while (iterator.hasNext())
322 {
323 KpiLaneDirection kpiLaneDirection = iterator.next();
324 if (now().ge(this.stopRecordingMap.get(kpiLaneDirection)))
325 {
326 stopRecording(kpiLaneDirection);
327 }
328 iterator.remove();
329 }
330 }
331
332
333
334
335
336
337 private static class GTUTransceiver implements Transceiver
338 {
339
340 private final IMBSampler sampler;
341
342
343 private final IMBConnector imbConnector;
344
345
346
347
348
349 public GTUTransceiver(final IMBSampler sampler, final IMBConnector imbConnector)
350 {
351 this.imbConnector = imbConnector;
352 this.sampler = sampler;
353 }
354
355
356 @Override
357 @SuppressWarnings("unused")
358 public void handleMessageFromIMB(String imbEventName, TByteBuffer imbPayload) throws IMBException
359 {
360 int imbEventTypeNr = imbPayload.readInt32();
361 switch (imbEventTypeNr)
362 {
363 case TEventEntry.ACTION_NEW:
364 {
365 double timeStamp = imbPayload.readDouble();
366 String gtuId = imbPayload.readString();
367 double x = imbPayload.readDouble();
368 double y = imbPayload.readDouble();
369 double z = imbPayload.readDouble();
370 double rotZ = imbPayload.readDouble();
371 String networkId = imbPayload.readString();
372 String linkId = imbPayload.readString();
373
374 String laneId = linkId + "." + imbPayload.readString();
375 double longitudinalPosition = imbPayload.readDouble();
376 double length = imbPayload.readDouble();
377 double width = imbPayload.readDouble();
378 byte r = imbPayload.readByte();
379 byte g = imbPayload.readByte();
380 byte b = imbPayload.readByte();
381
382
383 GtuData gtuData = new GtuData(gtuId, this.sampler.defaultGtuType, this.sampler.defaultRoute);
384 this.sampler.gtus.put(gtuId, gtuData);
385 break;
386 }
387
388 case TEventEntry.ACTION_CHANGE:
389 {
390 double timeStamp = imbPayload.readDouble();
391 String gtuId = imbPayload.readString();
392 double x = imbPayload.readDouble();
393 double y = imbPayload.readDouble();
394 double z = imbPayload.readDouble();
395 double rotZ = imbPayload.readDouble();
396 String networkId = imbPayload.readString();
397 String linkId = imbPayload.readString();
398
399 String laneId = linkId + "." + imbPayload.readString();
400 double longitudinalPosition = imbPayload.readDouble();
401 double speed = imbPayload.readDouble();
402 double acceleration = imbPayload.readDouble();
403 String turnIndicatorStatus = imbPayload.readString();
404 boolean brakingLights = imbPayload.readBoolean();
405 double odometer = imbPayload.readDouble();
406 boolean forward = true;
407
408 this.sampler.sample(timeStamp, gtuId, laneId, forward, longitudinalPosition, speed, acceleration);
409
410 break;
411 }
412
413 case TEventEntry.ACTION_DELETE:
414 {
415
416 break;
417 }
418
419 default:
420 break;
421 }
422 }
423
424 @Override
425 public String getId()
426 {
427 return "GTU";
428 }
429
430 @Override
431 public Connector getConnector()
432 {
433 return this.imbConnector;
434 }
435 }
436
437
438
439
440
441
442 private static class NodeTransceiver implements Transceiver
443 {
444
445 private final IMBSampler sampler;
446
447
448 private final IMBConnector imbConnector;
449
450
451
452
453
454 public NodeTransceiver(final IMBSampler sampler, final IMBConnector imbConnector)
455 {
456 this.imbConnector = imbConnector;
457 this.sampler = sampler;
458 }
459
460
461 @Override
462 @SuppressWarnings("unused")
463 public void handleMessageFromIMB(String imbEventName, TByteBuffer imbPayload) throws IMBException
464 {
465 int imbEventTypeNr = imbPayload.readInt32();
466 switch (imbEventTypeNr)
467 {
468 case TEventEntry.ACTION_NEW:
469 {
470 double timeStamp = imbPayload.readDouble();
471 String networkId = imbPayload.readString();
472 String nodeId = imbPayload.readString();
473 double x = imbPayload.readDouble();
474 double y = imbPayload.readDouble();
475 double z = imbPayload.readDouble();
476 CartesianPoint p = new CartesianPoint(x, y, z);
477 NodeData nodeData = new NodeData(nodeId, p);
478 this.sampler.nodes.put(nodeId, nodeData);
479 break;
480 }
481
482 case TEventEntry.ACTION_CHANGE:
483 {
484
485 break;
486 }
487
488 case TEventEntry.ACTION_DELETE:
489 {
490
491 break;
492 }
493
494 default:
495 break;
496 }
497 }
498
499 @Override
500 public String getId()
501 {
502 return "Node";
503 }
504
505 @Override
506 public Connector getConnector()
507 {
508 return this.imbConnector;
509 }
510 }
511
512
513
514
515
516
517 private static class LinkTransceiver implements Transceiver
518 {
519
520 private final IMBSampler sampler;
521
522
523 private final IMBConnector imbConnector;
524
525
526
527
528
529 public LinkTransceiver(final IMBSampler sampler, final IMBConnector imbConnector)
530 {
531 this.imbConnector = imbConnector;
532 this.sampler = sampler;
533 }
534
535
536 @Override
537 @SuppressWarnings("unused")
538 public void handleMessageFromIMB(String imbEventName, TByteBuffer imbPayload) throws IMBException
539 {
540 int imbEventTypeNr = imbPayload.readInt32();
541 switch (imbEventTypeNr)
542 {
543 case TEventEntry.ACTION_NEW:
544 {
545 double timeStamp = imbPayload.readDouble();
546 String networkId = imbPayload.readString();
547 String linkId = imbPayload.readString();
548 String startNodeId = imbPayload.readString();
549 String endNodeId = imbPayload.readString();
550 int dlNumPoints = imbPayload.readInt32();
551 double len = 0.0;
552 double x = imbPayload.readDouble();
553 double y = imbPayload.readDouble();
554 double z = imbPayload.readDouble();
555 CartesianPoint p1 = new CartesianPoint(x, y, z);
556 for (int i = 1; i < dlNumPoints; i++)
557 {
558 x = imbPayload.readDouble();
559 y = imbPayload.readDouble();
560 z = imbPayload.readDouble();
561 CartesianPoint p2 = new CartesianPoint(x, y, z);
562 len += p1.distance(p2);
563 p1 = p2;
564 }
565 Length length = new Length(len, LengthUnit.SI);
566 LinkData linkData = new LinkData(linkId, this.sampler.nodes.get(startNodeId),
567 this.sampler.nodes.get(endNodeId), length);
568 this.sampler.links.put(linkId, linkData);
569 break;
570 }
571
572 case TEventEntry.ACTION_CHANGE:
573 {
574
575 break;
576 }
577
578 case TEventEntry.ACTION_DELETE:
579 {
580
581 break;
582 }
583
584 default:
585 break;
586 }
587 }
588
589 @Override
590 public String getId()
591 {
592 return "Link_GTU";
593 }
594
595 @Override
596 public Connector getConnector()
597 {
598 return this.imbConnector;
599 }
600 }
601
602
603
604
605
606
607 private static class LaneTransceiver implements Transceiver
608 {
609
610 private final IMBSampler sampler;
611
612
613 private final IMBConnector imbConnector;
614
615
616
617
618
619 public LaneTransceiver(final IMBSampler sampler, final IMBConnector imbConnector)
620 {
621 this.imbConnector = imbConnector;
622 this.sampler = sampler;
623 }
624
625
626 @Override
627 @SuppressWarnings("unused")
628 public void handleMessageFromIMB(String imbEventName, TByteBuffer imbPayload) throws IMBException
629 {
630 int imbEventTypeNr = imbPayload.readInt32();
631 switch (imbEventTypeNr)
632 {
633 case TEventEntry.ACTION_NEW:
634 {
635 double timeStamp = imbPayload.readDouble();
636 String networkId = imbPayload.readString();
637 String linkId = imbPayload.readString();
638
639 String laneId = linkId + "." + imbPayload.readString();
640 int laneNumber = imbPayload.readInt32();
641 int dlNumPoints = imbPayload.readInt32();
642 double len = 0.0;
643 double x = imbPayload.readDouble();
644 double y = imbPayload.readDouble();
645 double z = imbPayload.readDouble();
646 CartesianPoint p1 = new CartesianPoint(x, y, z);
647 for (int i = 1; i < dlNumPoints; i++)
648 {
649 x = imbPayload.readDouble();
650 y = imbPayload.readDouble();
651 z = imbPayload.readDouble();
652 CartesianPoint p2 = new CartesianPoint(x, y, z);
653 len += p1.distance(p2);
654 p1 = p2;
655 }
656 Length length = new Length(len, LengthUnit.SI);
657 LaneData laneData = new LaneData(this.sampler.links.get(linkId), laneId, length);
658 if (this.sampler.lanes.containsKey(laneId))
659 {
660 System.out.println("Lanes not unique.");
661 }
662 this.sampler.lanes.put(laneId, laneData);
663 break;
664 }
665
666 case TEventEntry.ACTION_CHANGE:
667 {
668
669 break;
670 }
671
672 case TEventEntry.ACTION_DELETE:
673 {
674
675 break;
676 }
677
678 default:
679 break;
680 }
681 }
682
683 @Override
684 public String getId()
685 {
686 return "Lane_GTU";
687 }
688
689 @Override
690 public Connector getConnector()
691 {
692 return this.imbConnector;
693 }
694 }
695
696 }