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