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