1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
10 /// @file MSE2Collector.cpp
11 /// @author Christian Roessel
12 /// @author Daniel Krajzewicz
13 /// @author Sascha Krieg
14 /// @author Michael Behrisch
15 /// @author Robbin Blokpoel
16 /// @author Jakob Erdmann
17 /// @author Leonhard Luecken
18 /// @date Mon Feb 03 2014 10:13 CET
19 /// @version $Id$
20 ///
21 // An areal detector covering a sequence of consecutive lanes
22 /****************************************************************************/
23
24
25 /* TODO:
26 * tests:
27 * - subsecond variant, ballistic variant
28 * allow omitting jam processing (?)
29 *
30 * Meso-compatibility? (esp. enteredLane-argument for MSBaseVehicle::notifyEnter() is not treated)
31 * Compatibility without internal lanes?
32 * Include leftVehicles into output?
33 */
34
35 // ===========================================================================
36 // included modules
37 // ===========================================================================
38 #include <config.h>
39
40 #include <cassert>
41 #include <algorithm>
42 #include "MSE2Collector.h"
43 #include <microsim/MSLane.h>
44 #include <microsim/MSNet.h>
45 #include <microsim/MSVehicle.h>
46 #include <microsim/MSVehicleType.h>
47
48 //#define DEBUG_E2_CONSTRUCTOR
49 //#define DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
50 //#define DEBUG_E2_NOTIFY_MOVE
51 //#define DEBUG_E2_MAKE_VEHINFO
52 //#define DEBUG_E2_DETECTOR_UPDATE
53 //#define DEBUG_E2_TIME_ON_DETECTOR
54 //#define DEBUG_E2_JAMS
55 //#define DEBUG_E2_XML_OUT
56 //#define DEBUG_COND (true)
57 //#define DEBUG_COND (getID()=="e2Detector_e5.601A_1_SAa")
58 //#define DEBUG_COND (getID()=="702057")
59 //#define DEBUG_COND (getID()=="det0")
60
MSE2Collector(const std::string & id,DetectorUsage usage,MSLane * lane,double startPos,double endPos,double length,SUMOTime haltingTimeThreshold,double haltingSpeedThreshold,double jamDistThreshold,const std::string & vTypes)61 MSE2Collector::MSE2Collector(const std::string& id,
62 DetectorUsage usage, MSLane* lane, double startPos, double endPos, double length,
63 SUMOTime haltingTimeThreshold, double haltingSpeedThreshold, double jamDistThreshold,
64 const std::string& vTypes) :
65 MSMoveReminder(id, lane, false),
66 MSDetectorFileOutput(id, vTypes),
67 myUsage(usage),
68 myJamHaltingSpeedThreshold(haltingSpeedThreshold),
69 myJamHaltingTimeThreshold(haltingTimeThreshold),
70 myJamDistanceThreshold(jamDistThreshold),
71 myNumberOfEnteredVehicles(0),
72 myNumberOfSeenVehicles(0),
73 myNumberOfLeftVehicles(0) {
74 reset();
75
76 #ifdef DEBUG_E2_CONSTRUCTOR
77 if DEBUG_COND {
78 std::cout << "\n" << "Creating MSE2Collector " << id
79 << " with lane = " << lane->getID()
80 << " startPos = " << startPos
81 << " endPos = " << endPos
82 << " length = " << length
83 << " haltingTimeThreshold = " << haltingTimeThreshold
84 << " haltingSpeedThreshold = " << haltingSpeedThreshold
85 << " jamDistThreshold = " << jamDistThreshold
86 << std::endl;
87 }
88 #endif
89
90 assert(lane != 0);
91
92 // check that exactly one of length, startPos, endPos is invalid
93 bool lengthInvalid = length == std::numeric_limits<double>::max() || length <= 0;
94 bool endPosInvalid = endPos == std::numeric_limits<double>::max();
95 bool posInvalid = startPos == std::numeric_limits<double>::max();
96
97 // check and normalize positions (assure positive values for pos and endPos, snap to lane-ends)
98 if (lengthInvalid) {
99 // assume that the detector is only located on a single lane
100 if (posInvalid) {
101 WRITE_WARNING("No valid detector length and start position given. Assuming startPos = 0 and length = end position");
102 startPos = 0;
103 }
104 if (endPosInvalid) {
105 WRITE_WARNING("No valid detector length and end position given. Assuming endPos = lane length and length = endPos-startPos");
106 endPos = lane->getLength();
107 }
108 endPos = endPos < 0 ? lane->getLength() + endPos : endPos;
109 startPos = startPos < 0 ? lane->getLength() + startPos : startPos;
110 bool valid = endPos <= lane->getLength() && 0 <= startPos && startPos < endPos;
111 if (!valid) {
112 throw InvalidArgument("Error in specification for E2Detector '" + id + "'. Positional argument is malformed. 0 <= pos < endPos <= lane.getLength() is required.");
113 }
114 // snap detector ends to lane ends
115 endPos = snap(endPos, lane->getLength(), POSITION_EPS);
116 startPos = snap(startPos, 0., POSITION_EPS);
117 length = endPos - startPos;
118 } else if (posInvalid) {
119 // endPosInvalid == false
120 endPos = endPos < 0 ? lane->getLength() + endPos : endPos;
121 endPos = snap(endPos, lane->getLength(), POSITION_EPS);
122 } else {
123 // posInvalid == false
124 startPos = startPos < 0 ? lane->getLength() + startPos : startPos;
125 startPos = snap(startPos, 0., POSITION_EPS);
126 }
127
128 myStartPos = startPos;
129 myEndPos = endPos;
130
131 std::vector<MSLane*> lanes;
132 if (posInvalid) {
133 lanes = selectLanes(lane, length, "bw");
134 } else if (endPosInvalid) {
135 lanes = selectLanes(lane, length, "fw");
136 } else {
137 // assuming detector is only located at a single lane
138 lanes.push_back(lane);
139 }
140
141 initAuxiliaries(lanes);
142 checkPositioning(endPosInvalid, length);
143 addDetectorToLanes(lanes);
144 }
145
146
147 MSE2Collector::MSE2Collector(const std::string& id,
148 DetectorUsage usage, std::vector<MSLane*> lanes, double startPos, double endPos,
149 SUMOTime haltingTimeThreshold, double haltingSpeedThreshold, double jamDistThreshold,
150 const std::string& vTypes) :
151 MSMoveReminder(id, lanes[lanes.size() - 1], false), // assure that lanes.size() > 0 at caller side!!!
152 MSDetectorFileOutput(id, vTypes),
153 myUsage(usage),
154 myFirstLane(lanes[0]),
155 myLastLane(lanes[lanes.size() - 1]),
156 myStartPos(startPos),
157 myEndPos(endPos),
158 myJamHaltingSpeedThreshold(haltingSpeedThreshold),
159 myJamHaltingTimeThreshold(haltingTimeThreshold),
160 myJamDistanceThreshold(jamDistThreshold),
161 myNumberOfEnteredVehicles(0),
162 myNumberOfSeenVehicles(0),
163 myNumberOfLeftVehicles(0) {
164 reset();
165
166 for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
167 assert((*i) != 0);
168 }
169
170 #ifdef DEBUG_E2_CONSTRUCTOR
171 if DEBUG_COND {
172 std::cout << "\n" << "Creating MSE2Collector " << id
173 << " with endLane = " << myLastLane->getID()
174 << " endPos = " << endPos
175 << " startLane = " << myFirstLane->getID()
176 << " startPos = " << startPos
177 << " haltingTimeThreshold = " << haltingTimeThreshold
178 << " haltingSpeedThreshold = " << haltingSpeedThreshold
179 << " jamDistThreshold = " << jamDistThreshold
180 << std::endl;
181 }
182 #endif
183
184 myStartPos = myStartPos < 0 ? lanes[0]->getLength() + myStartPos : myStartPos;
185 myEndPos = myEndPos < 0 ? lanes[lanes.size() - 1]->getLength() + myEndPos : myEndPos;
186
187 if (myStartPos < POSITION_EPS) {
188 myStartPos = 0;
189 }
190 if (myEndPos > lanes[lanes.size() - 1]->getLength() - POSITION_EPS) {
191 myEndPos = lanes[lanes.size() - 1]->getLength();
192 }
193
194
195 initAuxiliaries(lanes);
196 checkPositioning();
197 addDetectorToLanes(lanes);
198 }
199
200
201 void
202 MSE2Collector::checkPositioning(bool posGiven, double desiredLength) {
203 // check if detector was truncated
204 if (desiredLength > 0 && myDetectorLength < desiredLength - NUMERICAL_EPS) {
205 std::stringstream ss;
206 ss << "Cannot build detector of length " << desiredLength
207 << " because no further continuation lane was found for lane '" << (posGiven ? myLastLane->getID() : myFirstLane->getID())
208 << "'! Truncated detector at length " << myDetectorLength << ".";
209 WRITE_WARNING(ss.str());
210 }
211
212 if (myDetectorLength < POSITION_EPS && (myStartPos > 0. || myEndPos < myLastLane->getLength())) {
213 // assure minimal detector length
214 double prolong = POSITION_EPS - myDetectorLength;
215 double startPos = MAX2(0., myStartPos - prolong); // new startPos
216 prolong -= myStartPos - startPos;
217 myStartPos = startPos;
218 if (prolong > 0.) {
219 myEndPos = MIN2(myEndPos + prolong, myLastLane->getLength());
220 }
221 WRITE_WARNING("Adjusted detector positioning to meet requirement length >= " + toString(POSITION_EPS)
222 + ". New position is [" + toString(myStartPos) + "," + toString(myEndPos) + "]");
223 }
224
225 // do some regularization snapping...
226 myStartPos = snap(myStartPos, 0., POSITION_EPS);
227 myStartPos = snap(myStartPos, myFirstLane->getLength() - POSITION_EPS, POSITION_EPS);
228 myStartPos = snap(myStartPos, 0., POSITION_EPS);
229 myEndPos = snap(myEndPos, myFirstLane->getLength(), POSITION_EPS);
230 myEndPos = snap(myEndPos, POSITION_EPS, POSITION_EPS);
231 myEndPos = snap(myEndPos, myFirstLane->getLength(), POSITION_EPS);
232 recalculateDetectorLength();
233
234 #ifdef DEBUG_E2_CONSTRUCTOR
235 if DEBUG_COND {
236 std::stringstream ss;
237 // ss << std::setprecision(32) << myEndPos << " : " << POSITION_EPS;
238 // std::cout << ss.str() << std::endl;
239 std::cout << "myStartPos = " << myStartPos << std::endl;
240 std::cout << "myEndPos = " << myEndPos << std::endl;
241 std::cout << "myLastLane->getLength() = " << myLastLane->getLength() << std::endl;
242 }
243 #endif
244
245
246 assert((myStartPos >= POSITION_EPS || myStartPos == 0) && myStartPos < myFirstLane->getLength());
247 assert(myEndPos <= myLastLane->getLength() - POSITION_EPS || myEndPos == myLastLane->getLength());
248 assert(myFirstLane != myLastLane || myEndPos - myStartPos > 0);
249 }
250
251
252 double
253 MSE2Collector::snap(double value, double snapPoint, double snapDist) {
254 if (fabs(value - snapPoint) < snapDist) {
255 return snapPoint;
256 } else {
257 return value;
258 }
259 }
260
261
262 void
263 MSE2Collector::recalculateDetectorLength() {
264 std::vector<std::string>::const_iterator i;
265 std::vector<MSLane*> lanes;
266 // get real lanes
267 for (i = myLanes.begin(); i != myLanes.end(); ++i) {
268 MSLane* lane = MSLane::dictionary(*i);
269 lanes.push_back(lane);
270 }
271
272 // sum up their lengths
273 std::vector<MSLane*>::const_iterator j;
274 MSLane* previous = nullptr;
275 myDetectorLength = 0;
276 for (j = lanes.begin(); j != lanes.end(); ++j) {
277 // lane length
278 myDetectorLength += (*j)->getLength();
279 if (previous != nullptr && !MSGlobals::gUsingInternalLanes) {
280 // eventually link length
281 myDetectorLength += previous->getLinkTo(*j)->getLength();
282 }
283 previous = *j;
284 }
285 // substract uncovered area on first and last lane
286 myDetectorLength -= myStartPos;
287 myDetectorLength -= myLastLane->getLength() - myEndPos;
288
289 #ifdef DEBUG_E2_CONSTRUCTOR
290 if DEBUG_COND {
291 std::cout << "Total detector length after recalculation = " << myDetectorLength << std::endl;
292 }
293 #endif
294 }
295
296
297 MSE2Collector::~MSE2Collector() {
298 // clear move notifications
299 for (std::vector<MoveNotificationInfo*>::iterator j = myMoveNotifications.begin(); j != myMoveNotifications.end(); ++j) {
300 delete *j;
301 }
302 myMoveNotifications.clear();
303
304 // clear vehicle infos
305 for (VehicleInfoMap::iterator j = myVehicleInfos.begin(); j != myVehicleInfos.end(); ++j) {
306 delete j->second;
307 }
308 myVehicleInfos.clear();
309 }
310
311
312 std::vector<MSLane*>
313 MSE2Collector::selectLanes(MSLane* lane, double length, std::string dir) {
314 // direction of detector extension
315 assert(dir == "fw" || dir == "bw");
316 bool fw = dir == "fw";
317 double linkLength = 0; // linkLength (used if no internal lanes are present)
318 bool substractedLinkLength = false; // whether linkLength was substracted during the last iteration.
319
320 #ifdef DEBUG_E2_CONSTRUCTOR
321 if DEBUG_COND {
322 std::cout << "\n" << "selectLanes()" << (fw ? "(forward)" : "(backward)") << std::endl;
323 }
324 #endif
325 std::vector<MSLane*> lanes;
326 // Selected lanes are stacked into vector 'lanes'. If dir == "bw" lanes will be reversed after this is done.
327 // The length is reduced while adding lanes to the detector
328 // First we adjust the starting value for length (in the first iteration, the whole length of the first considered lane is substracted,
329 // while it might only be partially covered by the detector)
330 if (fw) {
331 assert(myStartPos != std::numeric_limits<double>::max());
332 length += myStartPos;
333 } else {
334 assert(myEndPos != std::numeric_limits<double>::max());
335 length += lane->getLength() - myEndPos;
336 }
337 length = MAX2(POSITION_EPS, length); // this assures to add at least one lane to lanes
338 while (length >= POSITION_EPS && lane != nullptr) {
339 // Break loop for length <= NUMERICAL_EPS to avoid placement of very small
340 // detector piece on the end or beginning of one lane due to numerical rounding errors.
341 lanes.push_back(lane);
342 #ifdef DEBUG_E2_CONSTRUCTOR
343 if DEBUG_COND {
344 std::cout << "Added lane " << lane->getID()
345 << " (length: " << lane->getLength() << ")" << std::endl;
346 }
347 #endif
348
349 length -= lane->getLength();
350
351 // proceed to upstream predecessor
352 if (fw) {
353 lane = lane->getCanonicalSuccessorLane();
354 } else {
355 lane = lane->getCanonicalPredecessorLane();
356 }
357
358
359 substractedLinkLength = false;
360 if (lane != nullptr && !MSGlobals::gUsingInternalLanes && length > POSITION_EPS) {
361 // In case wher no internal lanes are used,
362 // take into account the link length for the detector range
363 linkLength = 0;
364 if (fw) {
365 linkLength = lanes.back()->getLinkTo(lane)->getLength();
366 } else {
367 linkLength = lane->getLinkTo(lanes.back())->getLength();
368 }
369 length -= linkLength;
370 substractedLinkLength = true;
371 }
372
373
374 #ifdef DEBUG_E2_CONSTRUCTOR
375 if DEBUG_COND {
376 if (lane != 0) {
377 std::cout << (fw ? "Successor lane: " : "Predecessor lane: ") << "'" << lane->getID() << "'";
378 }
379 std::cout << std::endl;
380 }
381 #endif
382 }
383
384 if (substractedLinkLength) {
385 // if the link's length was substracted during the last step,
386 // the start/endPos would lie on a non-existing internal lane,
387 // therefore revert and truncate detector part on the non-existing internal lane.
388 length += linkLength;
389 }
390
391
392 // 1) At this point a negative <length> means that not the whole last stored lane lanes[lanes.size()-1]
393 // should be added to the detector, but the detector should spare out a part with length = -<length>
394 // If this part is too small (of length < POSITION_EPS) we take the whole lane
395 // 2) The whole lane is also taken for the case that <length> is positive. This corresponds to on
396 // of three situations: i) <length> < POSITION_EPS (break condition -> don't take too small pieces on the next lane)
397 // ii&iii) <length> >= POS_EPSILON may arise either if no continuation lane was found (lane==0), or
398 // in case of not using internal lanes if the detector end/start falls on a link.
399 // In all cases we take the whole last lane.
400 if (fw) {
401 if (length > -POSITION_EPS) {
402 myEndPos = lanes[lanes.size() - 1]->getLength();
403 } else if (length < 0) {
404 myEndPos = lanes[lanes.size() - 1]->getLength() + length;
405 }
406 } else {
407 if (length > -POSITION_EPS) {
408 myStartPos = 0;
409 } else if (length < 0) {
410 myStartPos = -length;
411 }
412 }
413
414 // reverse lanes if lane selection was backwards
415 if (!fw) {
416 std::reverse(lanes.begin(), lanes.end());
417 }
418
419 return lanes;
420 }
421
422 void
423 MSE2Collector::addDetectorToLanes(std::vector<MSLane*>& lanes) {
424 #ifdef DEBUG_E2_CONSTRUCTOR
425 if DEBUG_COND {
426 std::cout << "\n" << "Adding detector " << myID << " to lanes:" << std::endl;
427 }
428 #endif
429 for (std::vector<MSLane*>::iterator l = lanes.begin(); l != lanes.end(); ++l) {
430 (*l)->addMoveReminder(this);
431 #ifdef DEBUG_E2_CONSTRUCTOR
432 if DEBUG_COND {
433 std::cout << (*l)->getID() << std::endl;
434 }
435 #endif
436 }
437 }
438
439 void
440 MSE2Collector::initAuxiliaries(std::vector<MSLane*>& lanes) {
441 // Checks integrity of myLanes, adds internal-lane information, inits myLength, myFirstLane, myLastLane, myOffsets, myEndPos/myStartPos
442 myFirstLane = lanes[0];
443 myLastLane = lanes[lanes.size() - 1];
444
445 #ifdef DEBUG_E2_CONSTRUCTOR
446 if DEBUG_COND {
447 std::cout << "\n" << "Initializing auxiliaries:"
448 << "\nFirst lane: " << myFirstLane->getID() << " (startPos = " << myStartPos << ")"
449 << "\nLast lane: " << myLastLane->getID() << " (endPos = " << myEndPos << ")"
450 << std::endl;
451 }
452 #endif
453
454 // Init myOffsets and myDetectorLength.
455 // The loop below runs through the given lanes assuming the possibility that only non-internal lanes are given
456 // or at least not all relevant internal lanes are considered. During this a new, complete list of lane ids is
457 // built into myLanes.
458 myLanes.clear();
459
460 // myDetectorLength will be increased in the loop below, always giving
461 // the offset of the currently considered lane to the detector start
462 myDetectorLength = -myStartPos;
463 myOffsets.clear();
464
465 // loop over detector lanes and accumulate offsets with respect to the first lane's begin
466 // (these will be corrected afterwards by substracting the start position.)
467 std::vector<MSLane*>::iterator il = lanes.begin();
468
469 // start on an internal lane?
470 // (This may happen if specifying the detector by its upstream
471 // length starting from a given end position)
472 const MSLane* internal = (*il)->isInternal() ? *il : nullptr;
473
474 #ifdef DEBUG_E2_CONSTRUCTOR
475 if DEBUG_COND {
476 std::cout << "\n" << "Initializing offsets:" << std::endl;
477 }
478 #endif
479
480 #ifdef _MSC_VER
481 #pragma warning(push)
482 #pragma warning(disable: 4127) // do not warn about constant conditional expression
483 #endif
484 while (true) {
485 #ifdef _MSC_VER
486 #pragma warning(pop)
487 #endif
488 // Consider the next internal lanes
489 while (internal != nullptr) {
490 myLanes.push_back(internal->getID());
491 myOffsets.push_back(myDetectorLength);
492
493 #ifdef DEBUG_E2_CONSTRUCTOR
494 if DEBUG_COND {
495 std::cout << "Offset for lane " << internal->getID() << " = " << myDetectorLength
496 << std::endl;
497 }
498 #endif
499
500 myDetectorLength += internal->getLength();
501 if (internal->getID() == myLastLane->getID()) {
502 break;
503 }
504
505 // There should be a unique continuation for each internal lane
506 assert(internal->getLinkCont().size() == 1);
507
508 internal = internal->getLinkCont()[0]->getViaLaneOrLane();
509 if (!internal->isInternal()) {
510 // passed the junction
511 internal = nullptr;
512 break;
513 }
514 }
515
516 // Consider the next non-internal lane
517 // This is the first lane in the first iteration, if it is non-internal
518 // However, it can equal myLanes.end() if the myLastLane is internal. In that case we break.
519
520 // Move il to next non-internal
521 while (il != lanes.end() && (*il)->isInternal()) {
522 il++;
523 }
524 if (il == lanes.end()) {
525 break;
526 }
527
528 // There is still a non-internal lane to consider
529 MSLane* lane = *il;
530 myLanes.push_back(lane->getID());
531
532 #ifdef DEBUG_E2_CONSTRUCTOR
533 if DEBUG_COND {
534 std::cout << "Offset for lane " << lane->getID() << " = " << myDetectorLength
535 << std::endl;
536 }
537 #endif
538
539 // Offset to detector start for this lane
540 myOffsets.push_back(myDetectorLength);
541
542 // Add the lanes length to the detector offset
543 myDetectorLength += lane->getLength();
544
545 // Get the next lane if this lane isn't the last one
546 if (++il == lanes.end()) {
547 break;
548 }
549
550 if ((*il)->isInternal()) {
551 // next lane in myLanes is internal
552 internal = *il;
553 continue;
554 }
555
556 // find the connection to next
557 const MSLink* link = lane->getLinkTo(*il);
558 if (link == nullptr) {
559 throw InvalidArgument("Lanes '" + lane->getID() + "' and '" + (*il)->getID() + "' are not consecutive in defintion of e2Detector '" + getID() + "'");
560 }
561
562 if (!MSGlobals::gUsingInternalLanes) {
563 myDetectorLength += link->getLength();
564 } else {
565 internal = link->getViaLane();
566 }
567 }
568
569 // Substract distance not covered on the last considered lane
570 bool fw = myEndPos == std::numeric_limits<double>::max();
571 if (fw) {
572 myDetectorLength -= myStartPos;
573 } else {
574 myDetectorLength -= myLastLane->getLength() - myEndPos;
575 }
576
577 #ifdef DEBUG_E2_CONSTRUCTOR
578 if DEBUG_COND {
579 std::cout << "Total detector length after initAuxiliaries() = " << myDetectorLength << std::endl;
580 }
581 #endif
582
583 // make lanes a complete list including internal lanes
584 lanes = getLanes();
585 }
586
587
588 std::vector<MSLane*>
589 MSE2Collector::getLanes() {
590 std::vector<MSLane*> res;
591 for (std::vector<std::string>::const_iterator i = myLanes.begin(); i != myLanes.end(); ++i) {
592 res.push_back(MSLane::dictionary(*i));
593 }
594 return res;
595 }
596
597
598 bool
599 MSE2Collector::notifyMove(SUMOTrafficObject& tObject, double oldPos,
600 double newPos, double newSpeed) {
601 if (!tObject.isVehicle()) {
602 return false;
603 }
604 SUMOVehicle& veh = static_cast<SUMOVehicle&>(tObject);
605 VehicleInfoMap::iterator vi = myVehicleInfos.find(veh.getID());
606 assert(vi != myVehicleInfos.end()); // all vehicles calling notifyMove() should have called notifyEnter() before
607
608 const std::string& vehID = veh.getID();
609 VehicleInfo& vehInfo = *(vi->second);
610
611 // position relative to the detector start
612 double relPos = vehInfo.entryOffset + newPos;
613
614 // update current distance to the detector end
615 vehInfo.distToDetectorEnd = myDetectorLength - relPos;
616
617 #ifdef DEBUG_E2_NOTIFY_MOVE
618 if DEBUG_COND {
619 std::cout << "\n" << SIMTIME
620 << " MSE2Collector::notifyMove() (detID = " << myID << "on lane '" << myLane->getID() << "')"
621 << " called by vehicle '" << vehID << "'"
622 << " at relative position " << relPos
623 << ", distToDetectorEnd = " << vehInfo.distToDetectorEnd << std::endl;
624 }
625 #endif
626
627 // Check whether vehicle has reached the detector begin
628 if (relPos <= 0) {
629 // detector not yet reached, request being informed further
630 #ifdef DEBUG_E2_NOTIFY_MOVE
631 if DEBUG_COND {
632 std::cout << "Vehicle has not yet reached the detector start position." << std::endl;
633 }
634 #endif
635 return true;
636 } else if (!vehInfo.hasEntered) {
637 vehInfo.hasEntered = true;
638 myNumberOfEnteredVehicles++;
639 myNumberOfSeenVehicles++;
640 }
641
642
643 // determine whether vehicle has moved beyond the detector's end
644 bool vehPassedDetectorEnd = - vehInfo.exitOffset <= newPos - veh.getVehicleType().getLength();
645
646 // determine whether vehicle has been on the detector at all
647 bool vehicleEnteredLaneAfterDetector = vehPassedDetectorEnd && (-vehInfo.exitOffset <= oldPos - veh.getVehicleType().getLength());
648 // ... if not, dont create any notification at all
649 if (vehicleEnteredLaneAfterDetector) {
650 #ifdef DEBUG_E2_NOTIFY_MOVE
651 if DEBUG_COND {
652 std::cout << "Vehicle entered lane behind detector." << std::endl;
653 }
654 #endif
655 } else {
656 myMoveNotifications.push_back(makeMoveNotification(veh, oldPos, newPos, newSpeed, vehInfo));
657 }
658
659
660 if (vehPassedDetectorEnd) {
661 #ifdef DEBUG_E2_NOTIFY_MOVE
662 if DEBUG_COND {
663 std::cout << "Vehicle has left the detector longitudinally." << std::endl;
664 }
665 #endif
666 // Vehicle is beyond the detector, unsubscribe and register removal from myVehicleInfos
667 myLeftVehicles.insert(vehID);
668 return false;
669 } else {
670 // Receive further notifications
671 return true;
672 }
673 }
674
675 bool
676 MSE2Collector::notifyLeave(SUMOTrafficObject& veh, double /* lastPos */, MSMoveReminder::Notification reason, const MSLane* enteredLane) {
677 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
678 if DEBUG_COND {
679 std::cout << "\n" << SIMTIME << " notifyLeave() (detID = " << myID << "on lane '" << myLane->getID() << "')"
680 << "called by vehicle '" << veh.getID() << "'" << std::endl;
681 }
682 #endif
683
684 if (reason == MSMoveReminder::NOTIFICATION_JUNCTION) {
685 // vehicle left lane via junction, unsubscription and registering in myLeftVehicles when
686 // moving beyond the detector end is controlled in notifyMove.
687 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
688 if DEBUG_COND {
689 std::cout << SIMTIME << " Left longitudinally (along junction) -> keep subscription [handle exit in notifyMove()]" << std::endl;
690 }
691 #endif
692
693 if (std::find(myLanes.begin(), myLanes.end(), enteredLane->getID()) == myLanes.end()) {
694 // Entered lane is not part of the detector
695 VehicleInfoMap::iterator vi = myVehicleInfos.find(veh.getID());
696 // Determine exit offset, where vehicle left the detector
697 double exitOffset = vi->second->entryOffset - myOffsets[vi->second->currentOffsetIndex] - vi->second->currentLane->getLength();
698 vi->second->exitOffset = MAX2(vi->second->exitOffset, exitOffset);
699 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
700 if DEBUG_COND {
701 std::cout << SIMTIME << " Vehicle '" << veh.getID() << "' leaves the detector. Exit offset = " << vi->second->exitOffset << std::endl;
702 }
703 #endif
704 }
705
706 return true;
707 } else {
708 VehicleInfoMap::iterator vi = myVehicleInfos.find(veh.getID());
709 // erase vehicle, which leaves in a non-longitudinal way, immediately
710 if (vi->second->hasEntered) {
711 myNumberOfLeftVehicles++;
712 }
713 delete vi->second;
714 myVehicleInfos.erase(vi);
715 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
716 if DEBUG_COND {
717 std::cout << SIMTIME << " Left non-longitudinally (lanechange, teleport, parking, etc) -> discard subscription" << std::endl;
718 }
719 #endif
720 return false;
721 }
722 }
723
724
725 bool
726 MSE2Collector::notifyEnter(SUMOTrafficObject& tObject, MSMoveReminder::Notification reason, const MSLane* enteredLane) {
727 if (!tObject.isVehicle()) {
728 return false;
729 }
730 SUMOVehicle& veh = static_cast<SUMOVehicle&>(tObject);
731 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
732 if DEBUG_COND {
733 std::cout << std::endl << SIMTIME << " notifyEnter() (detID = " << myID << " on lane '" << myLane->getID() << "')"
734 << " called by vehicle '" << veh.getID()
735 << "' entering lane '" << (enteredLane != 0 ? enteredLane->getID() : "NULL") << "'" << std::endl;
736 }
737 #endif
738
739 // notifyEnter() should only be called for lanes of the detector
740 assert(std::find(myLanes.begin(), myLanes.end(), enteredLane->getID()) != myLanes.end());
741
742 assert(veh.getLane() == enteredLane);
743
744 if (!vehicleApplies(veh)) {
745 // That's not my type...
746 return false;
747 }
748
749 // determine whether the vehicle entered the lane behind the detector end
750 // e.g. due to lane change manoeuver
751 if (reason != NOTIFICATION_JUNCTION) {
752 const double vehBackPos = veh.getBackPositionOnLane(enteredLane);
753 bool vehEnteredBehindDetectorEnd = (enteredLane == myLastLane) && myEndPos <= vehBackPos;
754 if (vehEnteredBehindDetectorEnd) {
755 // this vehicle cannot influence detector readings, do not subscribe
756 // to move notifications
757 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
758 if DEBUG_COND {
759 std::cout << "Vehicle entered the lane behind the detector, ignoring it." << std::endl;
760 std::cout << "(myEndPos = " << this->myEndPos << ", veh.getBackPositionOnLane() = " << vehBackPos << ")" << std::endl;
761 }
762 #endif
763 return false;
764 }
765 }
766
767 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
768 if DEBUG_COND {
769 if (!veh.isOnRoad()) {
770 // Vehicle is teleporting over the edge
771 std::cout << "Vehicle is off road (teleporting over edge)..." << std::endl;
772 }
773 }
774 #endif
775
776 const std::string& vehID = veh.getID();
777 VehicleInfoMap::iterator vi = myVehicleInfos.find(vehID);
778 if (vi != myVehicleInfos.end()) {
779 // Register move current offset to the next lane
780 vi->second->currentOffsetIndex++;
781 vi->second->currentLane = enteredLane;
782
783 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
784 if DEBUG_COND {
785 std::cout << SIMTIME << " Vehicle '" << veh.getID() << "' on lane '" << veh.getLane()->getID()
786 << "' already known. No new VehicleInfo is created.\n"
787 << "enteredLane = " << enteredLane->getID() << "\nmyLanes[vi->offset] = " << myLanes[vi->second->currentOffsetIndex]
788 << std::endl;
789 }
790 #endif
791 assert(myLanes[vi->second->currentOffsetIndex] == enteredLane->getID());
792
793 // but don't add a second subscription for another lane
794 return false;
795 }
796
797
798
799 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
800 if DEBUG_COND {
801 std::cout << SIMTIME << " Adding VehicleInfo for vehicle '" << veh.getID() << "'." << std::endl;
802 }
803 #endif
804
805 // Add vehicle info
806 myVehicleInfos.insert(std::make_pair(vehID, makeVehicleInfo(veh, enteredLane)));
807 // Subscribe to vehicle's movement notifications
808 return true;
809 }
810
811 MSE2Collector::VehicleInfo*
812 MSE2Collector::makeVehicleInfo(const SUMOVehicle& veh, const MSLane* enteredLane) const {
813 // The vehicle's distance to the detector end
814 int j = (int)(std::find(myLanes.begin(), myLanes.end(), enteredLane->getID()) - myLanes.begin());
815 assert(j >= 0 && j < (int)myLanes.size());
816 double entryOffset = myOffsets[j];
817 double distToDetectorEnd = myDetectorLength - (entryOffset + veh.getPositionOnLane());
818 bool onDetector = -entryOffset < veh.getPositionOnLane() && distToDetectorEnd > -veh.getVehicleType().getLength();
819
820 #ifdef DEBUG_E2_MAKE_VEHINFO
821 if DEBUG_COND {
822 std::cout << SIMTIME << " Making VehicleInfo for vehicle '" << veh.getID() << "'."
823 << "\ndistToDetectorEnd = " << distToDetectorEnd
824 << "\nveh.getPositionOnLane() = " << veh.getPositionOnLane()
825 << "\nentry lane offset (lane begin from detector begin) = " << entryOffset
826 << std::endl;
827 }
828 #endif
829 return new VehicleInfo(veh.getID(), veh.getVehicleType().getID(), veh.getVehicleType().getLength(), veh.getVehicleType().getMinGap(), enteredLane, entryOffset, j,
830 myOffsets[j] - myDetectorLength, distToDetectorEnd, onDetector);
831 }
832
833 void
834 MSE2Collector::detectorUpdate(const SUMOTime /* step */) {
835
836 #ifdef DEBUG_E2_DETECTOR_UPDATE
837 if DEBUG_COND {
838 std::cout << "\n" << SIMTIME << " detectorUpdate() for detector '" << myID << "'"
839 << "\nmyCurrentMeanSpeed = " << myCurrentMeanSpeed
840 << "\nmyCurrentMeanLength = " << myCurrentMeanLength
841 << "\nmyNumberOfEnteredVehicles = " << myNumberOfEnteredVehicles
842 << "\nmyNumberOfLeftVehicles = " << myNumberOfLeftVehicles
843 << "\nmyNumberOfSeenVehicles = " << myNumberOfSeenVehicles
844 << std::endl;
845 }
846 #endif
847
848 // sort myMoveNotifications (required for jam processing) ascendingly according to vehicle's distance to the detector end
849 // (min = myMoveNotifications[0].distToDetectorEnd)
850 std::sort(myMoveNotifications.begin(), myMoveNotifications.end(), compareMoveNotification);
851
852 // reset values concerning current time step (these are updated in integrateMoveNotification() and aggregateOutputValues())
853 myCurrentMeanSpeed = 0;
854 myCurrentMeanLength = 0;
855 myCurrentStartedHalts = 0;
856 myCurrentHaltingsNumber = 0;
857
858 JamInfo* currentJam = nullptr;
859 std::vector<JamInfo*> jams;
860 std::map<std::string, SUMOTime> haltingVehicles;
861 std::map<std::string, SUMOTime> intervalHaltingVehicles;
862
863 // go through the list of vehicles positioned on the detector
864 for (std::vector<MoveNotificationInfo*>::iterator i = myMoveNotifications.begin(); i != myMoveNotifications.end(); ++i) {
865 // The ID of the vehicle that has sent this notification in the last step
866 const std::string& vehID = (*i)->id;
867 VehicleInfoMap::iterator vi = myVehicleInfos.find(vehID);
868
869 if (vi == myVehicleInfos.end()) {
870 // The vehicle has already left the detector by lanechange, teleport, etc. (not longitudinal)
871 integrateMoveNotification(nullptr, *i);
872 } else {
873 // Add move notification infos to detector values and VehicleInfo
874 integrateMoveNotification(vi->second, *i);
875 }
876 // construct jam structure
877 bool isInJam = checkJam(i, haltingVehicles, intervalHaltingVehicles);
878 buildJam(isInJam, i, currentJam, jams);
879 }
880
881 // extract some aggregated values from the jam structure
882 processJams(jams, currentJam);
883
884 // Aggregate and normalize values for the detector output
885 aggregateOutputValues();
886
887 // save information about halting vehicles
888 myHaltingVehicleDurations = haltingVehicles;
889 myIntervalHaltingVehicleDurations = intervalHaltingVehicles;
890
891 #ifdef DEBUG_E2_DETECTOR_UPDATE
892 if DEBUG_COND {
893 std::cout << "\n" << SIMTIME << " Current lanes for vehicles still on or approaching the detector:" << std::endl;
894 }
895 #endif
896 // update current and entered lanes for remaining vehicles
897 VehicleInfoMap::iterator iv;
898 for (iv = myVehicleInfos.begin(); iv != myVehicleInfos.end(); ++iv) {
899 #ifdef DEBUG_E2_DETECTOR_UPDATE
900 if DEBUG_COND {
901 std::cout << " Vehicle '" << iv->second->id << "'" << ": '"
902 << iv->second->currentLane->getID() << "'"
903 << std::endl;
904 }
905 #endif
906 }
907
908 #ifdef DEBUG_E2_DETECTOR_UPDATE
909 if DEBUG_COND {
910 std::cout << SIMTIME << " Discarding vehicles that have left the detector:" << std::endl;
911 }
912 #endif
913 // Remove the vehicles that have left the detector
914 std::set<std::string>::const_iterator i;
915 for (i = myLeftVehicles.begin(); i != myLeftVehicles.end(); ++i) {
916 VehicleInfoMap::iterator j = myVehicleInfos.find(*i);
917 delete j->second;
918 myVehicleInfos.erase(*i);
919 myNumberOfLeftVehicles++;
920 #ifdef DEBUG_E2_DETECTOR_UPDATE
921 if DEBUG_COND {
922 std::cout << "Erased vehicle '" << *i << "'" << std::endl;
923 }
924 #endif
925 }
926 myLeftVehicles.clear();
927
928 // reset move notifications
929 for (std::vector<MoveNotificationInfo*>::iterator j = myMoveNotifications.begin(); j != myMoveNotifications.end(); ++j) {
930 delete *j;
931 }
932 myMoveNotifications.clear();
933 }
934
935
936 void
937 MSE2Collector::aggregateOutputValues() {
938 myTimeSamples += 1;
939 // compute occupancy values (note myCurrentMeanLength is still not normalized here, but holds the sum of all vehicle lengths)
940 const double currentOccupancy = myCurrentMeanLength / myDetectorLength * (double) 100.;
941 myCurrentOccupancy = currentOccupancy;
942 myOccupancySum += currentOccupancy;
943 myMaxOccupancy = MAX2(myMaxOccupancy, currentOccupancy);
944 // compute jam values
945 myMeanMaxJamInVehicles += myCurrentMaxJamLengthInVehicles;
946 myMeanMaxJamInMeters += myCurrentMaxJamLengthInMeters;
947 myMaxJamInVehicles = MAX2(myMaxJamInVehicles, myCurrentMaxJamLengthInVehicles);
948 myMaxJamInMeters = MAX2(myMaxJamInMeters, myCurrentMaxJamLengthInMeters);
949 // compute information about vehicle numbers
950 const int numVehicles = (int)myMoveNotifications.size();
951 myMeanVehicleNumber += numVehicles;
952 myMaxVehicleNumber = MAX2(numVehicles, myMaxVehicleNumber);
953 // norm current values
954 myCurrentMeanSpeed = numVehicles != 0 ? myCurrentMeanSpeed / (double) numVehicles : -1;
955 myCurrentMeanLength = numVehicles != 0 ? myCurrentMeanLength / (double) numVehicles : -1;
956 }
957
958
959
960 void
961 MSE2Collector::integrateMoveNotification(VehicleInfo* vi, const MoveNotificationInfo* mni) {
962
963 #ifdef DEBUG_E2_DETECTOR_UPDATE
964 if DEBUG_COND {
965 std::cout << SIMTIME << " integrateMoveNotification() for vehicle '" << mni->id << "'"
966 << "\ntimeOnDetector = " << mni->timeOnDetector
967 << "\nlengthOnDetector = " << mni->lengthOnDetector
968 << "\ntimeLoss = " << mni->timeLoss
969 << "\nspeed = " << mni->speed
970 << std::endl;
971 }
972 #endif
973
974 // Accumulate detector values
975 myVehicleSamples += mni->timeOnDetector;
976 myTotalTimeLoss += mni->timeLoss;
977 mySpeedSum += mni->speed * mni->timeOnDetector;
978 myCurrentMeanSpeed += mni->speed * mni->timeOnDetector;
979 myCurrentMeanLength += mni->lengthOnDetector;
980
981 if (vi != nullptr) {
982 // Accumulate individual values for the vehicle.
983 // @note vi==0 occurs, if the vehicle info has been erased at
984 // notifyLeave() in case of a non-longitudinal exit (lanechange, teleport, etc.)
985 vi->totalTimeOnDetector += mni->timeOnDetector;
986 vi->accumulatedTimeLoss += mni->timeLoss;
987 vi->lastAccel = mni->accel;
988 vi->lastSpeed = mni->speed;
989 vi->lastPos = myStartPos + vi->entryOffset + mni->newPos;
990 vi->onDetector = mni->onDetector;
991 }
992 }
993
994
995
996 MSE2Collector::MoveNotificationInfo*
997 MSE2Collector::makeMoveNotification(const SUMOVehicle& veh, double oldPos, double newPos, double newSpeed, const VehicleInfo& vehInfo) const {
998 #ifdef DEBUG_E2_NOTIFY_MOVE
999 if DEBUG_COND {
1000 std::cout << SIMTIME << " makeMoveNotification() for vehicle '" << veh.getID() << "'"
1001 << " oldPos = " << oldPos << " newPos = " << newPos << " newSpeed = " << newSpeed
1002 << std::endl;
1003 }
1004 #endif
1005
1006 // Timefraction in [0,TS] the vehicle has spend on the detector in the last step
1007 double timeOnDetector;
1008 // Note that at this point, vehInfo.currentLane points to the lane at the beginning of the last timestep,
1009 // and vehInfo.enteredLanes is a list of lanes entered in the last timestep
1010 double timeLoss;
1011 calculateTimeLossAndTimeOnDetector(veh, oldPos, newPos, vehInfo, timeOnDetector, timeLoss);
1012
1013 // The length of the part of the vehicle on the detector at the end of the last time step
1014 // may be shorter than vehicle's length if its back reaches out
1015 double lengthOnDetector = MAX2(MIN2(vehInfo.length, newPos + vehInfo.entryOffset), 0.);
1016
1017 // XXX: Fulfulling the specifications of the documentation (lengthOnDetector = time integral
1018 // over length of the vehicle's part on the detector) would be much more cumbersome.
1019 double distToExit = -vehInfo.exitOffset - newPos;
1020 // Eventually decrease further to account for the front reaching out
1021 lengthOnDetector = MAX2(0., lengthOnDetector + MIN2(0., distToExit));
1022
1023 // whether the vehicle is still on the detector at the end of the time step
1024 bool stillOnDetector = -distToExit < vehInfo.length;
1025
1026 #ifdef DEBUG_E2_NOTIFY_MOVE
1027 if DEBUG_COND {
1028 std::cout << SIMTIME << " lengthOnDetector = " << lengthOnDetector
1029 << "\nvehInfo.exitOffset = " << vehInfo.exitOffset
1030 << " vehInfo.entryOffset = " << vehInfo.entryOffset
1031 << " distToExit = " << distToExit
1032 << std::endl;
1033 }
1034 #endif
1035
1036 /* Store new infos */
1037 return new MoveNotificationInfo(veh.getID(), oldPos, newPos, newSpeed, veh.getAcceleration(),
1038 myDetectorLength - (vehInfo.entryOffset + newPos),
1039 timeOnDetector, lengthOnDetector, timeLoss, stillOnDetector);
1040 }
1041
1042 void
1043 MSE2Collector::buildJam(bool isInJam, std::vector<MoveNotificationInfo*>::const_iterator mni, JamInfo*& currentJam, std::vector<JamInfo*>& jams) {
1044 #ifdef DEBUG_E2_JAMS
1045 if DEBUG_COND {
1046 std::cout << SIMTIME << " buildJam() for vehicle '" << (*mni)->id << "'" << std::endl;
1047 }
1048 #endif
1049 if (isInJam) {
1050 // The vehicle is in a jam;
1051 // it may be a new one or already an existing one
1052 if (currentJam == nullptr) {
1053 #ifdef DEBUG_E2_JAMS
1054 if DEBUG_COND {
1055 std::cout << SIMTIME << " vehicle '" << (*mni)->id << "' forms the start of the first jam" << std::endl;
1056 }
1057 #endif
1058 // the vehicle is the first vehicle in a jam
1059 currentJam = new JamInfo();
1060 currentJam->firstStandingVehicle = mni;
1061 } else {
1062 // ok, we have a jam already. But - maybe it is too far away
1063 // ... honestly, I can hardly find a reason for doing this,
1064 // but jams were defined this way in an earlier version...
1065 MoveNotificationInfo* lastVeh = *currentJam->lastStandingVehicle;
1066 MoveNotificationInfo* currVeh = *mni;
1067 if (lastVeh->distToDetectorEnd - currVeh->distToDetectorEnd > myJamDistanceThreshold) {
1068 #ifdef DEBUG_E2_JAMS
1069 if DEBUG_COND {
1070 std::cout << SIMTIME << " vehicle '" << (*mni)->id << "' forms the start of a new jam" << std::endl;
1071 }
1072 #endif
1073 // yep, yep, yep - it's a new one...
1074 // close the frist, build a new
1075 jams.push_back(currentJam);
1076 currentJam = new JamInfo();
1077 currentJam->firstStandingVehicle = mni;
1078 }
1079 }
1080 currentJam->lastStandingVehicle = mni;
1081 } else {
1082 // the vehicle is not part of a jam...
1083 // maybe we have to close an already computed jam
1084 if (currentJam != nullptr) {
1085 #ifdef DEBUG_E2_JAMS
1086 if DEBUG_COND {
1087 std::cout << SIMTIME << " Closing current jam." << std::endl;
1088 }
1089 #endif
1090 jams.push_back(currentJam);
1091 currentJam = nullptr;
1092 }
1093 }
1094 }
1095
1096
1097 bool
1098 MSE2Collector::checkJam(std::vector<MoveNotificationInfo*>::const_iterator mni, std::map<std::string, SUMOTime>& haltingVehicles, std::map<std::string, SUMOTime>& intervalHaltingVehicles) {
1099 #ifdef DEBUG_E2_JAMS
1100 if DEBUG_COND {
1101 std::cout << SIMTIME << " CheckJam() for vehicle '" << (*mni)->id << "'" << std::endl;
1102 }
1103 #endif
1104 // jam-checking begins
1105 bool isInJam = false;
1106 // first, check whether the vehicle is slow enough to be counted as halting
1107 if ((*mni)->speed < myJamHaltingSpeedThreshold) {
1108 myCurrentHaltingsNumber++;
1109 // we have to track the time it was halting;
1110 // so let's look up whether it was halting before and compute the overall halting time
1111 bool wasHalting = myHaltingVehicleDurations.count((*mni)->id) > 0;
1112 if (wasHalting) {
1113 haltingVehicles[(*mni)->id] = myHaltingVehicleDurations[(*mni)->id] + DELTA_T;
1114 intervalHaltingVehicles[(*mni)->id] = myIntervalHaltingVehicleDurations[(*mni)->id] + DELTA_T;
1115 } else {
1116 #ifdef DEBUG_E2_JAMS
1117 if DEBUG_COND {
1118 std::cout << SIMTIME << " vehicle '" << (*mni)->id << "' starts halting." << std::endl;
1119 }
1120 #endif
1121 haltingVehicles[(*mni)->id] = DELTA_T;
1122 intervalHaltingVehicles[(*mni)->id] = DELTA_T;
1123 myCurrentStartedHalts++;
1124 myStartedHalts++;
1125 }
1126 // we now check whether the halting time is large enough
1127 if (haltingVehicles[(*mni)->id] > myJamHaltingTimeThreshold) {
1128 // yep --> the vehicle is a part of a jam
1129 isInJam = true;
1130 }
1131 } else {
1132 // is not standing anymore; keep duration information
1133 std::map<std::string, SUMOTime>::iterator v = myHaltingVehicleDurations.find((*mni)->id);
1134 if (v != myHaltingVehicleDurations.end()) {
1135 myPastStandingDurations.push_back(v->second);
1136 myHaltingVehicleDurations.erase(v);
1137 }
1138 v = myIntervalHaltingVehicleDurations.find((*mni)->id);
1139 if (v != myIntervalHaltingVehicleDurations.end()) {
1140 myPastIntervalStandingDurations.push_back((*v).second);
1141 myIntervalHaltingVehicleDurations.erase(v);
1142 }
1143 }
1144 #ifdef DEBUG_E2_JAMS
1145 if DEBUG_COND {
1146 std::cout << SIMTIME << " vehicle '" << (*mni)->id << "'" << (isInJam ? "is jammed." : "is not jammed.") << std::endl;
1147 }
1148 #endif
1149 return isInJam;
1150 }
1151
1152
1153 void
1154 MSE2Collector::processJams(std::vector<JamInfo*>& jams, JamInfo* currentJam) {
1155 // push last jam
1156 if (currentJam != nullptr) {
1157 jams.push_back(currentJam);
1158 currentJam = nullptr;
1159 }
1160
1161 #ifdef DEBUG_E2_JAMS
1162 if DEBUG_COND {
1163 std::cout << "\n" << SIMTIME << " processJams()"
1164 << "\nNumber of jams: " << jams.size() << std::endl;
1165 }
1166 #endif
1167
1168 // process jam information
1169 myCurrentMaxJamLengthInMeters = 0;
1170 myCurrentMaxJamLengthInVehicles = 0;
1171 myCurrentJamLengthInMeters = 0;
1172 myCurrentJamLengthInVehicles = 0;
1173 for (std::vector<JamInfo*>::const_iterator i = jams.begin(); i != jams.end(); ++i) {
1174 // compute current jam's values
1175 MoveNotificationInfo* lastVeh = *((*i)->lastStandingVehicle);
1176 MoveNotificationInfo* firstVeh = *((*i)->firstStandingVehicle);
1177 const double jamLengthInMeters = lastVeh->distToDetectorEnd
1178 - firstVeh->distToDetectorEnd
1179 + lastVeh->lengthOnDetector;
1180 const int jamLengthInVehicles = (int) distance((*i)->firstStandingVehicle, (*i)->lastStandingVehicle) + 1;
1181 // apply them to the statistics
1182 myCurrentMaxJamLengthInMeters = MAX2(myCurrentMaxJamLengthInMeters, jamLengthInMeters);
1183 myCurrentMaxJamLengthInVehicles = MAX2(myCurrentMaxJamLengthInVehicles, jamLengthInVehicles);
1184 myJamLengthInMetersSum += jamLengthInMeters;
1185 myJamLengthInVehiclesSum += jamLengthInVehicles;
1186 myCurrentJamLengthInMeters += jamLengthInMeters;
1187 myCurrentJamLengthInVehicles += jamLengthInVehicles;
1188 #ifdef DEBUG_E2_JAMS
1189 if DEBUG_COND {
1190 std::cout << SIMTIME << " processing jam nr." << ((int) distance((std::vector<JamInfo*>::const_iterator) jams.begin(), i) + 1)
1191 << "\njamLengthInMeters = " << jamLengthInMeters
1192 << " jamLengthInVehicles = " << jamLengthInVehicles
1193 << std::endl;
1194 }
1195 #endif
1196 }
1197 myCurrentJamNo = (int) jams.size();
1198
1199 // clean up jam structure
1200 for (std::vector<JamInfo*>::iterator i = jams.begin(); i != jams.end(); ++i) {
1201 delete *i;
1202 }
1203 }
1204
1205 void
1206 MSE2Collector::calculateTimeLossAndTimeOnDetector(const SUMOVehicle& veh, double oldPos, double newPos, const VehicleInfo& vi, double& timeOnDetector, double& timeLoss) const {
1207 assert(veh.getID() == vi.id);
1208 assert(newPos + vi.entryOffset >= 0);
1209
1210 if (oldPos == newPos) {
1211 // vehicle is stopped
1212 timeLoss = TS;
1213 timeOnDetector = TS;
1214 return;
1215 }
1216
1217 // Eventual positional offset of the detector start from the lane's start
1218 double entryPos = MAX2(-vi.entryOffset, 0.);
1219 // Time of this vehicle entering the detector in the last time step
1220 double entryTime = 0;
1221 // Take into account the time before entering the detector, if there is.
1222 if (oldPos < entryPos) {
1223 // Vehicle entered the detector in the last step, either traversing the detector start or somewhere in the middle.
1224 entryTime = MSCFModel::passingTime(oldPos, entryPos, newPos, veh.getPreviousSpeed(), veh.getSpeed());
1225 }
1226 // speed at detector entry
1227 double entrySpeed = MSCFModel::speedAfterTime(entryTime, veh.getPreviousSpeed(), newPos - oldPos);
1228 // Calculate time spent on detector until reaching newPos or a detector exit
1229 double exitPos = MIN2(newPos, -vi.exitOffset + vi.length);
1230 assert(entryPos < exitPos);
1231
1232 // calculate vehicle's time spent on the detector
1233 double exitTime;
1234 if (exitPos == newPos) {
1235 exitTime = TS;
1236 } else {
1237 exitTime = MSCFModel::passingTime(oldPos, exitPos, newPos, veh.getPreviousSpeed(), veh.getSpeed());
1238 }
1239
1240 // Vehicle's Speed when leaving the detector
1241 double exitSpeed = MSCFModel::speedAfterTime(exitTime, veh.getPreviousSpeed(), newPos - oldPos);
1242
1243 // Maximal speed on vehicle's current lane (== lane before last time step)
1244 // Note: this disregards the possibility of different maximal speeds on different traversed lanes.
1245 // (we accept this as discretization error)
1246 double vmax = MAX2(veh.getLane()->getVehicleMaxSpeed(&veh), NUMERICAL_EPS);
1247
1248 // Time loss suffered on the detector
1249 timeOnDetector = exitTime - entryTime;
1250 timeLoss = MAX2(0., timeOnDetector * (vmax - (entrySpeed + exitSpeed) / 2) / vmax);
1251
1252 #ifdef DEBUG_E2_TIME_ON_DETECTOR
1253 if DEBUG_COND {
1254 std::cout << SIMTIME << " calculateTimeLoss() for vehicle '" << veh.getID() << "'"
1255 << " oldPos = " << oldPos << " newPos = " << newPos
1256 << " entryPos = " << entryPos << " exitPos = " << exitPos
1257 << " timeOnDetector = " << timeOnDetector
1258 << " timeLoss = " << timeLoss
1259 << std::endl;
1260 }
1261 #endif
1262 }
1263
1264
1265 void
1266 MSE2Collector::writeXMLDetectorProlog(OutputDevice& dev) const {
1267 dev.writeXMLHeader("detector", "det_e2_file.xsd");
1268 }
1269
1270 void
1271 MSE2Collector::writeXMLOutput(OutputDevice& dev, SUMOTime startTime, SUMOTime stopTime) {
1272 dev << " <interval begin=\"" << time2string(startTime) << "\" end=\"" << time2string(stopTime) << "\" " << "id=\"" << getID() << "\" ";
1273
1274 const double meanSpeed = myVehicleSamples != 0 ? mySpeedSum / myVehicleSamples : -1;
1275 const double meanOccupancy = myTimeSamples != 0 ? myOccupancySum / (double) myTimeSamples : 0;
1276 const double meanJamLengthInMeters = myTimeSamples != 0 ? myMeanMaxJamInMeters / (double) myTimeSamples : 0;
1277 const double meanJamLengthInVehicles = myTimeSamples != 0 ? myMeanMaxJamInVehicles / (double) myTimeSamples : 0;
1278 const double meanVehicleNumber = myTimeSamples != 0 ? (double) myMeanVehicleNumber / (double) myTimeSamples : 0;
1279 const double meanTimeLoss = myNumberOfSeenVehicles != 0 ? myTotalTimeLoss / myNumberOfSeenVehicles : -1;
1280
1281 SUMOTime haltingDurationSum = 0;
1282 SUMOTime maxHaltingDuration = 0;
1283 int haltingNo = 0;
1284 for (std::vector<SUMOTime>::iterator i = myPastStandingDurations.begin(); i != myPastStandingDurations.end(); ++i) {
1285 haltingDurationSum += (*i);
1286 maxHaltingDuration = MAX2(maxHaltingDuration, (*i));
1287 haltingNo++;
1288 }
1289 for (std::map<std::string, SUMOTime> ::iterator i = myHaltingVehicleDurations.begin(); i != myHaltingVehicleDurations.end(); ++i) {
1290 haltingDurationSum += (*i).second;
1291 maxHaltingDuration = MAX2(maxHaltingDuration, (*i).second);
1292 haltingNo++;
1293 }
1294 const SUMOTime meanHaltingDuration = haltingNo != 0 ? haltingDurationSum / haltingNo : 0;
1295
1296 SUMOTime intervalHaltingDurationSum = 0;
1297 SUMOTime intervalMaxHaltingDuration = 0;
1298 int intervalHaltingNo = 0;
1299 for (std::vector<SUMOTime>::iterator i = myPastIntervalStandingDurations.begin(); i != myPastIntervalStandingDurations.end(); ++i) {
1300 intervalHaltingDurationSum += (*i);
1301 intervalMaxHaltingDuration = MAX2(intervalMaxHaltingDuration, (*i));
1302 intervalHaltingNo++;
1303 }
1304 for (std::map<std::string, SUMOTime> ::iterator i = myIntervalHaltingVehicleDurations.begin(); i != myIntervalHaltingVehicleDurations.end(); ++i) {
1305 intervalHaltingDurationSum += (*i).second;
1306 intervalMaxHaltingDuration = MAX2(intervalMaxHaltingDuration, (*i).second);
1307 intervalHaltingNo++;
1308 }
1309 const SUMOTime intervalMeanHaltingDuration = intervalHaltingNo != 0 ? intervalHaltingDurationSum / intervalHaltingNo : 0;
1310
1311 #ifdef DEBUG_E2_XML_OUT
1312 if DEBUG_COND {
1313 std::stringstream ss;
1314 ss << "sampledSeconds=\"" << myVehicleSamples << "\" "
1315 << "myTimeSamples=\"" << myTimeSamples << "\" "
1316 << "myOccupancySum=\"" << myOccupancySum << "\" "
1317 << "myMeanVehicleNumber=\"" << myMeanVehicleNumber << "\" "
1318 << "nVehEntered=\"" << myNumberOfEnteredVehicles << "\" "
1319 << "meanSpeed=\"" << meanSpeed << "\"";
1320 std::cout << ss.str() << std::endl;
1321 }
1322 #endif
1323
1324
1325 dev << "sampledSeconds=\"" << myVehicleSamples << "\" "
1326 << "nVehEntered=\"" << myNumberOfEnteredVehicles << "\" "
1327 << "nVehLeft=\"" << myNumberOfLeftVehicles << "\" "
1328 << "nVehSeen=\"" << myNumberOfSeenVehicles << "\" "
1329 << "meanSpeed=\"" << meanSpeed << "\" "
1330 << "meanTimeLoss=\"" << meanTimeLoss << "\" "
1331 << "meanOccupancy=\"" << meanOccupancy << "\" "
1332 << "maxOccupancy=\"" << myMaxOccupancy << "\" "
1333 << "meanMaxJamLengthInVehicles=\"" << meanJamLengthInVehicles << "\" "
1334 << "meanMaxJamLengthInMeters=\"" << meanJamLengthInMeters << "\" "
1335 << "maxJamLengthInVehicles=\"" << myMaxJamInVehicles << "\" "
1336 << "maxJamLengthInMeters=\"" << myMaxJamInMeters << "\" "
1337 << "jamLengthInVehiclesSum=\"" << myJamLengthInVehiclesSum << "\" "
1338 << "jamLengthInMetersSum=\"" << myJamLengthInMetersSum << "\" "
1339 << "meanHaltingDuration=\"" << STEPS2TIME(meanHaltingDuration) << "\" "
1340 << "maxHaltingDuration=\"" << STEPS2TIME(maxHaltingDuration) << "\" "
1341 << "haltingDurationSum=\"" << STEPS2TIME(haltingDurationSum) << "\" "
1342 << "meanIntervalHaltingDuration=\"" << STEPS2TIME(intervalMeanHaltingDuration) << "\" "
1343 << "maxIntervalHaltingDuration=\"" << STEPS2TIME(intervalMaxHaltingDuration) << "\" "
1344 << "intervalHaltingDurationSum=\"" << STEPS2TIME(intervalHaltingDurationSum) << "\" "
1345 << "startedHalts=\"" << myStartedHalts << "\" "
1346 << "meanVehicleNumber=\"" << meanVehicleNumber << "\" "
1347 << "maxVehicleNumber=\"" << myMaxVehicleNumber << "\" "
1348 << "/>\n";
1349 reset();
1350
1351 }
1352
1353 void
1354 MSE2Collector::reset() {
1355 myVehicleSamples = 0;
1356 myTotalTimeLoss = 0.;
1357 myNumberOfEnteredVehicles = 0;
1358 myNumberOfSeenVehicles -= myNumberOfLeftVehicles;
1359 myNumberOfLeftVehicles = 0;
1360 myMaxVehicleNumber = 0;
1361
1362 mySpeedSum = 0;
1363 myStartedHalts = 0;
1364 myJamLengthInMetersSum = 0;
1365 myJamLengthInVehiclesSum = 0;
1366 myOccupancySum = 0;
1367 myMaxOccupancy = 0;
1368 myMeanMaxJamInVehicles = 0;
1369 myMeanMaxJamInMeters = 0;
1370 myMaxJamInVehicles = 0;
1371 myMaxJamInMeters = 0;
1372 myTimeSamples = 0;
1373 myMeanVehicleNumber = 0;
1374 for (std::map<std::string, SUMOTime>::iterator i = myIntervalHaltingVehicleDurations.begin(); i != myIntervalHaltingVehicleDurations.end(); ++i) {
1375 (*i).second = 0;
1376 }
1377 myPastStandingDurations.clear();
1378 myPastIntervalStandingDurations.clear();
1379 }
1380
1381
1382 int
1383 MSE2Collector::getCurrentVehicleNumber() const {
1384 int result = 0;
1385 for (VehicleInfoMap::const_iterator it = myVehicleInfos.begin(); it != myVehicleInfos.end(); it++) {
1386 if (it->second->onDetector) {
1387 result++;
1388 }
1389 }
1390 return result;
1391 }
1392
1393
1394
1395 std::vector<std::string>
1396 MSE2Collector::getCurrentVehicleIDs() const {
1397 std::vector<std::string> ret;
1398 for (VehicleInfoMap::const_iterator i = myVehicleInfos.begin(); i != myVehicleInfos.end(); ++i) {
1399 if (i->second->onDetector) {
1400 ret.push_back(i->second->id);
1401 }
1402 }
1403 std::sort(ret.begin(), ret.end());
1404 return ret;
1405 }
1406
1407
1408 std::vector<MSE2Collector::VehicleInfo*>
1409 MSE2Collector::getCurrentVehicles() const {
1410 std::vector<VehicleInfo*> res;
1411 VehicleInfoMap::const_iterator i;
1412 for (i = myVehicleInfos.begin(); i != myVehicleInfos.end(); ++i) {
1413 if (i->second->onDetector) {
1414 res.push_back(i->second);
1415 }
1416 }
1417 return res;
1418 }
1419
1420
1421
1422 int
1423 MSE2Collector::getEstimatedCurrentVehicleNumber(double speedThreshold) const {
1424
1425 // double distance = std::numeric_limits<double>::max();
1426 double thresholdSpeed = myLane->getSpeedLimit() / speedThreshold;
1427
1428 int count = 0;
1429 for (VehicleInfoMap::const_iterator it = myVehicleInfos.begin();
1430 it != myVehicleInfos.end(); it++) {
1431 if (it->second->onDetector) {
1432 // if (it->position < distance) {
1433 // distance = it->position;
1434 // }
1435 // const double realDistance = myLane->getLength() - distance; // the closer vehicle get to the light the greater is the distance
1436 const double realDistance = it->second->distToDetectorEnd;
1437 if (it->second->lastSpeed <= thresholdSpeed || it->second->lastAccel > 0) { //TODO speed less half of the maximum speed for the lane NEED TUNING
1438 count = (int)(realDistance / (it->second->length + it->second->minGap)) + 1;
1439 }
1440 }
1441 }
1442
1443 return count;
1444 }
1445
1446 double
1447 MSE2Collector::getEstimateQueueLength() const {
1448
1449 if (myVehicleInfos.empty()) {
1450 return -1;
1451 }
1452
1453 double distance = std::numeric_limits<double>::max();
1454 double realDistance = 0;
1455 bool flowing = true;
1456 for (VehicleInfoMap::const_iterator it = myVehicleInfos.begin();
1457 it != myVehicleInfos.end(); it++) {
1458 if (it->second->onDetector) {
1459 distance = MIN2(it->second->lastPos, distance);
1460 // double distanceTemp = myLane->getLength() - distance;
1461 if (it->second->lastSpeed <= 0.5) {
1462 realDistance = distance - it->second->length + it->second->minGap;
1463 flowing = false;
1464 }
1465 // DBG(
1466 // std::ostringstream str;
1467 // str << time2string(MSNet::getInstance()->getCurrentTimeStep())
1468 // << " MSE2Collector::getEstimateQueueLength::"
1469 // << " lane " << myLane->getID()
1470 // << " vehicle " << it->second.id
1471 // << " positionOnLane " << it->second.position
1472 // << " vel " << it->second.speed
1473 // << " realDistance " << realDistance;
1474 // WRITE_MESSAGE(str.str());
1475 // )
1476 }
1477 }
1478 if (flowing) {
1479 return 0;
1480 } else {
1481 return myLane->getLength() - realDistance;
1482 }
1483 }
1484
1485 /****************************************************************************/
1486
1487