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() == ¤tLane->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