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