Eclipse SUMO - Simulation of Urban MObility
Helper.cpp
Go to the documentation of this file.
1/****************************************************************************/
2// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3// Copyright (C) 2017-2022 German Aerospace Center (DLR) and others.
4// This program and the accompanying materials are made available under the
5// terms of the Eclipse Public License 2.0 which is available at
6// https://www.eclipse.org/legal/epl-2.0/
7// This Source Code may also be made available under the following Secondary
8// Licenses when the conditions for such availability set forth in the Eclipse
9// Public License 2.0 are satisfied: GNU General Public License, version 2
10// or later which is available at
11// https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12// SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13/****************************************************************************/
20// C++ TraCI client API implementation
21/****************************************************************************/
22#include <config.h>
23
24#include <cstring>
27#include <microsim/MSNet.h>
31#include <microsim/MSEdge.h>
32#include <microsim/MSLane.h>
33#include <microsim/MSLink.h>
35#include <microsim/MSVehicle.h>
42#include <libsumo/TraCIDefs.h>
43#include <libsumo/BusStop.h>
44#include <libsumo/Calibrator.h>
46#include <libsumo/Edge.h>
47#ifdef HAVE_LIBSUMOGUI
48#include <libsumo/GUI.h>
49#endif
51#include <libsumo/Junction.h>
52#include <libsumo/Lane.h>
53#include <libsumo/LaneArea.h>
54#include <libsumo/MeanData.h>
57#include <libsumo/ParkingArea.h>
58#include <libsumo/Person.h>
59#include <libsumo/POI.h>
60#include <libsumo/Polygon.h>
61#include <libsumo/Rerouter.h>
62#include <libsumo/Route.h>
63#include <libsumo/RouteProbe.h>
64#include <libsumo/Simulation.h>
67#include <libsumo/Vehicle.h>
68#include <libsumo/VehicleType.h>
70#include "Helper.h"
71
72#define FAR_AWAY 1000.0
73
74//#define DEBUG_MOVEXY
75//#define DEBUG_MOVEXY_ANGLE
76//#define DEBUG_SURROUNDING
77
78namespace libsumo {
79// ===========================================================================
80// static member initializations
81// ===========================================================================
82std::vector<Subscription> Helper::mySubscriptions;
83Subscription* Helper::myLastContextSubscription = nullptr;
84std::map<int, std::shared_ptr<VariableWrapper> > Helper::myWrapper;
85Helper::VehicleStateListener Helper::myVehicleStateListener;
86Helper::TransportableStateListener Helper::myTransportableStateListener;
88std::map<std::string, MSVehicle*> Helper::myRemoteControlledVehicles;
89std::map<std::string, MSPerson*> Helper::myRemoteControlledPersons;
90
91
92// ===========================================================================
93// static member definitions
94// ===========================================================================
95
96void
98 if (veh != nullptr) {
99 if (veh->isVehicle()) {
100 std::cout << " '" << veh->getID() << "' on lane '" << ((SUMOVehicle*)veh)->getLane()->getID() << "'\n";
101 } else {
102 std::cout << " '" << veh->getID() << "' on edge '" << veh->getEdge()->getID() << "'\n";
103 }
104 }
105}
106
107
108void
109Helper::subscribe(const int commandId, const std::string& id, const std::vector<int>& variables,
110 const double beginTime, const double endTime, const libsumo::TraCIResults& params,
111 const int contextDomain, const double range) {
113 if (variables.empty()) {
114 for (std::vector<libsumo::Subscription>::iterator j = mySubscriptions.begin(); j != mySubscriptions.end();) {
115 if (j->id == id && j->commandId == commandId && j->contextDomain == contextDomain) {
116 j = mySubscriptions.erase(j);
117 } else {
118 ++j;
119 }
120 }
121 return;
122 }
123 std::vector<std::shared_ptr<tcpip::Storage> > parameters;
124 for (const int var : variables) {
125 const auto& p = params.find(var);
126 if (p == params.end()) {
127 parameters.push_back(std::make_shared<tcpip::Storage>());
128 } else {
129 parameters.push_back(libsumo::StorageHelper::toStorage(*p->second));
130 }
131 }
132 const SUMOTime begin = beginTime == INVALID_DOUBLE_VALUE ? 0 : TIME2STEPS(beginTime);
133 const SUMOTime end = endTime == INVALID_DOUBLE_VALUE || endTime > STEPS2TIME(SUMOTime_MAX) ? SUMOTime_MAX : TIME2STEPS(endTime);
134 libsumo::Subscription s(commandId, id, variables, parameters, begin, end, contextDomain, range);
135 if (s.variables.size() == 1 && s.variables.front() == -1) {
136 s.variables.clear();
137 }
139 libsumo::Subscription* modifiedSubscription = nullptr;
140 needNewSubscription(s, mySubscriptions, modifiedSubscription);
141 if (modifiedSubscription->isVehicleToVehicleContextSubscription()
142 || modifiedSubscription->isVehicleToPersonContextSubscription()) {
143 // Set last modified vehicle context subscription active for filter modifications
144 myLastContextSubscription = modifiedSubscription;
145 }
146}
147
148
149void
151 for (auto& wrapper : myWrapper) {
152 wrapper.second->clear();
153 }
154 for (std::vector<libsumo::Subscription>::iterator i = mySubscriptions.begin(); i != mySubscriptions.end();) {
155 const libsumo::Subscription& s = *i;
156 const bool isArrivedVehicle = (s.commandId == CMD_SUBSCRIBE_VEHICLE_VARIABLE || s.commandId == CMD_SUBSCRIBE_VEHICLE_CONTEXT)
159 && MSNet::getInstance()->getPersonControl().get(s.id) == nullptr;
160 if (s.endTime < t || isArrivedVehicle || isArrivedPerson) {
161 i = mySubscriptions.erase(i);
162 continue;
163 }
164 ++i;
165 }
166 for (const libsumo::Subscription& s : mySubscriptions) {
167 if (s.beginTime <= t) {
169 }
170 }
171}
172
173
174bool
175Helper::needNewSubscription(libsumo::Subscription& s, std::vector<Subscription>& subscriptions, libsumo::Subscription*& modifiedSubscription) {
176 for (libsumo::Subscription& o : subscriptions) {
177 if (s.commandId == o.commandId && s.id == o.id &&
178 s.beginTime == o.beginTime && s.endTime == o.endTime &&
179 s.contextDomain == o.contextDomain && s.range == o.range) {
180 std::vector<std::shared_ptr<tcpip::Storage> >::const_iterator k = s.parameters.begin();
181 for (const int v : s.variables) {
182 const int offset = (int)(std::find(o.variables.begin(), o.variables.end(), v) - o.variables.begin());
183 if (offset == (int)o.variables.size() || o.parameters[offset]->size() != (*k)->size() || !std::equal((*k)->begin(), (*k)->end(), o.parameters[offset]->begin())) {
184 o.variables.push_back(v);
185 o.parameters.push_back(*k);
186 }
187 ++k;
188 }
189 modifiedSubscription = &o;
190 return false;
191 }
192 }
193 subscriptions.push_back(s);
194 modifiedSubscription = &subscriptions.back();
195 return true;
196}
197
198
199void
201 mySubscriptions.clear();
203}
204
205
208 if (myLastContextSubscription != nullptr) {
210 } else {
211 // The following code relies on the fact that the filter is 2^(filterType-1),
212 // see Subscription.h and the corresponding TraCIConstants.h.
213 // It is only for getting similar error messages with libsumo and traci.
214 int index = (int)filter;
215 int filterType = 0;
216 if (index != 0) {
217 ++filterType;
218 while (index >>= 1) {
219 ++filterType;
220 }
221 }
222 throw TraCIException("No previous vehicle context subscription exists to apply filter type " + toHex(filterType, 2));
223 }
225}
226
227
228void
230 const int getCommandId = s.contextDomain > 0 ? s.contextDomain : s.commandId - 0x30;
231 std::set<std::string> objIDs;
232 if (s.contextDomain > 0) {
233 if ((s.activeFilters & SUBS_FILTER_NO_RTREE) == 0) {
234 PositionVector shape;
235 findObjectShape(s.commandId, s.id, shape);
236 collectObjectIDsInRange(s.contextDomain, shape, s.range, objIDs);
237 }
238 applySubscriptionFilters(s, objIDs);
239 } else {
240 objIDs.insert(s.id);
241 }
242 if (myWrapper.empty()) {
243 myWrapper[libsumo::CMD_GET_BUSSTOP_VARIABLE] = BusStop::makeWrapper();
244 myWrapper[libsumo::CMD_GET_CALIBRATOR_VARIABLE] = Calibrator::makeWrapper();
245 myWrapper[libsumo::CMD_GET_CHARGINGSTATION_VARIABLE] = ChargingStation::makeWrapper();
246 myWrapper[libsumo::CMD_GET_EDGE_VARIABLE] = Edge::makeWrapper();
247#ifdef HAVE_LIBSUMOGUI
248 myWrapper[libsumo::CMD_GET_GUI_VARIABLE] = GUI::makeWrapper();
249#endif
250 myWrapper[libsumo::CMD_GET_INDUCTIONLOOP_VARIABLE] = InductionLoop::makeWrapper();
251 myWrapper[libsumo::CMD_GET_JUNCTION_VARIABLE] = Junction::makeWrapper();
252 myWrapper[libsumo::CMD_GET_LANE_VARIABLE] = Lane::makeWrapper();
253 myWrapper[libsumo::CMD_GET_LANEAREA_VARIABLE] = LaneArea::makeWrapper();
254 myWrapper[libsumo::CMD_GET_MEANDATA_VARIABLE] = MeanData::makeWrapper();
255 myWrapper[libsumo::CMD_GET_MULTIENTRYEXIT_VARIABLE] = MultiEntryExit::makeWrapper();
256 myWrapper[libsumo::CMD_GET_OVERHEADWIRE_VARIABLE] = OverheadWire::makeWrapper();
257 myWrapper[libsumo::CMD_GET_PARKINGAREA_VARIABLE] = ParkingArea::makeWrapper();
258 myWrapper[libsumo::CMD_GET_PERSON_VARIABLE] = Person::makeWrapper();
259 myWrapper[libsumo::CMD_GET_POI_VARIABLE] = POI::makeWrapper();
260 myWrapper[libsumo::CMD_GET_POLYGON_VARIABLE] = Polygon::makeWrapper();
261 myWrapper[libsumo::CMD_GET_REROUTER_VARIABLE] = Rerouter::makeWrapper();
262 myWrapper[libsumo::CMD_GET_ROUTE_VARIABLE] = Route::makeWrapper();
263 myWrapper[libsumo::CMD_GET_ROUTEPROBE_VARIABLE] = RouteProbe::makeWrapper();
264 myWrapper[libsumo::CMD_GET_SIM_VARIABLE] = Simulation::makeWrapper();
265 myWrapper[libsumo::CMD_GET_TL_VARIABLE] = TrafficLight::makeWrapper();
266 myWrapper[libsumo::CMD_GET_VARIABLESPEEDSIGN_VARIABLE] = VariableSpeedSign::makeWrapper();
267 myWrapper[libsumo::CMD_GET_VEHICLE_VARIABLE] = Vehicle::makeWrapper();
268 myWrapper[libsumo::CMD_GET_VEHICLETYPE_VARIABLE] = VehicleType::makeWrapper();
269 }
270 auto wrapper = myWrapper.find(getCommandId);
271 if (wrapper == myWrapper.end()) {
272 throw TraCIException("Unsupported command specified");
273 }
274 std::shared_ptr<VariableWrapper> handler = wrapper->second;
275 VariableWrapper* container = handler.get();
276 if (s.contextDomain > 0) {
277 auto containerWrapper = myWrapper.find(s.commandId + 0x20);
278 if (containerWrapper == myWrapper.end()) {
279 throw TraCIException("Unsupported domain specified");
280 }
281 container = containerWrapper->second.get();
282 container->setContext(&s.id);
283 } else {
284 container->setContext(nullptr);
285 }
286 for (const std::string& objID : objIDs) {
287 if (!s.variables.empty()) {
288 std::vector<std::shared_ptr<tcpip::Storage> >::const_iterator k = s.parameters.begin();
289 for (const int variable : s.variables) {
290 if (s.contextDomain > 0 && variable == libsumo::TRACI_ID_LIST) {
291 container->empty(objID);
292 } else {
293 (*k)->resetPos();
294 handler->handle(objID, variable, container, k->get());
295 ++k;
296 }
297 }
298 } else {
299 if (s.contextDomain == 0 && getCommandId == libsumo::CMD_GET_VEHICLE_VARIABLE) {
300 // default for vehicles is edge id and lane position
301 handler->handle(objID, VAR_ROAD_ID, container, nullptr);
302 handler->handle(objID, VAR_LANEPOSITION, container, nullptr);
303 } else if (s.contextDomain > 0) {
304 // default for contexts is an empty map (similar to id list)
305 container->empty(objID);
306 } else if (!handler->handle(objID, libsumo::LAST_STEP_VEHICLE_NUMBER, container, nullptr)) {
307 // default for detectors is vehicle number, for all others id list
308 handler->handle(objID, libsumo::TRACI_ID_LIST, container, nullptr);
309 }
310 }
311 }
312}
313
314
315void
316Helper::fuseLaneCoverage(std::shared_ptr<LaneCoverageInfo> aggregatedLaneCoverage, const std::shared_ptr<LaneCoverageInfo> newLaneCoverage) {
317 for (auto& p : *newLaneCoverage) {
318 const MSLane* lane = p.first;
319 if (aggregatedLaneCoverage->find(lane) == aggregatedLaneCoverage->end()) {
320 // Lane has no coverage in aggregatedLaneCoverage, yet
321 (*aggregatedLaneCoverage)[lane] = (*newLaneCoverage)[lane];
322 } else {
323 // Lane is covered in aggregatedLaneCoverage as well
324 std::pair<double, double>& range1 = (*aggregatedLaneCoverage)[lane];
325 std::pair<double, double>& range2 = (*newLaneCoverage)[lane];
326 std::pair<double, double> hull = std::make_pair(MIN2(range1.first, range2.first), MAX2(range1.second, range2.second));
327 (*aggregatedLaneCoverage)[lane] = hull;
328 }
329 }
330}
331
332
336 for (int i = 0; i < (int)positionVector.size(); ++i) {
337 tp.value.push_back(makeTraCIPosition(positionVector[i]));
338 }
339 return tp;
340}
341
342
346 for (const TraCIPosition& pos : vector.value) {
347 if (std::isnan(pos.x) || std::isnan(pos.y)) {
348 throw libsumo::TraCIException("NaN-Value in shape.");
349 }
350 pv.push_back(Position(pos.x, pos.y));
351 }
352 return pv;
353}
354
355
358 TraCIColor tc;
359 tc.a = color.alpha();
360 tc.b = color.blue();
361 tc.g = color.green();
362 tc.r = color.red();
363 return tc;
364}
365
366
369 return RGBColor((unsigned char)c.r, (unsigned char)c.g, (unsigned char)c.b, (unsigned char)c.a);
370}
371
372
374Helper::makeTraCIPosition(const Position& position, const bool includeZ) {
376 p.x = position.x();
377 p.y = position.y();
378 p.z = includeZ ? position.z() : INVALID_DOUBLE_VALUE;
379 return p;
380}
381
382
385 return Position(tpos.x, tpos.y, tpos.z);
386}
387
388
389MSEdge*
390Helper::getEdge(const std::string& edgeID) {
391 MSEdge* edge = MSEdge::dictionary(edgeID);
392 if (edge == nullptr) {
393 throw TraCIException("Referenced edge '" + edgeID + "' is not known.");
394 }
395 return edge;
396}
397
398
399const MSLane*
400Helper::getLaneChecking(const std::string& edgeID, int laneIndex, double pos) {
401 const MSEdge* edge = MSEdge::dictionary(edgeID);
402 if (edge == nullptr) {
403 throw TraCIException("Unknown edge " + edgeID);
404 }
405 if (laneIndex < 0 || laneIndex >= (int)edge->getLanes().size()) {
406 throw TraCIException("Invalid lane index for " + edgeID);
407 }
408 const MSLane* lane = edge->getLanes()[laneIndex];
409 if (pos < 0 || pos > lane->getLength()) {
410 throw TraCIException("Position on lane invalid");
411 }
412 return lane;
413}
414
415
416std::pair<MSLane*, double>
418 const PositionVector shape({ pos });
419 std::pair<MSLane*, double> result(nullptr, -1);
420 double range = 1000.;
421 const Boundary& netBounds = GeoConvHelper::getFinal().getConvBoundary();
422 const double maxRange = MAX2(1001., netBounds.getWidth() + netBounds.getHeight() + netBounds.distanceTo2D(pos));
423 while (range < maxRange) {
424 std::set<const Named*> lanes;
426 double minDistance = std::numeric_limits<double>::max();
427 for (const Named* named : lanes) {
428 MSLane* lane = const_cast<MSLane*>(dynamic_cast<const MSLane*>(named));
429 if (lane->allowsVehicleClass(vClass)) {
430 // @todo this may be a place where 3D is required but 2D is used
431 const double newDistance = lane->getShape().distance2D(pos);
432 if (newDistance < minDistance ||
433 (newDistance == minDistance
434 && result.first != nullptr
435 && lane->getID() < result.first->getID())) {
436 minDistance = newDistance;
437 result.first = lane;
438 }
439 }
440 }
441 if (minDistance < std::numeric_limits<double>::max()) {
442 result.second = result.first->interpolateGeometryPosToLanePos(result.first->getShape().nearest_offset_to_point2D(pos, false));
443 break;
444 }
445 range *= 2;
446 }
447 return result;
448}
449
450
451double
452Helper::getDrivingDistance(std::pair<const MSLane*, double>& roadPos1, std::pair<const MSLane*, double>& roadPos2) {
453 if (roadPos1.first == roadPos2.first && roadPos1.second <= roadPos2.second) {
454 // same edge
455 return roadPos2.second - roadPos1.second;
456 }
457 double distance = 0.0;
458 ConstMSEdgeVector newRoute;
459 while (roadPos2.first->isInternal() && roadPos2.first != roadPos1.first) {
460 distance += roadPos2.second;
461 roadPos2.first = roadPos2.first->getLogicalPredecessorLane();
462 roadPos2.second = roadPos2.first->getLength();
463 }
464 MSNet::getInstance()->getRouterTT(0).compute(&roadPos1.first->getEdge(), &roadPos2.first->getEdge(), nullptr, SIMSTEP, newRoute, true);
465 if (newRoute.empty()) {
467 }
468 MSRoute route("", newRoute, false, nullptr, std::vector<SUMOVehicleParameter::Stop>());
469 return distance + route.getDistanceBetween(roadPos1.second, roadPos2.second, &roadPos1.first->getEdge(), &roadPos2.first->getEdge());
470}
471
472
474Helper::getVehicle(const std::string& id) {
476 if (sumoVehicle == nullptr) {
477 throw TraCIException("Vehicle '" + id + "' is not known.");
478 }
479 MSBaseVehicle* v = dynamic_cast<MSBaseVehicle*>(sumoVehicle);
480 if (v == nullptr) {
481 throw TraCIException("Vehicle '" + id + "' is not a proper vehicle.");
482 }
483 return v;
484}
485
486
488Helper::getPerson(const std::string& personID) {
490 MSPerson* p = dynamic_cast<MSPerson*>(c.get(personID));
491 if (p == nullptr) {
492 throw TraCIException("Person '" + personID + "' is not known");
493 }
494 return p;
495}
496
498Helper::getTrafficObject(int domain, const std::string& id) {
499 if (domain == CMD_GET_VEHICLE_VARIABLE) {
500 return getVehicle(id);
501 } else if (domain == CMD_GET_PERSON_VARIABLE) {
502 return getPerson(id);
503 } else {
504 throw TraCIException("Cannot retrieve traffic object for domain " + toString(domain));
505 }
506}
507
508const MSVehicleType&
509Helper::getVehicleType(const std::string& vehicleID) {
510 return getVehicle(vehicleID)->getVehicleType();
511}
512
513
515Helper::getTLS(const std::string& id) {
516 if (!MSNet::getInstance()->getTLSControl().knows(id)) {
517 throw TraCIException("Traffic light '" + id + "' is not known");
518 }
519 return MSNet::getInstance()->getTLSControl().get(id);
520}
521
522
524Helper::getStoppingPlace(const std::string& id, const SumoXMLTag type) {
526 if (s == nullptr) {
527 throw TraCIException(toString(type) + " '" + id + "' is not known");
528 }
529 return s;
530}
531
532
534Helper::buildStopParameters(const std::string& edgeOrStoppingPlaceID,
535 double pos, int laneIndex, double startPos, int flags, double duration, double until) {
537 newStop.duration = duration == INVALID_DOUBLE_VALUE ? SUMOTime_MAX : TIME2STEPS(duration);
538 newStop.until = until == INVALID_DOUBLE_VALUE ? -1 : TIME2STEPS(until);
539 newStop.index = STOP_INDEX_FIT;
540 if (newStop.duration >= 0) {
542 }
543 if (newStop.until >= 0) {
544 newStop.parametersSet |= STOP_UNTIL_SET;
545 }
546 if ((flags & 1) != 0) {
549 }
550 if ((flags & 2) != 0) {
551 newStop.triggered = true;
553 }
554 if ((flags & 4) != 0) {
555 newStop.containerTriggered = true;
557 }
558
559 SumoXMLTag stoppingPlaceType = SUMO_TAG_NOTHING;
560 if ((flags & 8) != 0) {
561 stoppingPlaceType = SUMO_TAG_BUS_STOP;
562 }
563 if ((flags & 16) != 0) {
564 stoppingPlaceType = SUMO_TAG_CONTAINER_STOP;
565 }
566 if ((flags & 32) != 0) {
567 stoppingPlaceType = SUMO_TAG_CHARGING_STATION;
568 }
569 if ((flags & 64) != 0) {
570 stoppingPlaceType = SUMO_TAG_PARKING_AREA;
571 }
572 if ((flags & 128) != 0) {
573 stoppingPlaceType = SUMO_TAG_OVERHEAD_WIRE_SEGMENT;
574 }
575
576 if (stoppingPlaceType != SUMO_TAG_NOTHING) {
577 MSStoppingPlace* bs = MSNet::getInstance()->getStoppingPlace(edgeOrStoppingPlaceID, stoppingPlaceType);
578 if (bs == nullptr) {
579 throw TraCIException("The " + toString(stoppingPlaceType) + " '" + edgeOrStoppingPlaceID + "' is not known");
580 }
581 newStop.lane = bs->getLane().getID();
582 newStop.edge = bs->getLane().getEdge().getID();
583 newStop.endPos = bs->getEndLanePosition();
584 newStop.startPos = bs->getBeginLanePosition();
585 switch (stoppingPlaceType) {
587 newStop.busstop = edgeOrStoppingPlaceID;
588 break;
590 newStop.containerstop = edgeOrStoppingPlaceID;
591 break;
593 newStop.chargingStation = edgeOrStoppingPlaceID;
594 break;
596 newStop.parkingarea = edgeOrStoppingPlaceID;
597 break;
599 newStop.overheadWireSegment = edgeOrStoppingPlaceID;
600 break;
601 default:
602 throw TraCIException("Unknown stopping place type '" + toString(stoppingPlaceType) + "'.");
603 }
604 } else {
605 if (startPos == INVALID_DOUBLE_VALUE) {
606 startPos = MAX2(0.0, pos - POSITION_EPS);
607 }
608 if (startPos < 0.) {
609 throw TraCIException("Position on lane must not be negative.");
610 }
611 if (pos < startPos) {
612 throw TraCIException("End position on lane must be after start position.");
613 }
614 // get the actual lane that is referenced by laneIndex
615 MSEdge* road = MSEdge::dictionary(edgeOrStoppingPlaceID);
616 if (road == nullptr) {
617 throw TraCIException("Edge '" + edgeOrStoppingPlaceID + "' is not known.");
618 }
619 const std::vector<MSLane*>& allLanes = road->getLanes();
620 if ((laneIndex < 0) || laneIndex >= (int)(allLanes.size())) {
621 throw TraCIException("No lane with index '" + toString(laneIndex) + "' on edge '" + edgeOrStoppingPlaceID + "'.");
622 }
623 newStop.lane = allLanes[laneIndex]->getID();
624 newStop.edge = allLanes[laneIndex]->getEdge().getID();
625 newStop.endPos = pos;
626 newStop.startPos = startPos;
628 }
629 return newStop;
630}
631
632
635 std::string stoppingPlaceID = "";
636 if (stopPar.busstop != "") {
637 stoppingPlaceID = stopPar.busstop;
638 }
639 if (stopPar.containerstop != "") {
640 stoppingPlaceID = stopPar.containerstop;
641 }
642 if (stopPar.parkingarea != "") {
643 stoppingPlaceID = stopPar.parkingarea;
644 }
645 if (stopPar.chargingStation != "") {
646 stoppingPlaceID = stopPar.chargingStation;
647 }
648 if (stopPar.overheadWireSegment != "") {
649 stoppingPlaceID = stopPar.overheadWireSegment;
650 }
651
652 return TraCINextStopData(stopPar.lane,
653 stopPar.startPos,
654 stopPar.endPos,
655 stoppingPlaceID,
656 stopPar.getFlags(),
657 // negative duration is permitted to indicate that a vehicle cannot
658 // re-enter traffic after parking
659 stopPar.duration != -1 ? STEPS2TIME(stopPar.duration) : INVALID_DOUBLE_VALUE,
660 stopPar.until >= 0 ? STEPS2TIME(stopPar.until) : INVALID_DOUBLE_VALUE,
661 stopPar.arrival >= 0 ? STEPS2TIME(stopPar.arrival) : INVALID_DOUBLE_VALUE,
662 stopPar.started >= 0 ? STEPS2TIME(stopPar.started) : INVALID_DOUBLE_VALUE,
663 stopPar.ended >= 0 ? STEPS2TIME(stopPar.ended) : INVALID_DOUBLE_VALUE,
664 stopPar.split,
665 stopPar.join,
666 stopPar.actType,
667 stopPar.tripId,
668 stopPar.line,
669 stopPar.speed);
670}
671
672
673void
675 // clean up NamedRTrees
676 InductionLoop::cleanup();
677 Junction::cleanup();
678 LaneArea::cleanup();
679 POI::cleanup();
680 Polygon::cleanup();
683 delete myLaneTree;
684 myLaneTree = nullptr;
685}
686
687
688void
690 if (MSNet::hasInstance()) {
693 }
694}
695
696
697const std::vector<std::string>&
700}
701
702
703const std::vector<std::string>&
706}
707
708
709void
712 i.second.clear();
713 }
715 i.second.clear();
716 }
717}
718
719
722 try {
723 return c->getCurrentStateInterval();
724 } catch (ProcessError& e) {
725 throw TraCIException(e.what());
726 }
727}
728
729
730void
731Helper::findObjectShape(int domain, const std::string& id, PositionVector& shape) {
732 switch (domain) {
735 shape.push_back(stop->getLane().getShape().positionAtOffset(stop->getBeginLanePosition()));
736 shape.push_back(stop->getLane().getShape().positionAtOffset(stop->getEndLanePosition()));
737 break;
738 }
740 MSCalibrator* const calib = Calibrator::getCalibrator(id);
741 shape.push_back(calib->getLane()->getShape()[0]);
742 break;
743 }
746 shape.push_back(stop->getLane().getShape().positionAtOffset(stop->getBeginLanePosition()));
747 shape.push_back(stop->getLane().getShape().positionAtOffset(stop->getEndLanePosition()));
748 break;
749 }
751 Edge::storeShape(id, shape);
752 break;
754 InductionLoop::storeShape(id, shape);
755 break;
757 Junction::storeShape(id, shape);
758 break;
760 Lane::storeShape(id, shape);
761 break;
763 LaneArea::storeShape(id, shape);
764 break;
766 MSE3Collector* const det = MultiEntryExit::getDetector(id);
767 for (const MSCrossSection& cs : det->getEntries()) {
768 shape.push_back(cs.myLane->getShape().positionAtOffset(cs.myPosition));
769 }
770 for (const MSCrossSection& cs : det->getExits()) {
771 shape.push_back(cs.myLane->getShape().positionAtOffset(cs.myPosition));
772 }
773 break;
774 }
777 shape.push_back(stop->getLane().getShape().positionAtOffset(stop->getBeginLanePosition()));
778 shape.push_back(stop->getLane().getShape().positionAtOffset(stop->getEndLanePosition()));
779 break;
780 }
782 Person::storeShape(id, shape);
783 break;
785 POI::storeShape(id, shape);
786 break;
788 Polygon::storeShape(id, shape);
789 break;
791 Vehicle::storeShape(id, shape);
792 break;
793 default:
794 Simulation::storeShape(shape);
795 break;
796 }
797}
798
799
800void
801Helper::collectObjectIDsInRange(int domain, const PositionVector& shape, double range, std::set<std::string>& into) {
802 std::set<const Named*> objects;
803 collectObjectsInRange(domain, shape, range, objects);
804 for (const Named* obj : objects) {
805 into.insert(obj->getID());
806 }
807}
808
809
810void
811Helper::collectObjectsInRange(int domain, const PositionVector& shape, double range, std::set<const Named*>& into) {
812 const Boundary b = shape.getBoxBoundary().grow(range);
813 const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
814 const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
815 switch (domain) {
817 for (const auto& stop : MSNet::getInstance()->getStoppingPlaces(SUMO_TAG_BUS_STOP)) {
818 if (shape.distance2D(stop.second->getCenterPos()) <= range) {
819 into.insert(stop.second);
820 }
821 }
822 break;
825 if (shape.distance2D(stop.second->getCenterPos()) <= range) {
826 into.insert(stop.second);
827 }
828 }
829 break;
831 for (const auto& calib : MSCalibrator::getInstances()) {
832 if (shape.distance2D(calib.second->getLane()->getShape()[0]) <= range) {
833 into.insert(calib.second);
834 }
835 }
836 break;
838 InductionLoop::getTree()->Search(cmin, cmax, Named::StoringVisitor(into));
839 break;
841 Junction::getTree()->Search(cmin, cmax, Named::StoringVisitor(into));
842 break;
844 LaneArea::getTree()->Search(cmin, cmax, Named::StoringVisitor(into));
845 break;
847 for (const auto& stop : MSNet::getInstance()->getStoppingPlaces(SUMO_TAG_PARKING_AREA)) {
848 if (shape.distance2D(stop.second->getCenterPos()) <= range) {
849 into.insert(stop.second);
850 }
851 }
852 break;
853 }
855 POI::getTree()->Search(cmin, cmax, Named::StoringVisitor(into));
856 break;
858 Polygon::getTree()->Search(cmin, cmax, Named::StoringVisitor(into));
859 break;
864 if (myLaneTree == nullptr) {
867 }
868 MSLane::StoringVisitor lsv(into, shape, range, domain);
869 myLaneTree->Search(cmin, cmax, lsv);
870 }
871 break;
872 default:
873 throw TraCIException("Infeasible context domain (" + toString(domain) + ")");
874 break;
875 }
876}
877
878
879
880void
881Helper::applySubscriptionFilters(const Subscription& s, std::set<std::string>& objIDs) {
882#ifdef DEBUG_SURROUNDING
883 MSBaseVehicle* _veh = getVehicle(s.id);
884 std::cout << SIMTIME << " applySubscriptionFilters for vehicle '" << _veh->getID() << "' on lane '" << _veh->getLane()->getID() << "'"
885 << "\n on edge '" << _veh->getLane()->getEdge().getID() << "' (" << toString(_veh->getLane()->getEdge().getLanes()) << ")\n"
886 << "objIDs = " << toString(objIDs) << std::endl;
887#endif
888
889 if (s.activeFilters == 0) {
890 // No filters set
891 return;
892 }
893
894 MSVehicle* v = dynamic_cast<MSVehicle*>(getVehicle(s.id));
895
896 // Whether vehicles on opposite lanes shall be taken into account
897 const bool disregardOppositeDirection = (s.activeFilters & SUBS_FILTER_NOOPPOSITE) != 0;
898
899 // Check filter specification consistency
900 if (disregardOppositeDirection && (s.activeFilters & SUBS_FILTER_NO_RTREE) == 0) {
901 WRITE_WARNINGF(TL("Ignoring veh '%' no-opposite subscription filter for geographic range object collection. Consider using the 'lanes' filter."), v->getID())
902 }
903 // TODO: Treat case, where ego vehicle is currently on opposite lane
904
905 std::set<const SUMOTrafficObject*> vehs;
907 // Set defaults for upstream/downstream/lateral distances
908 double downstreamDist = s.range, upstreamDist = s.range, lateralDist = s.range;
910 // Specifies maximal downstream distance for vehicles in context subscription result
911 downstreamDist = s.filterDownstreamDist;
912 }
914 // Specifies maximal downstream distance for vehicles in context subscription result
915 upstreamDist = s.filterUpstreamDist;
916 }
918 // Specifies maximal lateral distance for vehicles in context subscription result
919 lateralDist = s.filterLateralDist;
920 }
921 if (v == nullptr) {
922 throw TraCIException("Subscription filter not yet implemented for meso vehicle");
923 }
924 if (!v->isOnRoad()) {
925 return;
926 }
927 const MSLane* vehLane = v->getLane();
928 if (vehLane == nullptr) {
929 return;
930 }
931 MSEdge* vehEdge = &vehLane->getEdge();
932 std::vector<int> filterLanes;
933 if ((s.activeFilters & SUBS_FILTER_LANES) == 0) {
934 // No lane indices are specified (but downstream and/or upstream distance)
935 // -> use only vehicle's current lane as origin for the searches
936 filterLanes = {0};
937 // Lane indices must be specified when leader/follower information is requested.
938 assert((s.activeFilters & SUBS_FILTER_LEAD_FOLLOW) == 0);
939 } else {
940 filterLanes = s.filterLanes;
941 }
942
943#ifdef DEBUG_SURROUNDING
944 std::cout << "Filter lanes: " << toString(filterLanes) << std::endl;
945 std::cout << "Downstream distance: " << downstreamDist << std::endl;
946 std::cout << "Upstream distance: " << upstreamDist << std::endl;
947 std::cout << "Lateral distance: " << lateralDist << std::endl;
948#endif
949
950 if ((s.activeFilters & SUBS_FILTER_MANEUVER) != 0) {
951 // Maneuver filters disables road net search for all surrounding vehicles
952 if ((s.activeFilters & SUBS_FILTER_LEAD_FOLLOW) != 0) {
953 // Return leader and follower on the specified lanes in context subscription result.
954 for (int offset : filterLanes) {
955 MSLane* lane = v->getLane()->getParallelLane(offset, false);
956 if (lane != nullptr) {
957 // this is a non-opposite lane
958 MSVehicle* leader = lane->getLeader(v, v->getPositionOnLane(), v->getBestLanesContinuation(lane), downstreamDist).first;
959 MSVehicle* follower = lane->getFollower(v, v->getPositionOnLane(), upstreamDist,
960 MSLane::MinorLinkMode::FOLLOW_ALWAYS).first;
961 vehs.insert(vehs.end(), leader);
962 vehs.insert(vehs.end(), follower);
963
964#ifdef DEBUG_SURROUNDING
965 std::cout << "Lane at index " << offset << ": '" << lane->getID() << std::endl;
966 std::cout << "Leader: '" << (leader != nullptr ? leader->getID() : "NULL") << "'" << std::endl;
967 std::cout << "Follower: '" << (follower != nullptr ? follower->getID() : "NULL") << "'" << std::endl;
968#endif
969
970 } else if (!disregardOppositeDirection && offset > 0) { // TODO: offset<0 may indicate opposite query when vehicle is on opposite itself
971 // check whether ix points to an opposite lane
972 const MSEdge* opposite = vehEdge->getOppositeEdge();
973 if (opposite == nullptr) {
974#ifdef DEBUG_SURROUNDING
975 std::cout << "No lane at index " << offset << std::endl;
976#endif
977 // no opposite edge
978 continue;
979 }
980 // Index of opposite lane at relative offset
981 const int ix_opposite = (int)opposite->getLanes().size() - 1 - (vehLane->getIndex() + offset - (int)vehEdge->getLanes().size());
982 if (ix_opposite < 0) {
983#ifdef DEBUG_SURROUNDING
984 std::cout << "No lane on opposite at index " << ix_opposite << std::endl;
985#endif
986 // no opposite edge
987 continue;
988 }
989 lane = opposite->getLanes()[ix_opposite];
990 // Search vehs along opposite lanes (swap upstream and downstream distance)
991 // XXX transformations for curved geometries
992 double posOnOpposite = MAX2(0., opposite->getLength() - v->getPositionOnLane());
993 // Get leader on opposite
994 vehs.insert(vehs.end(), lane->getFollower(v, posOnOpposite, downstreamDist, MSLane::MinorLinkMode::FOLLOW_NEVER).first);
995 // Get follower (no search on consecutive lanes
996 vehs.insert(vehs.end(), lane->getLeader(v, posOnOpposite - v->getLength(), std::vector<MSLane*>()).first);
997 }
998 }
999 }
1000
1004 applySubscriptionFilterLanes(s, vehs, filterLanes, downstreamDist, upstreamDist, disregardOppositeDirection);
1005 }
1007 applySubscriptionFilterLateralDistance(s, vehs, downstreamDist, upstreamDist, lateralDist);
1008 }
1009 }
1010#ifdef DEBUG_SURROUNDING
1011 std::cout << SIMTIME << " applySubscriptionFilters() for veh '" << v->getID() << "'. Found the following vehicles:\n";
1012 for (auto veh : vehs) {
1013 debugPrint(veh);
1014 }
1015#endif
1016 } else if (s.activeFilters & SUBS_FILTER_LATERAL_DIST) {
1017 applySubscriptionFilterLateralDistance(s, vehs, downstreamDist, upstreamDist, lateralDist);
1018 } else {
1019 // No maneuver or lateral distance filters requested, but only lanes filter (directly, or indirectly by specifying downstream or upstream distance)
1020 applySubscriptionFilterLanes(s, vehs, filterLanes, downstreamDist, upstreamDist, disregardOppositeDirection);
1021 }
1022 // Write vehs IDs in objIDs
1023 for (const SUMOTrafficObject* veh : vehs) {
1024 if (veh != nullptr) {
1025 objIDs.insert(objIDs.end(), veh->getID());
1026 }
1027 }
1028 }
1029
1031 // Only return vehicles of the given vClass in context subscription result
1032 auto i = objIDs.begin();
1033 while (i != objIDs.end()) {
1034 MSBaseVehicle* veh = getVehicle(*i);
1035 if ((veh->getVehicleType().getVehicleClass() & s.filterVClasses) == 0) {
1036 i = objIDs.erase(i);
1037 } else {
1038 ++i;
1039 }
1040 }
1041 }
1043 // Only return vehicles of the given vType in context subscription result
1044 auto i = objIDs.begin();
1045 while (i != objIDs.end()) {
1046 MSBaseVehicle* veh = getVehicle(*i);
1047 if (s.filterVTypes.find(veh->getVehicleType().getID()) == s.filterVTypes.end()) {
1048 i = objIDs.erase(i);
1049 } else {
1050 ++i;
1051 }
1052 }
1053 }
1055 // Only return vehicles within field of vision in context subscription result
1057 }
1058}
1059
1060void
1061Helper::applySubscriptionFilterLanes(const Subscription& s, std::set<const SUMOTrafficObject*>& vehs, std::vector<int>& filterLanes, double downstreamDist,
1062 double upstreamDist, bool disregardOppositeDirection) {
1064 WRITE_WARNINGF(TL("Lanes filter is only feasible for context domain 'vehicle' (current is '%'), ignoring filter..."), toHex(s.contextDomain, 2));
1065 return;
1066 }
1067 assert(filterLanes.size() > 0);
1068 MSVehicle* v = dynamic_cast<MSVehicle*>(getVehicle(s.id));
1069 const MSLane* vehLane = v->getLane();
1070 MSEdge* vehEdge = &vehLane->getEdge();
1071 // This is to remember the lanes checked in the driving direction of the vehicle (their opposites can be added in a second pass)
1072 auto checkedLanesInDrivingDir = std::make_shared<LaneCoverageInfo>();
1073 for (int offset : filterLanes) {
1074 MSLane* lane = vehLane->getParallelLane(offset, false);
1075 if (lane != nullptr) {
1076#ifdef DEBUG_SURROUNDING
1077 std::cout << "Checking for surrounding vehicles starting on lane '" << lane->getID() << "' at index " << offset << std::endl;
1078#endif
1079 // Search vehs along this lane
1080 // (Coverage info is collected per origin lane since lanes reached from neighboring lanes may have different distances
1081 // and aborting at previously scanned when coming from a closer origin may prevent scanning of parts that should be included.)
1082 std::shared_ptr<LaneCoverageInfo> checkedLanes = std::make_shared<LaneCoverageInfo>();
1083 const std::set<MSVehicle*> new_vehs =
1084 lane->getSurroundingVehicles(v->getPositionOnLane(), downstreamDist, upstreamDist + v->getLength(), checkedLanes);
1085 vehs.insert(new_vehs.begin(), new_vehs.end());
1086 fuseLaneCoverage(checkedLanesInDrivingDir, checkedLanes);
1087 } else if (!disregardOppositeDirection && offset > 0) {
1088 // Check opposite edge, too
1089 assert(vehLane->getIndex() + offset >= (int)vehEdge->getLanes().size()); // index points beyond this edge
1090 const MSEdge* opposite = vehEdge->getOppositeEdge();
1091 if (opposite == nullptr) {
1092#ifdef DEBUG_SURROUNDING
1093 std::cout << "No opposite edge, thus no lane at index " << offset << std::endl;
1094#endif
1095 // no opposite edge
1096 continue;
1097 }
1098 // Index of opposite lane at relative offset
1099 const int ix_opposite = (int)opposite->getLanes().size() - 1 - (vehLane->getIndex() + offset - (int)vehEdge->getLanes().size());
1100 if (ix_opposite < 0) {
1101#ifdef DEBUG_SURROUNDING
1102 std::cout << "No lane on opposite at index " << ix_opposite << std::endl;
1103#endif
1104 // no opposite edge
1105 continue;
1106 }
1107 lane = opposite->getLanes()[ix_opposite];
1108 // Search vehs along opposite lanes (swap upstream and downstream distance)
1109 const std::set<MSVehicle*> new_vehs = lane->getSurroundingVehicles(lane->getLength() - v->getPositionOnLane(), upstreamDist + v->getLength(),
1110 downstreamDist, std::make_shared<LaneCoverageInfo>());
1111 vehs.insert(new_vehs.begin(), new_vehs.end());
1112 }
1113#ifdef DEBUG_SURROUNDING
1114 else {
1115 std::cout << "No lane at index " << offset << std::endl;
1116 }
1117#endif
1118
1119 if (!disregardOppositeDirection) {
1120 // If opposite should be checked, do this for each lane of the search tree in checkedLanesInDrivingDir
1121 // (For instance, some opposite lanes of these would not be obtained if the ego lane does not have an opposite.)
1122
1123 // Number of opposite lanes to be checked (assumes filterLanes.size()>0, see assertion above) determined as hypothetical offset
1124 // overlap into opposing edge from the vehicle's current lane.
1125 // TODO: offset<0 may indicate opposite query when vehicle is on opposite itself (-> use min_element(filterLanes...) instead, etc)
1126 const int nOpp = MAX2(0, (*std::max_element(filterLanes.begin(), filterLanes.end())) - ((int)vehEdge->getLanes().size() - 1 - vehLane->getIndex()));
1127 // Collect vehicles from opposite lanes
1128 if (nOpp > 0) {
1129 for (auto& laneCov : *checkedLanesInDrivingDir) {
1130 const MSLane* const l = laneCov.first;
1131 if (l == nullptr || l->getEdge().getOppositeEdge() == nullptr) {
1132 continue;
1133 }
1134 const MSEdge* opposite = l->getEdge().getOppositeEdge();
1135 const std::pair<double, double>& range = laneCov.second;
1136 auto leftMostOppositeLaneIt = opposite->getLanes().rbegin();
1137 for (auto oppositeLaneIt = leftMostOppositeLaneIt; oppositeLaneIt != opposite->getLanes().rend(); ++oppositeLaneIt) {
1138 if ((int)(oppositeLaneIt - leftMostOppositeLaneIt) == nOpp) {
1139 break;
1140 }
1141 // Add vehicles from corresponding range on opposite direction
1142 const MSLane* oppositeLane = *oppositeLaneIt;
1143 auto new_vehs = oppositeLane->getVehiclesInRange(l->getLength() - range.second, l->getLength() - range.first);
1144 vehs.insert(new_vehs.begin(), new_vehs.end());
1145 }
1146 }
1147 }
1148 }
1149#ifdef DEBUG_SURROUNDING
1150 std::cout << SIMTIME << " applySubscriptionFilterLanes() for veh '" << v->getID() << "', lane offset '" << offset << "'. Found the following vehicles so far:\n";
1151 for (auto veh : vehs) {
1152 debugPrint(veh);
1153 }
1154#endif
1155 }
1156}
1157
1158void
1159Helper::applySubscriptionFilterTurn(const Subscription& s, std::set<const SUMOTrafficObject*>& vehs) {
1161 WRITE_WARNINGF(TL("Turn filter is only feasible for context domain 'vehicle' (current is '%'), ignoring filter..."), toHex(s.contextDomain, 2));
1162 return;
1163 }
1164 // Get upcoming junctions and vialanes within downstream distance, where foe links exist or at least the link direction is not straight
1165 MSVehicle* v = dynamic_cast<MSVehicle*>(getVehicle(s.id));
1166 const MSLane* lane = v->getLane();
1167 std::vector<const MSLink*> links = lane->getUpcomingLinks(v->getPositionOnLane(), s.filterDownstreamDist, v->getBestLanesContinuation());
1168#ifdef DEBUG_SURROUNDING
1169 std::cout << "Applying turn filter for vehicle '" << v->getID() << "'\n Gathering foes ..." << std::endl;
1170#endif
1171 // Iterate through junctions and find approaching foes within foeDistToJunction.
1172 for (auto& l : links) {
1173#ifdef DEBUG_SURROUNDING
1174 std::cout << " On junction '" << l->getJunction()->getID() << "' (no. foe links = " << l->getFoeLinks().size() << "):" << std::endl;
1175#endif
1176 for (auto& foeLane : l->getFoeLanes()) {
1177 if (foeLane->getEdge().isCrossing()) {
1178#ifdef DEBUG_SURROUNDING
1179 std::cout << " skipping crossing foeLane '" << foeLane->getID() << "'" << std::endl;
1180#endif
1181 continue;
1182 }
1183#ifdef DEBUG_SURROUNDING
1184 std::cout << " foeLane '" << foeLane->getID() << "'" << std::endl;
1185#endif
1186 // Check vehicles approaching the entry link corresponding to this lane
1187 const MSLink* foeLink = foeLane->getEntryLink();
1188 for (auto& vi : foeLink->getApproaching()) {
1189 if (vi.second.dist <= s.filterFoeDistToJunction) {
1190#ifdef DEBUG_SURROUNDING
1191 std::cout << " Approaching foeLane entry link '" << vi.first->getID() << "'" << std::endl;
1192#endif
1193 vehs.insert(vehs.end(), dynamic_cast<const MSVehicle*>(vi.first));
1194 }
1195 }
1196 // add vehicles currently on the junction
1197 for (const MSVehicle* foe : foeLane->getVehiclesSecure()) {
1198#ifdef DEBUG_SURROUNDING
1199 std::cout << " On foeLane '" << foe->getID() << "'" << std::endl;
1200#endif
1201 vehs.insert(vehs.end(), foe);
1202 }
1203 foeLane->releaseVehicles();
1204 for (auto& laneInfo : foeLane->getIncomingLanes()) {
1205 const MSLane* foeLanePred = laneInfo.lane;
1206 if (foeLanePred->isInternal()) {
1207#ifdef DEBUG_SURROUNDING
1208 std::cout << " foeLanePred '" << foeLanePred->getID() << "'" << std::endl;
1209#endif
1210 for (const MSVehicle* foe : foeLanePred->getVehiclesSecure()) {
1211#ifdef DEBUG_SURROUNDING
1212 std::cout << " On foeLanePred '" << foe->getID() << "'" << std::endl;
1213#endif
1214 vehs.insert(vehs.end(), foe);
1215 }
1216 foeLanePred->releaseVehicles();
1217 }
1218 }
1219 }
1220 }
1221}
1222
1223void
1224Helper::applySubscriptionFilterFieldOfVision(const Subscription& s, std::set<std::string>& objIDs) {
1226 WRITE_WARNINGF(TL("Field of vision opening angle ('%') should be within interval (0, 360), ignoring filter..."), s.filterFieldOfVisionOpeningAngle);
1227 return;
1228 }
1229
1230 MSBaseVehicle* egoVehicle = getVehicle(s.id);
1231 Position egoPosition = egoVehicle->getPosition();
1232 double openingAngle = DEG2RAD(s.filterFieldOfVisionOpeningAngle);
1233
1234#ifdef DEBUG_SURROUNDING
1235 std::cout << "FOVFILTER: ego direction = " << toString(RAD2DEG(egoVehicle->getAngle())) << " (deg)" << std::endl;
1236#endif
1237
1238 auto i = objIDs.begin();
1239 while (i != objIDs.end()) {
1240 if (s.id.compare(*i) == 0) { // skip if this is the ego vehicle
1241 ++i;
1242 continue;
1243 }
1245 double angleEgoToVeh = egoPosition.angleTo2D(obj->getPosition());
1246 double alpha = GeomHelper::angleDiff(egoVehicle->getAngle(), angleEgoToVeh);
1247
1248#ifdef DEBUG_SURROUNDING
1249 const std::string objType = s.isVehicleToPersonContextSubscription() ? "person" : "veh";
1250 std::cout << "FOVFILTER: " << objType << " '" << *i << "' dist = " << toString(egoPosition.distanceTo2D(obj->getPosition())) << std::endl;
1251 std::cout << "FOVFILTER: " << objType << " '" << *i << "' alpha = " << toString(RAD2DEG(alpha)) << " (deg)" << std::endl;
1252#endif
1253
1254 if (abs(alpha) > openingAngle * 0.5) {
1255 i = objIDs.erase(i);
1256 } else {
1257 ++i;
1258 }
1259 }
1260}
1261
1262void
1263Helper::applySubscriptionFilterLateralDistance(const Subscription& s, std::set<const SUMOTrafficObject*>& vehs, double downstreamDist, double upstreamDist,
1264 double lateralDist) {
1265 // collect all vehicles within maximum range of interest to get an upper bound
1266 PositionVector vehShape;
1267 findObjectShape(s.commandId, s.id, vehShape);
1268 double range = MAX3(downstreamDist, upstreamDist, lateralDist);
1269 std::set<std::string> objIDs;
1270 collectObjectIDsInRange(s.contextDomain, vehShape, range, objIDs);
1271
1272#ifdef DEBUG_SURROUNDING
1273 std::cout << "FILTER_LATERAL_DIST: collected object IDs (range " << range << "):" << std::endl;
1274 for (std::string i : objIDs) {
1275 std::cout << i << std::endl;
1276 }
1277#endif
1278
1279 MSVehicle* v = dynamic_cast<MSVehicle*>(getVehicle(s.id));
1280#ifdef DEBUG_SURROUNDING
1281 std::cout << "FILTER_LATERAL_DIST: myLane is '" << v->getLane()->getID() << "', pos " << v->getPositionOnLane() << std::endl;
1282 std::cout << "FILTER_LATERAL_DIST: opposite lane is '" << v->getLane()->getParallelOpposite()->getID() << "'" << std::endl;
1283#endif
1284 double frontPosOnLane = v->getPositionOnLane();
1285 if (v->getLaneChangeModel().isOpposite()) {
1286 frontPosOnLane = v->getLane()->getOppositePos(frontPosOnLane);
1287 }
1288 // 1st pass: downstream (make sure that the whole length of the vehicle is included in the match)
1289 const double backPosOnLane = MAX2(0.0, frontPosOnLane - v->getVehicleType().getLength());
1290 applySubscriptionFilterLateralDistanceSinglePass(s, objIDs, vehs, v->getUpcomingLanesUntil(downstreamDist), backPosOnLane, v->getLateralPositionOnLane(),
1291 true);
1292 // 2nd pass: upstream
1293 applySubscriptionFilterLateralDistanceSinglePass(s, objIDs, vehs, v->getPastLanesUntil(upstreamDist), frontPosOnLane, v->getLateralPositionOnLane(), false);
1294}
1295
1296void
1298 std::set<const SUMOTrafficObject*>& vehs,
1299 const std::vector<const MSLane*>& lanes, double posOnLane, double posLat, bool isDownstream) {
1300 const double streamDist = isDownstream ? s.filterDownstreamDist : s.filterUpstreamDist;
1301 double distRemaining = streamDist;
1302 bool isFirstLane = true;
1303 PositionVector combinedShape;
1304 for (const MSLane* lane : lanes) {
1305#ifdef DEBUG_SURROUNDING
1306 std::cout << "FILTER_LATERAL_DIST: current lane " << (isDownstream ? "down" : "up") << " is '" << lane->getID() << "', length " << lane->getLength()
1307 << ", pos " << posOnLane << ", distRemaining " << distRemaining << std::endl;
1308#endif
1309 PositionVector laneShape = lane->getShape();
1310 if (isFirstLane) {
1311 isFirstLane = false;
1312 double geometryPos = lane->interpolateLanePosToGeometryPos(posOnLane);
1313 if (geometryPos <= POSITION_EPS) {
1314 if (!isDownstream) {
1315 continue;
1316 }
1317 } else {
1318 if (geometryPos >= laneShape.length() - POSITION_EPS) {
1319 laneShape = isDownstream ? PositionVector() : laneShape;
1320 } else {
1321 auto pair = laneShape.splitAt(geometryPos, false);
1322 laneShape = isDownstream ? pair.second : pair.first;
1323 }
1324 }
1325 }
1326 double laneLength = lane->interpolateGeometryPosToLanePos(laneShape.length());
1327 if (distRemaining - laneLength < 0.) {
1328 double geometryPos = lane->interpolateLanePosToGeometryPos(isDownstream ? distRemaining : laneLength - distRemaining);
1329 if (geometryPos > POSITION_EPS && geometryPos < laneShape.length() - POSITION_EPS) {
1330 auto pair = laneShape.splitAt(geometryPos, false);
1331 laneShape = isDownstream ? pair.first : pair.second;
1332 }
1333 }
1334 distRemaining -= laneLength;
1335 try {
1336 laneShape.move2side(-posLat);
1337 } catch (ProcessError&) {
1338 WRITE_WARNING("addSubscriptionFilterLateralDistance could not determine shape of lane '" + lane->getID() + "' with lateral shift of " + toString(posLat));
1339 }
1340#ifdef DEBUG_SURROUNDING
1341 std::cout << " posLat=" << posLat << " laneShape=" << laneShape << "\n";
1342#endif
1343 if (isDownstream) {
1344 combinedShape.append(laneShape);
1345 } else {
1346 combinedShape.prepend(laneShape);
1347 }
1348 if (distRemaining <= POSITION_EPS) {
1349 break;
1350 }
1351 }
1352#ifdef DEBUG_SURROUNDING
1353 std::cout << " combinedShape=" << combinedShape << "\n";
1354#endif
1355 // check remaining objects' distances to the combined shape
1356 auto i = objIDs.begin();
1357 while (i != objIDs.end()) {
1359 double minPerpendicularDist = combinedShape.distance2D(obj->getPosition(), true);
1360#ifdef DEBUG_SURROUNDING
1361 std::cout << (isDownstream ? "DOWN" : "UP") << " obj " << obj->getID() << " perpendicular dist=" << minPerpendicularDist << " filterLateralDist=" << s.filterLateralDist << "\n";
1362#endif
1363 if ((minPerpendicularDist != GeomHelper::INVALID_OFFSET) && (minPerpendicularDist <= s.filterLateralDist)) {
1364 vehs.insert(obj);
1365 i = objIDs.erase(i);
1366 } else {
1367 ++i;
1368 }
1369 }
1370}
1371
1372void
1373Helper::setRemoteControlled(MSVehicle* v, Position xyPos, MSLane* l, double pos, double posLat, double angle,
1374 int edgeOffset, ConstMSEdgeVector route, SUMOTime t) {
1376 v->getInfluencer().setRemoteControlled(xyPos, l, pos, posLat, angle, edgeOffset, route, t);
1377}
1378
1379void
1380Helper::setRemoteControlled(MSPerson* p, Position xyPos, MSLane* l, double pos, double posLat, double angle,
1381 int edgeOffset, ConstMSEdgeVector route, SUMOTime t) {
1383 p->getInfluencer().setRemoteControlled(xyPos, l, pos, posLat, angle, edgeOffset, route, t);
1384}
1385
1386
1387int
1389 int numControlled = 0;
1390 for (auto& controlled : myRemoteControlledVehicles) {
1391 if (MSNet::getInstance()->getVehicleControl().getVehicle(controlled.first) != nullptr) {
1392 controlled.second->getInfluencer().postProcessRemoteControl(controlled.second);
1393 numControlled++;
1394 } else {
1395 WRITE_WARNING("Vehicle '" + controlled.first + "' was removed though being controlled by TraCI");
1396 }
1397 }
1399 for (auto& controlled : myRemoteControlledPersons) {
1400 if (MSNet::getInstance()->getPersonControl().get(controlled.first) != nullptr) {
1401 controlled.second->getInfluencer().postProcessRemoteControl(controlled.second);
1402 numControlled++;
1403 } else {
1404 WRITE_WARNING("Person '" + controlled.first + "' was removed though being controlled by TraCI");
1405 }
1406 }
1408 return numControlled;
1409}
1410
1411
1412bool
1413Helper::moveToXYMap(const Position& pos, double maxRouteDistance, bool mayLeaveNetwork, const std::string& origID, const double angle,
1414 double speed, const ConstMSEdgeVector& currentRoute, const int routePosition, const MSLane* currentLane, double currentLanePos, bool onRoad,
1415 SUMOVehicleClass vClass, bool setLateralPos,
1416 double& bestDistance, MSLane** lane, double& lanePos, int& routeOffset, ConstMSEdgeVector& edges) {
1417 // collect edges around the vehicle/person
1418#ifdef DEBUG_MOVEXY
1419 std::cout << SIMTIME << " moveToXYMap pos=" << pos << " angle=" << angle << " vClass=" << toString(vClass) << "\n";
1420#endif
1421 const MSEdge* const currentRouteEdge = currentRoute[routePosition];
1422 std::set<const Named*> into;
1423 PositionVector shape;
1424 shape.push_back(pos);
1425 collectObjectsInRange(libsumo::CMD_GET_EDGE_VARIABLE, shape, maxRouteDistance, into);
1426 double maxDist = 0;
1427 std::map<MSLane*, LaneUtility, ComparatorNumericalIdLess> lane2utility;
1428 // compute utility for all candidate edges
1429 for (const Named* namedEdge : into) {
1430 const MSEdge* e = dynamic_cast<const MSEdge*>(namedEdge);
1431 if ((e->getPermissions() & vClass) != vClass) {
1432 continue;
1433 }
1434 const MSEdge* prevEdge = nullptr;
1435 const MSEdge* nextEdge = nullptr;
1436 bool onRoute = false;
1437 // the next if/the clause sets "onRoute", "prevEdge", and "nextEdge", depending on
1438 // whether the currently seen edge is an internal one or a normal one
1439 if (e->isWalkingArea() || e->isCrossing()) {
1440 // find current intersection along the route
1441 const MSJunction* junction = e->getFromJunction();
1442 for (int i = routePosition; i < (int)currentRoute.size(); i++) {
1443 const MSEdge* cand = currentRoute[i];
1444 if (cand->getToJunction() == junction) {
1445 prevEdge = cand;
1446 if (i + 1 < (int)currentRoute.size()) {
1447 onRoute = true;
1448 nextEdge = currentRoute[i + 1];
1449 }
1450 break;
1451 }
1452 }
1453 if (onRoute == false) {
1454 // search backward
1455 for (int i = routePosition - 1; i >= 0; i--) {
1456 const MSEdge* cand = currentRoute[i];
1457 if (cand->getToJunction() == junction) {
1458 onRoute = true;
1459 prevEdge = cand;
1460 nextEdge = currentRoute[i + 1];
1461 break;
1462 }
1463 }
1464 }
1465 if (prevEdge == nullptr) {
1466 // use arbitrary predecessor
1467 if (e->getPredecessors().size() > 0) {
1468 prevEdge = e->getPredecessors().front();
1469 } else if (e->getSuccessors().size() > 1) {
1470 for (MSEdge* e2 : e->getSuccessors()) {
1471 if (e2 != nextEdge) {
1472 prevEdge = e2;
1473 break;
1474 }
1475 }
1476 }
1477 }
1478 if (nextEdge == nullptr) {
1479 if (e->getSuccessors().size() > 0) {
1480 nextEdge = e->getSuccessors().front();
1481 } else if (e->getPredecessors().size() > 1) {
1482 for (MSEdge* e2 : e->getPredecessors()) {
1483 if (e2 != prevEdge) {
1484 nextEdge = e2;
1485 break;
1486 }
1487 }
1488 }
1489 }
1490#ifdef DEBUG_MOVEXY_ANGLE
1491 std::cout << "walkingarea/crossing:" << e->getID() << " prev:" << Named::getIDSecure(prevEdge) << " next:" << Named::getIDSecure(nextEdge)
1492 << " pred=" << toString(e->getPredecessors()) << " succ=" << toString(e->getSuccessors())
1493 << "\n";
1494#endif
1495 } else if (e->isNormal()) {
1496 // a normal edge
1497 //
1498 // check whether the currently seen edge is in the vehicle's route
1499 // - either the one it's on or one of the next edges
1500 ConstMSEdgeVector::const_iterator searchStart = currentRoute.begin() + routePosition;
1501 if (onRoad && currentLane->getEdge().isInternal()) {
1502 ++searchStart;
1503 }
1504 ConstMSEdgeVector::const_iterator edgePos = std::find(searchStart, currentRoute.end(), e);
1505 onRoute = edgePos != currentRoute.end(); // no? -> onRoute is false
1506 if (edgePos == currentRoute.end() - 1 && currentRouteEdge == e) {
1507 // onRoute is false as well if the vehicle is beyond the edge
1508 onRoute &= currentRouteEdge->getLength() > currentLanePos + SPEED2DIST(speed);
1509 }
1510 // save prior and next edges
1511 prevEdge = e;
1512 nextEdge = !onRoute || edgePos == currentRoute.end() - 1 ? nullptr : *(edgePos + 1);
1513#ifdef DEBUG_MOVEXY_ANGLE
1514 std::cout << "normal:" << e->getID() << " prev:" << Named::getIDSecure(prevEdge) << " next:" << Named::getIDSecure(nextEdge) << "\n";
1515#endif
1516 } else if (e->isInternal()) {
1517 // an internal edge
1518 // get the previous edge
1519 prevEdge = e;
1520 while (prevEdge != nullptr && prevEdge->isInternal()) {
1521 MSLane* l = prevEdge->getLanes()[0];
1523 prevEdge = l == nullptr ? nullptr : &l->getEdge();
1524 }
1525 // check whether the previous edge is on the route (was on the route)
1526 ConstMSEdgeVector::const_iterator prevEdgePos = std::find(currentRoute.begin() + routePosition, currentRoute.end(), prevEdge);
1527 nextEdge = e;
1528 while (nextEdge != nullptr && nextEdge->isInternal()) {
1529 nextEdge = nextEdge->getSuccessors()[0]; // should be only one for an internal edge
1530 }
1531 if (prevEdgePos != currentRoute.end() && (prevEdgePos + 1) != currentRoute.end()) {
1532 onRoute = *(prevEdgePos + 1) == nextEdge;
1533 }
1534#ifdef DEBUG_MOVEXY_ANGLE
1535 std::cout << "internal:" << e->getID() << " prev:" << Named::getIDSecure(prevEdge) << " next:" << Named::getIDSecure(nextEdge) << "\n";
1536#endif
1537 }
1538
1539
1540 // weight the lanes...
1541 const bool perpendicular = false;
1542 for (MSLane* const l : e->getLanes()) {
1543 if (!l->allowsVehicleClass(vClass)) {
1544 continue;
1545 }
1546 if (l->getShape().length() == 0) {
1547 // mapping to shapeless lanes is a bad idea
1548 continue;
1549 }
1550 double langle = 180.;
1551 double dist = FAR_AWAY;
1552 double perpendicularDist = FAR_AWAY;
1553 // add some slack to avoid issues from tiny gaps between consecutive lanes
1554 // except when simulating with high precision where the slack can throw of mapping
1555 const double slack = POSITION_EPS * TS;
1556 PositionVector laneShape = l->getShape();
1557 laneShape.extrapolate2D(slack);
1558 double off = laneShape.nearest_offset_to_point2D(pos, true);
1559 if (off != GeomHelper::INVALID_OFFSET) {
1560 perpendicularDist = laneShape.distance2D(pos, true);
1561 }
1562 off = l->getShape().nearest_offset_to_point2D(pos, perpendicular);
1563 if (off != GeomHelper::INVALID_OFFSET) {
1564 dist = l->getShape().distance2D(pos, perpendicular);
1565 langle = GeomHelper::naviDegree(l->getShape().rotationAtOffset(off));
1566 }
1567 // cannot trust lanePos on walkingArea
1568 bool sameEdge = onRoad && e == &currentLane->getEdge() && currentRouteEdge->getLength() > currentLanePos + SPEED2DIST(speed) && !e->isWalkingArea();
1569 /*
1570 const MSEdge* rNextEdge = nextEdge;
1571 while(rNextEdge==0&&lane->getEdge().getPurpose()==MSEdge::EDGEFUNCTION_INTERNAL) {
1572 MSLane* next = lane->getLinkCont()[0]->getLane();
1573 rNextEdge = next == 0 ? 0 : &next->getEdge();
1574 }
1575 */
1576 double dist2 = dist;
1577 if (mayLeaveNetwork && fabs(dist - perpendicularDist) > slack) {
1578 // ambiguous mapping. Don't trust this
1579 dist2 = FAR_AWAY;
1580 }
1581 const double angleDiff = (angle == INVALID_DOUBLE_VALUE || l->getEdge().isWalkingArea() ? 0 : GeomHelper::getMinAngleDiff(angle, langle));
1582#ifdef DEBUG_MOVEXY_ANGLE
1583 std::cout << std::setprecision(gPrecision)
1584 << " candLane=" << l->getID() << " lAngle=" << langle << " lLength=" << l->getLength()
1585 << " angleDiff=" << angleDiff
1586 << " off=" << off
1587 << " pDist=" << perpendicularDist
1588 << " dist=" << dist
1589 << " dist2=" << dist2
1590 << "\n";
1591 std::cout << l->getID() << " param=" << l->getParameter(SUMO_PARAM_ORIGID, "") << " origID='" << origID << "\n";
1592#endif
1593
1594 bool origIDMatch = l->getParameter(SUMO_PARAM_ORIGID, l->getID()) == origID;
1595 if (origIDMatch && setLateralPos
1596 && perpendicularDist > l->getWidth() / 2) {
1597 origIDMatch = false;
1598 }
1599 lane2utility.emplace(l, LaneUtility(
1600 dist2, perpendicularDist, off, angleDiff,
1601 origIDMatch,
1602 onRoute, sameEdge, prevEdge, nextEdge));
1603 // update scaling value
1604 maxDist = MAX2(maxDist, MIN2(dist, SUMO_const_laneWidth));
1605
1606 }
1607 }
1608
1609 // get the best lane given the previously computed values
1610 double bestValue = 0;
1611 MSLane* bestLane = nullptr;
1612 for (const auto& it : lane2utility) {
1613 MSLane* const l = it.first;
1614 const LaneUtility& u = it.second;
1615 double distN = u.dist > 999 ? -10 : 1. - (u.dist / maxDist);
1616 double angleDiffN = 1. - (u.angleDiff / 180.);
1617 double idN = u.ID ? 1 : 0;
1618 double onRouteN = u.onRoute ? 1 : 0;
1619 double sameEdgeN = u.sameEdge ? MIN2(currentRouteEdge->getLength() / MAX2(NUMERICAL_EPS, speed), (double)1.) : 0;
1620 // distance is more important than angle because the vehicle might be driving in the opposite direction
1621 // also, distance becomes increasingly more important with lower step lengths (because position errors from one step to the next can result in large acceleration/speed errors)
1622 double value = (distN * .5 / TS
1623 + angleDiffN * 0.35 /*.5 */
1624 + idN * 1
1625 + onRouteN * 0.1
1626 + sameEdgeN * 0.1);
1627#ifdef DEBUG_MOVEXY
1628 std::cout << " x; l:" << l->getID() << " d:" << u.dist << " dN:" << distN << " aD:" << angleDiffN <<
1629 " ID:" << idN << " oRN:" << onRouteN << " sEN:" << sameEdgeN << " value:" << value << std::endl;
1630#endif
1631 if (value > bestValue || bestLane == nullptr) {
1632 bestValue = value;
1633 if (u.dist == FAR_AWAY) {
1634 bestLane = nullptr;
1635 } else {
1636 bestLane = l;
1637 }
1638 }
1639 }
1640 // no best lane found, return
1641 if (bestLane == nullptr) {
1642 return false;
1643 }
1644 const LaneUtility& u = lane2utility.find(bestLane)->second;
1645 bestDistance = u.dist;
1646 *lane = bestLane;
1647 lanePos = MAX2(0., MIN2(double((*lane)->getLength() - NUMERICAL_EPS),
1649 bestLane->getShape().nearest_offset_to_point25D(pos, false))));
1650 const MSEdge* prevEdge = u.prevEdge;
1651 if (u.onRoute) {
1652 ConstMSEdgeVector::const_iterator prevEdgePos = std::find(currentRoute.begin(), currentRoute.end(), prevEdge);
1653 routeOffset = (int)std::distance(currentRoute.begin(), prevEdgePos);
1654 //std::cout << SIMTIME << "moveToXYMap vehicle=" << veh.getID() << " currLane=" << veh.getLane()->getID() << " routeOffset=" << routeOffset << " edges=" << toString(ev) << " bestLane=" << bestLane->getID() << " prevEdge=" << prevEdge->getID() << "\n";
1655 } else {
1656 edges.push_back(u.prevEdge);
1657 /*
1658 if(bestLane->getEdge().getPurpose()!=MSEdge::EDGEFUNCTION_INTERNAL) {
1659 edges.push_back(&bestLane->getEdge());
1660 }
1661 */
1662 if (u.nextEdge != nullptr) {
1663 edges.push_back(u.nextEdge);
1664 }
1665 routeOffset = 0;
1666#ifdef DEBUG_MOVEXY_ANGLE
1667 std::cout << SIMTIME << " internal2: lane=" << bestLane->getID() << " prev=" << Named::getIDSecure(u.prevEdge) << " next=" << Named::getIDSecure(u.nextEdge) << "\n";
1668#endif
1669 }
1670 return true;
1671}
1672
1673
1674bool
1675Helper::findCloserLane(const MSEdge* edge, const Position& pos, SUMOVehicleClass vClass, double& bestDistance, MSLane** lane) {
1676 // TODO maybe there is a way to abort this early if the lane already found is good enough but simply
1677 // checking for bestDistance < POSITON_EPS gives ugly order dependencies (#7933), so think twice and profile first
1678 if (edge == nullptr) {
1679 return false;
1680 }
1681 bool newBest = false;
1682 for (MSLane* const candidateLane : edge->getLanes()) {
1683 if (!candidateLane->allowsVehicleClass(vClass)) {
1684 continue;
1685 }
1686 if (candidateLane->getShape().length() == 0) {
1687 // mapping to shapeless lanes is a bad idea
1688 continue;
1689 }
1690 const double dist = candidateLane->getShape().distance2D(pos); // get distance
1691#ifdef DEBUG_MOVEXY
1692 std::cout << " b at lane " << candidateLane->getID() << " dist:" << dist << " best:" << bestDistance << std::endl;
1693#endif
1694 if (dist < bestDistance || (dist == bestDistance && candidateLane->getNumericalID() < (*lane)->getNumericalID())) {
1695 // is the new distance the best one? keep then...
1696 bestDistance = dist;
1697 *lane = candidateLane;
1698 newBest = true;
1699 }
1700 }
1701 return newBest;
1702}
1703
1704
1705bool
1706Helper::moveToXYMap_matchingRoutePosition(const Position& pos, const std::string& origID,
1707 const ConstMSEdgeVector& currentRoute, int routeIndex,
1708 SUMOVehicleClass vClass, bool setLateralPos,
1709 double& bestDistance, MSLane** lane, double& lanePos, int& routeOffset) {
1710#ifdef DEBUG_MOVEXY
1711 std::cout << SIMTIME << " moveToXYMap_matchingRoutePosition pos=" << pos << " vClass=" << toString(vClass) << "\n";
1712#endif
1713 //std::cout << "moveToXYMap_matchingRoutePosition pos=" << pos << "\n";
1714 routeOffset = 0;
1715 // routes may be looped which makes routeOffset ambiguous. We first try to
1716 // find the closest upcoming edge on the route and then look for closer passed edges
1717
1718 // look forward along the route
1719 const MSEdge* prev = nullptr;
1720 for (int i = routeIndex; i < (int)currentRoute.size(); ++i) {
1721 const MSEdge* cand = currentRoute[i];
1722 while (prev != nullptr) {
1723 // check internal edge(s)
1724 const MSEdge* internalCand = prev->getInternalFollowingEdge(cand, vClass);
1725#ifdef DEBUG_MOVEXY
1726 std::cout << SIMTIME << " prev=" << Named::getIDSecure(prev) << " cand=" << Named::getIDSecure(cand) << " internal=" << Named::getIDSecure(internalCand) << "\n";
1727#endif
1728 findCloserLane(internalCand, pos, vClass, bestDistance, lane);
1729 prev = internalCand;
1730 }
1731 if (findCloserLane(cand, pos, vClass, bestDistance, lane)) {
1732 routeOffset = i;
1733 }
1734 prev = cand;
1735 }
1736 // look backward along the route
1737 const MSEdge* next = currentRoute[routeIndex];
1738 for (int i = routeIndex; i >= 0; --i) {
1739 const MSEdge* cand = currentRoute[i];
1740 //std::cout << " next=" << next->getID() << " cand=" << cand->getID() << " i=" << i << " routeIndex=" << routeIndex << "\n";
1741 prev = cand;
1742 while (prev != nullptr) {
1743 // check internal edge(s)
1744 const MSEdge* internalCand = prev->getInternalFollowingEdge(next, vClass);
1745 if (findCloserLane(internalCand, pos, vClass, bestDistance, lane)) {
1746 routeOffset = i;
1747 }
1748 prev = internalCand;
1749 }
1750 if (findCloserLane(cand, pos, vClass, bestDistance, lane)) {
1751 routeOffset = i;
1752 }
1753 next = cand;
1754 }
1755 if (vClass == SVC_PEDESTRIAN) {
1756 // consider all crossings and walkingareas along the route
1757 std::map<const MSJunction*, int> routeJunctions;
1758 for (int i = 0; i < (int)currentRoute.size() - 1; ++i) {
1759 routeJunctions[currentRoute[i]->getToJunction()] = i;
1760 }
1761 std::set<const Named*> into;
1762 PositionVector shape;
1763 shape.push_back(pos);
1765 for (const Named* named : into) {
1766 const MSLane* cand = dynamic_cast<const MSLane*>(named);
1767 if ((cand->getEdge().isWalkingArea() || cand->getEdge().isCrossing())
1768 && routeJunctions.count(cand->getEdge().getToJunction()) != 0) {
1769 if (findCloserLane(&cand->getEdge(), pos, vClass, bestDistance, lane)) {
1770 routeOffset = routeJunctions[cand->getEdge().getToJunction()];
1771 }
1772 }
1773 }
1774 }
1775
1776 assert(lane != 0);
1777 // quit if no solution was found, reporting a failure
1778 if (lane == nullptr) {
1779#ifdef DEBUG_MOVEXY
1780 std::cout << " b failed - no best route lane" << std::endl;
1781#endif
1782 return false;
1783 }
1784
1785
1786 // position may be inaccurate; let's check the given index, too
1787 // @note: this is enabled for non-internal lanes only, as otherwise the position information may ambiguous
1788 if (!(*lane)->getEdge().isInternal()) {
1789 const std::vector<MSLane*>& lanes = (*lane)->getEdge().getLanes();
1790 for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
1791 if ((*i)->getParameter(SUMO_PARAM_ORIGID, (*i)->getID()) == origID) {
1792 if (setLateralPos) {
1793 // vehicle might end up on top of another lane with a big
1794 // lateral offset to the lane with origID.
1795 const double dist = (*i)->getShape().distance2D(pos); // get distance
1796 if (dist < (*i)->getWidth() / 2) {
1797 *lane = *i;
1798 break;
1799 }
1800 } else {
1801 *lane = *i;
1802 break;
1803 }
1804 }
1805 }
1806 }
1807 // check position, stuff, we should have the best lane along the route
1808 lanePos = MAX2(0., MIN2(double((*lane)->getLength() - NUMERICAL_EPS),
1809 (*lane)->interpolateGeometryPosToLanePos(
1810 (*lane)->getShape().nearest_offset_to_point25D(pos, false))));
1811 //std::cout << SIMTIME << " moveToXYMap_matchingRoutePosition vehicle=" << veh.getID() << " currLane=" << veh.getLane()->getID() << " routeOffset=" << routeOffset << " edges=" << toString(edges) << " lane=" << (*lane)->getID() << "\n";
1812#ifdef DEBUG_MOVEXY
1813 std::cout << " b ok lane " << (*lane)->getID() << " lanePos:" << lanePos << std::endl;
1814#endif
1815 return true;
1816}
1817
1818
1820 : VariableWrapper(handler), myResults(into), myContextResults(context), myActiveResults(&into) {
1821
1822}
1823
1824
1825void
1826Helper::SubscriptionWrapper::setContext(const std::string* const refID) {
1827 myActiveResults = refID == nullptr ? &myResults : &myContextResults[*refID];
1828}
1829
1830
1831void
1833 myActiveResults = &myResults;
1834 myResults.clear();
1835 myContextResults.clear();
1836}
1837
1838
1839bool
1840Helper::SubscriptionWrapper::wrapDouble(const std::string& objID, const int variable, const double value) {
1841 (*myActiveResults)[objID][variable] = std::make_shared<TraCIDouble>(value);
1842 return true;
1843}
1844
1845
1846bool
1847Helper::SubscriptionWrapper::wrapInt(const std::string& objID, const int variable, const int value) {
1848 (*myActiveResults)[objID][variable] = std::make_shared<TraCIInt>(value);
1849 return true;
1850}
1851
1852
1853bool
1854Helper::SubscriptionWrapper::wrapString(const std::string& objID, const int variable, const std::string& value) {
1855 (*myActiveResults)[objID][variable] = std::make_shared<TraCIString>(value);
1856 return true;
1857}
1858
1859
1860bool
1861Helper::SubscriptionWrapper::wrapStringList(const std::string& objID, const int variable, const std::vector<std::string>& value) {
1862 auto sl = std::make_shared<TraCIStringList>();
1863 sl->value = value;
1864 (*myActiveResults)[objID][variable] = sl;
1865 return true;
1866}
1867
1868
1869bool
1870Helper::SubscriptionWrapper::wrapDoubleList(const std::string& objID, const int variable, const std::vector<double>& value) {
1871 auto sl = std::make_shared<TraCIDoubleList>();
1872 sl->value = value;
1873 (*myActiveResults)[objID][variable] = sl;
1874 return true;
1875}
1876
1877
1878bool
1879Helper::SubscriptionWrapper::wrapPosition(const std::string& objID, const int variable, const TraCIPosition& value) {
1880 (*myActiveResults)[objID][variable] = std::make_shared<TraCIPosition>(value);
1881 return true;
1882}
1883
1884
1885bool
1886Helper::SubscriptionWrapper::wrapPositionVector(const std::string& objID, const int variable, const TraCIPositionVector& value) {
1887 (*myActiveResults)[objID][variable] = std::make_shared<TraCIPositionVector>(value);
1888 return true;
1889}
1890
1891
1892bool
1893Helper::SubscriptionWrapper::wrapColor(const std::string& objID, const int variable, const TraCIColor& value) {
1894 (*myActiveResults)[objID][variable] = std::make_shared<TraCIColor>(value);
1895 return true;
1896}
1897
1898
1899bool
1900Helper::SubscriptionWrapper::wrapStringDoublePair(const std::string& objID, const int variable, const std::pair<std::string, double>& value) {
1901 (*myActiveResults)[objID][variable] = std::make_shared<TraCIRoadPosition>(value.first, value.second);
1902 return true;
1903}
1904
1905
1906bool
1907Helper::SubscriptionWrapper::wrapStringPair(const std::string& objID, const int variable, const std::pair<std::string, std::string>& value) {
1908 auto sl = std::make_shared<TraCIStringList>();
1909 sl->value.push_back(value.first);
1910 sl->value.push_back(value.second);
1911 (*myActiveResults)[objID][variable] = sl;
1912 return true;
1913}
1914
1915
1916void
1917Helper::SubscriptionWrapper::empty(const std::string& objID) {
1918 (*myActiveResults)[objID]; // initiate the empty map to track the objectID for TRACI_ID_LIST context subscriptions
1919}
1920
1921
1922void
1923Helper::VehicleStateListener::vehicleStateChanged(const SUMOVehicle* const vehicle, MSNet::VehicleState to, const std::string& /*info*/) {
1924 myVehicleStateChanges[to].push_back(vehicle->getID());
1925}
1926
1927
1928void
1930 myTransportableStateChanges[to].push_back(transportable->getID());
1931}
1932
1933
1934}
1935
1936
1937/****************************************************************************/
long long int SUMOTime
Definition: GUI.h:36
#define DEG2RAD(x)
Definition: GeomHelper.h:35
#define RAD2DEG(x)
Definition: GeomHelper.h:36
#define FAR_AWAY
Definition: Helper.cpp:72
std::vector< const MSEdge * > ConstMSEdgeVector
Definition: MSEdge.h:74
#define LANE_RTREE_QUAL
Definition: MSLane.h:1682
#define WRITE_WARNINGF(...)
Definition: MsgHandler.h:266
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:265
#define TL(string)
Definition: MsgHandler.h:282
#define STEPS2TIME(x)
Definition: SUMOTime.h:54
#define SPEED2DIST(x)
Definition: SUMOTime.h:44
#define SIMSTEP
Definition: SUMOTime.h:60
#define SUMOTime_MAX
Definition: SUMOTime.h:33
#define TS
Definition: SUMOTime.h:41
#define SIMTIME
Definition: SUMOTime.h:61
#define TIME2STEPS(x)
Definition: SUMOTime.h:56
SUMOVehicleClass
Definition of vehicle classes to differ between different lane usage and authority types.
@ SVC_PEDESTRIAN
pedestrian
const int STOP_DURATION_SET
const int STOP_UNTIL_SET
const int STOP_PARKING_SET
const int STOP_START_SET
const int STOP_CONTAINER_TRIGGER_SET
const int STOP_INDEX_FIT
const int STOP_TRIGGER_SET
const int STOP_END_SET
const std::string SUMO_PARAM_ORIGID
SumoXMLTag
Numbers representing SUMO-XML - element names.
@ SUMO_TAG_CHARGING_STATION
A Charging Station.
@ SUMO_TAG_NOTHING
invalid tag
@ SUMO_TAG_CONTAINER_STOP
A container stop.
@ SUMO_TAG_BUS_STOP
A bus stop.
@ SUMO_TAG_PARKING_AREA
A parking area.
@ SUMO_TAG_OVERHEAD_WIRE_SEGMENT
An overhead wire segment.
int gPrecision
the precision for floating point outputs
Definition: StdDefs.cpp:25
const double SUMO_const_laneWidth
Definition: StdDefs.h:48
T MIN2(T a, T b)
Definition: StdDefs.h:71
T MAX2(T a, T b)
Definition: StdDefs.h:77
T MAX3(T a, T b, T c)
Definition: StdDefs.h:91
std::string toHex(const T i, std::streamsize numDigits=0)
Definition: ToString.h:56
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:46
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:39
double ymin() const
Returns minimum y-coordinate.
Definition: Boundary.cpp:130
double xmin() const
Returns minimum x-coordinate.
Definition: Boundary.cpp:118
Boundary & grow(double by)
extends the boundary by the given amount
Definition: Boundary.cpp:300
double distanceTo2D(const Position &p) const
returns the euclidean distance in the x-y-plane
Definition: Boundary.cpp:222
double getHeight() const
Returns the height of the boundary (y-axis)
Definition: Boundary.cpp:160
double getWidth() const
Returns the width of the boudary (x-axis)
Definition: Boundary.cpp:154
double ymax() const
Returns maximum y-coordinate.
Definition: Boundary.cpp:136
double xmax() const
Returns maximum x-coordinate.
Definition: Boundary.cpp:124
static const GeoConvHelper & getFinal()
the coordinate transformation for writing the location element and for tracking the original coordina...
const Boundary & getConvBoundary() const
Returns the converted boundary.
static const double INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
Definition: GeomHelper.h:50
static double naviDegree(const double angle)
Definition: GeomHelper.cpp:192
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
Definition: GeomHelper.cpp:179
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
Definition: GeomHelper.cpp:173
The base class for microscopic and mesoscopic vehicles.
Definition: MSBaseVehicle.h:55
double getLength() const
Returns the vehicle's length.
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
Calibrates the flow on a segment to a specified one.
Definition: MSCalibrator.h:47
const MSLane * getLane() const
Definition: MSCalibrator.h:111
AspiredState getCurrentStateInterval() const
static const std::map< std::string, MSCalibrator * > & getInstances()
return all calibrator instances
Definition: MSCalibrator.h:92
A simple description of a position on a lane (crossing of a lane)
A detector of vehicles passing an area between entry/exit points.
Definition: MSE3Collector.h:59
const CrossSectionVector & getEntries() const
Returns the entry cross sections.
const CrossSectionVector & getExits() const
Returns the exit cross sections.
A road/street connecting two junctions.
Definition: MSEdge.h:77
bool isCrossing() const
return whether this edge is a pedestrian crossing
Definition: MSEdge.h:270
SVCPermissions getPermissions() const
Returns the combined permissions of all lanes of this edge.
Definition: MSEdge.h:622
bool isWalkingArea() const
return whether this edge is walking area
Definition: MSEdge.h:284
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
Definition: MSEdge.h:168
const MSEdge * getOppositeEdge() const
Returns the opposite direction edge if on exists else a nullptr.
Definition: MSEdge.cpp:1236
bool isNormal() const
return whether this edge is an internal edge
Definition: MSEdge.h:260
const MSJunction * getToJunction() const
Definition: MSEdge.h:415
double getLength() const
return the length of the edge
Definition: MSEdge.h:658
const MSJunction * getFromJunction() const
Definition: MSEdge.h:411
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:265
static bool dictionary(const std::string &id, MSEdge *edge)
Inserts edge into the static dictionary Returns true if the key id isn't already in the dictionary....
Definition: MSEdge.cpp:945
const MSEdgeVector & getPredecessors() const
Definition: MSEdge.h:406
const MSEdge * getInternalFollowingEdge(const MSEdge *followerAfterInternal, SUMOVehicleClass vClass) const
Definition: MSEdge.cpp:798
const MSEdgeVector & getSuccessors(SUMOVehicleClass vClass=SVC_IGNORING) const
Returns the following edges, restricted by vClass.
Definition: MSEdge.cpp:1155
The base class for an intersection.
Definition: MSJunction.h:58
Representation of a lane in the micro simulation.
Definition: MSLane.h:84
std::pair< MSVehicle *const, double > getFollower(const MSVehicle *ego, double egoPos, double dist, MinorLinkMode mLinkMode) const
Find follower vehicle for the given ego vehicle (which may be on the opposite direction lane)
Definition: MSLane.cpp:4022
MSLane * getParallelLane(int offset, bool includeOpposite=true) const
Returns the lane with the given offset parallel to this one or 0 if it does not exist.
Definition: MSLane.cpp:2521
void visit(const MSLane::StoringVisitor &cont) const
Callback for visiting the lane when traversing an RTree.
Definition: MSLane.h:1258
static void fill(RTREE &into)
Fills the given RTree with lane instances.
Definition: MSLane.cpp:2239
std::set< MSVehicle * > getVehiclesInRange(const double a, const double b) const
Returns all vehicles on the lane overlapping with the interval [a,b].
Definition: MSLane.cpp:3926
double getLength() const
Returns the lane's length.
Definition: MSLane.h:575
const PositionVector & getShape() const
Returns this lane's shape.
Definition: MSLane.h:506
std::vector< const MSLink * > getUpcomingLinks(double pos, double range, const std::vector< MSLane * > &contLanes) const
Returns all upcoming links within given range along the given (non-internal) continuation lanes measu...
Definition: MSLane.cpp:3957
std::set< MSVehicle * > getSurroundingVehicles(double startPos, double downstreamDist, double upstreamDist, std::shared_ptr< LaneCoverageInfo > checkedLanes) const
Returns all vehicles closer than downstreamDist along the road network starting on the given position...
Definition: MSLane.cpp:3874
bool allowsVehicleClass(SUMOVehicleClass vclass) const
Definition: MSLane.h:856
int getIndex() const
Returns the lane's index.
Definition: MSLane.h:597
double getOppositePos(double pos) const
return the corresponding position on the opposite lane
Definition: MSLane.cpp:4017
MSLane * getLogicalPredecessorLane() const
get the most likely precedecessor lane (sorted using by_connections_to_sorter). The result is cached ...
Definition: MSLane.cpp:2866
bool isInternal() const
Definition: MSLane.cpp:2330
double interpolateGeometryPosToLanePos(double geometryPos) const
Definition: MSLane.h:539
std::pair< MSVehicle *const, double > getLeader(const MSVehicle *veh, const double vehPos, const std::vector< MSLane * > &bestLaneConts, double dist=-1, bool checkTmpVehicles=false) const
Returns the immediate leader of veh and the distance to veh starting on this lane.
Definition: MSLane.cpp:2589
virtual const VehCont & getVehiclesSecure() const
Returns the vehicles container; locks it for microsimulation.
Definition: MSLane.h:456
virtual void releaseVehicles() const
Allows to use the container for microsimulation again.
Definition: MSLane.h:486
MSLane * getParallelOpposite() const
return the opposite direction lane of this lanes edge or nullptr
Definition: MSLane.cpp:4011
MSEdge & getEdge() const
Returns the lane's edge.
Definition: MSLane.h:713
VehicleState
Definition of a vehicle state.
Definition: MSNet.h:603
@ ARRIVED
The vehicle arrived at his destination (is deleted)
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:183
MSTLLogicControl & getTLSControl()
Returns the tls logics control.
Definition: MSNet.h:452
SUMOAbstractRouter< MSEdge, SUMOVehicle > & getRouterTT(const int rngIndex, const MSEdgeVector &prohibited=MSEdgeVector()) const
Definition: MSNet.cpp:1389
static bool hasInstance()
Returns whether the network was already constructed.
Definition: MSNet.h:154
MSStoppingPlace * getStoppingPlace(const std::string &id, const SumoXMLTag category) const
Returns the named stopping place of the given category.
Definition: MSNet.cpp:1287
void addVehicleStateListener(VehicleStateListener *listener)
Adds a vehicle states listener.
Definition: MSNet.cpp:1164
void addTransportableStateListener(TransportableStateListener *listener)
Adds a transportable states listener.
Definition: MSNet.cpp:1192
TransportableState
Definition of a transportable state.
Definition: MSNet.h:680
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
Definition: MSNet.h:379
virtual MSTransportableControl & getPersonControl()
Returns the person control.
Definition: MSNet.cpp:1096
const NamedObjectCont< MSStoppingPlace * > & getStoppingPlaces(SumoXMLTag category) const
Definition: MSNet.cpp:1310
void setRemoteControlled(Position xyPos, MSLane *l, double pos, double posLat, double angle, int edgeOffset, const ConstMSEdgeVector &route, SUMOTime t)
Definition: MSPerson.cpp:631
Influencer & getInfluencer()
Returns the velocity/lane influencer.
Definition: MSPerson.cpp:606
double getDistanceBetween(double fromPos, double toPos, const MSEdge *fromEdge, const MSEdge *toEdge, bool includeInternal=true, int routePosition=0) const
Compute the distance between 2 given edges on this route, including the length of internal lanes....
Definition: MSRoute.cpp:316
A lane area vehicles can halt at.
double getBeginLanePosition() const
Returns the begin position of this stop.
double getEndLanePosition() const
Returns the end position of this stop.
const MSLane & getLane() const
Returns the lane this stop is located at.
Storage for all programs of a single tls.
TLSLogicVariants & get(const std::string &id) const
Returns the variants of a named tls.
MSTransportable * get(const std::string &id) const
Returns the named transportable, if existing.
void setRemoteControlled(Position xyPos, MSLane *l, double pos, double posLat, double angle, int edgeOffset, const ConstMSEdgeVector &route, SUMOTime t)
Definition: MSVehicle.cpp:792
SUMOVehicle * getVehicle(const std::string &id) const
Returns the vehicle with the given id.
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:77
const std::vector< const MSLane * > getUpcomingLanesUntil(double distance) const
Returns the upcoming (best followed by default 0) sequence of lanes to continue the route starting at...
Definition: MSVehicle.cpp:5876
bool isOnRoad() const
Returns the information whether the vehicle is on a road (is simulated)
Definition: MSVehicle.h:599
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:5367
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:5848
const std::vector< const MSLane * > getPastLanesUntil(double distance) const
Returns the sequence of past lanes (right-most on edge) based on the route starting at the current la...
Definition: MSVehicle.cpp:5945
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:577
Influencer & getInfluencer()
Definition: MSVehicle.cpp:6773
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
Definition: MSVehicle.h:416
double getPositionOnLane() const
Get the vehicle's position along the lane.
Definition: MSVehicle.h:377
The car-following model and parameter.
Definition: MSVehicleType.h:63
SUMOVehicleClass getVehicleClass() const
Get this vehicle type's vehicle class.
const std::string & getID() const
Returns the name of the vehicle type.
Definition: MSVehicleType.h:91
double getLength() const
Get vehicle's length [m].
Allows to store the object; used as context while traveling the rtree in TraCI.
Definition: Named.h:90
Base class for objects which have an id.
Definition: Named.h:54
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
Definition: Named.h:67
const std::string & getID() const
Returns the id.
Definition: Named.h:74
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:37
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:252
double x() const
Returns the x-position.
Definition: Position.h:55
double z() const
Returns the z-position.
Definition: Position.h:65
double angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position
Definition: Position.h:262
double y() const
Returns the y-position.
Definition: Position.h:60
A list of positions.
void append(const PositionVector &v, double sameThreshold=2.0)
double length() const
Returns the length.
Position positionAtOffset(double pos, double lateralOffset=0) const
Returns the position at the given length.
double distance2D(const Position &p, bool perpendicular=false) const
closest 2D-distance to point p (or -1 if perpendicular is true and the point is beyond this vector)
void prepend(const PositionVector &v, double sameThreshold=2.0)
double nearest_offset_to_point2D(const Position &p, bool perpendicular=true) const
return the nearest offest to point 2D
std::pair< PositionVector, PositionVector > splitAt(double where, bool use2D=false) const
Returns the two lists made when this list vector is splitted at the given point.
void move2side(double amount, double maxExtension=100)
move position vector to side using certain ammount
Boundary getBoxBoundary() const
Returns a boundary enclosing this list of lines.
void extrapolate2D(const double val, const bool onlyFirst=false)
extrapolate position vector in two dimensions (Z is ignored)
double nearest_offset_to_point25D(const Position &p, bool perpendicular=true) const
return the nearest offest to point 2D projected onto the 3D geometry
unsigned char red() const
Returns the red-amount of the color.
Definition: RGBColor.cpp:74
unsigned char alpha() const
Returns the alpha-amount of the color.
Definition: RGBColor.cpp:92
unsigned char green() const
Returns the green-amount of the color.
Definition: RGBColor.cpp:80
unsigned char blue() const
Returns the blue-amount of the color.
Definition: RGBColor.cpp:86
virtual bool compute(const E *from, const E *to, const V *const vehicle, SUMOTime msTime, std::vector< const E * > &into, bool silent=false)=0
Builds the route between the given edges using the minimum effort at the given time The definition of...
Representation of a vehicle, person, or container.
virtual bool isVehicle() const
Whether it is a vehicle.
virtual const MSLane * getLane() const =0
Returns the lane the object is currently at.
virtual Position getPosition(const double offset=0) const =0
Return current position (x/y, cartesian)
virtual const MSEdge * getEdge() const =0
Returns the edge the object is currently at.
Representation of a vehicle.
Definition: SUMOVehicle.h:60
virtual double getAngle() const =0
Get the vehicle's angle.
Definition of vehicle stop (position and duration)
int getFlags() const
return flags as per Vehicle::getStops
SUMOTime started
the time at which this stop was reached
std::string edge
The edge to stop at (used only in NETEDIT)
ParkingType parking
whether the vehicle is removed from the net while stopping
std::string lane
The lane to stop at.
double speed
the speed at which this stop counts as reached (waypoint mode)
std::string parkingarea
(Optional) parking area if one is assigned to the stop
std::string split
the id of the vehicle (train portion) that splits of upon reaching this stop
double startPos
The stopping position start.
std::string line
the new line id of the trip within a cyclical public transport route
std::string chargingStation
(Optional) charging station if one is assigned to the stop
std::string overheadWireSegment
(Optional) overhead line segment if one is assigned to the stop
int parametersSet
Information for the output which parameter were set.
int index
at which position in the stops list
std::string join
the id of the vehicle (train portion) to which this vehicle shall be joined
SUMOTime until
The time at which the vehicle may continue its journey.
std::string actType
act Type (only used by Persons) (used by NETEDIT)
bool triggered
whether an arriving person lets the vehicle continue
SUMOTime ended
the time at which this stop was ended
double endPos
The stopping position end.
std::string busstop
(Optional) bus stop if one is assigned to the stop
std::string tripId
id of the trip within a cyclical public transport route
std::string containerstop
(Optional) container stop if one is assigned to the stop
bool containerTriggered
whether an arriving container lets the vehicle continue
SUMOTime arrival
The (expected) time at which the vehicle reaches the stop.
SUMOTime duration
The stopping duration.
const MSEdge * nextEdge
Definition: Helper.h:205
const MSEdge * prevEdge
Definition: Helper.h:204
bool wrapDouble(const std::string &objID, const int variable, const double value)
Definition: Helper.cpp:1840
void empty(const std::string &objID)
Definition: Helper.cpp:1917
bool wrapPositionVector(const std::string &objID, const int variable, const TraCIPositionVector &value)
Definition: Helper.cpp:1886
bool wrapColor(const std::string &objID, const int variable, const TraCIColor &value)
Definition: Helper.cpp:1893
bool wrapInt(const std::string &objID, const int variable, const int value)
Definition: Helper.cpp:1847
bool wrapStringList(const std::string &objID, const int variable, const std::vector< std::string > &value)
Definition: Helper.cpp:1861
bool wrapPosition(const std::string &objID, const int variable, const TraCIPosition &value)
Definition: Helper.cpp:1879
bool wrapString(const std::string &objID, const int variable, const std::string &value)
Definition: Helper.cpp:1854
bool wrapStringPair(const std::string &objID, const int variable, const std::pair< std::string, std::string > &value)
Definition: Helper.cpp:1907
bool wrapStringDoublePair(const std::string &objID, const int variable, const std::pair< std::string, double > &value)
Definition: Helper.cpp:1900
SubscriptionWrapper(VariableWrapper::SubscriptionHandler handler, SubscriptionResults &into, ContextSubscriptionResults &context)
Definition: Helper.cpp:1819
void setContext(const std::string *const refID)
Definition: Helper.cpp:1826
bool wrapDoubleList(const std::string &objID, const int variable, const std::vector< double > &value)
Definition: Helper.cpp:1870
void transportableStateChanged(const MSTransportable *const transportable, MSNet::TransportableState to, const std::string &info="")
Called if a transportable changes its state.
Definition: Helper.cpp:1929
std::map< MSNet::TransportableState, std::vector< std::string > > myTransportableStateChanges
Changes in the states of simulated transportables.
Definition: Helper.h:258
std::map< MSNet::VehicleState, std::vector< std::string > > myVehicleStateChanges
Changes in the states of simulated vehicles.
Definition: Helper.h:251
void vehicleStateChanged(const SUMOVehicle *const vehicle, MSNet::VehicleState to, const std::string &info="")
Called if a vehicle changes its state.
Definition: Helper.cpp:1923
static Position makePosition(const TraCIPosition &position)
Definition: Helper.cpp:384
static MSEdge * getEdge(const std::string &edgeID)
Definition: Helper.cpp:390
static void cleanup()
Definition: Helper.cpp:674
static double getDrivingDistance(std::pair< const MSLane *, double > &roadPos1, std::pair< const MSLane *, double > &roadPos2)
Definition: Helper.cpp:452
static void collectObjectsInRange(int domain, const PositionVector &shape, double range, std::set< const Named * > &into)
Definition: Helper.cpp:811
static MSCalibrator::AspiredState getCalibratorState(const MSCalibrator *c)
Definition: Helper.cpp:721
static TraCIPosition makeTraCIPosition(const Position &position, const bool includeZ=false)
Definition: Helper.cpp:374
static LANE_RTREE_QUAL * myLaneTree
A lookup tree of lanes.
Definition: Helper.h:277
static void applySubscriptionFilterTurn(const Subscription &s, std::set< const SUMOTrafficObject * > &vehs)
Apply the subscription filter "turn": Gather upcoming junctions and vialanes within downstream distan...
Definition: Helper.cpp:1159
static void findObjectShape(int domain, const std::string &id, PositionVector &shape)
Definition: Helper.cpp:731
static void clearStateChanges()
Definition: Helper.cpp:710
static PositionVector makePositionVector(const TraCIPositionVector &vector)
Definition: Helper.cpp:344
static void fuseLaneCoverage(std::shared_ptr< LaneCoverageInfo > aggregatedLaneCoverage, const std::shared_ptr< LaneCoverageInfo > newLaneCoverage)
Adds lane coverage information from newLaneCoverage into aggregatedLaneCoverage.
Definition: Helper.cpp:316
static bool moveToXYMap_matchingRoutePosition(const Position &pos, const std::string &origID, const ConstMSEdgeVector &currentRoute, int routeIndex, SUMOVehicleClass vClass, bool setLateralPos, double &bestDistance, MSLane **lane, double &lanePos, int &routeOffset)
Definition: Helper.cpp:1706
static void debugPrint(const SUMOTrafficObject *veh)
Definition: Helper.cpp:97
static MSPerson * getPerson(const std::string &id)
Definition: Helper.cpp:488
static void subscribe(const int commandId, const std::string &id, const std::vector< int > &variables, const double beginTime, const double endTime, const libsumo::TraCIResults &params, const int contextDomain=0, const double range=0.)
Definition: Helper.cpp:109
static TraCIPositionVector makeTraCIPositionVector(const PositionVector &positionVector)
helper functions
Definition: Helper.cpp:334
static const std::vector< std::string > & getTransportableStateChanges(const MSNet::TransportableState state)
Definition: Helper.cpp:704
static std::map< int, std::shared_ptr< VariableWrapper > > myWrapper
Map of commandIds -> their executors; applicable if the executor applies to the method footprint.
Definition: Helper.h:268
static void clearSubscriptions()
Definition: Helper.cpp:200
static MSBaseVehicle * getVehicle(const std::string &id)
Definition: Helper.cpp:474
static void applySubscriptionFilterLateralDistanceSinglePass(const Subscription &s, std::set< std::string > &objIDs, std::set< const SUMOTrafficObject * > &vehs, const std::vector< const MSLane * > &lanes, double posOnLane, double posLat, bool isDownstream)
Definition: Helper.cpp:1297
static MSStoppingPlace * getStoppingPlace(const std::string &id, const SumoXMLTag type)
Definition: Helper.cpp:524
static TraCIColor makeTraCIColor(const RGBColor &color)
Definition: Helper.cpp:357
static void applySubscriptionFilterFieldOfVision(const Subscription &s, std::set< std::string > &objIDs)
Definition: Helper.cpp:1224
static Subscription * myLastContextSubscription
The last context subscription.
Definition: Helper.h:265
static TraCINextStopData buildStopData(const SUMOVehicleParameter::Stop &stopPar)
Definition: Helper.cpp:634
static void registerStateListener()
Definition: Helper.cpp:689
static TransportableStateListener myTransportableStateListener
Changes in the states of simulated transportables.
Definition: Helper.h:274
static void setRemoteControlled(MSVehicle *v, Position xyPos, MSLane *l, double pos, double posLat, double angle, int edgeOffset, ConstMSEdgeVector route, SUMOTime t)
Definition: Helper.cpp:1373
static int postProcessRemoteControl()
return number of remote-controlled entities
Definition: Helper.cpp:1388
static void applySubscriptionFilters(const Subscription &s, std::set< std::string > &objIDs)
Filter the given ID-Set (which was obtained from an R-Tree search) according to the filters set by th...
Definition: Helper.cpp:881
static std::map< std::string, MSVehicle * > myRemoteControlledVehicles
Definition: Helper.h:279
static const MSVehicleType & getVehicleType(const std::string &vehicleID)
Definition: Helper.cpp:509
static bool moveToXYMap(const Position &pos, double maxRouteDistance, bool mayLeaveNetwork, const std::string &origID, const double angle, double speed, const ConstMSEdgeVector &currentRoute, const int routePosition, const MSLane *currentLane, double currentLanePos, bool onRoad, SUMOVehicleClass vClass, bool setLateralPos, double &bestDistance, MSLane **lane, double &lanePos, int &routeOffset, ConstMSEdgeVector &edges)
Definition: Helper.cpp:1413
static std::pair< MSLane *, double > convertCartesianToRoadMap(const Position &pos, const SUMOVehicleClass vClass)
Definition: Helper.cpp:417
static MSTLLogicControl::TLSLogicVariants & getTLS(const std::string &id)
Definition: Helper.cpp:515
static SUMOTrafficObject * getTrafficObject(int domain, const std::string &id)
Definition: Helper.cpp:498
static VehicleStateListener myVehicleStateListener
Changes in the states of simulated vehicles.
Definition: Helper.h:271
static std::vector< Subscription > mySubscriptions
The list of known, still valid subscriptions.
Definition: Helper.h:262
static SUMOVehicleParameter::Stop buildStopParameters(const std::string &edgeOrStoppingPlaceID, double pos, int laneIndex, double startPos, int flags, double duration, double until)
Definition: Helper.cpp:534
static void handleSingleSubscription(const Subscription &s)
Definition: Helper.cpp:229
static void applySubscriptionFilterLateralDistance(const Subscription &s, std::set< const SUMOTrafficObject * > &vehs, double downstreamDist, double upstreamDist, double lateralDist)
Apply the subscription filter "lateral distance": Only return vehicles within the given lateral dista...
Definition: Helper.cpp:1263
static const std::vector< std::string > & getVehicleStateChanges(const MSNet::VehicleState state)
Definition: Helper.cpp:698
static void collectObjectIDsInRange(int domain, const PositionVector &shape, double range, std::set< std::string > &into)
Definition: Helper.cpp:801
static void handleSubscriptions(const SUMOTime t)
Definition: Helper.cpp:150
static Subscription * addSubscriptionFilter(SubscriptionFilterType filter)
Definition: Helper.cpp:207
static const MSLane * getLaneChecking(const std::string &edgeID, int laneIndex, double pos)
Definition: Helper.cpp:400
static RGBColor makeRGBColor(const TraCIColor &color)
Definition: Helper.cpp:368
static void applySubscriptionFilterLanes(const Subscription &s, std::set< const SUMOTrafficObject * > &vehs, std::vector< int > &filterLanes, double downstreamDist, double upstreamDist, bool disregardOppositeDirection)
Apply the subscription filter "lanes": Only return vehicles on list of lanes relative to ego vehicle....
Definition: Helper.cpp:1061
static std::map< std::string, MSPerson * > myRemoteControlledPersons
Definition: Helper.h:280
static bool needNewSubscription(libsumo::Subscription &s, std::vector< Subscription > &subscriptions, libsumo::Subscription *&modifiedSubscription)
Definition: Helper.cpp:175
static bool findCloserLane(const MSEdge *edge, const Position &pos, SUMOVehicleClass vClass, double &bestDistance, MSLane **lane)
Definition: Helper.cpp:1675
static std::shared_ptr< tcpip::Storage > toStorage(const TraCIResult &v)
Definition: StorageHelper.h:33
Representation of a subscription.
Definition: Subscription.h:69
double filterUpstreamDist
Upstream distance specified by the upstream distance filter.
Definition: Subscription.h:136
int commandId
commandIdArg The command id of the subscription
Definition: Subscription.h:113
std::set< std::string > filterVTypes
vTypes specified by the vTypes filter
Definition: Subscription.h:140
double filterFieldOfVisionOpeningAngle
Opening angle (in deg) specified by the field of vision filter.
Definition: Subscription.h:144
std::vector< int > filterLanes
lanes specified by the lanes filter
Definition: Subscription.h:132
std::string id
The id of the object that is subscribed.
Definition: Subscription.h:115
int filterVClasses
vClasses specified by the vClasses filter,
Definition: Subscription.h:142
SUMOTime endTime
The end time of the subscription.
Definition: Subscription.h:123
int contextDomain
The domain ID of the context.
Definition: Subscription.h:125
double filterFoeDistToJunction
Foe distance to junction specified by the turn filter.
Definition: Subscription.h:138
bool isVehicleToVehicleContextSubscription() const
Definition: Subscription.h:104
SUMOTime beginTime
The begin time of the subscription.
Definition: Subscription.h:121
std::vector< int > variables
The subscribed variables.
Definition: Subscription.h:117
bool isVehicleToPersonContextSubscription() const
Definition: Subscription.h:108
double filterDownstreamDist
Downstream distance specified by the downstream distance filter.
Definition: Subscription.h:134
double filterLateralDist
Lateral distance specified by the lateral distance filter.
Definition: Subscription.h:146
int activeFilters
Active filters for the subscription (bitset,.
Definition: Subscription.h:130
double range
The range of the context.
Definition: Subscription.h:127
std::vector< std::shared_ptr< tcpip::Storage > > parameters
The parameters for the subscribed variables.
Definition: Subscription.h:119
An error which allows to continue.
Definition: TraCIDefs.h:138
virtual void empty(const std::string &)
Definition: Subscription.h:168
bool(* SubscriptionHandler)(const std::string &objID, const int variable, VariableWrapper *wrapper, tcpip::Storage *paramData)
Definition of a method to be called for serving an associated commandID.
Definition: Subscription.h:152
virtual void setContext(const std::string *const)
Definition: Subscription.h:156
TRACI_CONST double INVALID_DOUBLE_VALUE
TRACI_CONST int LAST_STEP_VEHICLE_NUMBER
TRACI_CONST int CMD_GET_CHARGINGSTATION_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_EDGE_CONTEXT
TRACI_CONST int CMD_SUBSCRIBE_LANE_CONTEXT
TRACI_CONST int TRACI_ID_LIST
TRACI_CONST int CMD_GET_PARKINGAREA_VARIABLE
TRACI_CONST int CMD_GET_POI_VARIABLE
TRACI_CONST int CMD_GET_TL_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_VEHICLE_CONTEXT
std::map< std::string, libsumo::SubscriptionResults > ContextSubscriptionResults
Definition: TraCIDefs.h:301
TRACI_CONST int CMD_GET_REROUTER_VARIABLE
TRACI_CONST int VAR_ROAD_ID
TRACI_CONST int CMD_GET_VEHICLE_VARIABLE
TRACI_CONST int CMD_GET_EDGE_VARIABLE
TRACI_CONST int CMD_GET_CALIBRATOR_VARIABLE
TRACI_CONST int CMD_GET_PERSON_VARIABLE
TRACI_CONST int CMD_GET_ROUTEPROBE_VARIABLE
TRACI_CONST int CMD_GET_LANEAREA_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_BUSSTOP_CONTEXT
TRACI_CONST int CMD_GET_BUSSTOP_VARIABLE
TRACI_CONST int CMD_GET_ROUTE_VARIABLE
TRACI_CONST int CMD_GET_MEANDATA_VARIABLE
TRACI_CONST int CMD_GET_JUNCTION_VARIABLE
std::map< std::string, libsumo::TraCIResults > SubscriptionResults
{object->{variable->value}}
Definition: TraCIDefs.h:300
TRACI_CONST int CMD_GET_VARIABLESPEEDSIGN_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_CALIBRATOR_CONTEXT
TRACI_CONST int VAR_LANEPOSITION
TRACI_CONST int CMD_GET_SIM_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_PERSON_CONTEXT
TRACI_CONST int CMD_GET_VEHICLETYPE_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_CHARGINGSTATION_CONTEXT
TRACI_CONST int CMD_SUBSCRIBE_POLYGON_CONTEXT
TRACI_CONST int CMD_GET_LANE_VARIABLE
TRACI_CONST int CMD_GET_GUI_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_PARKINGAREA_CONTEXT
TRACI_CONST int CMD_GET_POLYGON_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_MULTIENTRYEXIT_CONTEXT
TRACI_CONST int CMD_SUBSCRIBE_VEHICLE_VARIABLE
TRACI_CONST int CMD_GET_MULTIENTRYEXIT_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_INDUCTIONLOOP_CONTEXT
SubscriptionFilterType
Filter types for context subscriptions.
Definition: Subscription.h:36
@ SUBS_FILTER_LEAD_FOLLOW
Definition: Subscription.h:48
@ SUBS_FILTER_UPSTREAM_DIST
Definition: Subscription.h:46
@ SUBS_FILTER_VTYPE
Definition: Subscription.h:54
@ SUBS_FILTER_NO_RTREE
Definition: Subscription.h:61
@ SUBS_FILTER_LANES
Definition: Subscription.h:40
@ SUBS_FILTER_NOOPPOSITE
Definition: Subscription.h:42
@ SUBS_FILTER_DOWNSTREAM_DIST
Definition: Subscription.h:44
@ SUBS_FILTER_LATERAL_DIST
Definition: Subscription.h:59
@ SUBS_FILTER_TURN
Definition: Subscription.h:50
@ SUBS_FILTER_VCLASS
Definition: Subscription.h:52
@ SUBS_FILTER_MANEUVER
Definition: Subscription.h:63
@ SUBS_FILTER_FIELD_OF_VISION
Definition: Subscription.h:57
TRACI_CONST int CMD_SUBSCRIBE_POI_CONTEXT
TRACI_CONST int CMD_SUBSCRIBE_PERSON_VARIABLE
TRACI_CONST int CMD_GET_INDUCTIONLOOP_VARIABLE
TRACI_CONST int CMD_GET_OVERHEADWIRE_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_LANEAREA_CONTEXT
TRACI_CONST int CMD_SUBSCRIBE_JUNCTION_CONTEXT
std::map< int, std::shared_ptr< libsumo::TraCIResult > > TraCIResults
{variable->value}
Definition: TraCIDefs.h:298
auto get(const nlohmann::detail::iteration_proxy_value< IteratorType > &i) -> decltype(i.key())
Definition: json.hpp:4451
A 3D-position.
Definition: TraCIDefs.h:172
A list of positions.
Definition: TraCIDefs.h:215
std::vector< TraCIPosition > value
Definition: TraCIDefs.h:225