1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2017-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
10 /// @file    Helper.cpp
11 /// @author  Laura Bieker-Walz
12 /// @author  Robert Hilbrich
13 /// @author  Leonhard Luecken
14 /// @date    15.09.2017
15 /// @version $Id$
16 ///
17 // C++ TraCI client API implementation
18 /****************************************************************************/
19 
20 // ===========================================================================
21 // included modules
22 // ===========================================================================
23 #include <config.h>
24 
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/MSTransportableControl.h>
30 #include <microsim/MSEdgeControl.h>
31 #include <microsim/MSInsertionControl.h>
32 #include <microsim/MSEdge.h>
33 #include <microsim/MSLane.h>
34 #include <microsim/MSVehicle.h>
35 #include <microsim/MSTransportable.h>
36 #include <microsim/pedestrians/MSPerson.h>
37 #include <libsumo/TraCIDefs.h>
38 #include <libsumo/Edge.h>
39 #include <libsumo/InductionLoop.h>
40 #include <libsumo/Junction.h>
41 #include <libsumo/Lane.h>
42 #include <libsumo/LaneArea.h>
43 #include <libsumo/MultiEntryExit.h>
44 #include <libsumo/Person.h>
45 #include <libsumo/POI.h>
46 #include <libsumo/Polygon.h>
47 #include <libsumo/Route.h>
48 #include <libsumo/Simulation.h>
49 #include <libsumo/TrafficLight.h>
50 #include <libsumo/Vehicle.h>
51 #include <libsumo/VehicleType.h>
52 #include <libsumo/TraCIConstants.h>
53 #include "Helper.h"
54 
55 #define FAR_AWAY 1000.0
56 
57 //#define DEBUG_MOVEXY
58 //#define DEBUG_MOVEXY_ANGLE
59 //#define DEBUG_SURROUNDING
60 
61 
62 void
add(const MSLane * const l) const63 LaneStoringVisitor::add(const MSLane* const l) const {
64     switch (myDomain) {
65         case libsumo::CMD_GET_VEHICLE_VARIABLE: {
66             const MSLane::VehCont& vehs = l->getVehiclesSecure();
67             for (MSLane::VehCont::const_iterator j = vehs.begin(); j != vehs.end(); ++j) {
68                 if (myShape.distance2D((*j)->getPosition()) <= myRange) {
69                     myIDs.insert((*j)->getID());
70                 }
71             }
72             l->releaseVehicles();
73         }
74         break;
75         case libsumo::CMD_GET_PERSON_VARIABLE: {
76             l->getVehiclesSecure();
77             std::vector<MSTransportable*> persons = l->getEdge().getSortedPersons(MSNet::getInstance()->getCurrentTimeStep(), true);
78             for (auto p : persons) {
79                 if (myShape.distance2D(p->getPosition()) <= myRange) {
80                     myIDs.insert(p->getID());
81                 }
82             }
83             l->releaseVehicles();
84         }
85         break;
86         case libsumo::CMD_GET_EDGE_VARIABLE: {
87             if (myShape.size() != 1 || l->getShape().distance2D(myShape[0]) <= myRange) {
88                 myIDs.insert(l->getEdge().getID());
89             }
90         }
91         break;
92         case libsumo::CMD_GET_LANE_VARIABLE: {
93             if (myShape.size() != 1 || l->getShape().distance2D(myShape[0]) <= myRange) {
94                 myIDs.insert(l->getID());
95             }
96         }
97         break;
98         default:
99             break;
100 
101     }
102 }
103 
104 namespace libsumo {
105 // ===========================================================================
106 // static member initializations
107 // ===========================================================================
108 std::vector<Subscription> Helper::mySubscriptions;
109 std::map<int, std::shared_ptr<VariableWrapper> > Helper::myWrapper;
110 Helper::VehicleStateListener Helper::myVehicleStateListener;
111 std::map<int, NamedRTree*> Helper::myObjects;
112 LANE_RTREE_QUAL* Helper::myLaneTree;
113 std::map<std::string, MSVehicle*> Helper::myRemoteControlledVehicles;
114 std::map<std::string, MSPerson*> Helper::myRemoteControlledPersons;
115 
116 
117 // ===========================================================================
118 // static member definitions
119 // ===========================================================================
120 void
subscribe(const int commandId,const std::string & id,const std::vector<int> & variables,const double beginTime,const double endTime,const int contextDomain,const double range)121 Helper::subscribe(const int commandId, const std::string& id, const std::vector<int>& variables,
122                   const double beginTime, const double endTime, const int contextDomain, const double range) {
123     std::vector<std::vector<unsigned char> > parameters;
124     const SUMOTime begin = beginTime == INVALID_DOUBLE_VALUE ? 0 : TIME2STEPS(beginTime);
125     const SUMOTime end = endTime == INVALID_DOUBLE_VALUE || endTime > STEPS2TIME(SUMOTime_MAX) ? SUMOTime_MAX : TIME2STEPS(endTime);
126     libsumo::Subscription s(commandId, id, variables, parameters, begin, end, contextDomain, range);
127     mySubscriptions.push_back(s);
128     handleSingleSubscription(s);
129 }
130 
131 
132 void
handleSubscriptions(const SUMOTime t)133 Helper::handleSubscriptions(const SUMOTime t) {
134     for (const libsumo::Subscription& s : mySubscriptions) {
135         if (s.beginTime > t) {
136             continue;
137         }
138         handleSingleSubscription(s);
139     }
140 }
141 
142 
143 void
handleSingleSubscription(const Subscription & s)144 Helper::handleSingleSubscription(const Subscription& s) {
145     const int getCommandId = s.contextDomain > 0 ? s.contextDomain : s.commandId - 0x30;
146     std::set<std::string> objIDs;
147     if (s.contextDomain > 0) {
148         if ((s.activeFilters & SUBS_FILTER_NO_RTREE) == 0) {
149             PositionVector shape;
150             findObjectShape(s.commandId, s.id, shape);
151             collectObjectsInRange(s.contextDomain, shape, s.range, objIDs);
152         }
153         applySubscriptionFilters(s, objIDs);
154     } else {
155         objIDs.insert(s.id);
156     }
157     if (myWrapper.empty()) {
158         myWrapper[libsumo::CMD_GET_EDGE_VARIABLE] = Edge::makeWrapper();
159         myWrapper[libsumo::CMD_GET_INDUCTIONLOOP_VARIABLE] = InductionLoop::makeWrapper();
160         myWrapper[libsumo::CMD_GET_JUNCTION_VARIABLE] = Junction::makeWrapper();
161         myWrapper[libsumo::CMD_GET_LANE_VARIABLE] = Lane::makeWrapper();
162         myWrapper[libsumo::CMD_GET_LANEAREA_VARIABLE] = LaneArea::makeWrapper();
163         myWrapper[libsumo::CMD_GET_MULTIENTRYEXIT_VARIABLE] = MultiEntryExit::makeWrapper();
164         myWrapper[libsumo::CMD_GET_PERSON_VARIABLE] = Person::makeWrapper();
165         myWrapper[libsumo::CMD_GET_POI_VARIABLE] = POI::makeWrapper();
166         myWrapper[libsumo::CMD_GET_POLYGON_VARIABLE] = Polygon::makeWrapper();
167         myWrapper[libsumo::CMD_GET_ROUTE_VARIABLE] = Route::makeWrapper();
168         myWrapper[libsumo::CMD_GET_SIM_VARIABLE] = Simulation::makeWrapper();
169         myWrapper[libsumo::CMD_GET_TL_VARIABLE] = TrafficLight::makeWrapper();
170         myWrapper[libsumo::CMD_GET_VEHICLE_VARIABLE] = Vehicle::makeWrapper();
171         myWrapper[libsumo::CMD_GET_VEHICLETYPE_VARIABLE] = VehicleType::makeWrapper();
172     }
173     auto wrapper = myWrapper.find(getCommandId);
174     if (wrapper == myWrapper.end()) {
175         throw TraCIException("Unsupported command specified");
176     }
177     std::shared_ptr<VariableWrapper> handler = wrapper->second;
178     for (const std::string& objID : objIDs) {
179         if (!s.variables.empty()) {
180             for (const int variable : s.variables) {
181                 handler->handle(objID, variable, handler.get());
182             }
183         } else {
184             if (s.contextDomain == 0 && getCommandId == libsumo::CMD_GET_VEHICLE_VARIABLE) {
185                 // default for vehicles is edge id and lane position
186                 handler->handle(objID, VAR_ROAD_ID, handler.get());
187                 handler->handle(objID, VAR_LANEPOSITION, handler.get());
188             } else if (s.contextDomain > 0 || !handler->handle(objID, libsumo::LAST_STEP_VEHICLE_NUMBER, handler.get())) {
189                 // default for detectors is vehicle number, for all others (and contexts) id list
190                 handler->handle(objID, libsumo::TRACI_ID_LIST, handler.get());
191             }
192         }
193     }
194 }
195 
196 
197 void
fuseLaneCoverage(std::shared_ptr<LaneCoverageInfo> aggregatedLaneCoverage,const std::shared_ptr<LaneCoverageInfo> newLaneCoverage)198 Helper::fuseLaneCoverage(std::shared_ptr<LaneCoverageInfo> aggregatedLaneCoverage, const std::shared_ptr<LaneCoverageInfo> newLaneCoverage) {
199     for (auto& p : *newLaneCoverage) {
200         const MSLane* lane = p.first;
201         if (aggregatedLaneCoverage->find(lane) == aggregatedLaneCoverage->end()) {
202             // Lane has no coverage in aggregatedLaneCoverage, yet
203             (*aggregatedLaneCoverage)[lane] = (*newLaneCoverage)[lane];
204         } else {
205             // Lane is covered in aggregatedLaneCoverage as well
206             std::pair<double, double>& range1 = (*aggregatedLaneCoverage)[lane];
207             std::pair<double, double>& range2 = (*newLaneCoverage)[lane];
208             std::pair<double, double> hull = std::make_pair(MIN2(range1.first, range2.first), MAX2(range1.second, range2.second));
209             (*aggregatedLaneCoverage)[lane] = hull;
210         }
211     }
212 }
213 
214 
215 TraCIPositionVector
makeTraCIPositionVector(const PositionVector & positionVector)216 Helper::makeTraCIPositionVector(const PositionVector& positionVector) {
217     TraCIPositionVector tp;
218     for (int i = 0; i < (int)positionVector.size(); ++i) {
219         tp.push_back(makeTraCIPosition(positionVector[i]));
220     }
221     return tp;
222 }
223 
224 
225 PositionVector
makePositionVector(const TraCIPositionVector & vector)226 Helper::makePositionVector(const TraCIPositionVector& vector) {
227     PositionVector pv;
228     for (int i = 0; i < (int)vector.size(); i++) {
229         pv.push_back(Position(vector[i].x, vector[i].y));
230     }
231     return pv;
232 }
233 
234 
235 TraCIColor
makeTraCIColor(const RGBColor & color)236 Helper::makeTraCIColor(const RGBColor& color) {
237     TraCIColor tc;
238     tc.a = color.alpha();
239     tc.b = color.blue();
240     tc.g = color.green();
241     tc.r = color.red();
242     return tc;
243 }
244 
245 
246 RGBColor
makeRGBColor(const TraCIColor & c)247 Helper::makeRGBColor(const TraCIColor& c) {
248     return RGBColor((unsigned char)c.r, (unsigned char)c.g, (unsigned char)c.b, (unsigned char)c.a);
249 }
250 
251 
252 TraCIPosition
makeTraCIPosition(const Position & position,const bool includeZ)253 Helper::makeTraCIPosition(const Position& position, const bool includeZ) {
254     TraCIPosition p;
255     p.x = position.x();
256     p.y = position.y();
257     p.z = includeZ ? position.z() : INVALID_DOUBLE_VALUE;
258     return p;
259 }
260 
261 
262 Position
makePosition(const TraCIPosition & tpos)263 Helper::makePosition(const TraCIPosition& tpos) {
264     return Position(tpos.x, tpos.y, tpos.z);
265 }
266 
267 
268 MSEdge*
getEdge(const std::string & edgeID)269 Helper::getEdge(const std::string& edgeID) {
270     MSEdge* edge = MSEdge::dictionary(edgeID);
271     if (edge == nullptr) {
272         throw TraCIException("Referenced edge '" + edgeID + "' is not known.");
273     }
274     return edge;
275 }
276 
277 
278 const MSLane*
getLaneChecking(const std::string & edgeID,int laneIndex,double pos)279 Helper::getLaneChecking(const std::string& edgeID, int laneIndex, double pos) {
280     const MSEdge* edge = MSEdge::dictionary(edgeID);
281     if (edge == nullptr) {
282         throw TraCIException("Unknown edge " + edgeID);
283     }
284     if (laneIndex < 0 || laneIndex >= (int)edge->getLanes().size()) {
285         throw TraCIException("Invalid lane index for " + edgeID);
286     }
287     const MSLane* lane = edge->getLanes()[laneIndex];
288     if (pos < 0 || pos > lane->getLength()) {
289         throw TraCIException("Position on lane invalid");
290     }
291     return lane;
292 }
293 
294 
295 std::pair<MSLane*, double>
convertCartesianToRoadMap(const Position & pos,const SUMOVehicleClass vClass)296 Helper::convertCartesianToRoadMap(const Position& pos, const SUMOVehicleClass vClass) {
297     const PositionVector shape({ pos });
298     std::pair<MSLane*, double> result;
299     double range = 1000.;
300     const Boundary& netBounds = GeoConvHelper::getFinal().getConvBoundary();
301     const double maxRange = MAX2(1001., netBounds.getWidth() + netBounds.getHeight() + netBounds.distanceTo2D(pos));
302     while (range < maxRange) {
303         std::set<std::string> laneIds;
304         collectObjectsInRange(libsumo::CMD_GET_LANE_VARIABLE, shape, range, laneIds);
305         double minDistance = std::numeric_limits<double>::max();
306         for (const std::string& laneID : laneIds) {
307             MSLane* const lane = MSLane::dictionary(laneID);
308             if (lane->allowsVehicleClass(vClass)) {
309                 const double newDistance = lane->getShape().distance2D(pos);
310                 if (newDistance < minDistance) {
311                     minDistance = newDistance;
312                     result.first = lane;
313                 }
314             }
315         }
316         if (minDistance < std::numeric_limits<double>::max()) {
317             // @todo this may be a place where 3D is required but 2D is delivered
318             result.second = result.first->getShape().nearest_offset_to_point2D(pos, false);
319             break;
320         }
321         range *= 2;
322     }
323     return result;
324 }
325 
326 
327 void
cleanup()328 Helper::cleanup() {
329     for (const auto i : myObjects) {
330         delete i.second;
331     }
332     myObjects.clear();
333     delete myLaneTree;
334     myLaneTree = nullptr;
335 }
336 
337 
338 void
registerVehicleStateListener()339 Helper::registerVehicleStateListener() {
340     if (MSNet::hasInstance()) {
341         MSNet::getInstance()->addVehicleStateListener(&myVehicleStateListener);
342     }
343 }
344 
345 
346 const std::vector<std::string>&
getVehicleStateChanges(const MSNet::VehicleState state)347 Helper::getVehicleStateChanges(const MSNet::VehicleState state) {
348     return myVehicleStateListener.myVehicleStateChanges[state];
349 }
350 
351 
352 void
clearVehicleStates()353 Helper::clearVehicleStates() {
354     for (auto& i : myVehicleStateListener.myVehicleStateChanges) {
355         i.second.clear();
356     }
357 }
358 
359 
360 void
findObjectShape(int domain,const std::string & id,PositionVector & shape)361 Helper::findObjectShape(int domain, const std::string& id, PositionVector& shape) {
362     switch (domain) {
363         case libsumo::CMD_SUBSCRIBE_INDUCTIONLOOP_CONTEXT:
364             InductionLoop::storeShape(id, shape);
365             break;
366         case libsumo::CMD_SUBSCRIBE_LANE_CONTEXT:
367             Lane::storeShape(id, shape);
368             break;
369         case libsumo::CMD_SUBSCRIBE_VEHICLE_CONTEXT:
370             Vehicle::storeShape(id, shape);
371             break;
372         case libsumo::CMD_SUBSCRIBE_PERSON_CONTEXT:
373             Person::storeShape(id, shape);
374             break;
375         case libsumo::CMD_SUBSCRIBE_POI_CONTEXT:
376             POI::storeShape(id, shape);
377             break;
378         case libsumo::CMD_SUBSCRIBE_POLYGON_CONTEXT:
379             Polygon::storeShape(id, shape);
380             break;
381         case libsumo::CMD_SUBSCRIBE_JUNCTION_CONTEXT:
382             Junction::storeShape(id, shape);
383             break;
384         case libsumo::CMD_SUBSCRIBE_EDGE_CONTEXT:
385             Edge::storeShape(id, shape);
386             break;
387         default:
388             break;
389     }
390 }
391 
392 
393 void
collectObjectsInRange(int domain,const PositionVector & shape,double range,std::set<std::string> & into)394 Helper::collectObjectsInRange(int domain, const PositionVector& shape, double range, std::set<std::string>& into) {
395     // build the look-up tree if not yet existing
396     if (myObjects.find(domain) == myObjects.end()) {
397         switch (domain) {
398             case libsumo::CMD_GET_INDUCTIONLOOP_VARIABLE:
399                 myObjects[libsumo::CMD_GET_INDUCTIONLOOP_VARIABLE] = InductionLoop::getTree();
400                 break;
401             case libsumo::CMD_GET_EDGE_VARIABLE:
402             case libsumo::CMD_GET_LANE_VARIABLE:
403             case libsumo::CMD_GET_PERSON_VARIABLE:
404             case libsumo::CMD_GET_VEHICLE_VARIABLE:
405                 myObjects[libsumo::CMD_GET_EDGE_VARIABLE] = nullptr;
406                 myObjects[libsumo::CMD_GET_LANE_VARIABLE] = nullptr;
407                 myObjects[libsumo::CMD_GET_PERSON_VARIABLE] = nullptr;
408                 myObjects[libsumo::CMD_GET_VEHICLE_VARIABLE] = nullptr;
409                 myLaneTree = new LANE_RTREE_QUAL(&MSLane::visit);
410                 MSLane::fill(*myLaneTree);
411                 break;
412             case libsumo::CMD_GET_POI_VARIABLE:
413                 myObjects[libsumo::CMD_GET_POI_VARIABLE] = POI::getTree();
414                 break;
415             case libsumo::CMD_GET_POLYGON_VARIABLE:
416                 myObjects[libsumo::CMD_GET_POLYGON_VARIABLE] = Polygon::getTree();
417                 break;
418             case libsumo::CMD_GET_JUNCTION_VARIABLE:
419                 myObjects[libsumo::CMD_GET_JUNCTION_VARIABLE] = Junction::getTree();
420                 break;
421             default:
422                 break;
423         }
424     }
425     const Boundary b = shape.getBoxBoundary().grow(range);
426     const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
427     const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
428     switch (domain) {
429         case libsumo::CMD_GET_INDUCTIONLOOP_VARIABLE:
430         case libsumo::CMD_GET_POI_VARIABLE:
431         case libsumo::CMD_GET_POLYGON_VARIABLE:
432         case libsumo::CMD_GET_JUNCTION_VARIABLE: {
433             Named::StoringVisitor sv(into);
434             myObjects[domain]->Search(cmin, cmax, sv);
435         }
436         break;
437         case libsumo::CMD_GET_EDGE_VARIABLE:
438         case libsumo::CMD_GET_LANE_VARIABLE:
439         case libsumo::CMD_GET_PERSON_VARIABLE:
440         case libsumo::CMD_GET_VEHICLE_VARIABLE: {
441             LaneStoringVisitor sv(into, shape, range, domain);
442             myLaneTree->Search(cmin, cmax, sv);
443         }
444         break;
445         default:
446             break;
447     }
448 }
449 
450 
451 
452 void
applySubscriptionFilters(const Subscription & s,std::set<std::string> & objIDs)453 Helper::applySubscriptionFilters(const Subscription& s, std::set<std::string>& objIDs) {
454 #ifdef DEBUG_SURROUNDING
455     MSVehicle* _veh = libsumo::Vehicle::getVehicle(s.id);
456     std::cout << SIMTIME << " applySubscriptionFilters for vehicle '" << _veh->getID() << "' on lane '" << _veh->getLane()->getID() << "'"
457               << "\n       on edge '" << _veh->getLane()->getEdge().getID() << "' (" << toString(_veh->getLane()->getEdge().getLanes()) << ")\n"
458               << "objIDs = " << toString(objIDs) << std::endl;
459 #endif
460 
461     if (s.activeFilters == 0) {
462         // No filters set
463         return;
464     }
465 
466     // Whether vehicles on opposite lanes shall be taken into account
467     const bool disregardOppositeDirection = (s.activeFilters & SUBS_FILTER_NOOPPOSITE) != 0;
468 
469     // Check filter specification consistency
470     // TODO: Warn only once
471     if (disregardOppositeDirection && (s.activeFilters & SUBS_FILTER_NO_RTREE) == 0) {
472         WRITE_WARNING("Ignoring no-opposite subscription filter for geographic range object collection. Consider using the 'lanes' filter.")
473     }
474 
475     // TODO: Treat case, where ego vehicle is currently on opposite lane
476 
477     std::set<const MSVehicle*> vehs;
478     if (s.activeFilters & SUBS_FILTER_NO_RTREE) {
479         // Set defaults for upstream and downstream distances
480         double downstreamDist = s.range, upstreamDist = s.range;
481         if (s.activeFilters & SUBS_FILTER_DOWNSTREAM_DIST) {
482             // Specifies maximal downstream distance for vehicles in context subscription result
483             downstreamDist = s.filterDownstreamDist;
484         }
485         if (s.activeFilters & SUBS_FILTER_UPSTREAM_DIST) {
486             // Specifies maximal downstream distance for vehicles in context subscription result
487             upstreamDist = s.filterUpstreamDist;
488         }
489         MSVehicle* v = libsumo::Vehicle::getVehicle(s.id);
490         if (!v->isOnRoad()) {
491             return;
492         }
493         MSLane* vehLane = v->getLane();
494         if (vehLane == nullptr) {
495             return;
496         }
497         MSEdge* vehEdge = &vehLane->getEdge();
498         std::vector<int> filterLanes;
499         if ((s.activeFilters & SUBS_FILTER_LANES) == 0) {
500             // No lane indices are specified (but downstream and/or upstream distance)
501             //   -> use only vehicle's current lane as origin for the searches
502             filterLanes = {0};
503             // Lane indices must be specified when leader/follower information is requested.
504             assert((s.activeFilters & SUBS_FILTER_LEAD_FOLLOW) == 0);
505         } else {
506             filterLanes = s.filterLanes;
507         }
508 
509 #ifdef DEBUG_SURROUNDING
510         std::cout << "Filter lanes: " << toString(filterLanes) << std::endl;
511         std::cout << "Downstream distance: " << downstreamDist << std::endl;
512         std::cout << "Upstream distance: " << upstreamDist << std::endl;
513 #endif
514 
515         if (s.activeFilters & SUBS_FILTER_MANEUVER) {
516             // Maneuver filters disables road net search for all surrounding vehicles
517             if (s.activeFilters & SUBS_FILTER_LEAD_FOLLOW) {
518                 // Return leader and follower on the specified lanes in context subscription result.
519                 for (int offset : filterLanes) {
520                     MSLane* lane = v->getLane()->getParallelLane(offset);
521                     if (lane != nullptr) {
522                         // this is a non-opposite lane
523                         MSVehicle* leader = lane->getLeader(v, v->getPositionOnLane(), v->getBestLanesContinuation(lane), downstreamDist).first;
524                         MSVehicle* follower = lane->getFollower(v, v->getPositionOnLane(), upstreamDist, false).first;
525                         vehs.insert(vehs.end(), leader);
526                         vehs.insert(vehs.end(), follower);
527 
528 #ifdef DEBUG_SURROUNDING
529                         std::cout << "Lane at index " << offset << ": '" << lane->getID() << std::endl;
530                         std::cout << "Leader: '" << (leader != nullptr ? leader->getID() : "NULL") << "'" << std::endl;
531                         std::cout << "Follower: '" << (follower != nullptr ? follower->getID() : "NULL") << "'" << std::endl;
532 #endif
533 
534                     } else if (!disregardOppositeDirection && offset > 0) { // TODO: offset<0 may indicate opposite query when vehicle is on opposite itself
535                         // check whether ix points to an opposite lane
536                         const MSEdge* opposite = vehEdge->getOppositeEdge();
537                         if (opposite == nullptr) {
538 #ifdef DEBUG_SURROUNDING
539                             std::cout << "No lane at index " << offset << std::endl;
540 #endif
541                             // no opposite edge
542                             continue;
543                         }
544                         // Index of opposite lane at relative offset
545                         const int ix_opposite = (int)opposite->getLanes().size() - 1 - (vehLane->getIndex() + offset - (int)vehEdge->getLanes().size());
546                         if (ix_opposite < 0) {
547 #ifdef DEBUG_SURROUNDING
548                             std::cout << "No lane on opposite at index " << ix_opposite << std::endl;
549 #endif
550                             // no opposite edge
551                             continue;
552                         }
553                         lane = opposite->getLanes()[ix_opposite];
554                         // Search vehs along opposite lanes (swap upstream and downstream distance)
555                         // XXX transformations for curved geometries
556                         double posOnOpposite = MAX2(0., opposite->getLength() - v->getPositionOnLane());
557                         // Get leader on opposite
558                         vehs.insert(vehs.end(), lane->getFollower(v, posOnOpposite, downstreamDist, true).first);
559                         // Get follower (no search on consecutive lanes
560                         vehs.insert(vehs.end(), lane->getLeader(v, posOnOpposite - v->getLength(), std::vector<MSLane*>()).first);
561                     }
562                 }
563             }
564 
565             if (s.activeFilters & SUBS_FILTER_TURN) {
566                 // Get upcoming junctions and vialanes within downstream distance, where foe links exist or at least the link direction is not straight
567                 MSLane* lane = v->getLane();
568                 std::vector<const MSLink*> links = lane->getUpcomingLinks(v->getPositionOnLane(), downstreamDist, v->getBestLanesContinuation());
569 #ifdef DEBUG_SURROUNDING
570                 std::cout << "Applying turn filter for vehicle '" << v->getID() << "'\n Gathering foes ..." << std::endl;
571 #endif
572                 // Iterate through junctions and find approaching foes within upstreamDist.
573                 for (auto& l : links) {
574 #ifdef DEBUG_SURROUNDING
575                     std::cout << "  On junction '" << l->getJunction()->getID() << "' (no. foe links = " << l->getFoeLinks().size() << "):" << std::endl;
576 #endif
577                     for (auto& foeLane : l->getFoeLanes()) {
578                         // Check vehicles approaching the entry link corresponding to this lane
579                         const MSLink* foeLink = foeLane->getEntryLink();
580                         for (auto& vi : foeLink->getApproaching()) {
581                             if (vi.second.dist <= upstreamDist) {
582 #ifdef DEBUG_SURROUNDING
583                                 std::cout << "    Approaching from foe-lane '" << vi.first->getID() << "'" << std::endl;
584 #endif
585                                 vehs.insert(vehs.end(), dynamic_cast<const MSVehicle*>(vi.first));
586                             }
587                         }
588                         // add vehicles currently on the junction
589                         for (const MSVehicle* foe : foeLane->getVehiclesSecure()) {
590                             vehs.insert(vehs.end(), foe);
591                         }
592                         foeLane->releaseVehicles();
593                     }
594                 }
595             }
596 #ifdef DEBUG_SURROUNDING
597             std::cout << SIMTIME << " applySubscriptionFilters() for veh '" << v->getID() << "'. Found the following vehicles:\n";
598             for (auto veh : vehs) {
599                 if (veh != nullptr) {
600                     std::cout << "  '" << veh->getID() << "' on lane '" << veh->getLane()->getID() << "'\n";
601                 }
602             }
603 #endif
604         } else {
605             // No maneuver filters requested, but only lanes filter (directly, or indirectly by specifying downstream or upstream distance)
606             assert(filterLanes.size() > 0);
607             // This is to remember the lanes checked in the driving direction of the vehicle (their opposites can be added in a second pass)
608             auto checkedLanesInDrivingDir = std::make_shared<LaneCoverageInfo>();
609             for (int offset : filterLanes) {
610                 MSLane* lane = vehLane->getParallelLane(offset);
611                 if (lane != nullptr) {
612 #ifdef DEBUG_SURROUNDING
613                     std::cout << "Checking for surrounding vehicles starting on lane '" << lane->getID() << "' at index " << offset << std::endl;
614 #endif
615                     // Search vehs along this lane
616                     // (Coverage info is collected per origin lane since lanes reached from neighboring lanes may have different distances
617                     // and aborting at previously scanned when coming from a closer origin may prevent scanning of parts that should be included.)
618                     std::shared_ptr<LaneCoverageInfo> checkedLanes = std::make_shared<LaneCoverageInfo>();
619                     const std::set<MSVehicle*> new_vehs = lane->getSurroundingVehicles(v->getPositionOnLane(), downstreamDist, upstreamDist + v->getLength(), checkedLanes);
620                     vehs.insert(new_vehs.begin(), new_vehs.end());
621                     fuseLaneCoverage(checkedLanesInDrivingDir, checkedLanes);
622                 } else if (!disregardOppositeDirection && offset > 0) {
623                     // Check opposite edge, too
624                     assert(vehLane->getIndex() + (unsigned int) offset >= vehEdge->getLanes().size()); // index points beyond this edge
625                     const MSEdge* opposite = vehEdge->getOppositeEdge();
626                     if (opposite == nullptr) {
627 #ifdef DEBUG_SURROUNDING
628                         std::cout << "No opposite edge, thus no lane at index " << offset << std::endl;
629 #endif
630                         // no opposite edge
631                         continue;
632                     }
633                     // Index of opposite lane at relative offset
634                     const int ix_opposite = (int)opposite->getLanes().size() - 1 - (vehLane->getIndex() + offset - (int)vehEdge->getLanes().size());
635                     if (ix_opposite < 0) {
636 #ifdef DEBUG_SURROUNDING
637                         std::cout << "No lane on opposite at index " << ix_opposite << std::endl;
638 #endif
639                         // no opposite edge
640                         continue;
641                     }
642                     lane = opposite->getLanes()[ix_opposite];
643                     // Search vehs along opposite lanes (swap upstream and downstream distance)
644                     const std::set<MSVehicle*> new_vehs = lane->getSurroundingVehicles(lane->getLength() - v->getPositionOnLane(), upstreamDist + v->getLength(), downstreamDist, std::make_shared<LaneCoverageInfo>());
645                     vehs.insert(new_vehs.begin(), new_vehs.end());
646                 }
647 #ifdef DEBUG_SURROUNDING
648                 else {
649                     std::cout << "No lane at index " << offset << std::endl;
650                 }
651 #endif
652 
653                 if (!disregardOppositeDirection) {
654                     // If opposite should be checked, do this for each lane of the search tree in checkedLanesInDrivingDir
655                     // (For instance, some opposite lanes of these would not be obtained if the ego lane does not have an opposite.)
656 
657                     // Number of opposite lanes to be checked (assumes filterLanes.size()>0, see assertion above) determined as hypothetical offset
658                     // overlap into opposing edge from the vehicle's current lane.
659                     // TODO: offset<0 may indicate opposite query when vehicle is on opposite itself (-> use min_element(filterLanes...) instead, etc)
660                     const int nOpp = MAX2(0, (*std::max_element(filterLanes.begin(), filterLanes.end())) - ((int)vehEdge->getLanes().size() - 1 - vehLane->getIndex()));
661                     // Collect vehicles from opposite lanes
662                     if (nOpp > 0) {
663                         for (auto& laneCov : *checkedLanesInDrivingDir) {
664                             const MSLane* lane = laneCov.first;
665                             if (lane == nullptr || lane->getEdge().getOppositeEdge() == nullptr) {
666                                 continue;
667                             }
668                             const MSEdge* edge = &(lane->getEdge());
669                             const MSEdge* opposite = edge->getOppositeEdge();
670                             const std::pair<double, double>& range = laneCov.second;
671                             auto leftMostOppositeLaneIt = opposite->getLanes().rbegin();
672                             for (auto oppositeLaneIt = leftMostOppositeLaneIt;
673                                     oppositeLaneIt != opposite->getLanes().rend(); ++oppositeLaneIt) {
674                                 if ((int)(oppositeLaneIt - leftMostOppositeLaneIt) == nOpp) {
675                                     break;
676                                 }
677                                 // Add vehicles from corresponding range on opposite direction
678                                 const MSLane* oppositeLane = *oppositeLaneIt;
679                                 auto new_vehs = oppositeLane->getVehiclesInRange(lane->getLength() - range.second, lane->getLength() - range.first);
680                                 vehs.insert(new_vehs.begin(), new_vehs.end());
681                             }
682                         }
683                     }
684                 }
685 #ifdef DEBUG_SURROUNDING
686                 std::cout << SIMTIME << " applySubscriptionFilters() for veh '" << v->getID() << "'. Found the following vehicles:\n";
687                 for (auto veh : vehs) {
688                     if (veh != nullptr) {
689                         std::cout << "  '" << veh->getID() << "' on lane '" << veh->getLane()->getID() << "'\n";
690                     }
691                 }
692 #endif
693             }
694 
695             // filter vehicles in vehs by class and/or type if requested
696             if (s.activeFilters & SUBS_FILTER_VCLASS) {
697                 // Only return vehicles of the given vClass in context subscription result
698                 auto i = vehs.begin();
699                 while (i != vehs.end()) {
700                     if (((*i)->getVehicleType().getVehicleClass() & s.filterVClasses) == 0) {
701                         i = vehs.erase(i);
702                     } else {
703                         ++i;
704                     }
705                 }
706             }
707             if (s.activeFilters & SUBS_FILTER_VTYPE) {
708                 // Only return vehicles of the given vType in context subscription result
709                 auto i = vehs.begin();
710                 while (i != vehs.end()) {
711                     if (s.filterVTypes.find((*i)->getVehicleType().getID()) == s.filterVTypes.end()) {
712                         i = vehs.erase(i);
713                     } else {
714                         ++i;
715                     }
716                 }
717             }
718         }
719         // Write vehs IDs in objIDs
720         for (const MSVehicle* veh : vehs) {
721             if (veh != nullptr) {
722                 objIDs.insert(objIDs.end(), veh->getID());
723             }
724         }
725     } else {
726         // filter vehicles in vehs by class and/or type if requested
727         if (s.activeFilters & SUBS_FILTER_VCLASS) {
728             // Only return vehicles of the given vClass in context subscription result
729             auto i = objIDs.begin();
730             while (i != objIDs.end()) {
731                 MSVehicle* veh = libsumo::Vehicle::getVehicle(*i);
732                 if ((veh->getVehicleType().getVehicleClass() & s.filterVClasses) == 0) {
733                     i = objIDs.erase(i);
734                 } else {
735                     ++i;
736                 }
737             }
738         }
739         if (s.activeFilters & SUBS_FILTER_VTYPE) {
740             // Only return vehicles of the given vType in context subscription result
741             auto i = objIDs.begin();
742             while (i != objIDs.end()) {
743                 MSVehicle* veh = libsumo::Vehicle::getVehicle(*i);
744                 if (s.filterVTypes.find(veh->getVehicleType().getID()) == s.filterVTypes.end()) {
745                     i = objIDs.erase(i);
746                 } else {
747                     ++i;
748                 }
749             }
750         }
751     }
752 }
753 
754 void
setRemoteControlled(MSVehicle * v,Position xyPos,MSLane * l,double pos,double posLat,double angle,int edgeOffset,ConstMSEdgeVector route,SUMOTime t)755 Helper::setRemoteControlled(MSVehicle* v, Position xyPos, MSLane* l, double pos, double posLat, double angle,
756                             int edgeOffset, ConstMSEdgeVector route, SUMOTime t) {
757     myRemoteControlledVehicles[v->getID()] = v;
758     v->getInfluencer().setRemoteControlled(xyPos, l, pos, posLat, angle, edgeOffset, route, t);
759 }
760 
761 void
setRemoteControlled(MSPerson * p,Position xyPos,MSLane * l,double pos,double posLat,double angle,int edgeOffset,ConstMSEdgeVector route,SUMOTime t)762 Helper::setRemoteControlled(MSPerson* p, Position xyPos, MSLane* l, double pos, double posLat, double angle,
763                             int edgeOffset, ConstMSEdgeVector route, SUMOTime t) {
764     myRemoteControlledPersons[p->getID()] = p;
765     p->getInfluencer().setRemoteControlled(xyPos, l, pos, posLat, angle, edgeOffset, route, t);
766 }
767 
768 
769 void
postProcessRemoteControl()770 Helper::postProcessRemoteControl() {
771     for (auto& controlled : myRemoteControlledVehicles) {
772         if (MSNet::getInstance()->getVehicleControl().getVehicle(controlled.first) != nullptr) {
773             controlled.second->getInfluencer().postProcessRemoteControl(controlled.second);
774         } else {
775             WRITE_WARNING("Vehicle '" + controlled.first + "' was removed though being controlled by TraCI");
776         }
777     }
778     myRemoteControlledVehicles.clear();
779     for (auto& controlled : myRemoteControlledPersons) {
780         if (MSNet::getInstance()->getPersonControl().get(controlled.first) != nullptr) {
781             controlled.second->getInfluencer().postProcessRemoteControl(controlled.second);
782         } else {
783             WRITE_WARNING("Person '" + controlled.first + "' was removed though being controlled by TraCI");
784         }
785     }
786     myRemoteControlledPersons.clear();
787 }
788 
789 
790 bool
moveToXYMap(const Position & pos,double maxRouteDistance,bool mayLeaveNetwork,const std::string & origID,const double angle,double speed,const ConstMSEdgeVector & currentRoute,const int routePosition,MSLane * currentLane,double currentLanePos,bool onRoad,double & bestDistance,MSLane ** lane,double & lanePos,int & routeOffset,ConstMSEdgeVector & edges)791 Helper::moveToXYMap(const Position& pos, double maxRouteDistance, bool mayLeaveNetwork, const std::string& origID, const double angle,
792                     double speed, const ConstMSEdgeVector& currentRoute, const int routePosition, MSLane* currentLane, double currentLanePos, bool onRoad,
793                     double& bestDistance, MSLane** lane, double& lanePos, int& routeOffset, ConstMSEdgeVector& edges) {
794     // collect edges around the vehicle/person
795     const MSEdge* const currentRouteEdge = currentRoute[routePosition];
796     std::set<std::string> into;
797     PositionVector shape;
798     shape.push_back(pos);
799     collectObjectsInRange(libsumo::CMD_GET_EDGE_VARIABLE, shape, maxRouteDistance, into);
800     double maxDist = 0;
801     std::map<MSLane*, LaneUtility> lane2utility;
802     // compute utility for all candidate edges
803     for (std::set<std::string>::const_iterator j = into.begin(); j != into.end(); ++j) {
804         const MSEdge* const e = MSEdge::dictionary(*j);
805         const MSEdge* prevEdge = nullptr;
806         const MSEdge* nextEdge = nullptr;
807         bool onRoute = false;
808         // the next if/the clause sets "onRoute", "prevEdge", and "nextEdge", depending on
809         //  whether the currently seen edge is an internal one or a normal one
810         if (!e->isInternal()) {
811 #ifdef DEBUG_MOVEXY_ANGLE
812             std::cout << "Ego on normal" << std::endl;
813 #endif
814             // a normal edge
815             //
816             // check whether the currently seen edge is in the vehicle's route
817             //  - either the one it's on or one of the next edges
818             ConstMSEdgeVector::const_iterator searchStart = currentRoute.begin() + routePosition;
819             if (onRoad && currentLane->getEdge().isInternal()) {
820                 ++searchStart;
821             }
822             ConstMSEdgeVector::const_iterator edgePos = std::find(searchStart, currentRoute.end(), e);
823             onRoute = edgePos != currentRoute.end(); // no? -> onRoute is false
824             if (edgePos == currentRoute.end() - 1 && currentRouteEdge == e) {
825                 // onRoute is false as well if the vehicle is beyond the edge
826                 onRoute &= currentRouteEdge->getLength() > currentLanePos + SPEED2DIST(speed);
827             }
828             // save prior and next edges
829             prevEdge = e;
830             nextEdge = !onRoute || edgePos == currentRoute.end() - 1 ? nullptr : *(edgePos + 1);
831 #ifdef DEBUG_MOVEXY_ANGLE
832             std::cout << "normal:" << e->getID() << " prev:" << prevEdge->getID() << " next:";
833             if (nextEdge != 0) {
834                 std::cout << nextEdge->getID();
835             }
836             std::cout << std::endl;
837 #endif
838         } else {
839 #ifdef DEBUG_MOVEXY_ANGLE
840             std::cout << "Ego on internal" << std::endl;
841 #endif
842             // an internal edge
843             // get the previous edge
844             prevEdge = e;
845             while (prevEdge != nullptr && prevEdge->isInternal()) {
846                 MSLane* l = prevEdge->getLanes()[0];
847                 l = l->getLogicalPredecessorLane();
848                 prevEdge = l == nullptr ? nullptr : &l->getEdge();
849             }
850             // check whether the previous edge is on the route (was on the route)
851             ConstMSEdgeVector::const_iterator prevEdgePos = std::find(currentRoute.begin() + routePosition, currentRoute.end(), prevEdge);
852             nextEdge = e;
853             while (nextEdge != nullptr && nextEdge->isInternal()) {
854                 nextEdge = nextEdge->getSuccessors()[0]; // should be only one for an internal edge
855             }
856             if (prevEdgePos != currentRoute.end() && (prevEdgePos + 1) != currentRoute.end()) {
857                 onRoute = *(prevEdgePos + 1) == nextEdge;
858             }
859 #ifdef DEBUG_MOVEXY_ANGLE
860             std::cout << "internal:" << e->getID() << " prev:" << prevEdge->getID() << " next:" << nextEdge->getID() << std::endl;
861 #endif
862         }
863 
864 
865         // weight the lanes...
866         const std::vector<MSLane*>& lanes = e->getLanes();
867         const bool perpendicular = false;
868         for (std::vector<MSLane*>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
869             MSLane* lane = *k;
870             double langle = 180.;
871             double dist = FAR_AWAY;
872             double perpendicularDist = FAR_AWAY;
873             double off = lane->getShape().nearest_offset_to_point2D(pos, true);
874             if (off != GeomHelper::INVALID_OFFSET) {
875                 perpendicularDist = lane->getShape().distance2D(pos, true);
876             }
877             off = lane->getShape().nearest_offset_to_point2D(pos, perpendicular);
878             if (off != GeomHelper::INVALID_OFFSET) {
879                 dist = lane->getShape().distance2D(pos, perpendicular);
880                 langle = GeomHelper::naviDegree(lane->getShape().rotationAtOffset(off));
881             }
882             bool sameEdge = onRoad && &lane->getEdge() == &currentLane->getEdge() && currentRouteEdge->getLength() > currentLanePos + SPEED2DIST(speed);
883             /*
884             const MSEdge* rNextEdge = nextEdge;
885             while(rNextEdge==0&&lane->getEdge().getPurpose()==MSEdge::EDGEFUNCTION_INTERNAL) {
886                 MSLane* next = lane->getLinkCont()[0]->getLane();
887                 rNextEdge = next == 0 ? 0 : &next->getEdge();
888             }
889             */
890             double dist2 = dist;
891             if (mayLeaveNetwork && dist != perpendicularDist) {
892                 // ambiguous mapping. Don't trust this
893                 dist2 = FAR_AWAY;
894             }
895             const double angleDiff = (angle == INVALID_DOUBLE_VALUE ? 0 : GeomHelper::getMinAngleDiff(angle, langle));
896 #ifdef DEBUG_MOVEXY_ANGLE
897             std::cout << lane->getID() << " lAngle:" << langle << " lLength=" << lane->getLength()
898                       << " angleDiff:" << angleDiff
899                       << " off:" << off
900                       << " pDist=" << perpendicularDist
901                       << " dist=" << dist
902                       << " dist2=" << dist2
903                       << "\n";
904             std::cout << lane->getID() << " param=" << lane->getParameter(SUMO_PARAM_ORIGID, lane->getID()) << " origID='" << origID << "\n";
905 #endif
906             lane2utility[lane] = LaneUtility(
907                                      dist2, perpendicularDist, off, angleDiff,
908                                      lane->getParameter(SUMO_PARAM_ORIGID, lane->getID()) == origID,
909                                      onRoute, sameEdge, prevEdge, nextEdge);
910             // update scaling value
911             maxDist = MAX2(maxDist, MIN2(dist, SUMO_const_laneWidth));
912 
913         }
914     }
915 
916     // get the best lane given the previously computed values
917     double bestValue = 0;
918     MSLane* bestLane = nullptr;
919     for (std::map<MSLane*, LaneUtility>::iterator i = lane2utility.begin(); i != lane2utility.end(); ++i) {
920         MSLane* l = (*i).first;
921         const LaneUtility& u = (*i).second;
922         double distN = u.dist > 999 ? -10 : 1. - (u.dist / maxDist);
923         double angleDiffN = 1. - (u.angleDiff / 180.);
924         double idN = u.ID ? 1 : 0;
925         double onRouteN = u.onRoute ? 1 : 0;
926         double sameEdgeN = u.sameEdge ? MIN2(currentRouteEdge->getLength() / speed, (double)1.) : 0;
927         double value = (distN * .5 // distance is more important than angle because the vehicle might be driving in the opposite direction
928                         + angleDiffN * 0.35 /*.5 */
929                         + idN * 1
930                         + onRouteN * 0.1
931                         + sameEdgeN * 0.1);
932 #ifdef DEBUG_MOVEXY
933         std::cout << " x; l:" << l->getID() << " d:" << u.dist << " dN:" << distN << " aD:" << angleDiffN <<
934                   " ID:" << idN << " oRN:" << onRouteN << " sEN:" << sameEdgeN << " value:" << value << std::endl;
935 #endif
936         if (value > bestValue || bestLane == nullptr) {
937             bestValue = value;
938             if (u.dist == FAR_AWAY) {
939                 bestLane = nullptr;
940             } else {
941                 bestLane = l;
942             }
943         }
944     }
945     // no best lane found, return
946     if (bestLane == nullptr) {
947         return false;
948     }
949     const LaneUtility& u = lane2utility.find(bestLane)->second;
950     bestDistance = u.dist;
951     *lane = bestLane;
952     lanePos = bestLane->getShape().nearest_offset_to_point25D(pos, false);
953     const MSEdge* prevEdge = u.prevEdge;
954     if (u.onRoute) {
955         ConstMSEdgeVector::const_iterator prevEdgePos = std::find(currentRoute.begin(), currentRoute.end(), prevEdge);
956         routeOffset = (int)std::distance(currentRoute.begin(), prevEdgePos);
957         //std::cout << SIMTIME << "moveToXYMap vehicle=" << veh.getID() << " currLane=" << veh.getLane()->getID() << " routeOffset=" << routeOffset << " edges=" << toString(ev) << " bestLane=" << bestLane->getID() << " prevEdge=" << prevEdge->getID() << "\n";
958     } else {
959         edges.push_back(u.prevEdge);
960         /*
961            if(bestLane->getEdge().getPurpose()!=MSEdge::EDGEFUNCTION_INTERNAL) {
962            edges.push_back(&bestLane->getEdge());
963            }
964         */
965         if (u.nextEdge != nullptr) {
966             edges.push_back(u.nextEdge);
967         }
968         routeOffset = 0;
969 #ifdef DEBUG_MOVEXY_ANGLE
970         std::cout << "internal2: lane=" << bestLane->getID() << " prev=" << Named::getIDSecure(u.prevEdge) << " next=" << Named::getIDSecure(u.nextEdge) << "\n";;
971 #endif
972     }
973     return true;
974 }
975 
976 
977 bool
findCloserLane(const MSEdge * edge,const Position & pos,double & bestDistance,MSLane ** lane)978 Helper::findCloserLane(const MSEdge* edge, const Position& pos, double& bestDistance, MSLane** lane) {
979     if (edge == nullptr) {
980         return false;
981     }
982     const std::vector<MSLane*>& lanes = edge->getLanes();
983     bool newBest = false;
984     for (std::vector<MSLane*>::const_iterator k = lanes.begin(); k != lanes.end() && bestDistance > POSITION_EPS; ++k) {
985         MSLane* candidateLane = *k;
986         const double dist = candidateLane->getShape().distance2D(pos); // get distance
987 #ifdef DEBUG_MOVEXY
988         std::cout << "   b at lane " << candidateLane->getID() << " dist:" << dist << " best:" << bestDistance << std::endl;
989 #endif
990         if (dist < bestDistance) {
991             // is the new distance the best one? keep then...
992             bestDistance = dist;
993             *lane = candidateLane;
994             newBest = true;
995         }
996     }
997     return newBest;
998 }
999 
1000 
1001 bool
moveToXYMap_matchingRoutePosition(const Position & pos,const std::string & origID,const ConstMSEdgeVector & currentRoute,int routeIndex,double & bestDistance,MSLane ** lane,double & lanePos,int & routeOffset)1002 Helper::moveToXYMap_matchingRoutePosition(const Position& pos, const std::string& origID,
1003         const ConstMSEdgeVector& currentRoute, int routeIndex,
1004         double& bestDistance, MSLane** lane, double& lanePos, int& routeOffset) {
1005     //std::cout << "moveToXYMap_matchingRoutePosition pos=" << pos << "\n";
1006     routeOffset = 0;
1007     // routes may be looped which makes routeOffset ambiguous. We first try to
1008     // find the closest upcoming edge on the route and then look for closer passed edges
1009 
1010     // look forward along the route
1011     const MSEdge* prev = nullptr;
1012     for (int i = routeIndex; i < (int)currentRoute.size(); ++i) {
1013         const MSEdge* cand = currentRoute[i];
1014         while (prev != nullptr) {
1015             // check internal edge(s)
1016             const MSEdge* internalCand = prev->getInternalFollowingEdge(cand);
1017             findCloserLane(internalCand, pos, bestDistance, lane);
1018             prev = internalCand;
1019         }
1020         if (findCloserLane(cand, pos, bestDistance, lane)) {
1021             routeOffset = i;
1022         }
1023         prev = cand;
1024     }
1025     // look backward along the route
1026     const MSEdge* next = currentRoute[routeIndex];
1027     for (int i = routeIndex; i >= 0; --i) {
1028         const MSEdge* cand = currentRoute[i];
1029         //std::cout << "  next=" << next->getID() << " cand=" << cand->getID() << " i=" << i << " routeIndex=" << routeIndex << "\n";
1030         prev = cand;
1031         while (prev != nullptr) {
1032             // check internal edge(s)
1033             const MSEdge* internalCand = prev->getInternalFollowingEdge(next);
1034             if (findCloserLane(internalCand, pos, bestDistance, lane)) {
1035                 routeOffset = i;
1036             }
1037             prev = internalCand;
1038         }
1039         if (findCloserLane(cand, pos, bestDistance, lane)) {
1040             routeOffset = i;
1041         }
1042         next = cand;
1043     }
1044 
1045     assert(lane != 0);
1046     // quit if no solution was found, reporting a failure
1047     if (lane == nullptr) {
1048 #ifdef DEBUG_MOVEXY
1049         std::cout << "  b failed - no best route lane" << std::endl;
1050 #endif
1051         return false;
1052     }
1053 
1054 
1055     // position may be inaccurate; let's checkt the given index, too
1056     // @note: this is enabled for non-internal lanes only, as otherwise the position information may ambiguous
1057     if (!(*lane)->getEdge().isInternal()) {
1058         const std::vector<MSLane*>& lanes = (*lane)->getEdge().getLanes();
1059         for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
1060             if ((*i)->getParameter(SUMO_PARAM_ORIGID, (*i)->getID()) == origID) {
1061                 *lane = *i;
1062                 break;
1063             }
1064         }
1065     }
1066     // check position, stuff, we should have the best lane along the route
1067     lanePos = MAX2(0., MIN2(double((*lane)->getLength() - POSITION_EPS),
1068                             (*lane)->interpolateGeometryPosToLanePos(
1069                                 (*lane)->getShape().nearest_offset_to_point25D(pos, false))));
1070     //std::cout << SIMTIME << " moveToXYMap_matchingRoutePosition vehicle=" << veh.getID() << " currLane=" << veh.getLane()->getID() << " routeOffset=" << routeOffset << " edges=" << toString(edges) << " lane=" << (*lane)->getID() << "\n";
1071 #ifdef DEBUG_MOVEXY
1072     std::cout << "  b ok lane " << (*lane)->getID() << " lanePos:" << lanePos << std::endl;
1073 #endif
1074     return true;
1075 }
1076 
1077 
SubscriptionWrapper(VariableWrapper::SubscriptionHandler handler,SubscriptionResults & into,ContextSubscriptionResults & context)1078 Helper::SubscriptionWrapper::SubscriptionWrapper(VariableWrapper::SubscriptionHandler handler, SubscriptionResults& into, ContextSubscriptionResults& context)
1079     : VariableWrapper(handler), myResults(into), myContextResults(context), myActiveResults(into) {
1080 
1081 }
1082 
1083 
1084 void
setContext(const std::string & refID)1085 Helper::SubscriptionWrapper::setContext(const std::string& refID) {
1086     myActiveResults = refID == "" ? myResults : myContextResults[refID];
1087 }
1088 
1089 
1090 bool
wrapDouble(const std::string & objID,const int variable,const double value)1091 Helper::SubscriptionWrapper::wrapDouble(const std::string& objID, const int variable, const double value) {
1092     myActiveResults[objID][variable] = std::make_shared<TraCIDouble>(value);
1093     return true;
1094 }
1095 
1096 
1097 bool
wrapInt(const std::string & objID,const int variable,const int value)1098 Helper::SubscriptionWrapper::wrapInt(const std::string& objID, const int variable, const int value) {
1099     myActiveResults[objID][variable] = std::make_shared<TraCIInt>(value);
1100     return true;
1101 }
1102 
1103 
1104 bool
wrapString(const std::string & objID,const int variable,const std::string & value)1105 Helper::SubscriptionWrapper::wrapString(const std::string& objID, const int variable, const std::string& value) {
1106     myActiveResults[objID][variable] = std::make_shared<TraCIString>(value);
1107     return true;
1108 }
1109 
1110 
1111 bool
wrapStringList(const std::string & objID,const int variable,const std::vector<std::string> & value)1112 Helper::SubscriptionWrapper::wrapStringList(const std::string& objID, const int variable, const std::vector<std::string>& value) {
1113     auto sl = std::make_shared<TraCIStringList>();
1114     sl->value = value;
1115     myActiveResults[objID][variable] = sl;
1116     return true;
1117 }
1118 
1119 
1120 bool
wrapPosition(const std::string & objID,const int variable,const TraCIPosition & value)1121 Helper::SubscriptionWrapper::wrapPosition(const std::string& objID, const int variable, const TraCIPosition& value) {
1122     myActiveResults[objID][variable] = std::make_shared<TraCIPosition>(value);
1123     return true;
1124 }
1125 
1126 
1127 bool
wrapColor(const std::string & objID,const int variable,const TraCIColor & value)1128 Helper::SubscriptionWrapper::wrapColor(const std::string& objID, const int variable, const TraCIColor& value) {
1129     myActiveResults[objID][variable] = std::make_shared<TraCIColor>(value);
1130     return true;
1131 }
1132 
1133 
1134 bool
wrapRoadPosition(const std::string & objID,const int variable,const TraCIRoadPosition & value)1135 Helper::SubscriptionWrapper::wrapRoadPosition(const std::string& objID, const int variable, const TraCIRoadPosition& value) {
1136     myActiveResults[objID][variable] = std::make_shared<TraCIRoadPosition>(value);
1137     return true;
1138 }
1139 
1140 
1141 void
vehicleStateChanged(const SUMOVehicle * const vehicle,MSNet::VehicleState to,const std::string &)1142 Helper::VehicleStateListener::vehicleStateChanged(const SUMOVehicle* const vehicle, MSNet::VehicleState to, const std::string& /*info*/) {
1143     myVehicleStateChanges[to].push_back(vehicle->getID());
1144 }
1145 
1146 
1147 }
1148 
1149 
1150 /****************************************************************************/
1151