Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2017-2024 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 : /****************************************************************************/
14 : /// @file Helper.cpp
15 : /// @author Laura Bieker-Walz
16 : /// @author Robert Hilbrich
17 : /// @author Leonhard Luecken
18 : /// @date 15.09.2017
19 : ///
20 : // C++ TraCI client API implementation
21 : /****************************************************************************/
22 : #include <config.h>
23 :
24 : #include <cstring>
25 : #include <utils/geom/GeomHelper.h>
26 : #include <utils/geom/GeoConvHelper.h>
27 : #include <microsim/MSNet.h>
28 : #include <microsim/MSVehicleControl.h>
29 : #include <microsim/MSEdgeControl.h>
30 : #include <microsim/MSInsertionControl.h>
31 : #include <microsim/MSEdge.h>
32 : #include <microsim/MSLane.h>
33 : #include <microsim/MSLink.h>
34 : #include <microsim/MSStoppingPlace.h>
35 : #include <microsim/MSVehicle.h>
36 : #include <microsim/output/MSE3Collector.h>
37 : #include <microsim/transportables/MSTransportable.h>
38 : #include <microsim/transportables/MSTransportableControl.h>
39 : #include <microsim/transportables/MSPerson.h>
40 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
41 : #include <libsumo/StorageHelper.h>
42 : #include <libsumo/TraCIDefs.h>
43 : #include <libsumo/BusStop.h>
44 : #include <libsumo/Calibrator.h>
45 : #include <libsumo/ChargingStation.h>
46 : #include <libsumo/Edge.h>
47 : #ifdef HAVE_LIBSUMOGUI
48 : #include <libsumo/GUI.h>
49 : #endif
50 : #include <libsumo/InductionLoop.h>
51 : #include <libsumo/Junction.h>
52 : #include <libsumo/Lane.h>
53 : #include <libsumo/LaneArea.h>
54 : #include <libsumo/MeanData.h>
55 : #include <libsumo/MultiEntryExit.h>
56 : #include <libsumo/OverheadWire.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>
65 : #include <libsumo/TrafficLight.h>
66 : #include <libsumo/VariableSpeedSign.h>
67 : #include <libsumo/Vehicle.h>
68 : #include <libsumo/VehicleType.h>
69 : #include <libsumo/TraCIConstants.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 :
78 : namespace libsumo {
79 : // ===========================================================================
80 : // static member initializations
81 : // ===========================================================================
82 : std::vector<Subscription> Helper::mySubscriptions;
83 : Subscription* Helper::myLastContextSubscription = nullptr;
84 : std::map<int, std::shared_ptr<VariableWrapper> > Helper::myWrapper;
85 : Helper::VehicleStateListener Helper::myVehicleStateListener;
86 : Helper::TransportableStateListener Helper::myTransportableStateListener;
87 : LANE_RTREE_QUAL* Helper::myLaneTree;
88 : std::map<std::string, MSVehicle*> Helper::myRemoteControlledVehicles;
89 : std::map<std::string, MSPerson*> Helper::myRemoteControlledPersons;
90 :
91 :
92 : // ===========================================================================
93 : // static member definitions
94 : // ===========================================================================
95 :
96 : void
97 0 : Helper::debugPrint(const SUMOTrafficObject* veh) {
98 0 : if (veh != nullptr) {
99 0 : if (veh->isVehicle()) {
100 0 : std::cout << " '" << veh->getID() << "' on lane '" << ((SUMOVehicle*)veh)->getLane()->getID() << "'\n";
101 : } else {
102 0 : std::cout << " '" << veh->getID() << "' on edge '" << veh->getEdge()->getID() << "'\n";
103 : }
104 : }
105 0 : }
106 :
107 :
108 : void
109 2848 : Helper::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) {
112 2848 : myLastContextSubscription = nullptr;
113 2848 : if (variables.empty()) {
114 1397 : for (std::vector<libsumo::Subscription>::iterator j = mySubscriptions.begin(); j != mySubscriptions.end();) {
115 713 : if (j->id == id && j->commandId == commandId && j->contextDomain == contextDomain) {
116 : j = mySubscriptions.erase(j);
117 : } else {
118 : ++j;
119 : }
120 : }
121 684 : return;
122 : }
123 : std::vector<std::shared_ptr<tcpip::Storage> > parameters;
124 4511 : for (const int var : variables) {
125 : const auto& p = params.find(var);
126 2347 : if (p == params.end()) {
127 2299 : parameters.push_back(std::make_shared<tcpip::Storage>());
128 : } else {
129 96 : parameters.push_back(libsumo::StorageHelper::toStorage(*p->second));
130 : }
131 : }
132 2164 : const SUMOTime begin = beginTime == INVALID_DOUBLE_VALUE ? 0 : TIME2STEPS(beginTime);
133 2164 : const SUMOTime end = endTime == INVALID_DOUBLE_VALUE || endTime > STEPS2TIME(SUMOTime_MAX) ? SUMOTime_MAX : TIME2STEPS(endTime);
134 2164 : libsumo::Subscription s(commandId, id, variables, parameters, begin, end, contextDomain, range);
135 2164 : if (commandId == libsumo::CMD_SUBSCRIBE_SIM_CONTEXT) {
136 98 : s.range = std::numeric_limits<double>::max();
137 : }
138 2164 : if (s.variables.size() == 1 && s.variables.front() == -1) {
139 : s.variables.clear();
140 : }
141 2164 : handleSingleSubscription(s);
142 1691 : libsumo::Subscription* modifiedSubscription = nullptr;
143 1691 : needNewSubscription(s, mySubscriptions, modifiedSubscription);
144 1691 : if (modifiedSubscription->isVehicleToVehicleContextSubscription()
145 : || modifiedSubscription->isVehicleToPersonContextSubscription()) {
146 : // Set last modified vehicle context subscription active for filter modifications
147 138 : myLastContextSubscription = modifiedSubscription;
148 : }
149 2637 : }
150 :
151 :
152 : void
153 156459 : Helper::handleSubscriptions(const SUMOTime t) {
154 1255395 : for (auto& wrapper : myWrapper) {
155 1098936 : wrapper.second->clear();
156 : }
157 248127 : for (std::vector<libsumo::Subscription>::iterator i = mySubscriptions.begin(); i != mySubscriptions.end();) {
158 : const libsumo::Subscription& s = *i;
159 91668 : const bool isArrivedVehicle = (s.commandId == CMD_SUBSCRIBE_VEHICLE_VARIABLE || s.commandId == CMD_SUBSCRIBE_VEHICLE_CONTEXT)
160 91668 : && (find(getVehicleStateChanges(MSNet::VehicleState::ARRIVED).begin(), getVehicleStateChanges(MSNet::VehicleState::ARRIVED).end(), s.id) != getVehicleStateChanges(MSNet::VehicleState::ARRIVED).end());
161 91668 : const bool isArrivedPerson = (s.commandId == libsumo::CMD_SUBSCRIBE_PERSON_VARIABLE || s.commandId == libsumo::CMD_SUBSCRIBE_PERSON_CONTEXT)
162 91668 : && MSNet::getInstance()->getPersonControl().get(s.id) == nullptr;
163 91668 : if (s.endTime < t || isArrivedVehicle || isArrivedPerson) {
164 : i = mySubscriptions.erase(i);
165 106 : continue;
166 : }
167 : ++i;
168 : }
169 248021 : for (const libsumo::Subscription& s : mySubscriptions) {
170 91562 : if (s.beginTime <= t) {
171 91562 : handleSingleSubscription(s);
172 : }
173 : }
174 156459 : }
175 :
176 :
177 : bool
178 9524 : Helper::needNewSubscription(libsumo::Subscription& s, std::vector<Subscription>& subscriptions, libsumo::Subscription*& modifiedSubscription) {
179 1123167 : for (libsumo::Subscription& o : subscriptions) {
180 1110154 : if (s.commandId == o.commandId && s.id == o.id &&
181 1946 : s.beginTime == o.beginTime && s.endTime == o.endTime &&
182 1117403 : s.contextDomain == o.contextDomain && s.range == o.range) {
183 : std::vector<std::shared_ptr<tcpip::Storage> >::const_iterator k = s.parameters.begin();
184 3696 : for (const int v : s.variables) {
185 1851 : const int offset = (int)(std::find(o.variables.begin(), o.variables.end(), v) - o.variables.begin());
186 1851 : if (offset == (int)o.variables.size() || o.parameters[offset]->size() != (*k)->size() || !std::equal((*k)->begin(), (*k)->end(), o.parameters[offset]->begin())) {
187 53 : o.variables.push_back(v);
188 53 : o.parameters.push_back(*k);
189 : }
190 : ++k;
191 : }
192 1845 : modifiedSubscription = &o;
193 : return false;
194 : }
195 : }
196 7679 : subscriptions.push_back(s);
197 7679 : modifiedSubscription = &subscriptions.back();
198 7679 : return true;
199 : }
200 :
201 :
202 : void
203 36934 : Helper::clearSubscriptions() {
204 : mySubscriptions.clear();
205 36934 : myLastContextSubscription = nullptr;
206 36934 : }
207 :
208 :
209 : Subscription*
210 371 : Helper::addSubscriptionFilter(SubscriptionFilterType filter) {
211 371 : if (myLastContextSubscription != nullptr) {
212 370 : myLastContextSubscription->activeFilters |= filter;
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 1 : int index = (int)filter;
218 : int filterType = 0;
219 1 : if (index != 0) {
220 : ++filterType;
221 11 : while (index >>= 1) {
222 10 : ++filterType;
223 : }
224 : }
225 2 : throw TraCIException("No previous vehicle context subscription exists to apply filter type " + toHex(filterType, 2));
226 : }
227 370 : return myLastContextSubscription;
228 : }
229 :
230 :
231 : void
232 93726 : Helper::handleSingleSubscription(const Subscription& s) {
233 93726 : const int getCommandId = s.contextDomain > 0 ? s.contextDomain : s.commandId - 0x30;
234 : std::set<std::string> objIDs;
235 93726 : if (s.contextDomain > 0) {
236 59644 : if ((s.activeFilters & SUBS_FILTER_NO_RTREE) == 0) {
237 57948 : PositionVector shape;
238 57948 : findObjectShape(s.commandId, s.id, shape);
239 57948 : collectObjectIDsInRange(s.contextDomain, shape, s.range, objIDs);
240 57948 : }
241 59171 : applySubscriptionFilters(s, objIDs);
242 : } else {
243 34082 : objIDs.insert(s.id);
244 : }
245 93253 : if (myWrapper.empty()) {
246 267 : myWrapper[libsumo::CMD_GET_BUSSTOP_VARIABLE] = BusStop::makeWrapper();
247 267 : myWrapper[libsumo::CMD_GET_CALIBRATOR_VARIABLE] = Calibrator::makeWrapper();
248 267 : myWrapper[libsumo::CMD_GET_CHARGINGSTATION_VARIABLE] = ChargingStation::makeWrapper();
249 267 : myWrapper[libsumo::CMD_GET_EDGE_VARIABLE] = Edge::makeWrapper();
250 : #ifdef HAVE_LIBSUMOGUI
251 267 : myWrapper[libsumo::CMD_GET_GUI_VARIABLE] = GUI::makeWrapper();
252 : #endif
253 267 : myWrapper[libsumo::CMD_GET_INDUCTIONLOOP_VARIABLE] = InductionLoop::makeWrapper();
254 267 : myWrapper[libsumo::CMD_GET_JUNCTION_VARIABLE] = Junction::makeWrapper();
255 267 : myWrapper[libsumo::CMD_GET_LANE_VARIABLE] = Lane::makeWrapper();
256 267 : myWrapper[libsumo::CMD_GET_LANEAREA_VARIABLE] = LaneArea::makeWrapper();
257 267 : myWrapper[libsumo::CMD_GET_MEANDATA_VARIABLE] = MeanData::makeWrapper();
258 267 : myWrapper[libsumo::CMD_GET_MULTIENTRYEXIT_VARIABLE] = MultiEntryExit::makeWrapper();
259 267 : myWrapper[libsumo::CMD_GET_OVERHEADWIRE_VARIABLE] = OverheadWire::makeWrapper();
260 267 : myWrapper[libsumo::CMD_GET_PARKINGAREA_VARIABLE] = ParkingArea::makeWrapper();
261 267 : myWrapper[libsumo::CMD_GET_PERSON_VARIABLE] = Person::makeWrapper();
262 267 : myWrapper[libsumo::CMD_GET_POI_VARIABLE] = POI::makeWrapper();
263 267 : myWrapper[libsumo::CMD_GET_POLYGON_VARIABLE] = Polygon::makeWrapper();
264 267 : myWrapper[libsumo::CMD_GET_REROUTER_VARIABLE] = Rerouter::makeWrapper();
265 267 : myWrapper[libsumo::CMD_GET_ROUTE_VARIABLE] = Route::makeWrapper();
266 267 : myWrapper[libsumo::CMD_GET_ROUTEPROBE_VARIABLE] = RouteProbe::makeWrapper();
267 267 : myWrapper[libsumo::CMD_GET_SIM_VARIABLE] = Simulation::makeWrapper();
268 267 : myWrapper[libsumo::CMD_GET_TL_VARIABLE] = TrafficLight::makeWrapper();
269 267 : myWrapper[libsumo::CMD_GET_VARIABLESPEEDSIGN_VARIABLE] = VariableSpeedSign::makeWrapper();
270 267 : myWrapper[libsumo::CMD_GET_VEHICLE_VARIABLE] = Vehicle::makeWrapper();
271 1007 : myWrapper[libsumo::CMD_GET_VEHICLETYPE_VARIABLE] = VehicleType::makeWrapper();
272 : }
273 : auto wrapper = myWrapper.find(getCommandId);
274 93253 : if (wrapper == myWrapper.end()) {
275 0 : throw TraCIException("Unsupported command specified");
276 : }
277 : std::shared_ptr<VariableWrapper> handler = wrapper->second;
278 : VariableWrapper* container = handler.get();
279 93253 : if (s.contextDomain > 0) {
280 59171 : auto containerWrapper = myWrapper.find(s.commandId + 0x20);
281 59171 : if (containerWrapper == myWrapper.end()) {
282 0 : throw TraCIException("Unsupported domain specified");
283 : }
284 : container = containerWrapper->second.get();
285 59171 : container->setContext(&s.id);
286 : } else {
287 34082 : container->setContext(nullptr);
288 : }
289 317230 : for (const std::string& objID : objIDs) {
290 223977 : if (!s.variables.empty()) {
291 : std::vector<std::shared_ptr<tcpip::Storage> >::const_iterator k = s.parameters.begin();
292 493158 : for (const int variable : s.variables) {
293 271027 : if (s.contextDomain > 0 && variable == libsumo::TRACI_ID_LIST) {
294 4382 : container->empty(objID);
295 : } else {
296 266645 : (*k)->resetPos();
297 266645 : handler->handle(objID, variable, container, k->get());
298 : ++k;
299 : }
300 : }
301 : } else {
302 1846 : if (s.contextDomain == 0 && getCommandId == libsumo::CMD_GET_VEHICLE_VARIABLE) {
303 : // default for vehicles is edge id and lane position
304 654 : handler->handle(objID, VAR_ROAD_ID, container, nullptr);
305 654 : handler->handle(objID, VAR_LANEPOSITION, container, nullptr);
306 1192 : } else if (s.contextDomain > 0) {
307 : // default for contexts is an empty map (similar to id list)
308 34 : container->empty(objID);
309 1158 : } 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 612 : handler->handle(objID, libsumo::TRACI_ID_LIST, container, nullptr);
312 : }
313 : }
314 : }
315 93253 : }
316 :
317 :
318 : void
319 1660 : Helper::fuseLaneCoverage(std::shared_ptr<LaneCoverageInfo> aggregatedLaneCoverage, const std::shared_ptr<LaneCoverageInfo> newLaneCoverage) {
320 7405 : for (auto& p : *newLaneCoverage) {
321 5745 : const MSLane* lane = p.first;
322 5745 : if (aggregatedLaneCoverage->find(lane) == aggregatedLaneCoverage->end()) {
323 : // Lane has no coverage in aggregatedLaneCoverage, yet
324 5725 : (*aggregatedLaneCoverage)[lane] = (*newLaneCoverage)[lane];
325 : } else {
326 : // Lane is covered in aggregatedLaneCoverage as well
327 20 : std::pair<double, double>& range1 = (*aggregatedLaneCoverage)[lane];
328 20 : std::pair<double, double>& range2 = (*newLaneCoverage)[lane];
329 35 : std::pair<double, double> hull = std::make_pair(MIN2(range1.first, range2.first), MAX2(range1.second, range2.second));
330 20 : (*aggregatedLaneCoverage)[lane] = hull;
331 : }
332 : }
333 1660 : }
334 :
335 :
336 : TraCIPositionVector
337 10369 : Helper::makeTraCIPositionVector(const PositionVector& positionVector) {
338 : TraCIPositionVector tp;
339 53668 : for (int i = 0; i < (int)positionVector.size(); ++i) {
340 86598 : tp.value.push_back(makeTraCIPosition(positionVector[i]));
341 : }
342 10369 : return tp;
343 : }
344 :
345 :
346 : PositionVector
347 278 : Helper::makePositionVector(const TraCIPositionVector& vector) {
348 278 : PositionVector pv;
349 8807 : for (const TraCIPosition& pos : vector.value) {
350 8531 : if (std::isnan(pos.x) || std::isnan(pos.y)) {
351 4 : throw libsumo::TraCIException("NaN-Value in shape.");
352 : }
353 8529 : pv.push_back(Position(pos.x, pos.y));
354 : }
355 276 : return pv;
356 2 : }
357 :
358 :
359 : TraCIColor
360 1384 : Helper::makeTraCIColor(const RGBColor& color) {
361 : TraCIColor tc;
362 1384 : tc.a = color.alpha();
363 1384 : tc.b = color.blue();
364 1384 : tc.g = color.green();
365 1384 : tc.r = color.red();
366 1384 : return tc;
367 : }
368 :
369 :
370 : RGBColor
371 434 : Helper::makeRGBColor(const TraCIColor& c) {
372 434 : return RGBColor((unsigned char)c.r, (unsigned char)c.g, (unsigned char)c.b, (unsigned char)c.a);
373 : }
374 :
375 :
376 : TraCIPosition
377 2310956 : Helper::makeTraCIPosition(const Position& position, const bool includeZ) {
378 2310956 : TraCIPosition p;
379 2310956 : p.x = position.x();
380 2310956 : p.y = position.y();
381 2310956 : p.z = includeZ ? position.z() : INVALID_DOUBLE_VALUE;
382 2310956 : return p;
383 : }
384 :
385 :
386 : Position
387 0 : Helper::makePosition(const TraCIPosition& tpos) {
388 0 : return Position(tpos.x, tpos.y, tpos.z);
389 : }
390 :
391 :
392 : MSEdge*
393 3937 : Helper::getEdge(const std::string& edgeID) {
394 3937 : MSEdge* edge = MSEdge::dictionary(edgeID);
395 3937 : if (edge == nullptr) {
396 0 : throw TraCIException("Referenced edge '" + edgeID + "' is not known.");
397 : }
398 3937 : return edge;
399 : }
400 :
401 :
402 : const MSLane*
403 832 : Helper::getLaneChecking(const std::string& edgeID, int laneIndex, double pos) {
404 832 : const MSEdge* edge = MSEdge::dictionary(edgeID);
405 832 : if (edge == nullptr) {
406 2 : throw TraCIException("Unknown edge " + edgeID);
407 : }
408 831 : if (laneIndex < 0 || laneIndex >= (int)edge->getLanes().size()) {
409 2 : throw TraCIException("Invalid lane index for " + edgeID);
410 : }
411 830 : const MSLane* lane = edge->getLanes()[laneIndex];
412 830 : if (pos < 0 || pos > lane->getLength()) {
413 6 : throw TraCIException("Position on lane invalid");
414 : }
415 827 : return lane;
416 : }
417 :
418 :
419 : std::pair<MSLane*, double>
420 655 : Helper::convertCartesianToRoadMap(const Position& pos, const SUMOVehicleClass vClass) {
421 655 : const PositionVector shape({ pos });
422 : std::pair<MSLane*, double> result(nullptr, -1);
423 : double range = 1000.;
424 655 : const Boundary& netBounds = GeoConvHelper::getFinal().getConvBoundary();
425 655 : const double maxRange = MAX2(1001., netBounds.getWidth() + netBounds.getHeight() + netBounds.distanceTo2D(pos));
426 726 : while (range < maxRange) {
427 : std::set<const Named*> lanes;
428 726 : collectObjectsInRange(libsumo::CMD_GET_LANE_VARIABLE, shape, range, lanes);
429 : double minDistance = std::numeric_limits<double>::max();
430 24262 : for (const Named* named : lanes) {
431 23536 : MSLane* lane = const_cast<MSLane*>(dynamic_cast<const MSLane*>(named));
432 23536 : if (lane->allowsVehicleClass(vClass)) {
433 : // @todo this may be a place where 3D is required but 2D is used
434 23530 : const double newDistance = lane->getShape().distance2D(pos);
435 23530 : if (newDistance < minDistance ||
436 : (newDistance == minDistance
437 861 : && result.first != nullptr
438 861 : && lane->getID() < result.first->getID())) {
439 : minDistance = newDistance;
440 : result.first = lane;
441 : }
442 : }
443 : }
444 726 : if (minDistance < std::numeric_limits<double>::max()) {
445 655 : result.second = result.first->interpolateGeometryPosToLanePos(result.first->getShape().nearest_offset_to_point2D(pos, false));
446 : break;
447 : }
448 71 : range *= 2;
449 : }
450 655 : return result;
451 655 : }
452 :
453 :
454 : double
455 271 : Helper::getDrivingDistance(std::pair<const MSLane*, double>& roadPos1, std::pair<const MSLane*, double>& roadPos2) {
456 271 : if (roadPos1.first == roadPos2.first && roadPos1.second <= roadPos2.second) {
457 : // same edge
458 102 : return roadPos2.second - roadPos1.second;
459 : }
460 : double distance = 0.0;
461 : ConstMSEdgeVector newRoute;
462 206 : while (roadPos2.first->isInternal() && roadPos2.first != roadPos1.first) {
463 37 : distance += roadPos2.second;
464 37 : roadPos2.first = roadPos2.first->getLogicalPredecessorLane();
465 37 : roadPos2.second = roadPos2.first->getLength();
466 : }
467 338 : MSNet::getInstance()->getRouterTT(0).compute(&roadPos1.first->getEdge(), &roadPos2.first->getEdge(), nullptr, SIMSTEP, newRoute, true);
468 169 : if (newRoute.empty()) {
469 : return libsumo::INVALID_DOUBLE_VALUE;
470 : }
471 314 : MSRoute route("", newRoute, false, nullptr, std::vector<SUMOVehicleParameter::Stop>());
472 157 : return distance + route.getDistanceBetween(roadPos1.second, roadPos2.second, &roadPos1.first->getEdge(), &roadPos2.first->getEdge());
473 157 : }
474 :
475 :
476 : MSBaseVehicle*
477 19293768 : Helper::getVehicle(const std::string& id) {
478 19293768 : SUMOVehicle* sumoVehicle = MSNet::getInstance()->getVehicleControl().getVehicle(id);
479 19293768 : if (sumoVehicle == nullptr) {
480 212 : throw TraCIException("Vehicle '" + id + "' is not known.");
481 : }
482 19293662 : MSBaseVehicle* v = dynamic_cast<MSBaseVehicle*>(sumoVehicle);
483 19293662 : if (v == nullptr) {
484 0 : throw TraCIException("Vehicle '" + id + "' is not a proper vehicle.");
485 : }
486 19293662 : return v;
487 : }
488 :
489 :
490 : MSPerson*
491 206702 : Helper::getPerson(const std::string& personID) {
492 206702 : MSTransportableControl& c = MSNet::getInstance()->getPersonControl();
493 206702 : MSPerson* p = dynamic_cast<MSPerson*>(c.get(personID));
494 206411 : if (p == nullptr) {
495 582 : throw TraCIException("Person '" + personID + "' is not known");
496 : }
497 206411 : return p;
498 : }
499 :
500 : SUMOTrafficObject*
501 2275 : Helper::getTrafficObject(int domain, const std::string& id) {
502 2275 : if (domain == CMD_GET_VEHICLE_VARIABLE) {
503 2240 : return getVehicle(id);
504 35 : } else if (domain == CMD_GET_PERSON_VARIABLE) {
505 35 : return getPerson(id);
506 : } else {
507 0 : throw TraCIException("Cannot retrieve traffic object for domain " + toString(domain));
508 : }
509 : }
510 :
511 : const MSVehicleType&
512 29442 : Helper::getVehicleType(const std::string& vehicleID) {
513 29442 : return getVehicle(vehicleID)->getVehicleType();
514 : }
515 :
516 :
517 : MSTLLogicControl::TLSLogicVariants&
518 174170 : Helper::getTLS(const std::string& id) {
519 174170 : if (!MSNet::getInstance()->getTLSControl().knows(id)) {
520 6 : throw TraCIException("Traffic light '" + id + "' is not known");
521 : }
522 174167 : return MSNet::getInstance()->getTLSControl().get(id);
523 : }
524 :
525 :
526 : MSStoppingPlace*
527 1663 : Helper::getStoppingPlace(const std::string& id, const SumoXMLTag type) {
528 1663 : MSStoppingPlace* s = MSNet::getInstance()->getStoppingPlace(id, type);
529 1663 : if (s == nullptr) {
530 0 : throw TraCIException(toString(type) + " '" + id + "' is not known");
531 : }
532 1663 : return s;
533 : }
534 :
535 :
536 : SUMOVehicleParameter::Stop
537 89016 : Helper::buildStopParameters(const std::string& edgeOrStoppingPlaceID,
538 : double pos, int laneIndex, double startPos, int flags, double duration, double until) {
539 89016 : SUMOVehicleParameter::Stop newStop;
540 89022 : newStop.duration = duration == INVALID_DOUBLE_VALUE ? SUMOTime_MAX : TIME2STEPS(duration);
541 89039 : newStop.until = until == INVALID_DOUBLE_VALUE ? -1 : TIME2STEPS(until);
542 89016 : newStop.index = STOP_INDEX_FIT;
543 89016 : if (newStop.duration >= 0) {
544 89010 : newStop.parametersSet |= STOP_DURATION_SET;
545 : }
546 89016 : if (newStop.until >= 0) {
547 6 : newStop.parametersSet |= STOP_UNTIL_SET;
548 : }
549 89016 : if ((flags & 1) != 0) {
550 62 : newStop.parking = ParkingType::OFFROAD;
551 62 : newStop.parametersSet |= STOP_PARKING_SET;
552 : }
553 89016 : if ((flags & 2) != 0) {
554 0 : newStop.triggered = true;
555 0 : newStop.parametersSet |= STOP_TRIGGER_SET;
556 : }
557 89016 : if ((flags & 4) != 0) {
558 0 : newStop.containerTriggered = true;
559 0 : newStop.parametersSet |= STOP_CONTAINER_TRIGGER_SET;
560 : }
561 :
562 89016 : SumoXMLTag stoppingPlaceType = SUMO_TAG_NOTHING;
563 89016 : if ((flags & 8) != 0) {
564 330 : stoppingPlaceType = SUMO_TAG_BUS_STOP;
565 : }
566 89016 : if ((flags & 16) != 0) {
567 0 : stoppingPlaceType = SUMO_TAG_CONTAINER_STOP;
568 : }
569 89016 : if ((flags & 32) != 0) {
570 0 : stoppingPlaceType = SUMO_TAG_CHARGING_STATION;
571 : }
572 89016 : if ((flags & 64) != 0) {
573 10 : stoppingPlaceType = SUMO_TAG_PARKING_AREA;
574 : }
575 89016 : if ((flags & 128) != 0) {
576 0 : stoppingPlaceType = SUMO_TAG_OVERHEAD_WIRE_SEGMENT;
577 : }
578 :
579 89016 : if (stoppingPlaceType != SUMO_TAG_NOTHING) {
580 340 : MSStoppingPlace* bs = MSNet::getInstance()->getStoppingPlace(edgeOrStoppingPlaceID, stoppingPlaceType);
581 340 : if (bs == nullptr) {
582 0 : throw TraCIException("The " + toString(stoppingPlaceType) + " '" + edgeOrStoppingPlaceID + "' is not known");
583 : }
584 340 : newStop.lane = bs->getLane().getID();
585 340 : newStop.edge = bs->getLane().getEdge().getID();
586 340 : newStop.endPos = bs->getEndLanePosition();
587 340 : newStop.startPos = bs->getBeginLanePosition();
588 340 : switch (stoppingPlaceType) {
589 330 : case SUMO_TAG_BUS_STOP:
590 330 : newStop.busstop = edgeOrStoppingPlaceID;
591 : break;
592 0 : case SUMO_TAG_CONTAINER_STOP:
593 0 : newStop.containerstop = edgeOrStoppingPlaceID;
594 : break;
595 0 : case SUMO_TAG_CHARGING_STATION:
596 0 : newStop.chargingStation = edgeOrStoppingPlaceID;
597 : break;
598 10 : case SUMO_TAG_PARKING_AREA:
599 10 : newStop.parkingarea = edgeOrStoppingPlaceID;
600 : break;
601 0 : case SUMO_TAG_OVERHEAD_WIRE_SEGMENT:
602 0 : newStop.overheadWireSegment = edgeOrStoppingPlaceID;
603 : break;
604 0 : default:
605 0 : throw TraCIException("Unknown stopping place type '" + toString(stoppingPlaceType) + "'.");
606 : }
607 : } else {
608 88676 : if (startPos == INVALID_DOUBLE_VALUE) {
609 88652 : startPos = MAX2(0.0, pos - POSITION_EPS);
610 : }
611 88676 : if (startPos < 0.) {
612 0 : throw TraCIException("Position on lane must not be negative.");
613 : }
614 88676 : if (pos < startPos) {
615 0 : throw TraCIException("End position on lane must be after start position.");
616 : }
617 : // get the actual lane that is referenced by laneIndex
618 88676 : MSEdge* road = MSEdge::dictionary(edgeOrStoppingPlaceID);
619 88676 : if (road == nullptr) {
620 10 : throw TraCIException("Edge '" + edgeOrStoppingPlaceID + "' is not known.");
621 : }
622 : const std::vector<MSLane*>& allLanes = road->getLanes();
623 88671 : if ((laneIndex < 0) || laneIndex >= (int)(allLanes.size())) {
624 0 : throw TraCIException("No lane with index '" + toString(laneIndex) + "' on edge '" + edgeOrStoppingPlaceID + "'.");
625 : }
626 88671 : newStop.lane = allLanes[laneIndex]->getID();
627 88671 : newStop.edge = allLanes[laneIndex]->getEdge().getID();
628 88671 : newStop.endPos = pos;
629 88671 : newStop.startPos = startPos;
630 88671 : newStop.parametersSet |= STOP_START_SET | STOP_END_SET;
631 : }
632 89011 : return newStop;
633 5 : }
634 :
635 :
636 : TraCINextStopData
637 10802 : Helper::buildStopData(const SUMOVehicleParameter::Stop& stopPar) {
638 10802 : std::string stoppingPlaceID = "";
639 10802 : if (stopPar.busstop != "") {
640 : stoppingPlaceID = stopPar.busstop;
641 : }
642 10802 : if (stopPar.containerstop != "") {
643 : stoppingPlaceID = stopPar.containerstop;
644 : }
645 10802 : if (stopPar.parkingarea != "") {
646 : stoppingPlaceID = stopPar.parkingarea;
647 : }
648 10802 : if (stopPar.chargingStation != "") {
649 : stoppingPlaceID = stopPar.chargingStation;
650 : }
651 10802 : if (stopPar.overheadWireSegment != "") {
652 : stoppingPlaceID = stopPar.overheadWireSegment;
653 : }
654 :
655 10802 : return TraCINextStopData(stopPar.lane,
656 10802 : stopPar.startPos,
657 10802 : 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 20989 : stopPar.duration != -1 ? STEPS2TIME(stopPar.duration) : INVALID_DOUBLE_VALUE,
663 16943 : stopPar.until >= 0 ? STEPS2TIME(stopPar.until) : INVALID_DOUBLE_VALUE,
664 14078 : stopPar.arrival >= 0 ? STEPS2TIME(stopPar.arrival) : INVALID_DOUBLE_VALUE,
665 13336 : stopPar.started >= 0 ? STEPS2TIME(stopPar.started) : INVALID_DOUBLE_VALUE,
666 11797 : stopPar.ended >= 0 ? STEPS2TIME(stopPar.ended) : INVALID_DOUBLE_VALUE,
667 10802 : stopPar.split,
668 10802 : stopPar.join,
669 10802 : stopPar.actType,
670 10802 : stopPar.tripId,
671 10802 : stopPar.line,
672 10802 : stopPar.speed);
673 : }
674 :
675 :
676 : void
677 35156 : Helper::cleanup() {
678 : // clean up NamedRTrees
679 35156 : InductionLoop::cleanup();
680 35156 : Junction::cleanup();
681 35156 : LaneArea::cleanup();
682 35156 : POI::cleanup();
683 35156 : Polygon::cleanup();
684 35156 : Helper::clearStateChanges();
685 35156 : Helper::clearSubscriptions();
686 35156 : delete myLaneTree;
687 35156 : myLaneTree = nullptr;
688 35156 : }
689 :
690 :
691 : void
692 1022 : Helper::registerStateListener() {
693 1022 : if (MSNet::hasInstance()) {
694 1022 : MSNet::getInstance()->addVehicleStateListener(&myVehicleStateListener);
695 1022 : MSNet::getInstance()->addTransportableStateListener(&myTransportableStateListener);
696 : }
697 1022 : }
698 :
699 :
700 : const std::vector<std::string>&
701 140985 : Helper::getVehicleStateChanges(const MSNet::VehicleState state) {
702 140985 : return myVehicleStateListener.myVehicleStateChanges[state];
703 : }
704 :
705 :
706 : const std::vector<std::string>&
707 24 : Helper::getTransportableStateChanges(const MSNet::TransportableState state) {
708 24 : return myTransportableStateListener.myTransportableStateChanges[state];
709 : }
710 :
711 :
712 : void
713 191837 : Helper::clearStateChanges() {
714 734783 : for (auto& i : myVehicleStateListener.myVehicleStateChanges) {
715 : i.second.clear();
716 : }
717 253661 : for (auto& i : myTransportableStateListener.myTransportableStateChanges) {
718 : i.second.clear();
719 : }
720 191837 : }
721 :
722 :
723 : MSCalibrator::AspiredState
724 90 : Helper::getCalibratorState(const MSCalibrator* c) {
725 : try {
726 90 : return c->getCurrentStateInterval();
727 0 : } catch (ProcessError& e) {
728 0 : throw TraCIException(e.what());
729 0 : }
730 : }
731 :
732 :
733 : void
734 145455 : Helper::findObjectShape(int domain, const std::string& id, PositionVector& shape) {
735 145455 : switch (domain) {
736 185 : case libsumo::CMD_SUBSCRIBE_BUSSTOP_CONTEXT: {
737 185 : MSStoppingPlace* const stop = getStoppingPlace(id, SUMO_TAG_BUS_STOP);
738 185 : shape.push_back(stop->getLane().getShape().positionAtOffset(stop->getBeginLanePosition()));
739 185 : shape.push_back(stop->getLane().getShape().positionAtOffset(stop->getEndLanePosition()));
740 185 : break;
741 : }
742 185 : case libsumo::CMD_SUBSCRIBE_CALIBRATOR_CONTEXT: {
743 185 : MSCalibrator* const calib = Calibrator::getCalibrator(id);
744 185 : shape.push_back(calib->getLane()->getShape()[0]);
745 185 : break;
746 : }
747 185 : case libsumo::CMD_SUBSCRIBE_CHARGINGSTATION_CONTEXT: {
748 185 : MSStoppingPlace* const stop = getStoppingPlace(id, SUMO_TAG_CHARGING_STATION);
749 185 : shape.push_back(stop->getLane().getShape().positionAtOffset(stop->getBeginLanePosition()));
750 185 : shape.push_back(stop->getLane().getShape().positionAtOffset(stop->getEndLanePosition()));
751 185 : break;
752 : }
753 20227 : case libsumo::CMD_SUBSCRIBE_EDGE_CONTEXT:
754 20227 : Edge::storeShape(id, shape);
755 20227 : break;
756 185 : case libsumo::CMD_SUBSCRIBE_INDUCTIONLOOP_CONTEXT:
757 185 : InductionLoop::storeShape(id, shape);
758 185 : break;
759 20215 : case libsumo::CMD_SUBSCRIBE_JUNCTION_CONTEXT:
760 20215 : Junction::storeShape(id, shape);
761 20215 : break;
762 20256 : case libsumo::CMD_SUBSCRIBE_LANE_CONTEXT:
763 20256 : Lane::storeShape(id, shape);
764 20256 : break;
765 185 : case libsumo::CMD_SUBSCRIBE_LANEAREA_CONTEXT:
766 185 : LaneArea::storeShape(id, shape);
767 185 : break;
768 185 : case libsumo::CMD_SUBSCRIBE_MULTIENTRYEXIT_CONTEXT: {
769 185 : MSE3Collector* const det = MultiEntryExit::getDetector(id);
770 370 : for (const MSCrossSection& cs : det->getEntries()) {
771 370 : shape.push_back(cs.myLane->getShape().positionAtOffset(cs.myPosition));
772 : }
773 370 : for (const MSCrossSection& cs : det->getExits()) {
774 370 : shape.push_back(cs.myLane->getShape().positionAtOffset(cs.myPosition));
775 : }
776 : break;
777 : }
778 185 : case libsumo::CMD_SUBSCRIBE_PARKINGAREA_CONTEXT: {
779 185 : MSStoppingPlace* const stop = getStoppingPlace(id, SUMO_TAG_PARKING_AREA);
780 185 : shape.push_back(stop->getLane().getShape().positionAtOffset(stop->getBeginLanePosition()));
781 185 : shape.push_back(stop->getLane().getShape().positionAtOffset(stop->getEndLanePosition()));
782 185 : break;
783 : }
784 20219 : case libsumo::CMD_SUBSCRIBE_PERSON_CONTEXT:
785 20219 : Person::storeShape(id, shape);
786 20219 : break;
787 20371 : case libsumo::CMD_SUBSCRIBE_POI_CONTEXT:
788 20371 : POI::storeShape(id, shape);
789 20371 : break;
790 20257 : case libsumo::CMD_SUBSCRIBE_POLYGON_CONTEXT:
791 20257 : Polygon::storeShape(id, shape);
792 20257 : break;
793 21199 : case libsumo::CMD_SUBSCRIBE_VEHICLE_CONTEXT:
794 21199 : Vehicle::storeShape(id, shape);
795 21199 : break;
796 1416 : default:
797 1416 : Simulation::storeShape(shape);
798 1416 : break;
799 : }
800 145455 : }
801 :
802 :
803 : void
804 145455 : Helper::collectObjectIDsInRange(int domain, const PositionVector& shape, double range, std::set<std::string>& into) {
805 : std::set<const Named*> objects;
806 145455 : collectObjectsInRange(domain, shape, range, objects);
807 610529 : for (const Named* obj : objects) {
808 : into.insert(obj->getID());
809 : }
810 144289 : }
811 :
812 :
813 : void
814 155024 : Helper::collectObjectsInRange(int domain, const PositionVector& shape, double range, std::set<const Named*>& into) {
815 155024 : const Boundary b = shape.getBoxBoundary().grow(range);
816 155024 : const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
817 155024 : const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
818 155024 : switch (domain) {
819 212 : case libsumo::CMD_GET_BUSSTOP_VARIABLE:
820 848 : for (const auto& stop : MSNet::getInstance()->getStoppingPlaces(SUMO_TAG_BUS_STOP)) {
821 636 : if (shape.distance2D(stop.second->getCenterPos()) <= range) {
822 284 : into.insert(stop.second);
823 : }
824 : }
825 : break;
826 212 : case libsumo::CMD_GET_CHARGINGSTATION_VARIABLE:
827 848 : for (const auto& stop : MSNet::getInstance()->getStoppingPlaces(SUMO_TAG_CHARGING_STATION)) {
828 636 : if (shape.distance2D(stop.second->getCenterPos()) <= range) {
829 282 : into.insert(stop.second);
830 : }
831 : }
832 : break;
833 : case libsumo::CMD_GET_CALIBRATOR_VARIABLE:
834 636 : for (const auto& calib : MSCalibrator::getInstances()) {
835 424 : if (shape.distance2D(calib.second->getLane()->getShape()[0]) <= range) {
836 1390 : into.insert(calib.second);
837 : }
838 : }
839 : break;
840 212 : case libsumo::CMD_GET_INDUCTIONLOOP_VARIABLE:
841 212 : InductionLoop::getTree()->Search(cmin, cmax, Named::StoringVisitor(into));
842 212 : break;
843 212 : case libsumo::CMD_GET_JUNCTION_VARIABLE:
844 212 : Junction::getTree()->Search(cmin, cmax, Named::StoringVisitor(into));
845 212 : break;
846 212 : case libsumo::CMD_GET_LANEAREA_VARIABLE:
847 212 : LaneArea::getTree()->Search(cmin, cmax, Named::StoringVisitor(into));
848 212 : break;
849 212 : case libsumo::CMD_GET_PARKINGAREA_VARIABLE: {
850 848 : for (const auto& stop : MSNet::getInstance()->getStoppingPlaces(SUMO_TAG_PARKING_AREA)) {
851 636 : if (shape.distance2D(stop.second->getCenterPos()) <= range) {
852 279 : into.insert(stop.second);
853 : }
854 : }
855 : break;
856 : }
857 254 : case libsumo::CMD_GET_POI_VARIABLE:
858 254 : POI::getTree()->Search(cmin, cmax, Named::StoringVisitor(into));
859 254 : break;
860 236 : case libsumo::CMD_GET_POLYGON_VARIABLE:
861 236 : Polygon::getTree()->Search(cmin, cmax, Named::StoringVisitor(into));
862 236 : break;
863 151884 : case libsumo::CMD_GET_EDGE_VARIABLE:
864 : case libsumo::CMD_GET_LANE_VARIABLE:
865 : case libsumo::CMD_GET_PERSON_VARIABLE:
866 : case libsumo::CMD_GET_VEHICLE_VARIABLE: {
867 151884 : if (myLaneTree == nullptr) {
868 634 : myLaneTree = new LANE_RTREE_QUAL(&MSLane::visit);
869 634 : MSLane::fill(*myLaneTree);
870 : }
871 151884 : MSLane::StoringVisitor lsv(into, shape, range, domain);
872 151884 : myLaneTree->Search(cmin, cmax, lsv);
873 : }
874 151884 : break;
875 1166 : default:
876 2332 : throw TraCIException("Infeasible context domain (" + toString(domain) + ")");
877 : break;
878 : }
879 155024 : }
880 :
881 :
882 :
883 : void
884 148119 : Helper::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 148119 : if (s.activeFilters == 0) {
893 : // No filters set
894 143839 : return;
895 : }
896 :
897 4285 : MSVehicle* v = dynamic_cast<MSVehicle*>(getVehicle(s.id));
898 :
899 : // Whether vehicles on opposite lanes shall be taken into account
900 4285 : const bool disregardOppositeDirection = (s.activeFilters & SUBS_FILTER_NOOPPOSITE) != 0;
901 :
902 : // Check filter specification consistency
903 4285 : if (disregardOppositeDirection && (s.activeFilters & SUBS_FILTER_NO_RTREE) == 0) {
904 0 : 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;
909 4285 : if (s.activeFilters & SUBS_FILTER_NO_RTREE) {
910 : // Set defaults for upstream/downstream/lateral distances
911 4250 : double downstreamDist = s.range, upstreamDist = s.range, lateralDist = s.range;
912 4250 : if (s.activeFilters & SUBS_FILTER_DOWNSTREAM_DIST) {
913 : // Specifies maximal downstream distance for vehicles in context subscription result
914 3440 : downstreamDist = s.filterDownstreamDist;
915 : }
916 4250 : if (s.activeFilters & SUBS_FILTER_UPSTREAM_DIST) {
917 : // Specifies maximal downstream distance for vehicles in context subscription result
918 2895 : upstreamDist = s.filterUpstreamDist;
919 : }
920 4250 : if (s.activeFilters & SUBS_FILTER_LATERAL_DIST) {
921 : // Specifies maximal lateral distance for vehicles in context subscription result
922 425 : lateralDist = s.filterLateralDist;
923 : }
924 4250 : if (v == nullptr) {
925 0 : throw TraCIException("Subscription filter not yet implemented for meso vehicle");
926 : }
927 4250 : if (!v->isOnRoad()) {
928 5 : return;
929 : }
930 4245 : const MSLane* vehLane = v->getLane();
931 4245 : if (vehLane == nullptr) {
932 : return;
933 : }
934 : MSEdge* vehEdge = &vehLane->getEdge();
935 : std::vector<int> filterLanes;
936 4245 : 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 1930 : 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 3280 : 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 4245 : if ((s.activeFilters & SUBS_FILTER_MANEUVER) != 0) {
954 : // Maneuver filters disables road net search for all surrounding vehicles
955 1805 : if ((s.activeFilters & SUBS_FILTER_LEAD_FOLLOW) != 0) {
956 : // Return leader and follower on the specified lanes in context subscription result.
957 2160 : for (int offset : filterLanes) {
958 1440 : MSLane* lane = v->getLane()->getParallelLane(offset, false);
959 1440 : if (lane != nullptr) {
960 : // this is a non-opposite lane
961 1285 : MSVehicle* leader = lane->getLeader(v, v->getPositionOnLane(), v->getBestLanesContinuation(lane), downstreamDist).first;
962 1285 : MSVehicle* follower = lane->getFollower(v, v->getPositionOnLane(), upstreamDist,
963 1285 : MSLane::MinorLinkMode::FOLLOW_ALWAYS).first;
964 1285 : vehs.insert(vehs.end(), leader);
965 1285 : 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 155 : } 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 155 : const MSEdge* opposite = vehEdge->getOppositeEdge();
976 155 : if (opposite == nullptr) {
977 : #ifdef DEBUG_SURROUNDING
978 : std::cout << "No lane at index " << offset << std::endl;
979 : #endif
980 : // no opposite edge
981 0 : continue;
982 : }
983 : // Index of opposite lane at relative offset
984 155 : const int ix_opposite = (int)opposite->getLanes().size() - 1 - (vehLane->getIndex() + offset - (int)vehEdge->getLanes().size());
985 155 : 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 0 : continue;
991 : }
992 155 : lane = opposite->getLanes()[ix_opposite];
993 : // Search vehs along opposite lanes (swap upstream and downstream distance)
994 : // XXX transformations for curved geometries
995 155 : double posOnOpposite = MAX2(0., opposite->getLength() - v->getPositionOnLane());
996 : // Get leader on opposite
997 155 : vehs.insert(vehs.end(), lane->getFollower(v, posOnOpposite, downstreamDist, MSLane::MinorLinkMode::FOLLOW_NEVER).first);
998 : // Get follower (no search on consecutive lanes
999 155 : vehs.insert(vehs.end(), lane->getLeader(v, posOnOpposite - v->getLength(), std::vector<MSLane*>()).first);
1000 : }
1001 : }
1002 : }
1003 :
1004 1805 : if (s.activeFilters & SUBS_FILTER_TURN) {
1005 1090 : applySubscriptionFilterTurn(s, vehs);
1006 1090 : if (s.activeFilters & SUBS_FILTER_LANES) {
1007 275 : applySubscriptionFilterLanes(s, vehs, filterLanes, downstreamDist, upstreamDist, disregardOppositeDirection);
1008 : }
1009 1090 : if (s.activeFilters & SUBS_FILTER_LATERAL_DIST) {
1010 270 : 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 2440 : } else if (s.activeFilters & SUBS_FILTER_LATERAL_DIST) {
1020 150 : 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 2290 : applySubscriptionFilterLanes(s, vehs, filterLanes, downstreamDist, upstreamDist, disregardOppositeDirection);
1024 : }
1025 : // Write vehs IDs in objIDs
1026 15709 : for (const SUMOTrafficObject* veh : vehs) {
1027 11464 : if (veh != nullptr) {
1028 : objIDs.insert(objIDs.end(), veh->getID());
1029 : }
1030 : }
1031 : }
1032 :
1033 4280 : if (s.activeFilters & SUBS_FILTER_VCLASS) {
1034 : // Only return vehicles of the given vClass in context subscription result
1035 : auto i = objIDs.begin();
1036 749 : while (i != objIDs.end()) {
1037 584 : MSBaseVehicle* veh = getVehicle(*i);
1038 584 : if ((veh->getVehicleType().getVehicleClass() & s.filterVClasses) == 0) {
1039 : i = objIDs.erase(i);
1040 : } else {
1041 : ++i;
1042 : }
1043 : }
1044 : }
1045 4280 : if (s.activeFilters & SUBS_FILTER_VTYPE) {
1046 : // Only return vehicles of the given vType in context subscription result
1047 : auto i = objIDs.begin();
1048 749 : while (i != objIDs.end()) {
1049 584 : MSBaseVehicle* veh = getVehicle(*i);
1050 584 : if (s.filterVTypes.find(veh->getVehicleType().getID()) == s.filterVTypes.end()) {
1051 : i = objIDs.erase(i);
1052 : } else {
1053 : ++i;
1054 : }
1055 : }
1056 : }
1057 4280 : if (s.activeFilters & SUBS_FILTER_FIELD_OF_VISION) {
1058 : // Only return vehicles within field of vision in context subscription result
1059 45 : applySubscriptionFilterFieldOfVision(s, objIDs);
1060 : }
1061 : }
1062 :
1063 : void
1064 2565 : Helper::applySubscriptionFilterLanes(const Subscription& s, std::set<const SUMOTrafficObject*>& vehs, std::vector<int>& filterLanes, double downstreamDist,
1065 : double upstreamDist, bool disregardOppositeDirection) {
1066 : if (!s.isVehicleToVehicleContextSubscription()) {
1067 0 : WRITE_WARNINGF(TL("Lanes filter is only feasible for context domain 'vehicle' (current is '%'), ignoring filter..."), toHex(s.contextDomain, 2));
1068 0 : return;
1069 : }
1070 : assert(filterLanes.size() > 0);
1071 2565 : MSVehicle* v = dynamic_cast<MSVehicle*>(getVehicle(s.id));
1072 2565 : 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 7675 : for (int offset : filterLanes) {
1077 5110 : MSLane* lane = vehLane->getParallelLane(offset, false);
1078 5110 : 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 3320 : lane->getSurroundingVehicles(v->getPositionOnLane(), downstreamDist, upstreamDist + v->getLength(), checkedLanes);
1088 1660 : vehs.insert(new_vehs.begin(), new_vehs.end());
1089 4980 : fuseLaneCoverage(checkedLanesInDrivingDir, checkedLanes);
1090 3450 : } 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 2505 : const MSEdge* opposite = vehEdge->getOppositeEdge();
1094 2505 : 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 450 : continue;
1100 : }
1101 : // Index of opposite lane at relative offset
1102 2505 : const int ix_opposite = (int)opposite->getLanes().size() - 1 - (vehLane->getIndex() + offset - (int)vehEdge->getLanes().size());
1103 2505 : 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 450 : continue;
1109 : }
1110 2055 : lane = opposite->getLanes()[ix_opposite];
1111 : // Search vehs along opposite lanes (swap upstream and downstream distance)
1112 2055 : const std::set<MSVehicle*> new_vehs = lane->getSurroundingVehicles(lane->getLength() - v->getPositionOnLane(), upstreamDist + v->getLength(),
1113 4110 : downstreamDist, std::make_shared<LaneCoverageInfo>());
1114 2055 : 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 4660 : 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 4175 : 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 4175 : if (nOpp > 0) {
1132 8506 : for (auto& laneCov : *checkedLanesInDrivingDir) {
1133 5086 : const MSLane* const l = laneCov.first;
1134 5086 : if (l == nullptr || l->getEdge().getOppositeEdge() == nullptr) {
1135 : continue;
1136 : }
1137 3618 : const MSEdge* opposite = l->getEdge().getOppositeEdge();
1138 : const std::pair<double, double>& range = laneCov.second;
1139 : auto leftMostOppositeLaneIt = opposite->getLanes().rbegin();
1140 7236 : for (auto oppositeLaneIt = leftMostOppositeLaneIt; oppositeLaneIt != opposite->getLanes().rend(); ++oppositeLaneIt) {
1141 5798 : if ((int)(oppositeLaneIt - leftMostOppositeLaneIt) == nOpp) {
1142 : break;
1143 : }
1144 : // Add vehicles from corresponding range on opposite direction
1145 3618 : const MSLane* oppositeLane = *oppositeLaneIt;
1146 3618 : auto new_vehs = oppositeLane->getVehiclesInRange(l->getLength() - range.second, l->getLength() - range.first);
1147 3618 : 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 :
1161 : void
1162 1090 : Helper::applySubscriptionFilterTurn(const Subscription& s, std::set<const SUMOTrafficObject*>& vehs) {
1163 : if (!s.isVehicleToVehicleContextSubscription()) {
1164 280 : WRITE_WARNINGF(TL("Turn filter is only feasible for context domain 'vehicle' (current is '%'), ignoring filter..."), toHex(s.contextDomain, 2));
1165 140 : 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 950 : MSVehicle* v = dynamic_cast<MSVehicle*>(getVehicle(s.id));
1169 950 : const MSLane* lane = v->getLane();
1170 950 : 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 1710 : 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 5740 : for (auto& foeLane : l->getFoeLanes()) {
1180 4980 : if (foeLane->getEdge().isCrossing()) {
1181 : #ifdef DEBUG_SURROUNDING
1182 : std::cout << " skipping crossing foeLane '" << foeLane->getID() << "'" << std::endl;
1183 : #endif
1184 150 : 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 4830 : const MSLink* foeLink = foeLane->getEntryLink();
1191 6320 : for (auto& vi : foeLink->getApproaching()) {
1192 1490 : 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 1135 : vehs.insert(vehs.end(), dynamic_cast<const MSVehicle*>(vi.first));
1197 : }
1198 : }
1199 : // add vehicles currently on the junction
1200 4850 : for (const MSVehicle* foe : foeLane->getVehiclesSecure()) {
1201 : #ifdef DEBUG_SURROUNDING
1202 : std::cout << " On foeLane '" << foe->getID() << "'" << std::endl;
1203 : #endif
1204 20 : vehs.insert(vehs.end(), foe);
1205 : }
1206 4830 : foeLane->releaseVehicles();
1207 9660 : for (auto& laneInfo : foeLane->getIncomingLanes()) {
1208 4830 : const MSLane* foeLanePred = laneInfo.lane;
1209 4830 : if (foeLanePred->isInternal()) {
1210 : #ifdef DEBUG_SURROUNDING
1211 : std::cout << " foeLanePred '" << foeLanePred->getID() << "'" << std::endl;
1212 : #endif
1213 1135 : for (const MSVehicle* foe : foeLanePred->getVehiclesSecure()) {
1214 : #ifdef DEBUG_SURROUNDING
1215 : std::cout << " On foeLanePred '" << foe->getID() << "'" << std::endl;
1216 : #endif
1217 155 : vehs.insert(vehs.end(), foe);
1218 : }
1219 980 : foeLanePred->releaseVehicles();
1220 : }
1221 : }
1222 : }
1223 : }
1224 : }
1225 :
1226 : void
1227 45 : Helper::applySubscriptionFilterFieldOfVision(const Subscription& s, std::set<std::string>& objIDs) {
1228 45 : if (s.filterFieldOfVisionOpeningAngle <= 0. || s.filterFieldOfVisionOpeningAngle >= 360.) {
1229 10 : WRITE_WARNINGF(TL("Field of vision opening angle ('%') should be within interval (0, 360), ignoring filter..."), s.filterFieldOfVisionOpeningAngle);
1230 10 : return;
1231 : }
1232 :
1233 35 : MSBaseVehicle* egoVehicle = getVehicle(s.id);
1234 35 : Position egoPosition = egoVehicle->getPosition();
1235 35 : 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 134 : while (i != objIDs.end()) {
1243 99 : if (s.id.compare(*i) == 0) { // skip if this is the ego vehicle
1244 : ++i;
1245 29 : continue;
1246 : }
1247 70 : SUMOTrafficObject* obj = getTrafficObject(s.contextDomain, *i);
1248 70 : double angleEgoToVeh = egoPosition.angleTo2D(obj->getPosition());
1249 70 : 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 70 : if (abs(alpha) > openingAngle * 0.5) {
1258 : i = objIDs.erase(i);
1259 : } else {
1260 : ++i;
1261 : }
1262 : }
1263 : }
1264 :
1265 : void
1266 420 : Helper::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 420 : PositionVector vehShape;
1270 420 : findObjectShape(s.commandId, s.id, vehShape);
1271 : double range = MAX3(downstreamDist, upstreamDist, lateralDist);
1272 : std::set<std::string> objIDs;
1273 420 : 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 420 : 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 420 : double frontPosOnLane = v->getPositionOnLane();
1288 420 : if (v->getLaneChangeModel().isOpposite()) {
1289 30 : 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 420 : const double backPosOnLane = MAX2(0.0, frontPosOnLane - v->getVehicleType().getLength());
1293 420 : applySubscriptionFilterLateralDistanceSinglePass(s, objIDs, vehs, v->getUpcomingLanesUntil(downstreamDist), backPosOnLane, v->getLateralPositionOnLane(),
1294 : true);
1295 : // 2nd pass: upstream
1296 840 : applySubscriptionFilterLateralDistanceSinglePass(s, objIDs, vehs, v->getPastLanesUntil(upstreamDist), frontPosOnLane, v->getLateralPositionOnLane(), false);
1297 420 : }
1298 :
1299 : void
1300 840 : Helper::applySubscriptionFilterLateralDistanceSinglePass(const Subscription& s, std::set<std::string>& objIDs,
1301 : std::set<const SUMOTrafficObject*>& vehs,
1302 : const std::vector<const MSLane*>& lanes, double posOnLane, double posLat, bool isDownstream) {
1303 840 : const double streamDist = isDownstream ? s.filterDownstreamDist : s.filterUpstreamDist;
1304 : double distRemaining = streamDist;
1305 : bool isFirstLane = true;
1306 840 : PositionVector combinedShape;
1307 1565 : 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 1355 : if (isFirstLane) {
1314 : isFirstLane = false;
1315 : double geometryPos = lane->interpolateLanePosToGeometryPos(posOnLane);
1316 705 : if (geometryPos <= POSITION_EPS) {
1317 50 : if (!isDownstream) {
1318 : continue;
1319 : }
1320 : } else {
1321 655 : if (geometryPos >= laneShape.length() - POSITION_EPS) {
1322 0 : laneShape = isDownstream ? PositionVector() : laneShape;
1323 : } else {
1324 655 : auto pair = laneShape.splitAt(geometryPos, false);
1325 655 : laneShape = isDownstream ? pair.second : pair.first;
1326 : }
1327 : }
1328 : }
1329 1355 : double laneLength = lane->interpolateGeometryPosToLanePos(laneShape.length());
1330 1355 : if (distRemaining - laneLength < 0.) {
1331 610 : double geometryPos = lane->interpolateLanePosToGeometryPos(isDownstream ? distRemaining : laneLength - distRemaining);
1332 610 : if (geometryPos > POSITION_EPS && geometryPos < laneShape.length() - POSITION_EPS) {
1333 610 : auto pair = laneShape.splitAt(geometryPos, false);
1334 610 : laneShape = isDownstream ? pair.first : pair.second;
1335 : }
1336 : }
1337 : distRemaining -= laneLength;
1338 : try {
1339 1355 : laneShape.move2side(-posLat);
1340 0 : } catch (ProcessError&) {
1341 0 : WRITE_WARNINGF(TL("addSubscriptionFilterLateralDistance could not determine shape of lane '%' with a lateral shift of %."),
1342 : lane->getID(), toString(posLat));
1343 0 : }
1344 : #ifdef DEBUG_SURROUNDING
1345 : std::cout << " posLat=" << posLat << " laneShape=" << laneShape << "\n";
1346 : #endif
1347 1355 : if (isDownstream) {
1348 885 : combinedShape.append(laneShape);
1349 : } else {
1350 470 : combinedShape.prepend(laneShape);
1351 : }
1352 1355 : if (distRemaining <= POSITION_EPS) {
1353 : break;
1354 : }
1355 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 3045 : while (i != objIDs.end()) {
1362 2205 : SUMOTrafficObject* obj = getTrafficObject(s.contextDomain, *i);
1363 2205 : 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 2205 : if ((minPerpendicularDist != GeomHelper::INVALID_OFFSET) && (minPerpendicularDist <= s.filterLateralDist)) {
1368 : vehs.insert(obj);
1369 : i = objIDs.erase(i);
1370 : } else {
1371 : ++i;
1372 : }
1373 : }
1374 840 : }
1375 :
1376 : void
1377 8769 : Helper::setRemoteControlled(MSVehicle* v, Position xyPos, MSLane* l, double pos, double posLat, double angle,
1378 : int edgeOffset, ConstMSEdgeVector route, SUMOTime t) {
1379 8769 : myRemoteControlledVehicles[v->getID()] = v;
1380 8769 : v->getInfluencer().setRemoteControlled(xyPos, l, pos, posLat, angle, edgeOffset, route, t);
1381 8769 : }
1382 :
1383 : void
1384 16339 : Helper::setRemoteControlled(MSPerson* p, Position xyPos, MSLane* l, double pos, double posLat, double angle,
1385 : int edgeOffset, ConstMSEdgeVector route, SUMOTime t) {
1386 16339 : myRemoteControlledPersons[p->getID()] = p;
1387 16339 : p->getInfluencer().setRemoteControlled(xyPos, l, pos, posLat, angle, edgeOffset, route, t);
1388 16339 : }
1389 :
1390 :
1391 : int
1392 74944773 : Helper::postProcessRemoteControl() {
1393 : int numControlled = 0;
1394 74953512 : for (auto& controlled : myRemoteControlledVehicles) {
1395 8739 : if (MSNet::getInstance()->getVehicleControl().getVehicle(controlled.first) != nullptr) {
1396 8739 : controlled.second->getInfluencer().postProcessRemoteControl(controlled.second);
1397 8739 : numControlled++;
1398 : } else {
1399 0 : WRITE_WARNINGF(TL("Vehicle '%' was removed though being controlled by TraCI"), controlled.first);
1400 : }
1401 : }
1402 : myRemoteControlledVehicles.clear();
1403 74961112 : for (auto& controlled : myRemoteControlledPersons) {
1404 16339 : if (MSNet::getInstance()->getPersonControl().get(controlled.first) != nullptr) {
1405 16339 : controlled.second->getInfluencer().postProcessRemoteControl(controlled.second);
1406 16339 : numControlled++;
1407 : } else {
1408 0 : WRITE_WARNINGF(TL("Person '%' was removed though being controlled by TraCI"), controlled.first);
1409 : }
1410 : }
1411 : myRemoteControlledPersons.clear();
1412 74944773 : return numControlled;
1413 : }
1414 :
1415 :
1416 : bool
1417 5632 : Helper::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 5632 : const MSEdge* const currentRouteEdge = currentRoute[routePosition];
1426 : std::set<const Named*> into;
1427 5632 : PositionVector shape;
1428 5632 : shape.push_back(pos);
1429 5632 : 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 234148 : for (const Named* namedEdge : into) {
1434 228516 : const MSEdge* e = dynamic_cast<const MSEdge*>(namedEdge);
1435 228516 : if ((e->getPermissions() & vClass) != vClass) {
1436 19876 : continue;
1437 : }
1438 208640 : 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 208640 : if (e->isWalkingArea() || e->isCrossing()) {
1444 : // find current intersection along the route
1445 : const MSJunction* junction = e->getFromJunction();
1446 16856 : for (int i = routePosition; i < (int)currentRoute.size(); i++) {
1447 11943 : const MSEdge* cand = currentRoute[i];
1448 11943 : if (cand->getToJunction() == junction) {
1449 5709 : prevEdge = cand;
1450 5709 : if (i + 1 < (int)currentRoute.size()) {
1451 : onRoute = true;
1452 4521 : nextEdge = currentRoute[i + 1];
1453 : }
1454 : break;
1455 : }
1456 : }
1457 10622 : if (!onRoute) {
1458 : // search backward
1459 6983 : for (int i = routePosition - 1; i >= 0; i--) {
1460 4482 : const MSEdge* cand = currentRoute[i];
1461 4482 : if (cand->getToJunction() == junction) {
1462 : onRoute = true;
1463 3600 : prevEdge = cand;
1464 3600 : nextEdge = currentRoute[i + 1];
1465 3600 : break;
1466 : }
1467 : }
1468 : }
1469 10622 : if (prevEdge == nullptr) {
1470 : // use arbitrary predecessor
1471 1313 : if (e->getPredecessors().size() > 0) {
1472 1313 : prevEdge = e->getPredecessors().front();
1473 0 : } else if (e->getSuccessors().size() > 1) {
1474 0 : for (MSEdge* e2 : e->getSuccessors()) {
1475 0 : if (e2 != nextEdge) {
1476 0 : prevEdge = e2;
1477 0 : break;
1478 : }
1479 : }
1480 : }
1481 : }
1482 10622 : if (nextEdge == nullptr) {
1483 2501 : if (e->getSuccessors().size() > 0) {
1484 2435 : nextEdge = e->getSuccessors().front();
1485 66 : } else if (e->getPredecessors().size() > 1) {
1486 66 : for (MSEdge* e2 : e->getPredecessors()) {
1487 66 : 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 198018 : } 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 51332 : if (onRoad && currentLane->getEdge().isInternal()) {
1506 : ++searchStart;
1507 : }
1508 51332 : ConstMSEdgeVector::const_iterator edgePos = std::find(searchStart, currentRoute.end(), e);
1509 : onRoute = edgePos != currentRoute.end(); // no? -> onRoute is false
1510 51332 : if (edgePos == currentRoute.end() - 1 && currentRouteEdge == e) {
1511 : // onRoute is false as well if the vehicle is beyond the edge
1512 2223 : onRoute &= currentRouteEdge->getLength() > currentLanePos + SPEED2DIST(speed);
1513 : }
1514 : // save prior and next edges
1515 51332 : prevEdge = e;
1516 51332 : 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 146686 : } else if (e->isInternal()) {
1521 : // an internal edge
1522 : // get the previous edge
1523 146686 : prevEdge = e;
1524 339646 : while (prevEdge != nullptr && prevEdge->isInternal()) {
1525 192960 : MSLane* l = prevEdge->getLanes()[0];
1526 192960 : l = l->getLogicalPredecessorLane();
1527 385920 : prevEdge = l == nullptr ? nullptr : &l->getEdge();
1528 : }
1529 : // check whether the previous edge is on the route (was on the route)
1530 146686 : ConstMSEdgeVector::const_iterator prevEdgePos = std::find(currentRoute.begin() + routePosition, currentRoute.end(), prevEdge);
1531 : nextEdge = e;
1532 293372 : while (nextEdge != nullptr && nextEdge->isInternal()) {
1533 146686 : nextEdge = nextEdge->getSuccessors()[0]; // should be only one for an internal edge
1534 : }
1535 146686 : if (prevEdgePos != currentRoute.end() && (prevEdgePos + 1) != currentRoute.end()) {
1536 17555 : 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 479010 : for (MSLane* const l : e->getLanes()) {
1547 270370 : if (!l->allowsVehicleClass(vClass)) {
1548 28976 : continue;
1549 : }
1550 241682 : if (l->getShape().length() == 0) {
1551 : // mapping to shapeless lanes is a bad idea
1552 288 : 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 241394 : const double slack = POSITION_EPS * TS;
1560 241394 : PositionVector laneShape = l->getShape();
1561 241394 : laneShape.extrapolate2D(slack);
1562 241394 : double off = laneShape.nearest_offset_to_point2D(pos, true);
1563 241394 : if (off != GeomHelper::INVALID_OFFSET) {
1564 69314 : perpendicularDist = laneShape.distance2D(pos, true);
1565 : }
1566 241394 : off = l->getShape().nearest_offset_to_point2D(pos, perpendicular);
1567 241394 : if (off != GeomHelper::INVALID_OFFSET) {
1568 241394 : dist = l->getShape().distance2D(pos, perpendicular);
1569 241394 : langle = GeomHelper::naviDegree(l->getShape().rotationAtOffset(off));
1570 : }
1571 : // cannot trust lanePos on walkingArea
1572 241394 : bool sameEdge = onRoad && e == ¤tLane->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 241394 : if (mayLeaveNetwork && fabs(dist - perpendicularDist) > slack) {
1582 : // ambiguous mapping. Don't trust this
1583 : dist2 = FAR_AWAY;
1584 : }
1585 241394 : 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 482788 : bool origIDMatch = l->getParameter(SUMO_PARAM_ORIGID, l->getID()) == origID;
1599 241394 : if (origIDMatch && setLateralPos
1600 241394 : && perpendicularDist > l->getWidth() / 2) {
1601 : origIDMatch = false;
1602 : }
1603 241394 : 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 241394 : }
1611 : }
1612 :
1613 : // get the best lane given the previously computed values
1614 : double bestValue = 0;
1615 5632 : MSLane* bestLane = nullptr;
1616 247026 : for (const auto& it : lane2utility) {
1617 241394 : MSLane* const l = it.first;
1618 : const LaneUtility& u = it.second;
1619 241394 : double distN = u.dist > 999 ? -10 : 1. - (u.dist / maxDist);
1620 241394 : double angleDiffN = 1. - (u.angleDiff / 180.);
1621 241394 : double idN = u.ID ? 1 : 0;
1622 241394 : double onRouteN = u.onRoute ? 1 : 0;
1623 248830 : 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 241394 : double value = (distN * .5 / TS
1627 241394 : + angleDiffN * 0.35 /*.5 */
1628 241394 : + idN * 1
1629 241394 : + onRouteN * 0.1
1630 241394 : + 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 241394 : if (value > bestValue || bestLane == nullptr) {
1636 : bestValue = value;
1637 40288 : if (u.dist == FAR_AWAY) {
1638 13662 : bestLane = nullptr;
1639 : } else {
1640 26626 : bestLane = l;
1641 : }
1642 : }
1643 : }
1644 : // no best lane found, return
1645 5632 : if (bestLane == nullptr) {
1646 : return false;
1647 : }
1648 : const LaneUtility& u = lane2utility.find(bestLane)->second;
1649 5462 : bestDistance = u.dist;
1650 5462 : *lane = bestLane;
1651 5462 : lanePos = MAX2(0., MIN2(double((*lane)->getLength() - NUMERICAL_EPS),
1652 : bestLane->interpolateGeometryPosToLanePos(
1653 : bestLane->getShape().nearest_offset_to_point25D(pos, false))));
1654 5462 : const MSEdge* prevEdge = u.prevEdge;
1655 5462 : if (u.onRoute) {
1656 5287 : ConstMSEdgeVector::const_iterator prevEdgePos = std::find(currentRoute.begin(), currentRoute.end(), prevEdge);
1657 5287 : 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 175 : edges.push_back(u.prevEdge);
1661 : /*
1662 : if(bestLane->getEdge().getPurpose()!=MSEdge::EDGEFUNCTION_INTERNAL) {
1663 : edges.push_back(&bestLane->getEdge());
1664 : }
1665 : */
1666 175 : if (u.nextEdge != nullptr) {
1667 55 : edges.push_back(u.nextEdge);
1668 : }
1669 175 : 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 5632 : }
1676 :
1677 :
1678 : bool
1679 980533 : Helper::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 980533 : if (edge == nullptr) {
1683 : return false;
1684 : }
1685 : bool newBest = false;
1686 1291953 : for (MSLane* const candidateLane : edge->getLanes()) {
1687 655045 : if (!candidateLane->allowsVehicleClass(vClass)) {
1688 6033 : continue;
1689 : }
1690 649012 : if (candidateLane->getShape().length() == 0) {
1691 : // mapping to shapeless lanes is a bad idea
1692 472 : continue;
1693 : }
1694 648540 : 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 648540 : if (dist < bestDistance || (dist == bestDistance && candidateLane->getNumericalID() < (*lane)->getNumericalID())) {
1699 : // is the new distance the best one? keep then...
1700 23476 : bestDistance = dist;
1701 23476 : *lane = candidateLane;
1702 : newBest = true;
1703 : }
1704 : }
1705 636908 : 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 8152 : for (const MSLane* const l : edge->getLanes()) {
1708 5435 : if (l->getIndex() == 0) {
1709 2717 : continue;
1710 : }
1711 5436 : for (const MSLink* const link : l->getLinkCont()) {
1712 2718 : if (link->isInternalJunctionLink()) {
1713 21 : if (findCloserLane(&link->getViaLane()->getEdge(), pos, vClass, bestDistance, lane)) {
1714 : newBest = true;
1715 : }
1716 : }
1717 : }
1718 : }
1719 : }
1720 : return newBest;
1721 : }
1722 :
1723 :
1724 : bool
1725 19490 : Helper::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 19490 : 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 271763 : for (int i = routeIndex; i < (int)currentRoute.size(); ++i) {
1740 252273 : const MSEdge* cand = currentRoute[i];
1741 683325 : while (prev != nullptr) {
1742 : // check internal edge(s)
1743 431052 : 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 431052 : findCloserLane(internalCand, pos, vClass, bestDistance, lane);
1748 : prev = internalCand;
1749 : }
1750 252273 : if (findCloserLane(cand, pos, vClass, bestDistance, lane)) {
1751 20761 : routeOffset = i;
1752 : }
1753 : prev = cand;
1754 : }
1755 : // look backward along the route
1756 19490 : const MSEdge* next = currentRoute[routeIndex];
1757 130332 : for (int i = routeIndex; i >= 0; --i) {
1758 110842 : const MSEdge* cand = currentRoute[i];
1759 : //std::cout << " next=" << next->getID() << " cand=" << cand->getID() << " i=" << i << " routeIndex=" << routeIndex << "\n";
1760 : prev = cand;
1761 293287 : while (prev != nullptr) {
1762 : // check internal edge(s)
1763 182445 : const MSEdge* internalCand = prev->getInternalFollowingEdge(next, vClass);
1764 182445 : if (findCloserLane(internalCand, pos, vClass, bestDistance, lane)) {
1765 12 : routeOffset = i;
1766 : }
1767 : prev = internalCand;
1768 : }
1769 110842 : if (findCloserLane(cand, pos, vClass, bestDistance, lane)) {
1770 6 : routeOffset = i;
1771 : }
1772 : next = cand;
1773 : }
1774 19490 : if (vClass == SVC_PEDESTRIAN) {
1775 : // consider all crossings and walkingareas along the route
1776 : std::map<const MSJunction*, int> routeJunctions;
1777 1043 : for (int i = 0; i < (int)currentRoute.size() - 1; ++i) {
1778 516 : routeJunctions[currentRoute[i]->getToJunction()] = i;
1779 : }
1780 : std::set<const Named*> into;
1781 527 : PositionVector shape;
1782 527 : shape.push_back(pos);
1783 527 : collectObjectsInRange(libsumo::CMD_GET_LANE_VARIABLE, shape, 100, into);
1784 45031 : for (const Named* named : into) {
1785 44504 : const MSLane* cand = dynamic_cast<const MSLane*>(named);
1786 41576 : if ((cand->getEdge().isWalkingArea() || cand->getEdge().isCrossing())
1787 44504 : && routeJunctions.count(cand->getEdge().getToJunction()) != 0) {
1788 3900 : if (findCloserLane(&cand->getEdge(), pos, vClass, bestDistance, lane)) {
1789 270 : routeOffset = routeJunctions[cand->getEdge().getToJunction()];
1790 : }
1791 : }
1792 : }
1793 527 : }
1794 :
1795 : assert(lane != nullptr);
1796 : // quit if no solution was found, reporting a failure
1797 19490 : 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 19490 : if (!(*lane)->getEdge().isInternal()) {
1808 : const std::vector<MSLane*>& lanes = (*lane)->getEdge().getLanes();
1809 36819 : for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
1810 21113 : if ((*i)->getParameter(SUMO_PARAM_ORIGID, (*i)->getID()) == origID) {
1811 2652 : if (setLateralPos) {
1812 : // vehicle might end up on top of another lane with a big
1813 : // lateral offset to the lane with origID.
1814 2595 : const double dist = (*i)->getShape().distance2D(pos); // get distance
1815 2595 : if (dist < (*i)->getWidth() / 2) {
1816 2589 : *lane = *i;
1817 2589 : break;
1818 : }
1819 : } else {
1820 57 : *lane = *i;
1821 57 : break;
1822 : }
1823 : }
1824 : }
1825 : }
1826 : // check position, stuff, we should have the best lane along the route
1827 19490 : 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 19490 : return true;
1835 : }
1836 :
1837 :
1838 6408 : Helper::SubscriptionWrapper::SubscriptionWrapper(VariableWrapper::SubscriptionHandler handler, SubscriptionResults& into, ContextSubscriptionResults& context)
1839 6408 : : VariableWrapper(handler), myResults(into), myContextResults(context), myActiveResults(&into) {
1840 :
1841 6408 : }
1842 :
1843 :
1844 : void
1845 93253 : Helper::SubscriptionWrapper::setContext(const std::string* const refID) {
1846 93253 : myActiveResults = refID == nullptr ? &myResults : &myContextResults[*refID];
1847 93253 : }
1848 :
1849 :
1850 : void
1851 1098936 : Helper::SubscriptionWrapper::clear() {
1852 1098936 : myActiveResults = &myResults;
1853 : myResults.clear();
1854 1098936 : myContextResults.clear();
1855 1098936 : }
1856 :
1857 :
1858 : bool
1859 32738 : Helper::SubscriptionWrapper::wrapDouble(const std::string& objID, const int variable, const double value) {
1860 32738 : (*myActiveResults)[objID][variable] = std::make_shared<TraCIDouble>(value);
1861 32738 : return true;
1862 : }
1863 :
1864 :
1865 : bool
1866 13748 : Helper::SubscriptionWrapper::wrapInt(const std::string& objID, const int variable, const int value) {
1867 13748 : (*myActiveResults)[objID][variable] = std::make_shared<TraCIInt>(value);
1868 13748 : return true;
1869 : }
1870 :
1871 :
1872 : bool
1873 27128 : Helper::SubscriptionWrapper::wrapString(const std::string& objID, const int variable, const std::string& value) {
1874 27128 : (*myActiveResults)[objID][variable] = std::make_shared<TraCIString>(value);
1875 27128 : return true;
1876 : }
1877 :
1878 :
1879 : bool
1880 714 : Helper::SubscriptionWrapper::wrapStringList(const std::string& objID, const int variable, const std::vector<std::string>& value) {
1881 : auto sl = std::make_shared<TraCIStringList>();
1882 714 : sl->value = value;
1883 714 : (*myActiveResults)[objID][variable] = sl;
1884 714 : return true;
1885 : }
1886 :
1887 :
1888 : bool
1889 0 : Helper::SubscriptionWrapper::wrapDoubleList(const std::string& objID, const int variable, const std::vector<double>& value) {
1890 : auto sl = std::make_shared<TraCIDoubleList>();
1891 0 : sl->value = value;
1892 0 : (*myActiveResults)[objID][variable] = sl;
1893 0 : return true;
1894 : }
1895 :
1896 :
1897 : bool
1898 185665 : Helper::SubscriptionWrapper::wrapPosition(const std::string& objID, const int variable, const TraCIPosition& value) {
1899 185665 : (*myActiveResults)[objID][variable] = std::make_shared<TraCIPosition>(value);
1900 185665 : return true;
1901 : }
1902 :
1903 :
1904 : bool
1905 0 : Helper::SubscriptionWrapper::wrapPositionVector(const std::string& objID, const int variable, const TraCIPositionVector& value) {
1906 0 : (*myActiveResults)[objID][variable] = std::make_shared<TraCIPositionVector>(value);
1907 0 : return true;
1908 : }
1909 :
1910 :
1911 : bool
1912 0 : Helper::SubscriptionWrapper::wrapColor(const std::string& objID, const int variable, const TraCIColor& value) {
1913 0 : (*myActiveResults)[objID][variable] = std::make_shared<TraCIColor>(value);
1914 0 : return true;
1915 : }
1916 :
1917 :
1918 : bool
1919 9076 : Helper::SubscriptionWrapper::wrapStringDoublePair(const std::string& objID, const int variable, const std::pair<std::string, double>& value) {
1920 9076 : (*myActiveResults)[objID][variable] = std::make_shared<TraCIRoadPosition>(value.first, value.second);
1921 9076 : return true;
1922 : }
1923 :
1924 :
1925 : bool
1926 42 : Helper::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 42 : sl->value.push_back(value.first);
1929 42 : sl->value.push_back(value.second);
1930 42 : (*myActiveResults)[objID][variable] = sl;
1931 42 : return true;
1932 : }
1933 :
1934 :
1935 : void
1936 4416 : Helper::SubscriptionWrapper::empty(const std::string& objID) {
1937 4416 : (*myActiveResults)[objID]; // initiate the empty map to track the objectID for TRACI_ID_LIST context subscriptions
1938 4416 : }
1939 :
1940 :
1941 : void
1942 42936 : Helper::VehicleStateListener::vehicleStateChanged(const SUMOVehicle* const vehicle, MSNet::VehicleState to, const std::string& /*info*/) {
1943 42936 : myVehicleStateChanges[to].push_back(vehicle->getID());
1944 42936 : }
1945 :
1946 :
1947 : void
1948 789 : Helper::TransportableStateListener::transportableStateChanged(const MSTransportable* const transportable, MSNet::TransportableState to, const std::string& /*info*/) {
1949 789 : myTransportableStateChanges[to].push_back(transportable->getID());
1950 789 : }
1951 :
1952 :
1953 : }
1954 :
1955 :
1956 : /****************************************************************************/
|
