Eclipse SUMO - Simulation of Urban MObility
Helper.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2017-2022 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials are made available under the
5 // terms of the Eclipse Public License 2.0 which is available at
6 // https://www.eclipse.org/legal/epl-2.0/
7 // This Source Code may also be made available under the following Secondary
8 // Licenses when the conditions for such availability set forth in the Eclipse
9 // Public License 2.0 are satisfied: GNU General Public License, version 2
10 // or later which is available at
11 // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 /****************************************************************************/
20 // C++ TraCI client API implementation
21 /****************************************************************************/
22 #include <config.h>
23 
24 #include <cstring>
25 #include <utils/geom/GeomHelper.h>
27 #include <microsim/MSNet.h>
29 #include <microsim/MSEdgeControl.h>
31 #include <microsim/MSEdge.h>
32 #include <microsim/MSLane.h>
33 #include <microsim/MSLink.h>
35 #include <microsim/MSVehicle.h>
40 #include <libsumo/StorageHelper.h>
41 #include <libsumo/TraCIDefs.h>
42 #include <libsumo/Edge.h>
43 #include <libsumo/InductionLoop.h>
44 #include <libsumo/Junction.h>
45 #include <libsumo/Lane.h>
46 #include <libsumo/LaneArea.h>
47 #include <libsumo/MultiEntryExit.h>
48 #include <libsumo/Person.h>
49 #include <libsumo/POI.h>
50 #include <libsumo/Polygon.h>
51 #include <libsumo/Route.h>
52 #include <libsumo/Simulation.h>
53 #include <libsumo/TrafficLight.h>
54 #include <libsumo/Vehicle.h>
55 #include <libsumo/VehicleType.h>
56 #include <libsumo/TraCIConstants.h>
57 #include "Helper.h"
58 
59 #define FAR_AWAY 1000.0
60 
61 //#define DEBUG_MOVEXY
62 //#define DEBUG_MOVEXY_ANGLE
63 //#define DEBUG_SURROUNDING
64 
65 void
66 LaneStoringVisitor::add(const MSLane* const l) const {
67  switch (myDomain) {
69  for (const MSVehicle* veh : l->getVehiclesSecure()) {
70  if (myShape.distance2D(veh->getPosition()) <= myRange) {
71  myObjects.insert(veh);
72  }
73  }
74  for (const MSBaseVehicle* veh : l->getParkingVehicles()) {
75  if (myShape.distance2D(veh->getPosition()) <= myRange) {
76  myObjects.insert(veh);
77  }
78  }
79  l->releaseVehicles();
80  }
81  break;
83  l->getVehiclesSecure();
84  std::vector<MSTransportable*> persons = l->getEdge().getSortedPersons(MSNet::getInstance()->getCurrentTimeStep(), true);
85  for (auto p : persons) {
86  if (myShape.distance2D(p->getPosition()) <= myRange) {
87  myObjects.insert(p);
88  }
89  }
90  l->releaseVehicles();
91  }
92  break;
94  if (myShape.size() != 1 || l->getShape().distance2D(myShape[0]) <= myRange) {
95  myObjects.insert(&l->getEdge());
96  }
97  }
98  break;
100  if (myShape.size() != 1 || l->getShape().distance2D(myShape[0]) <= myRange) {
101  myObjects.insert(l);
102  }
103  }
104  break;
105  default:
106  break;
107 
108  }
109 }
110 
111 namespace libsumo {
112 // ===========================================================================
113 // static member initializations
114 // ===========================================================================
115 std::vector<Subscription> Helper::mySubscriptions;
116 Subscription* Helper::myLastContextSubscription = nullptr;
117 std::map<int, std::shared_ptr<VariableWrapper> > Helper::myWrapper;
118 Helper::VehicleStateListener Helper::myVehicleStateListener;
119 Helper::TransportableStateListener Helper::myTransportableStateListener;
121 std::map<std::string, MSVehicle*> Helper::myRemoteControlledVehicles;
122 std::map<std::string, MSPerson*> Helper::myRemoteControlledPersons;
123 
124 
125 // ===========================================================================
126 // static member definitions
127 // ===========================================================================
128 
129 void
131  if (veh != nullptr) {
132  if (veh->isVehicle()) {
133  std::cout << " '" << veh->getID() << "' on lane '" << ((SUMOVehicle*)veh)->getLane()->getID() << "'\n";
134  } else {
135  std::cout << " '" << veh->getID() << "' on edge '" << veh->getEdge()->getID() << "'\n";
136  }
137  }
138 }
139 
140 
141 void
142 Helper::subscribe(const int commandId, const std::string& id, const std::vector<int>& variables,
143  const double beginTime, const double endTime, const libsumo::TraCIResults& params,
144  const int contextDomain, const double range) {
145  myLastContextSubscription = nullptr;
146  if (variables.empty()) {
147  for (std::vector<libsumo::Subscription>::iterator j = mySubscriptions.begin(); j != mySubscriptions.end();) {
148  if (j->id == id && j->commandId == commandId && j->contextDomain == contextDomain) {
149  j = mySubscriptions.erase(j);
150  } else {
151  ++j;
152  }
153  }
154  return;
155  }
156  std::vector<std::shared_ptr<tcpip::Storage> > parameters;
157  for (const int var : variables) {
158  const auto& p = params.find(var);
159  if (p == params.end()) {
160  parameters.push_back(std::make_shared<tcpip::Storage>());
161  } else {
162  parameters.push_back(libsumo::StorageHelper::toStorage(*p->second));
163  }
164  }
165  const SUMOTime begin = beginTime == INVALID_DOUBLE_VALUE ? 0 : TIME2STEPS(beginTime);
166  const SUMOTime end = endTime == INVALID_DOUBLE_VALUE || endTime > STEPS2TIME(SUMOTime_MAX) ? SUMOTime_MAX : TIME2STEPS(endTime);
167  libsumo::Subscription s(commandId, id, variables, parameters, begin, end, contextDomain, range);
168  if (s.variables.size() == 1 && s.variables.front() == -1) {
169  s.variables.clear();
170  }
172  libsumo::Subscription* modifiedSubscription = nullptr;
173  needNewSubscription(s, mySubscriptions, modifiedSubscription);
174  if (modifiedSubscription->isVehicleToVehicleContextSubscription()
175  || modifiedSubscription->isVehicleToPersonContextSubscription()) {
176  // Set last modified vehicle context subscription active for filter modifications
177  myLastContextSubscription = modifiedSubscription;
178  }
179 }
180 
181 
182 void
184  for (auto& wrapper : myWrapper) {
185  wrapper.second->clear();
186  }
187  for (std::vector<libsumo::Subscription>::iterator i = mySubscriptions.begin(); i != mySubscriptions.end();) {
188  const libsumo::Subscription& s = *i;
189  const bool isArrivedVehicle = (s.commandId == CMD_SUBSCRIBE_VEHICLE_VARIABLE || s.commandId == CMD_SUBSCRIBE_VEHICLE_CONTEXT)
192  && MSNet::getInstance()->getPersonControl().get(s.id) == nullptr;
193  if (s.endTime < t || isArrivedVehicle || isArrivedPerson) {
194  i = mySubscriptions.erase(i);
195  continue;
196  }
197  ++i;
198  }
199  for (const libsumo::Subscription& s : mySubscriptions) {
200  if (s.beginTime <= t) {
202  }
203  }
204 }
205 
206 
207 bool
208 Helper::needNewSubscription(libsumo::Subscription& s, std::vector<Subscription>& subscriptions, libsumo::Subscription*& modifiedSubscription) {
209  for (libsumo::Subscription& o : subscriptions) {
210  if (s.commandId == o.commandId && s.id == o.id &&
211  s.beginTime == o.beginTime && s.endTime == o.endTime &&
212  s.contextDomain == o.contextDomain && s.range == o.range) {
213  std::vector<std::shared_ptr<tcpip::Storage> >::const_iterator k = s.parameters.begin();
214  for (const int v : s.variables) {
215  const int offset = (int)(std::find(o.variables.begin(), o.variables.end(), v) - o.variables.begin());
216  if (offset == (int)o.variables.size() || o.parameters[offset]->size() != (*k)->size() || !std::equal((*k)->begin(), (*k)->end(), o.parameters[offset]->begin())) {
217  o.variables.push_back(v);
218  o.parameters.push_back(*k);
219  }
220  ++k;
221  }
222  modifiedSubscription = &o;
223  return false;
224  }
225  }
226  subscriptions.push_back(s);
227  modifiedSubscription = &subscriptions.back();
228  return true;
229 }
230 
231 
232 void
234  mySubscriptions.clear();
235  myLastContextSubscription = nullptr;
236 }
237 
238 
241  if (myLastContextSubscription != nullptr) {
243  } else {
244  WRITE_WARNING("addSubscriptionFilter: No previous vehicle context subscription exists to apply the context filter.");
245  }
247 }
248 
249 
250 void
252  const int getCommandId = s.contextDomain > 0 ? s.contextDomain : s.commandId - 0x30;
253  std::set<std::string> objIDs;
254  if (s.contextDomain > 0) {
255  if ((s.activeFilters & SUBS_FILTER_NO_RTREE) == 0) {
256  PositionVector shape;
257  findObjectShape(s.commandId, s.id, shape);
258  collectObjectIDsInRange(s.contextDomain, shape, s.range, objIDs);
259  }
260  applySubscriptionFilters(s, objIDs);
261  } else {
262  objIDs.insert(s.id);
263  }
264  if (myWrapper.empty()) {
265  myWrapper[libsumo::CMD_GET_EDGE_VARIABLE] = Edge::makeWrapper();
266  myWrapper[libsumo::CMD_GET_INDUCTIONLOOP_VARIABLE] = InductionLoop::makeWrapper();
267  myWrapper[libsumo::CMD_GET_JUNCTION_VARIABLE] = Junction::makeWrapper();
268  myWrapper[libsumo::CMD_GET_LANE_VARIABLE] = Lane::makeWrapper();
269  myWrapper[libsumo::CMD_GET_LANEAREA_VARIABLE] = LaneArea::makeWrapper();
270  myWrapper[libsumo::CMD_GET_MULTIENTRYEXIT_VARIABLE] = MultiEntryExit::makeWrapper();
271  myWrapper[libsumo::CMD_GET_PERSON_VARIABLE] = Person::makeWrapper();
272  myWrapper[libsumo::CMD_GET_POI_VARIABLE] = POI::makeWrapper();
273  myWrapper[libsumo::CMD_GET_POLYGON_VARIABLE] = Polygon::makeWrapper();
274  myWrapper[libsumo::CMD_GET_ROUTE_VARIABLE] = Route::makeWrapper();
275  myWrapper[libsumo::CMD_GET_SIM_VARIABLE] = Simulation::makeWrapper();
276  myWrapper[libsumo::CMD_GET_TL_VARIABLE] = TrafficLight::makeWrapper();
277  myWrapper[libsumo::CMD_GET_VEHICLE_VARIABLE] = Vehicle::makeWrapper();
278  myWrapper[libsumo::CMD_GET_VEHICLETYPE_VARIABLE] = VehicleType::makeWrapper();
279  }
280  auto wrapper = myWrapper.find(getCommandId);
281  if (wrapper == myWrapper.end()) {
282  throw TraCIException("Unsupported command specified");
283  }
284  std::shared_ptr<VariableWrapper> handler = wrapper->second;
285  VariableWrapper* container = handler.get();
286  if (s.contextDomain > 0) {
287  auto containerWrapper = myWrapper.find(s.commandId + 0x20);
288  if (containerWrapper == myWrapper.end()) {
289  throw TraCIException("Unsupported domain specified");
290  }
291  container = containerWrapper->second.get();
292  container->setContext(s.id);
293  } else {
294  container->setContext("");
295  }
296  for (const std::string& objID : objIDs) {
297  if (!s.variables.empty()) {
298  std::vector<std::shared_ptr<tcpip::Storage> >::const_iterator k = s.parameters.begin();
299  for (const int variable : s.variables) {
300  (*k)->resetPos();
301  handler->handle(objID, variable, container, k->get());
302  ++k;
303  }
304  } else {
305  if (s.contextDomain == 0 && getCommandId == libsumo::CMD_GET_VEHICLE_VARIABLE) {
306  // default for vehicles is edge id and lane position
307  handler->handle(objID, VAR_ROAD_ID, container, nullptr);
308  handler->handle(objID, VAR_LANEPOSITION, container, nullptr);
309  } else if (s.contextDomain > 0 || !handler->handle(objID, libsumo::LAST_STEP_VEHICLE_NUMBER, container, nullptr)) {
310  // default for detectors is vehicle number, for all others (and contexts) id list
311  handler->handle(objID, libsumo::TRACI_ID_LIST, container, nullptr);
312  }
313  }
314  }
315 }
316 
317 
318 
319 void
320 Helper::fuseLaneCoverage(std::shared_ptr<LaneCoverageInfo> aggregatedLaneCoverage, const std::shared_ptr<LaneCoverageInfo> newLaneCoverage) {
321  for (auto& p : *newLaneCoverage) {
322  const MSLane* lane = p.first;
323  if (aggregatedLaneCoverage->find(lane) == aggregatedLaneCoverage->end()) {
324  // Lane has no coverage in aggregatedLaneCoverage, yet
325  (*aggregatedLaneCoverage)[lane] = (*newLaneCoverage)[lane];
326  } else {
327  // Lane is covered in aggregatedLaneCoverage as well
328  std::pair<double, double>& range1 = (*aggregatedLaneCoverage)[lane];
329  std::pair<double, double>& range2 = (*newLaneCoverage)[lane];
330  std::pair<double, double> hull = std::make_pair(MIN2(range1.first, range2.first), MAX2(range1.second, range2.second));
331  (*aggregatedLaneCoverage)[lane] = hull;
332  }
333  }
334 }
335 
336 
340  for (int i = 0; i < (int)positionVector.size(); ++i) {
341  tp.value.push_back(makeTraCIPosition(positionVector[i]));
342  }
343  return tp;
344 }
345 
346 
349  PositionVector pv;
350  for (const TraCIPosition& pos : vector.value) {
351  if (std::isnan(pos.x) || std::isnan(pos.y)) {
352  throw libsumo::TraCIException("NaN-Value in shape.");
353  }
354  pv.push_back(Position(pos.x, pos.y));
355  }
356  return pv;
357 }
358 
359 
362  TraCIColor tc;
363  tc.a = color.alpha();
364  tc.b = color.blue();
365  tc.g = color.green();
366  tc.r = color.red();
367  return tc;
368 }
369 
370 
371 RGBColor
373  return RGBColor((unsigned char)c.r, (unsigned char)c.g, (unsigned char)c.b, (unsigned char)c.a);
374 }
375 
376 
378 Helper::makeTraCIPosition(const Position& position, const bool includeZ) {
379  TraCIPosition p;
380  p.x = position.x();
381  p.y = position.y();
382  p.z = includeZ ? position.z() : INVALID_DOUBLE_VALUE;
383  return p;
384 }
385 
386 
387 Position
389  return Position(tpos.x, tpos.y, tpos.z);
390 }
391 
392 
393 MSEdge*
394 Helper::getEdge(const std::string& edgeID) {
395  MSEdge* edge = MSEdge::dictionary(edgeID);
396  if (edge == nullptr) {
397  throw TraCIException("Referenced edge '" + edgeID + "' is not known.");
398  }
399  return edge;
400 }
401 
402 
403 const MSLane*
404 Helper::getLaneChecking(const std::string& edgeID, int laneIndex, double pos) {
405  const MSEdge* edge = MSEdge::dictionary(edgeID);
406  if (edge == nullptr) {
407  throw TraCIException("Unknown edge " + edgeID);
408  }
409  if (laneIndex < 0 || laneIndex >= (int)edge->getLanes().size()) {
410  throw TraCIException("Invalid lane index for " + edgeID);
411  }
412  const MSLane* lane = edge->getLanes()[laneIndex];
413  if (pos < 0 || pos > lane->getLength()) {
414  throw TraCIException("Position on lane invalid");
415  }
416  return lane;
417 }
418 
419 
420 std::pair<MSLane*, double>
422  const PositionVector shape({ pos });
423  std::pair<MSLane*, double> result;
424  double range = 1000.;
425  const Boundary& netBounds = GeoConvHelper::getFinal().getConvBoundary();
426  const double maxRange = MAX2(1001., netBounds.getWidth() + netBounds.getHeight() + netBounds.distanceTo2D(pos));
427  while (range < maxRange) {
428  std::set<const Named*> lanes;
430  double minDistance = std::numeric_limits<double>::max();
431  for (const Named* named : lanes) {
432  MSLane* lane = const_cast<MSLane*>(dynamic_cast<const MSLane*>(named));
433  if (lane->allowsVehicleClass(vClass)) {
434  // @todo this may be a place where 3D is required but 2D is used
435  const double newDistance = lane->getShape().distance2D(pos);
436  if (newDistance < minDistance) {
437  minDistance = newDistance;
438  result.first = lane;
439  }
440  }
441  }
442  if (minDistance < std::numeric_limits<double>::max()) {
443  result.second = result.first->interpolateGeometryPosToLanePos(result.first->getShape().nearest_offset_to_point2D(pos, false));
444  break;
445  }
446  range *= 2;
447  }
448  return result;
449 }
450 
451 
452 double
453 Helper::getDrivingDistance(std::pair<const MSLane*, double>& roadPos1, std::pair<const MSLane*, double>& roadPos2) {
454  if (roadPos1.first == roadPos2.first && roadPos1.second <= roadPos2.second) {
455  // same edge
456  return roadPos2.second - roadPos1.second;
457  }
458  double distance = 0.0;
459  ConstMSEdgeVector newRoute;
460  while (roadPos2.first->isInternal() && roadPos2.first != roadPos1.first) {
461  distance += roadPos2.second;
462  roadPos2.first = roadPos2.first->getLogicalPredecessorLane();
463  roadPos2.second = roadPos2.first->getLength();
464  }
465  MSNet::getInstance()->getRouterTT(0).compute(&roadPos1.first->getEdge(), &roadPos2.first->getEdge(), nullptr, SIMSTEP, newRoute, true);
466  if (newRoute.empty()) {
468  }
469  MSRoute route("", newRoute, false, nullptr, std::vector<SUMOVehicleParameter::Stop>());
470  return distance + route.getDistanceBetween(roadPos1.second, roadPos2.second, &roadPos1.first->getEdge(), &roadPos2.first->getEdge());
471 }
472 
473 
475 Helper::getVehicle(const std::string& id) {
477  if (sumoVehicle == nullptr) {
478  throw TraCIException("Vehicle '" + id + "' is not known.");
479  }
480  MSBaseVehicle* v = dynamic_cast<MSBaseVehicle*>(sumoVehicle);
481  if (v == nullptr) {
482  throw TraCIException("Vehicle '" + id + "' is not a proper vehicle.");
483  }
484  return v;
485 }
486 
487 
488 MSPerson*
489 Helper::getPerson(const std::string& personID) {
491  MSPerson* p = dynamic_cast<MSPerson*>(c.get(personID));
492  if (p == nullptr) {
493  throw TraCIException("Person '" + personID + "' is not known");
494  }
495  return p;
496 }
497 
499 Helper::getTrafficObject(int domain, const std::string& id) {
500  if (domain == CMD_GET_VEHICLE_VARIABLE) {
501  return getVehicle(id);
502  } else if (domain == CMD_GET_PERSON_VARIABLE) {
503  return getPerson(id);
504  } else {
505  throw TraCIException("Cannot retrieve traffic object for domain " + toString(domain));
506  }
507 }
508 
509 const MSVehicleType&
510 Helper::getVehicleType(const std::string& vehicleID) {
511  return getVehicle(vehicleID)->getVehicleType();
512 }
513 
514 
516 Helper::getTLS(const std::string& id) {
517  if (!MSNet::getInstance()->getTLSControl().knows(id)) {
518  throw TraCIException("Traffic light '" + id + "' is not known");
519  }
520  return MSNet::getInstance()->getTLSControl().get(id);
521 }
522 
523 
525 Helper::buildStopParameters(const std::string& edgeOrStoppingPlaceID,
526  double pos, int laneIndex, double startPos, int flags, double duration, double until) {
528  newStop.duration = duration == INVALID_DOUBLE_VALUE ? SUMOTime_MAX : TIME2STEPS(duration);
529  newStop.until = until == INVALID_DOUBLE_VALUE ? -1 : TIME2STEPS(until);
530  newStop.index = STOP_INDEX_FIT;
531  if (newStop.duration >= 0) {
532  newStop.parametersSet |= STOP_DURATION_SET;
533  }
534  if (newStop.until >= 0) {
535  newStop.parametersSet |= STOP_UNTIL_SET;
536  }
537  if ((flags & 1) != 0) {
538  newStop.parking = true;
539  newStop.parametersSet |= STOP_PARKING_SET;
540  }
541  if ((flags & 2) != 0) {
542  newStop.triggered = true;
543  newStop.parametersSet |= STOP_TRIGGER_SET;
544  }
545  if ((flags & 4) != 0) {
546  newStop.containerTriggered = true;
548  }
549 
550  SumoXMLTag stoppingPlaceType = SUMO_TAG_NOTHING;
551  if ((flags & 8) != 0) {
552  stoppingPlaceType = SUMO_TAG_BUS_STOP;
553  }
554  if ((flags & 16) != 0) {
555  stoppingPlaceType = SUMO_TAG_CONTAINER_STOP;
556  }
557  if ((flags & 32) != 0) {
558  stoppingPlaceType = SUMO_TAG_CHARGING_STATION;
559  }
560  if ((flags & 64) != 0) {
561  stoppingPlaceType = SUMO_TAG_PARKING_AREA;
562  }
563  if ((flags & 128) != 0) {
564  stoppingPlaceType = SUMO_TAG_OVERHEAD_WIRE_SEGMENT;
565  }
566 
567  if (stoppingPlaceType != SUMO_TAG_NOTHING) {
568  MSStoppingPlace* bs = MSNet::getInstance()->getStoppingPlace(edgeOrStoppingPlaceID, stoppingPlaceType);
569  if (bs == nullptr) {
570  throw TraCIException("The " + toString(stoppingPlaceType) + " '" + edgeOrStoppingPlaceID + "' is not known");
571  }
572  newStop.lane = bs->getLane().getID();
573  newStop.edge = bs->getLane().getEdge().getID();
574  newStop.endPos = bs->getEndLanePosition();
575  newStop.startPos = bs->getBeginLanePosition();
576  switch (stoppingPlaceType) {
577  case SUMO_TAG_BUS_STOP:
578  newStop.busstop = edgeOrStoppingPlaceID;
579  break;
581  newStop.containerstop = edgeOrStoppingPlaceID;
582  break;
584  newStop.chargingStation = edgeOrStoppingPlaceID;
585  break;
587  newStop.parkingarea = edgeOrStoppingPlaceID;
588  break;
590  newStop.overheadWireSegment = edgeOrStoppingPlaceID;
591  break;
592  default:
593  throw TraCIException("Unknown stopping place type '" + toString(stoppingPlaceType) + "'.");
594  }
595  } else {
596  if (startPos == INVALID_DOUBLE_VALUE) {
597  startPos = pos - POSITION_EPS;
598  }
599  if (startPos < 0.) {
600  throw TraCIException("Position on lane must not be negative.");
601  }
602  if (pos < startPos) {
603  throw TraCIException("End position on lane must be after start position.");
604  }
605  // get the actual lane that is referenced by laneIndex
606  MSEdge* road = MSEdge::dictionary(edgeOrStoppingPlaceID);
607  if (road == nullptr) {
608  throw TraCIException("Edge '" + edgeOrStoppingPlaceID + "' is not known.");
609  }
610  const std::vector<MSLane*>& allLanes = road->getLanes();
611  if ((laneIndex < 0) || laneIndex >= (int)(allLanes.size())) {
612  throw TraCIException("No lane with index '" + toString(laneIndex) + "' on edge '" + edgeOrStoppingPlaceID + "'.");
613  }
614  newStop.lane = allLanes[laneIndex]->getID();
615  newStop.edge = allLanes[laneIndex]->getEdge().getID();
616  newStop.endPos = pos;
617  newStop.startPos = startPos;
619  }
620  return newStop;
621 }
622 
623 
626  std::string stoppingPlaceID = "";
627  if (stopPar.busstop != "") {
628  stoppingPlaceID = stopPar.busstop;
629  }
630  if (stopPar.containerstop != "") {
631  stoppingPlaceID = stopPar.containerstop;
632  }
633  if (stopPar.parkingarea != "") {
634  stoppingPlaceID = stopPar.parkingarea;
635  }
636  if (stopPar.chargingStation != "") {
637  stoppingPlaceID = stopPar.chargingStation;
638  }
639  if (stopPar.overheadWireSegment != "") {
640  stoppingPlaceID = stopPar.overheadWireSegment;
641  }
642  int stopFlags = (
643  (stopPar.parking ? 1 : 0) +
644  (stopPar.triggered ? 2 : 0) +
645  (stopPar.containerTriggered ? 4 : 0) +
646  (stopPar.busstop != "" ? 8 : 0) +
647  (stopPar.containerstop != "" ? 16 : 0) +
648  (stopPar.chargingStation != "" ? 32 : 0) +
649  (stopPar.parkingarea != "" ? 64 : 0) +
650  (stopPar.overheadWireSegment != "" ? 128 : 0));
651 
652  return TraCINextStopData(stopPar.lane,
653  stopPar.startPos,
654  stopPar.endPos,
655  stoppingPlaceID,
656  stopFlags,
657  // negative duration is permitted to indicate that a vehicle cannot
658  // re-enter traffic after parking
659  stopPar.duration != -1 ? STEPS2TIME(stopPar.duration) : INVALID_DOUBLE_VALUE,
660  stopPar.until >= 0 ? STEPS2TIME(stopPar.until) : INVALID_DOUBLE_VALUE,
661  stopPar.arrival >= 0 ? STEPS2TIME(stopPar.arrival) : INVALID_DOUBLE_VALUE,
662  stopPar.started >= 0 ? STEPS2TIME(stopPar.started) : INVALID_DOUBLE_VALUE,
663  stopPar.ended >= 0 ? STEPS2TIME(stopPar.ended) : INVALID_DOUBLE_VALUE,
664  stopPar.split,
665  stopPar.join,
666  stopPar.actType,
667  stopPar.tripId,
668  stopPar.line,
669  stopPar.speed);
670 }
671 
672 
673 void
675  // clean up NamedRTrees
676  Polygon::cleanup();
677  POI::cleanup();
678  InductionLoop::cleanup();
679  Junction::cleanup();
683  delete myLaneTree;
684  myLaneTree = nullptr;
685 }
686 
687 
688 void
690  if (MSNet::hasInstance()) {
692  }
693 }
694 
695 
696 const std::vector<std::string>&
699 }
700 
701 
702 void
705  i.second.clear();
706  }
707 }
708 
709 
710 void
712  if (MSNet::hasInstance()) {
714  }
715 }
716 
717 
718 const std::vector<std::string>&
721 }
722 
723 
724 void
727  i.second.clear();
728  }
729 }
730 
731 
732 void
733 Helper::findObjectShape(int domain, const std::string& id, PositionVector& shape) {
734  switch (domain) {
736  InductionLoop::storeShape(id, shape);
737  break;
739  Lane::storeShape(id, shape);
740  break;
742  Vehicle::storeShape(id, shape);
743  break;
745  Person::storeShape(id, shape);
746  break;
748  POI::storeShape(id, shape);
749  break;
751  Polygon::storeShape(id, shape);
752  break;
754  Junction::storeShape(id, shape);
755  break;
757  Edge::storeShape(id, shape);
758  break;
759  default:
760  break;
761  }
762 }
763 
764 
765 void
766 Helper::collectObjectIDsInRange(int domain, const PositionVector& shape, double range, std::set<std::string>& into) {
767  std::set<const Named*> objects;
768  collectObjectsInRange(domain, shape, range, objects);
769  for (const Named* obj : objects) {
770  into.insert(obj->getID());
771  }
772 }
773 
774 
775 void
776 Helper::collectObjectsInRange(int domain, const PositionVector& shape, double range, std::set<const Named*>& into) {
777  const Boundary b = shape.getBoxBoundary().grow(range);
778  const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
779  const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
780  Named::StoringVisitor sv(into);
781  switch (domain) {
783  InductionLoop::getTree()->Search(cmin, cmax, sv);
784  break;
786  POI::getTree()->Search(cmin, cmax, sv);
787  break;
789  Polygon::getTree()->Search(cmin, cmax, sv);
790  break;
792  Junction::getTree()->Search(cmin, cmax, sv);
793  break;
798  if (myLaneTree == nullptr) {
801  }
802  LaneStoringVisitor lsv(into, shape, range, domain);
803  myLaneTree->Search(cmin, cmax, lsv);
804  }
805  break;
806  default:
807  break;
808  }
809 }
810 
811 
812 
813 void
814 Helper::applySubscriptionFilters(const Subscription& s, std::set<std::string>& objIDs) {
815 #ifdef DEBUG_SURROUNDING
816  MSBaseVehicle* _veh = getVehicle(s.id);
817  std::cout << SIMTIME << " applySubscriptionFilters for vehicle '" << _veh->getID() << "' on lane '" << _veh->getLane()->getID() << "'"
818  << "\n on edge '" << _veh->getLane()->getEdge().getID() << "' (" << toString(_veh->getLane()->getEdge().getLanes()) << ")\n"
819  << "objIDs = " << toString(objIDs) << std::endl;
820 #endif
821 
822  if (s.activeFilters == 0) {
823  // No filters set
824  return;
825  }
826 
827  MSVehicle* v = dynamic_cast<MSVehicle*>(getVehicle(s.id));
828 
829  // Whether vehicles on opposite lanes shall be taken into account
830  const bool disregardOppositeDirection = (s.activeFilters & SUBS_FILTER_NOOPPOSITE) != 0;
831 
832  // Check filter specification consistency
833  if (disregardOppositeDirection && (s.activeFilters & SUBS_FILTER_NO_RTREE) == 0) {
834  WRITE_WARNINGF("Ignoring veh '%' no-opposite subscription filter for geographic range object collection. Consider using the 'lanes' filter.", v->getID())
835  }
837  WRITE_WARNINGF("Ignoring veh '%' field of vision subscription filter due to incompatibility with other filter(s).", v->getID())
838  }
839 
840  // TODO: Treat case, where ego vehicle is currently on opposite lane
841 
842  std::set<const SUMOTrafficObject*> vehs;
844  // Set defaults for upstream/downstream/lateral distances
845  double downstreamDist = s.range, upstreamDist = s.range, lateralDist = s.range;
847  // Specifies maximal downstream distance for vehicles in context subscription result
848  downstreamDist = s.filterDownstreamDist;
849  }
851  // Specifies maximal downstream distance for vehicles in context subscription result
852  upstreamDist = s.filterUpstreamDist;
853  }
855  // Specifies maximal lateral distance for vehicles in context subscription result
856  lateralDist = s.filterLateralDist;
857  }
858  if (v == nullptr) {
859  throw TraCIException("Subscription filter not yet implemented for meso vehicle");
860  }
861  if (!v->isOnRoad()) {
862  return;
863  }
864  const MSLane* vehLane = v->getLane();
865  if (vehLane == nullptr) {
866  return;
867  }
868  MSEdge* vehEdge = &vehLane->getEdge();
869  std::vector<int> filterLanes;
870  if ((s.activeFilters & SUBS_FILTER_LANES) == 0) {
871  // No lane indices are specified (but downstream and/or upstream distance)
872  // -> use only vehicle's current lane as origin for the searches
873  filterLanes = {0};
874  // Lane indices must be specified when leader/follower information is requested.
875  assert((s.activeFilters & SUBS_FILTER_LEAD_FOLLOW) == 0);
876  } else {
877  filterLanes = s.filterLanes;
878  }
879 
880 #ifdef DEBUG_SURROUNDING
881  std::cout << "Filter lanes: " << toString(filterLanes) << std::endl;
882  std::cout << "Downstream distance: " << downstreamDist << std::endl;
883  std::cout << "Upstream distance: " << upstreamDist << std::endl;
884  std::cout << "Lateral distance: " << lateralDist << std::endl;
885 #endif
886 
887  if ((s.activeFilters & SUBS_FILTER_MANEUVER) != 0) {
888  // Maneuver filters disables road net search for all surrounding vehicles
889  if ((s.activeFilters & SUBS_FILTER_LEAD_FOLLOW) != 0) {
890  // Return leader and follower on the specified lanes in context subscription result.
891  for (int offset : filterLanes) {
892  MSLane* lane = v->getLane()->getParallelLane(offset, false);
893  if (lane != nullptr) {
894  // this is a non-opposite lane
895  MSVehicle* leader = lane->getLeader(v, v->getPositionOnLane(), v->getBestLanesContinuation(lane), downstreamDist).first;
896  MSVehicle* follower = lane->getFollower(v, v->getPositionOnLane(), upstreamDist, false).first;
897  vehs.insert(vehs.end(), leader);
898  vehs.insert(vehs.end(), follower);
899 
900 #ifdef DEBUG_SURROUNDING
901  std::cout << "Lane at index " << offset << ": '" << lane->getID() << std::endl;
902  std::cout << "Leader: '" << (leader != nullptr ? leader->getID() : "NULL") << "'" << std::endl;
903  std::cout << "Follower: '" << (follower != nullptr ? follower->getID() : "NULL") << "'" << std::endl;
904 #endif
905 
906  } else if (!disregardOppositeDirection && offset > 0) { // TODO: offset<0 may indicate opposite query when vehicle is on opposite itself
907  // check whether ix points to an opposite lane
908  const MSEdge* opposite = vehEdge->getOppositeEdge();
909  if (opposite == nullptr) {
910 #ifdef DEBUG_SURROUNDING
911  std::cout << "No lane at index " << offset << std::endl;
912 #endif
913  // no opposite edge
914  continue;
915  }
916  // Index of opposite lane at relative offset
917  const int ix_opposite = (int)opposite->getLanes().size() - 1 - (vehLane->getIndex() + offset - (int)vehEdge->getLanes().size());
918  if (ix_opposite < 0) {
919 #ifdef DEBUG_SURROUNDING
920  std::cout << "No lane on opposite at index " << ix_opposite << std::endl;
921 #endif
922  // no opposite edge
923  continue;
924  }
925  lane = opposite->getLanes()[ix_opposite];
926  // Search vehs along opposite lanes (swap upstream and downstream distance)
927  // XXX transformations for curved geometries
928  double posOnOpposite = MAX2(0., opposite->getLength() - v->getPositionOnLane());
929  // Get leader on opposite
930  vehs.insert(vehs.end(), lane->getFollower(v, posOnOpposite, downstreamDist, true).first);
931  // Get follower (no search on consecutive lanes
932  vehs.insert(vehs.end(), lane->getLeader(v, posOnOpposite - v->getLength(), std::vector<MSLane*>()).first);
933  }
934  }
935  }
936 
940  applySubscriptionFilterLanes(s, vehs, filterLanes, downstreamDist, upstreamDist, disregardOppositeDirection);
941  }
943  applySubscriptionFilterLateralDistance(s, vehs, downstreamDist, upstreamDist, lateralDist);
944  }
945  }
946 #ifdef DEBUG_SURROUNDING
947  std::cout << SIMTIME << " applySubscriptionFilters() for veh '" << v->getID() << "'. Found the following vehicles:\n";
948  for (auto veh : vehs) {
949  debugPrint(veh);
950  }
951 #endif
952  } else if (s.activeFilters & SUBS_FILTER_LATERAL_DIST) {
953  applySubscriptionFilterLateralDistance(s, vehs, downstreamDist, upstreamDist, lateralDist);
954  } else {
955  // No maneuver or lateral distance filters requested, but only lanes filter (directly, or indirectly by specifying downstream or upstream distance)
956  applySubscriptionFilterLanes(s, vehs, filterLanes, downstreamDist, upstreamDist, disregardOppositeDirection);
957 
958  // filter vehicles in vehs by class and/or type if requested
960  // Only return vehicles of the given vClass in context subscription result
961  auto i = vehs.begin();
962  while (i != vehs.end()) {
963  if (((*i)->getVehicleType().getVehicleClass() & s.filterVClasses) == 0) {
964  i = vehs.erase(i);
965  } else {
966  ++i;
967  }
968  }
969  }
971  // Only return vehicles of the given vType in context subscription result
972  auto i = vehs.begin();
973  while (i != vehs.end()) {
974  if (s.filterVTypes.find((*i)->getVehicleType().getID()) == s.filterVTypes.end()) {
975  i = vehs.erase(i);
976  } else {
977  ++i;
978  }
979  }
980  }
981  }
982  // Write vehs IDs in objIDs
983  for (const SUMOTrafficObject* veh : vehs) {
984  if (veh != nullptr) {
985  objIDs.insert(objIDs.end(), veh->getID());
986  }
987  }
988  } else { // apply rTree-based filters
990  // Only return vehicles of the given vClass in context subscription result
991  auto i = objIDs.begin();
992  while (i != objIDs.end()) {
993  MSBaseVehicle* veh = getVehicle(*i);
994  if ((veh->getVehicleType().getVehicleClass() & s.filterVClasses) == 0) {
995  i = objIDs.erase(i);
996  } else {
997  ++i;
998  }
999  }
1000  }
1001  if (s.activeFilters & SUBS_FILTER_VTYPE) {
1002  // Only return vehicles of the given vType in context subscription result
1003  auto i = objIDs.begin();
1004  while (i != objIDs.end()) {
1005  MSBaseVehicle* veh = getVehicle(*i);
1006  if (s.filterVTypes.find(veh->getVehicleType().getID()) == s.filterVTypes.end()) {
1007  i = objIDs.erase(i);
1008  } else {
1009  ++i;
1010  }
1011  }
1012  }
1014  // Only return vehicles within field of vision in context subscription result
1016  }
1017  }
1018 }
1019 
1020 void
1021 Helper::applySubscriptionFilterLanes(const Subscription& s, std::set<const SUMOTrafficObject*>& vehs, std::vector<int>& filterLanes, double downstreamDist,
1022  double upstreamDist, bool disregardOppositeDirection) {
1024  WRITE_WARNINGF("Lanes filter is only feasible for context domain 'vehicle' (current is '%'), ignoring filter...", toHex(s.contextDomain, 2));
1025  return;
1026  }
1027  assert(filterLanes.size() > 0);
1028  MSVehicle* v = dynamic_cast<MSVehicle*>(getVehicle(s.id));
1029  const MSLane* vehLane = v->getLane();
1030  MSEdge* vehEdge = &vehLane->getEdge();
1031  // This is to remember the lanes checked in the driving direction of the vehicle (their opposites can be added in a second pass)
1032  auto checkedLanesInDrivingDir = std::make_shared<LaneCoverageInfo>();
1033  for (int offset : filterLanes) {
1034  MSLane* lane = vehLane->getParallelLane(offset, false);
1035  if (lane != nullptr) {
1036 #ifdef DEBUG_SURROUNDING
1037  std::cout << "Checking for surrounding vehicles starting on lane '" << lane->getID() << "' at index " << offset << std::endl;
1038 #endif
1039  // Search vehs along this lane
1040  // (Coverage info is collected per origin lane since lanes reached from neighboring lanes may have different distances
1041  // and aborting at previously scanned when coming from a closer origin may prevent scanning of parts that should be included.)
1042  std::shared_ptr<LaneCoverageInfo> checkedLanes = std::make_shared<LaneCoverageInfo>();
1043  const std::set<MSVehicle*> new_vehs =
1044  lane->getSurroundingVehicles(v->getPositionOnLane(), downstreamDist, upstreamDist + v->getLength(), checkedLanes);
1045  vehs.insert(new_vehs.begin(), new_vehs.end());
1046  fuseLaneCoverage(checkedLanesInDrivingDir, checkedLanes);
1047  } else if (!disregardOppositeDirection && offset > 0) {
1048  // Check opposite edge, too
1049  assert(vehLane->getIndex() + offset >= (int)vehEdge->getLanes().size()); // index points beyond this edge
1050  const MSEdge* opposite = vehEdge->getOppositeEdge();
1051  if (opposite == nullptr) {
1052 #ifdef DEBUG_SURROUNDING
1053  std::cout << "No opposite edge, thus no lane at index " << offset << std::endl;
1054 #endif
1055  // no opposite edge
1056  continue;
1057  }
1058  // Index of opposite lane at relative offset
1059  const int ix_opposite = (int)opposite->getLanes().size() - 1 - (vehLane->getIndex() + offset - (int)vehEdge->getLanes().size());
1060  if (ix_opposite < 0) {
1061 #ifdef DEBUG_SURROUNDING
1062  std::cout << "No lane on opposite at index " << ix_opposite << std::endl;
1063 #endif
1064  // no opposite edge
1065  continue;
1066  }
1067  lane = opposite->getLanes()[ix_opposite];
1068  // Search vehs along opposite lanes (swap upstream and downstream distance)
1069  const std::set<MSVehicle*> new_vehs = lane->getSurroundingVehicles(lane->getLength() - v->getPositionOnLane(), upstreamDist + v->getLength(),
1070  downstreamDist, std::make_shared<LaneCoverageInfo>());
1071  vehs.insert(new_vehs.begin(), new_vehs.end());
1072  }
1073 #ifdef DEBUG_SURROUNDING
1074  else {
1075  std::cout << "No lane at index " << offset << std::endl;
1076  }
1077 #endif
1078 
1079  if (!disregardOppositeDirection) {
1080  // If opposite should be checked, do this for each lane of the search tree in checkedLanesInDrivingDir
1081  // (For instance, some opposite lanes of these would not be obtained if the ego lane does not have an opposite.)
1082 
1083  // Number of opposite lanes to be checked (assumes filterLanes.size()>0, see assertion above) determined as hypothetical offset
1084  // overlap into opposing edge from the vehicle's current lane.
1085  // TODO: offset<0 may indicate opposite query when vehicle is on opposite itself (-> use min_element(filterLanes...) instead, etc)
1086  const int nOpp = MAX2(0, (*std::max_element(filterLanes.begin(), filterLanes.end())) - ((int)vehEdge->getLanes().size() - 1 - vehLane->getIndex()));
1087  // Collect vehicles from opposite lanes
1088  if (nOpp > 0) {
1089  for (auto& laneCov : *checkedLanesInDrivingDir) {
1090  const MSLane* const l = laneCov.first;
1091  if (l == nullptr || l->getEdge().getOppositeEdge() == nullptr) {
1092  continue;
1093  }
1094  const MSEdge* opposite = l->getEdge().getOppositeEdge();
1095  const std::pair<double, double>& range = laneCov.second;
1096  auto leftMostOppositeLaneIt = opposite->getLanes().rbegin();
1097  for (auto oppositeLaneIt = leftMostOppositeLaneIt; oppositeLaneIt != opposite->getLanes().rend(); ++oppositeLaneIt) {
1098  if ((int)(oppositeLaneIt - leftMostOppositeLaneIt) == nOpp) {
1099  break;
1100  }
1101  // Add vehicles from corresponding range on opposite direction
1102  const MSLane* oppositeLane = *oppositeLaneIt;
1103  auto new_vehs = oppositeLane->getVehiclesInRange(l->getLength() - range.second, l->getLength() - range.first);
1104  vehs.insert(new_vehs.begin(), new_vehs.end());
1105  }
1106  }
1107  }
1108  }
1109 #ifdef DEBUG_SURROUNDING
1110  std::cout << SIMTIME << " applySubscriptionFilterLanes() for veh '" << v->getID() << "', lane offset '" << offset << "'. Found the following vehicles so far:\n";
1111  for (auto veh : vehs) {
1112  debugPrint(veh);
1113  }
1114 #endif
1115  }
1116 }
1117 
1118 void
1119 Helper::applySubscriptionFilterTurn(const Subscription& s, std::set<const SUMOTrafficObject*>& vehs) {
1121  WRITE_WARNINGF("Turn filter is only feasible for context domain 'vehicle' (current is '%'), ignoring filter...", toHex(s.contextDomain, 2));
1122  return;
1123  }
1124  // Get upcoming junctions and vialanes within downstream distance, where foe links exist or at least the link direction is not straight
1125  MSVehicle* v = dynamic_cast<MSVehicle*>(getVehicle(s.id));
1126  const MSLane* lane = v->getLane();
1127  std::vector<const MSLink*> links = lane->getUpcomingLinks(v->getPositionOnLane(), s.filterDownstreamDist, v->getBestLanesContinuation());
1128 #ifdef DEBUG_SURROUNDING
1129  std::cout << "Applying turn filter for vehicle '" << v->getID() << "'\n Gathering foes ..." << std::endl;
1130 #endif
1131  // Iterate through junctions and find approaching foes within foeDistToJunction.
1132  for (auto& l : links) {
1133 #ifdef DEBUG_SURROUNDING
1134  std::cout << " On junction '" << l->getJunction()->getID() << "' (no. foe links = " << l->getFoeLinks().size() << "):" << std::endl;
1135 #endif
1136  for (auto& foeLane : l->getFoeLanes()) {
1137  if (foeLane->getEdge().isCrossing()) {
1138 #ifdef DEBUG_SURROUNDING
1139  std::cout << " skipping crossing foeLane '" << foeLane->getID() << "'" << std::endl;
1140 #endif
1141  continue;
1142  }
1143 #ifdef DEBUG_SURROUNDING
1144  std::cout << " foeLane '" << foeLane->getID() << "'" << std::endl;
1145 #endif
1146  // Check vehicles approaching the entry link corresponding to this lane
1147  const MSLink* foeLink = foeLane->getEntryLink();
1148  for (auto& vi : foeLink->getApproaching()) {
1149  if (vi.second.dist <= s.filterFoeDistToJunction) {
1150 #ifdef DEBUG_SURROUNDING
1151  std::cout << " Approaching foeLane entry link '" << vi.first->getID() << "'" << std::endl;
1152 #endif
1153  vehs.insert(vehs.end(), dynamic_cast<const MSVehicle*>(vi.first));
1154  }
1155  }
1156  // add vehicles currently on the junction
1157  for (const MSVehicle* foe : foeLane->getVehiclesSecure()) {
1158 #ifdef DEBUG_SURROUNDING
1159  std::cout << " On foeLane '" << foe->getID() << "'" << std::endl;
1160 #endif
1161  vehs.insert(vehs.end(), foe);
1162  }
1163  foeLane->releaseVehicles();
1164  for (auto& laneInfo : foeLane->getIncomingLanes()) {
1165  const MSLane* foeLanePred = laneInfo.lane;
1166  if (foeLanePred->isInternal()) {
1167 #ifdef DEBUG_SURROUNDING
1168  std::cout << " foeLanePred '" << foeLanePred->getID() << "'" << std::endl;
1169 #endif
1170  for (const MSVehicle* foe : foeLanePred->getVehiclesSecure()) {
1171 #ifdef DEBUG_SURROUNDING
1172  std::cout << " On foeLanePred '" << foe->getID() << "'" << std::endl;
1173 #endif
1174  vehs.insert(vehs.end(), foe);
1175  }
1176  foeLanePred->releaseVehicles();
1177  }
1178  }
1179  }
1180  }
1181 }
1182 
1183 void
1184 Helper::applySubscriptionFilterFieldOfVision(const Subscription& s, std::set<std::string>& objIDs) {
1186  WRITE_WARNINGF("Field of vision opening angle ('%') should be within interval (0, 360), ignoring filter...", s.filterFieldOfVisionOpeningAngle);
1187  return;
1188  }
1189 
1190  MSBaseVehicle* egoVehicle = getVehicle(s.id);
1191  Position egoPosition = egoVehicle->getPosition();
1192  double openingAngle = DEG2RAD(s.filterFieldOfVisionOpeningAngle);
1193 
1194 #ifdef DEBUG_SURROUNDING
1195  std::cout << "FOVFILTER: ego direction = " << toString(RAD2DEG(egoVehicle->getAngle())) << " (deg)" << std::endl;
1196 #endif
1197 
1198  auto i = objIDs.begin();
1199  while (i != objIDs.end()) {
1200  if (s.id.compare(*i) == 0) { // skip if this is the ego vehicle
1201  ++i;
1202  continue;
1203  }
1205  double angleEgoToVeh = egoPosition.angleTo2D(obj->getPosition());
1206  double alpha = GeomHelper::angleDiff(egoVehicle->getAngle(), angleEgoToVeh);
1207 
1208 #ifdef DEBUG_SURROUNDING
1209  const std::string objType = s.isVehicleToPersonContextSubscription() ? "person" : "veh";
1210  std::cout << "FOVFILTER: " << objType << " '" << *i << "' dist = " << toString(egoPosition.distanceTo2D(obj->getPosition())) << std::endl;
1211  std::cout << "FOVFILTER: " << objType << " '" << *i << "' alpha = " << toString(RAD2DEG(alpha)) << " (deg)" << std::endl;
1212 #endif
1213 
1214  if (abs(alpha) > openingAngle * 0.5) {
1215  i = objIDs.erase(i);
1216  } else {
1217  ++i;
1218  }
1219  }
1220 }
1221 
1222 void
1223 Helper::applySubscriptionFilterLateralDistance(const Subscription& s, std::set<const SUMOTrafficObject*>& vehs, double downstreamDist, double upstreamDist,
1224  double lateralDist) {
1225  // collect all vehicles within maximum range of interest to get an upper bound
1226  PositionVector vehShape;
1227  findObjectShape(s.commandId, s.id, vehShape);
1228  double range = MAX3(downstreamDist, upstreamDist, lateralDist);
1229  std::set<std::string> objIDs;
1230  collectObjectIDsInRange(s.contextDomain, vehShape, range, objIDs);
1231 
1232 #ifdef DEBUG_SURROUNDING
1233  std::cout << "FILTER_LATERAL_DIST: collected object IDs (range " << range << "):" << std::endl;
1234  for (std::string i : objIDs) {
1235  std::cout << i << std::endl;
1236  }
1237 #endif
1238 
1239  MSVehicle* v = dynamic_cast<MSVehicle*>(getVehicle(s.id));
1240 #ifdef DEBUG_SURROUNDING
1241  std::cout << "FILTER_LATERAL_DIST: myLane is '" << v->getLane()->getID() << "', pos " << v->getPositionOnLane() << std::endl;
1242  std::cout << "FILTER_LATERAL_DIST: opposite lane is '" << v->getLane()->getParallelOpposite()->getID() << "'" << std::endl;
1243 #endif
1244  double frontPosOnLane = v->getPositionOnLane();
1245  if (v->getLaneChangeModel().isOpposite()) {
1246  frontPosOnLane = v->getLane()->getOppositePos(frontPosOnLane);
1247  }
1248  // 1st pass: downstream (make sure that the whole length of the vehicle is included in the match)
1249  const double backPosOnLane = MAX2(0.0, frontPosOnLane - v->getVehicleType().getLength());
1250  applySubscriptionFilterLateralDistanceSinglePass(s, objIDs, vehs, v->getUpcomingLanesUntil(downstreamDist), backPosOnLane, v->getLateralPositionOnLane(),
1251  true);
1252  // 2nd pass: upstream
1253  applySubscriptionFilterLateralDistanceSinglePass(s, objIDs, vehs, v->getPastLanesUntil(upstreamDist), frontPosOnLane, v->getLateralPositionOnLane(), false);
1254 }
1255 
1256 void
1258  std::set<const SUMOTrafficObject*>& vehs,
1259  const std::vector<const MSLane*>& lanes, double posOnLane, double posLat, bool isDownstream) {
1260  const double streamDist = isDownstream ? s.filterDownstreamDist : s.filterUpstreamDist;
1261  double distRemaining = streamDist;
1262  bool isFirstLane = true;
1263  PositionVector combinedShape;
1264  for (const MSLane* lane : lanes) {
1265 #ifdef DEBUG_SURROUNDING
1266  std::cout << "FILTER_LATERAL_DIST: current lane " << (isDownstream ? "down" : "up") << " is '" << lane->getID() << "', length " << lane->getLength()
1267  << ", pos " << posOnLane << ", distRemaining " << distRemaining << std::endl;
1268 #endif
1269  PositionVector laneShape = lane->getShape();
1270  if (isFirstLane) {
1271  isFirstLane = false;
1272  double geometryPos = lane->interpolateLanePosToGeometryPos(posOnLane);
1273  if (geometryPos <= POSITION_EPS) {
1274  if (!isDownstream) {
1275  continue;
1276  }
1277  } else {
1278  if (geometryPos >= laneShape.length() - POSITION_EPS) {
1279  laneShape = isDownstream ? PositionVector() : laneShape;
1280  } else {
1281  auto pair = laneShape.splitAt(geometryPos, false);
1282  laneShape = isDownstream ? pair.second : pair.first;
1283  }
1284  }
1285  }
1286  double laneLength = lane->interpolateGeometryPosToLanePos(laneShape.length());
1287  if (distRemaining - laneLength < 0.) {
1288  double geometryPos = lane->interpolateLanePosToGeometryPos(isDownstream ? distRemaining : laneLength - distRemaining);
1289  if (geometryPos > POSITION_EPS && geometryPos < laneShape.length() - POSITION_EPS) {
1290  auto pair = laneShape.splitAt(geometryPos, false);
1291  laneShape = isDownstream ? pair.first : pair.second;
1292  }
1293  }
1294  distRemaining -= laneLength;
1295  try {
1296  laneShape.move2side(-posLat);
1297  } catch (ProcessError&) {
1298  WRITE_WARNING("addSubscriptionFilterLateralDistance could not determine shape of lane '" + lane->getID() + "' with lateral shift of " + toString(posLat));
1299  }
1300 #ifdef DEBUG_SURROUNDING
1301  std::cout << " posLat=" << posLat << " laneShape=" << laneShape << "\n";
1302 #endif
1303  if (isDownstream) {
1304  combinedShape.append(laneShape);
1305  } else {
1306  combinedShape.prepend(laneShape);
1307  }
1308  if (distRemaining <= POSITION_EPS) {
1309  break;
1310  }
1311  }
1312 #ifdef DEBUG_SURROUNDING
1313  std::cout << " combinedShape=" << combinedShape << "\n";
1314 #endif
1315  // check remaining objects' distances to the combined shape
1316  auto i = objIDs.begin();
1317  while (i != objIDs.end()) {
1319  double minPerpendicularDist = combinedShape.distance2D(obj->getPosition(), true);
1320 #ifdef DEBUG_SURROUNDING
1321  std::cout << (isDownstream ? "DOWN" : "UP") << " obj " << obj->getID() << " perpendicular dist=" << minPerpendicularDist << " filterLateralDist=" << s.filterLateralDist << "\n";
1322 #endif
1323  if ((minPerpendicularDist != GeomHelper::INVALID_OFFSET) && (minPerpendicularDist <= s.filterLateralDist)) {
1324  vehs.insert(obj);
1325  i = objIDs.erase(i);
1326  } else {
1327  ++i;
1328  }
1329  }
1330 }
1331 
1332 void
1333 Helper::setRemoteControlled(MSVehicle* v, Position xyPos, MSLane* l, double pos, double posLat, double angle,
1334  int edgeOffset, ConstMSEdgeVector route, SUMOTime t) {
1336  v->getInfluencer().setRemoteControlled(xyPos, l, pos, posLat, angle, edgeOffset, route, t);
1337 }
1338 
1339 void
1340 Helper::setRemoteControlled(MSPerson* p, Position xyPos, MSLane* l, double pos, double posLat, double angle,
1341  int edgeOffset, ConstMSEdgeVector route, SUMOTime t) {
1342  myRemoteControlledPersons[p->getID()] = p;
1343  p->getInfluencer().setRemoteControlled(xyPos, l, pos, posLat, angle, edgeOffset, route, t);
1344 }
1345 
1346 
1347 void
1349  for (auto& controlled : myRemoteControlledVehicles) {
1350  if (MSNet::getInstance()->getVehicleControl().getVehicle(controlled.first) != nullptr) {
1351  controlled.second->getInfluencer().postProcessRemoteControl(controlled.second);
1352  } else {
1353  WRITE_WARNING("Vehicle '" + controlled.first + "' was removed though being controlled by TraCI");
1354  }
1355  }
1357  for (auto& controlled : myRemoteControlledPersons) {
1358  if (MSNet::getInstance()->getPersonControl().get(controlled.first) != nullptr) {
1359  controlled.second->getInfluencer().postProcessRemoteControl(controlled.second);
1360  } else {
1361  WRITE_WARNING("Person '" + controlled.first + "' was removed though being controlled by TraCI");
1362  }
1363  }
1364  myRemoteControlledPersons.clear();
1365 }
1366 
1367 
1368 bool
1369 Helper::moveToXYMap(const Position& pos, double maxRouteDistance, bool mayLeaveNetwork, const std::string& origID, const double angle,
1370  double speed, const ConstMSEdgeVector& currentRoute, const int routePosition, const MSLane* currentLane, double currentLanePos, bool onRoad,
1371  SUMOVehicleClass vClass, bool setLateralPos,
1372  double& bestDistance, MSLane** lane, double& lanePos, int& routeOffset, ConstMSEdgeVector& edges) {
1373  // collect edges around the vehicle/person
1374 #ifdef DEBUG_MOVEXY
1375  std::cout << SIMTIME << " moveToXYMap pos=" << pos << " angle=" << angle << " vClass=" << toString(vClass) << "\n";
1376 #endif
1377  const MSEdge* const currentRouteEdge = currentRoute[routePosition];
1378  std::set<const Named*> into;
1379  PositionVector shape;
1380  shape.push_back(pos);
1381  collectObjectsInRange(libsumo::CMD_GET_EDGE_VARIABLE, shape, maxRouteDistance, into);
1382  double maxDist = 0;
1383  std::map<MSLane*, LaneUtility, ComparatorNumericalIdLess> lane2utility;
1384  // compute utility for all candidate edges
1385  for (const Named* namedEdge : into) {
1386  const MSEdge* e = dynamic_cast<const MSEdge*>(namedEdge);
1387  if ((e->getPermissions() & vClass) != vClass) {
1388  continue;
1389  }
1390  const MSEdge* prevEdge = nullptr;
1391  const MSEdge* nextEdge = nullptr;
1392  bool onRoute = false;
1393  // the next if/the clause sets "onRoute", "prevEdge", and "nextEdge", depending on
1394  // whether the currently seen edge is an internal one or a normal one
1395  if (e->isWalkingArea() || e->isCrossing()) {
1396  // find current intersection along the route
1397  const MSJunction* junction = e->getFromJunction();
1398  for (int i = routePosition; i < (int)currentRoute.size(); i++) {
1399  const MSEdge* cand = currentRoute[i];
1400  if (cand->getToJunction() == junction) {
1401  prevEdge = cand;
1402  if (i + 1 < (int)currentRoute.size()) {
1403  onRoute = true;
1404  nextEdge = currentRoute[i + 1];
1405  }
1406  break;
1407  }
1408  }
1409  if (onRoute == false) {
1410  // search backward
1411  for (int i = routePosition - 1; i >= 0; i--) {
1412  const MSEdge* cand = currentRoute[i];
1413  if (cand->getToJunction() == junction) {
1414  onRoute = true;
1415  prevEdge = cand;
1416  nextEdge = currentRoute[i + 1];
1417  break;
1418  }
1419  }
1420  }
1421  if (prevEdge == nullptr) {
1422  // use arbitrary predecessor
1423  if (e->getPredecessors().size() > 0) {
1424  prevEdge = e->getPredecessors().front();
1425  } else if (e->getSuccessors().size() > 1) {
1426  for (MSEdge* e2 : e->getSuccessors()) {
1427  if (e2 != nextEdge) {
1428  prevEdge = e2;
1429  break;
1430  }
1431  }
1432  }
1433  }
1434  if (nextEdge == nullptr) {
1435  if (e->getSuccessors().size() > 0) {
1436  nextEdge = e->getSuccessors().front();
1437  } else if (e->getPredecessors().size() > 1) {
1438  for (MSEdge* e2 : e->getPredecessors()) {
1439  if (e2 != prevEdge) {
1440  nextEdge = e2;
1441  break;
1442  }
1443  }
1444  }
1445  }
1446 #ifdef DEBUG_MOVEXY_ANGLE
1447  std::cout << "walkingarea/crossing:" << e->getID() << " prev:" << Named::getIDSecure(prevEdge) << " next:" << Named::getIDSecure(nextEdge)
1448  << " pred=" << toString(e->getPredecessors()) << " succ=" << toString(e->getSuccessors())
1449  << "\n";
1450 #endif
1451  } else if (e->isNormal()) {
1452  // a normal edge
1453  //
1454  // check whether the currently seen edge is in the vehicle's route
1455  // - either the one it's on or one of the next edges
1456  ConstMSEdgeVector::const_iterator searchStart = currentRoute.begin() + routePosition;
1457  if (onRoad && currentLane->getEdge().isInternal()) {
1458  ++searchStart;
1459  }
1460  ConstMSEdgeVector::const_iterator edgePos = std::find(searchStart, currentRoute.end(), e);
1461  onRoute = edgePos != currentRoute.end(); // no? -> onRoute is false
1462  if (edgePos == currentRoute.end() - 1 && currentRouteEdge == e) {
1463  // onRoute is false as well if the vehicle is beyond the edge
1464  onRoute &= currentRouteEdge->getLength() > currentLanePos + SPEED2DIST(speed);
1465  }
1466  // save prior and next edges
1467  prevEdge = e;
1468  nextEdge = !onRoute || edgePos == currentRoute.end() - 1 ? nullptr : *(edgePos + 1);
1469 #ifdef DEBUG_MOVEXY_ANGLE
1470  std::cout << "normal:" << e->getID() << " prev:" << Named::getIDSecure(prevEdge) << " next:" << Named::getIDSecure(nextEdge) << "\n";
1471 #endif
1472  } else if (e->isInternal()) {
1473  // an internal edge
1474  // get the previous edge
1475  prevEdge = e;
1476  while (prevEdge != nullptr && prevEdge->isInternal()) {
1477  MSLane* l = prevEdge->getLanes()[0];
1478  l = l->getLogicalPredecessorLane();
1479  prevEdge = l == nullptr ? nullptr : &l->getEdge();
1480  }
1481  // check whether the previous edge is on the route (was on the route)
1482  ConstMSEdgeVector::const_iterator prevEdgePos = std::find(currentRoute.begin() + routePosition, currentRoute.end(), prevEdge);
1483  nextEdge = e;
1484  while (nextEdge != nullptr && nextEdge->isInternal()) {
1485  nextEdge = nextEdge->getSuccessors()[0]; // should be only one for an internal edge
1486  }
1487  if (prevEdgePos != currentRoute.end() && (prevEdgePos + 1) != currentRoute.end()) {
1488  onRoute = *(prevEdgePos + 1) == nextEdge;
1489  }
1490 #ifdef DEBUG_MOVEXY_ANGLE
1491  std::cout << "internal:" << e->getID() << " prev:" << Named::getIDSecure(prevEdge) << " next:" << Named::getIDSecure(nextEdge) << "\n";
1492 #endif
1493  }
1494 
1495 
1496  // weight the lanes...
1497  const bool perpendicular = false;
1498  for (MSLane* const l : e->getLanes()) {
1499  if (!l->allowsVehicleClass(vClass)) {
1500  continue;
1501  }
1502  if (l->getShape().length() == 0) {
1503  // mapping to shapeless lanes is a bad idea
1504  continue;
1505  }
1506  double langle = 180.;
1507  double dist = FAR_AWAY;
1508  double perpendicularDist = FAR_AWAY;
1509  // add some slack to avoid issues from tiny gaps between consecutive lanes
1510  const double slack = POSITION_EPS;
1511  PositionVector laneShape = l->getShape();
1512  laneShape.extrapolate2D(slack);
1513  double off = laneShape.nearest_offset_to_point2D(pos, true);
1514  if (off != GeomHelper::INVALID_OFFSET) {
1515  perpendicularDist = laneShape.distance2D(pos, true);
1516  }
1517  off = l->getShape().nearest_offset_to_point2D(pos, perpendicular);
1518  if (off != GeomHelper::INVALID_OFFSET) {
1519  dist = l->getShape().distance2D(pos, perpendicular);
1520  langle = GeomHelper::naviDegree(l->getShape().rotationAtOffset(off));
1521  }
1522  // cannot trust lanePos on walkingArea
1523  bool sameEdge = onRoad && e == &currentLane->getEdge() && currentRouteEdge->getLength() > currentLanePos + SPEED2DIST(speed) && !e->isWalkingArea();
1524  /*
1525  const MSEdge* rNextEdge = nextEdge;
1526  while(rNextEdge==0&&lane->getEdge().getPurpose()==MSEdge::EDGEFUNCTION_INTERNAL) {
1527  MSLane* next = lane->getLinkCont()[0]->getLane();
1528  rNextEdge = next == 0 ? 0 : &next->getEdge();
1529  }
1530  */
1531  double dist2 = dist;
1532  if (mayLeaveNetwork && fabs(dist - perpendicularDist) > slack) {
1533  // ambiguous mapping. Don't trust this
1534  dist2 = FAR_AWAY;
1535  }
1536  const double angleDiff = (angle == INVALID_DOUBLE_VALUE || l->getEdge().isWalkingArea() ? 0 : GeomHelper::getMinAngleDiff(angle, langle));
1537 #ifdef DEBUG_MOVEXY_ANGLE
1538  std::cout << std::setprecision(gPrecision)
1539  << " candLane=" << l->getID() << " lAngle=" << langle << " lLength=" << l->getLength()
1540  << " angleDiff=" << angleDiff
1541  << " off=" << off
1542  << " pDist=" << perpendicularDist
1543  << " dist=" << dist
1544  << " dist2=" << dist2
1545  << "\n";
1546  std::cout << l->getID() << " param=" << l->getParameter(SUMO_PARAM_ORIGID, "") << " origID='" << origID << "\n";
1547 #endif
1548 
1549  bool origIDMatch = l->getParameter(SUMO_PARAM_ORIGID, l->getID()) == origID;
1550  if (origIDMatch && setLateralPos
1551  && perpendicularDist > l->getWidth() / 2) {
1552  origIDMatch = false;
1553  }
1554  lane2utility.emplace(l, LaneUtility(
1555  dist2, perpendicularDist, off, angleDiff,
1556  origIDMatch,
1557  onRoute, sameEdge, prevEdge, nextEdge));
1558  // update scaling value
1559  maxDist = MAX2(maxDist, MIN2(dist, SUMO_const_laneWidth));
1560 
1561  }
1562  }
1563 
1564  // get the best lane given the previously computed values
1565  double bestValue = 0;
1566  MSLane* bestLane = nullptr;
1567  for (const auto& it : lane2utility) {
1568  MSLane* const l = it.first;
1569  const LaneUtility& u = it.second;
1570  double distN = u.dist > 999 ? -10 : 1. - (u.dist / maxDist);
1571  double angleDiffN = 1. - (u.angleDiff / 180.);
1572  double idN = u.ID ? 1 : 0;
1573  double onRouteN = u.onRoute ? 1 : 0;
1574  double sameEdgeN = u.sameEdge ? MIN2(currentRouteEdge->getLength() / MAX2(NUMERICAL_EPS, speed), (double)1.) : 0;
1575  double value = (distN * .5 // distance is more important than angle because the vehicle might be driving in the opposite direction
1576  + angleDiffN * 0.35 /*.5 */
1577  + idN * 1
1578  + onRouteN * 0.1
1579  + sameEdgeN * 0.1);
1580 #ifdef DEBUG_MOVEXY
1581  std::cout << " x; l:" << l->getID() << " d:" << u.dist << " dN:" << distN << " aD:" << angleDiffN <<
1582  " ID:" << idN << " oRN:" << onRouteN << " sEN:" << sameEdgeN << " value:" << value << std::endl;
1583 #endif
1584  if (value > bestValue || bestLane == nullptr) {
1585  bestValue = value;
1586  if (u.dist == FAR_AWAY) {
1587  bestLane = nullptr;
1588  } else {
1589  bestLane = l;
1590  }
1591  }
1592  }
1593  // no best lane found, return
1594  if (bestLane == nullptr) {
1595  return false;
1596  }
1597  const LaneUtility& u = lane2utility.find(bestLane)->second;
1598  bestDistance = u.dist;
1599  *lane = bestLane;
1600  lanePos = MAX2(0., MIN2(double((*lane)->getLength() - POSITION_EPS),
1602  bestLane->getShape().nearest_offset_to_point25D(pos, false))));
1603  const MSEdge* prevEdge = u.prevEdge;
1604  if (u.onRoute) {
1605  ConstMSEdgeVector::const_iterator prevEdgePos = std::find(currentRoute.begin(), currentRoute.end(), prevEdge);
1606  routeOffset = (int)std::distance(currentRoute.begin(), prevEdgePos);
1607  //std::cout << SIMTIME << "moveToXYMap vehicle=" << veh.getID() << " currLane=" << veh.getLane()->getID() << " routeOffset=" << routeOffset << " edges=" << toString(ev) << " bestLane=" << bestLane->getID() << " prevEdge=" << prevEdge->getID() << "\n";
1608  } else {
1609  edges.push_back(u.prevEdge);
1610  /*
1611  if(bestLane->getEdge().getPurpose()!=MSEdge::EDGEFUNCTION_INTERNAL) {
1612  edges.push_back(&bestLane->getEdge());
1613  }
1614  */
1615  if (u.nextEdge != nullptr) {
1616  edges.push_back(u.nextEdge);
1617  }
1618  routeOffset = 0;
1619 #ifdef DEBUG_MOVEXY_ANGLE
1620  std::cout << SIMTIME << " internal2: lane=" << bestLane->getID() << " prev=" << Named::getIDSecure(u.prevEdge) << " next=" << Named::getIDSecure(u.nextEdge) << "\n";
1621 #endif
1622  }
1623  return true;
1624 }
1625 
1626 
1627 bool
1628 Helper::findCloserLane(const MSEdge* edge, const Position& pos, SUMOVehicleClass vClass, double& bestDistance, MSLane** lane) {
1629  // TODO maybe there is a way to abort this early if the lane already found is good enough but simply
1630  // checking for bestDistance < POSITON_EPS gives ugly order dependencies (#7933), so think twice and profile first
1631  if (edge == nullptr) {
1632  return false;
1633  }
1634  bool newBest = false;
1635  for (MSLane* const candidateLane : edge->getLanes()) {
1636  if (!candidateLane->allowsVehicleClass(vClass)) {
1637  continue;
1638  }
1639  if (candidateLane->getShape().length() == 0) {
1640  // mapping to shapeless lanes is a bad idea
1641  continue;
1642  }
1643  const double dist = candidateLane->getShape().distance2D(pos); // get distance
1644 #ifdef DEBUG_MOVEXY
1645  std::cout << " b at lane " << candidateLane->getID() << " dist:" << dist << " best:" << bestDistance << std::endl;
1646 #endif
1647  if (dist < bestDistance || (dist == bestDistance && candidateLane->getNumericalID() < (*lane)->getNumericalID())) {
1648  // is the new distance the best one? keep then...
1649  bestDistance = dist;
1650  *lane = candidateLane;
1651  newBest = true;
1652  }
1653  }
1654  return newBest;
1655 }
1656 
1657 
1658 bool
1659 Helper::moveToXYMap_matchingRoutePosition(const Position& pos, const std::string& origID,
1660  const ConstMSEdgeVector& currentRoute, int routeIndex,
1661  SUMOVehicleClass vClass, bool setLateralPos,
1662  double& bestDistance, MSLane** lane, double& lanePos, int& routeOffset) {
1663 #ifdef DEBUG_MOVEXY
1664  std::cout << SIMTIME << " moveToXYMap_matchingRoutePosition pos=" << pos << " vClass=" << toString(vClass) << "\n";
1665 #endif
1666  //std::cout << "moveToXYMap_matchingRoutePosition pos=" << pos << "\n";
1667  routeOffset = 0;
1668  // routes may be looped which makes routeOffset ambiguous. We first try to
1669  // find the closest upcoming edge on the route and then look for closer passed edges
1670 
1671  // look forward along the route
1672  const MSEdge* prev = nullptr;
1673  for (int i = routeIndex; i < (int)currentRoute.size(); ++i) {
1674  const MSEdge* cand = currentRoute[i];
1675  while (prev != nullptr) {
1676  // check internal edge(s)
1677  const MSEdge* internalCand = prev->getInternalFollowingEdge(cand);
1678  findCloserLane(internalCand, pos, vClass, bestDistance, lane);
1679  prev = internalCand;
1680  }
1681  if (findCloserLane(cand, pos, vClass, bestDistance, lane)) {
1682  routeOffset = i;
1683  }
1684  prev = cand;
1685  }
1686  // look backward along the route
1687  const MSEdge* next = currentRoute[routeIndex];
1688  for (int i = routeIndex; i >= 0; --i) {
1689  const MSEdge* cand = currentRoute[i];
1690  //std::cout << " next=" << next->getID() << " cand=" << cand->getID() << " i=" << i << " routeIndex=" << routeIndex << "\n";
1691  prev = cand;
1692  while (prev != nullptr) {
1693  // check internal edge(s)
1694  const MSEdge* internalCand = prev->getInternalFollowingEdge(next);
1695  if (findCloserLane(internalCand, pos, vClass, bestDistance, lane)) {
1696  routeOffset = i;
1697  }
1698  prev = internalCand;
1699  }
1700  if (findCloserLane(cand, pos, vClass, bestDistance, lane)) {
1701  routeOffset = i;
1702  }
1703  next = cand;
1704  }
1705  if (vClass == SVC_PEDESTRIAN) {
1706  // consider all crossings and walkingareas along the route
1707  std::map<const MSJunction*, int> routeJunctions;
1708  for (int i = 0; i < (int)currentRoute.size() - 1; ++i) {
1709  routeJunctions[currentRoute[i]->getToJunction()] = i;
1710  }
1711  std::set<const Named*> into;
1712  PositionVector shape;
1713  shape.push_back(pos);
1715  for (const Named* named : into) {
1716  const MSLane* cand = dynamic_cast<const MSLane*>(named);
1717  if ((cand->getEdge().isWalkingArea() || cand->getEdge().isCrossing())
1718  && routeJunctions.count(cand->getEdge().getToJunction()) != 0) {
1719  if (findCloserLane(&cand->getEdge(), pos, vClass, bestDistance, lane)) {
1720  routeOffset = routeJunctions[cand->getEdge().getToJunction()];
1721  }
1722  }
1723  }
1724  }
1725 
1726  assert(lane != 0);
1727  // quit if no solution was found, reporting a failure
1728  if (lane == nullptr) {
1729 #ifdef DEBUG_MOVEXY
1730  std::cout << " b failed - no best route lane" << std::endl;
1731 #endif
1732  return false;
1733  }
1734 
1735 
1736  // position may be inaccurate; let's checkt the given index, too
1737  // @note: this is enabled for non-internal lanes only, as otherwise the position information may ambiguous
1738  if (!(*lane)->getEdge().isInternal()) {
1739  const std::vector<MSLane*>& lanes = (*lane)->getEdge().getLanes();
1740  for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
1741  if ((*i)->getParameter(SUMO_PARAM_ORIGID, (*i)->getID()) == origID) {
1742  if (setLateralPos) {
1743  // vehicle might end up on top of another lane with a big
1744  // lateral offset to the lane with origID.
1745  const double dist = (*i)->getShape().distance2D(pos); // get distance
1746  if (dist < (*i)->getWidth() / 2) {
1747  *lane = *i;
1748  break;
1749  }
1750  } else {
1751  *lane = *i;
1752  break;
1753  }
1754  }
1755  }
1756  }
1757  // check position, stuff, we should have the best lane along the route
1758  lanePos = MAX2(0., MIN2(double((*lane)->getLength() - POSITION_EPS),
1759  (*lane)->interpolateGeometryPosToLanePos(
1760  (*lane)->getShape().nearest_offset_to_point25D(pos, false))));
1761  //std::cout << SIMTIME << " moveToXYMap_matchingRoutePosition vehicle=" << veh.getID() << " currLane=" << veh.getLane()->getID() << " routeOffset=" << routeOffset << " edges=" << toString(edges) << " lane=" << (*lane)->getID() << "\n";
1762 #ifdef DEBUG_MOVEXY
1763  std::cout << " b ok lane " << (*lane)->getID() << " lanePos:" << lanePos << std::endl;
1764 #endif
1765  return true;
1766 }
1767 
1768 
1770  : VariableWrapper(handler), myResults(into), myContextResults(context), myActiveResults(&into) {
1771 
1772 }
1773 
1774 
1775 void
1776 Helper::SubscriptionWrapper::setContext(const std::string& refID) {
1777  myActiveResults = refID == "" ? &myResults : &myContextResults[refID];
1778 }
1779 
1780 
1781 void
1783  myActiveResults = &myResults;
1784  myResults.clear();
1785  myContextResults.clear();
1786 }
1787 
1788 
1789 bool
1790 Helper::SubscriptionWrapper::wrapDouble(const std::string& objID, const int variable, const double value) {
1791  (*myActiveResults)[objID][variable] = std::make_shared<TraCIDouble>(value);
1792  return true;
1793 }
1794 
1795 
1796 bool
1797 Helper::SubscriptionWrapper::wrapInt(const std::string& objID, const int variable, const int value) {
1798  (*myActiveResults)[objID][variable] = std::make_shared<TraCIInt>(value);
1799  return true;
1800 }
1801 
1802 
1803 bool
1804 Helper::SubscriptionWrapper::wrapString(const std::string& objID, const int variable, const std::string& value) {
1805  (*myActiveResults)[objID][variable] = std::make_shared<TraCIString>(value);
1806  return true;
1807 }
1808 
1809 
1810 bool
1811 Helper::SubscriptionWrapper::wrapStringList(const std::string& objID, const int variable, const std::vector<std::string>& value) {
1812  auto sl = std::make_shared<TraCIStringList>();
1813  sl->value = value;
1814  (*myActiveResults)[objID][variable] = sl;
1815  return true;
1816 }
1817 
1818 
1819 bool
1820 Helper::SubscriptionWrapper::wrapPosition(const std::string& objID, const int variable, const TraCIPosition& value) {
1821  (*myActiveResults)[objID][variable] = std::make_shared<TraCIPosition>(value);
1822  return true;
1823 }
1824 
1825 
1826 bool
1827 Helper::SubscriptionWrapper::wrapPositionVector(const std::string& objID, const int variable, const TraCIPositionVector& value) {
1828  (*myActiveResults)[objID][variable] = std::make_shared<TraCIPositionVector>(value);
1829  return true;
1830 }
1831 
1832 
1833 bool
1834 Helper::SubscriptionWrapper::wrapColor(const std::string& objID, const int variable, const TraCIColor& value) {
1835  (*myActiveResults)[objID][variable] = std::make_shared<TraCIColor>(value);
1836  return true;
1837 }
1838 
1839 
1840 bool
1841 Helper::SubscriptionWrapper::wrapStringDoublePair(const std::string& objID, const int variable, const std::pair<std::string, double>& value) {
1842  (*myActiveResults)[objID][variable] = std::make_shared<TraCIRoadPosition>(value.first, value.second);
1843  return true;
1844 }
1845 
1846 
1847 bool
1848 Helper::SubscriptionWrapper::wrapStringPair(const std::string& objID, const int variable, const std::pair<std::string, std::string>& value) {
1849  auto sl = std::make_shared<TraCIStringList>();
1850  sl->value.push_back(value.first);
1851  sl->value.push_back(value.second);
1852  (*myActiveResults)[objID][variable] = sl;
1853  return true;
1854 }
1855 
1856 
1857 void
1858 Helper::VehicleStateListener::vehicleStateChanged(const SUMOVehicle* const vehicle, MSNet::VehicleState to, const std::string& /*info*/) {
1859  myVehicleStateChanges[to].push_back(vehicle->getID());
1860 }
1861 
1862 
1863 void
1865  myTransportableStateChanges[to].push_back(transportable->getID());
1866 }
1867 
1868 
1869 }
1870 
1871 
1872 /****************************************************************************/
#define DEG2RAD(x)
Definition: GeomHelper.h:35
#define RAD2DEG(x)
Definition: GeomHelper.h:36
#define FAR_AWAY
Definition: Helper.cpp:59
#define LANE_RTREE_QUAL
Definition: Helper.h:80
std::vector< const MSEdge * > ConstMSEdgeVector
Definition: MSEdge.h:74
#define WRITE_WARNINGF(...)
Definition: MsgHandler.h:281
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:280
#define STEPS2TIME(x)
Definition: SUMOTime.h:53
#define SPEED2DIST(x)
Definition: SUMOTime.h:43
#define SIMSTEP
Definition: SUMOTime.h:59
#define SUMOTime_MAX
Definition: SUMOTime.h:33
#define SIMTIME
Definition: SUMOTime.h:60
#define TIME2STEPS(x)
Definition: SUMOTime.h:55
long long int SUMOTime
Definition: SUMOTime.h:32
SUMOVehicleClass
Definition of vehicle classes to differ between different lane usage and authority types.
@ SVC_PEDESTRIAN
pedestrian
const int STOP_DURATION_SET
const int STOP_UNTIL_SET
const int STOP_PARKING_SET
const int STOP_START_SET
const int STOP_CONTAINER_TRIGGER_SET
const int STOP_INDEX_FIT
const int STOP_TRIGGER_SET
const int STOP_END_SET
const std::string SUMO_PARAM_ORIGID
SumoXMLTag
Numbers representing SUMO-XML - element names.
@ SUMO_TAG_CHARGING_STATION
A Charging Station.
@ SUMO_TAG_NOTHING
invalid tag
@ SUMO_TAG_CONTAINER_STOP
A container stop.
@ SUMO_TAG_BUS_STOP
A bus stop.
@ SUMO_TAG_PARKING_AREA
A parking area.
@ SUMO_TAG_OVERHEAD_WIRE_SEGMENT
An overhead wire segment.
int gPrecision
the precision for floating point outputs
Definition: StdDefs.cpp:25
const double SUMO_const_laneWidth
Definition: StdDefs.h:48
T MIN2(T a, T b)
Definition: StdDefs.h:74
T MAX2(T a, T b)
Definition: StdDefs.h:80
T MAX3(T a, T b, T c)
Definition: StdDefs.h:94
std::string toHex(const T i, std::streamsize numDigits=0)
Definition: ToString.h:56
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:46
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:39
double ymin() const
Returns minimum y-coordinate.
Definition: Boundary.cpp:129
double xmin() const
Returns minimum x-coordinate.
Definition: Boundary.cpp:117
Boundary & grow(double by)
extends the boundary by the given amount
Definition: Boundary.cpp:299
double distanceTo2D(const Position &p) const
returns the euclidean distance in the x-y-plane
Definition: Boundary.cpp:221
double getHeight() const
Returns the height of the boundary (y-axis)
Definition: Boundary.cpp:159
double getWidth() const
Returns the width of the boudary (x-axis)
Definition: Boundary.cpp:153
double ymax() const
Returns maximum y-coordinate.
Definition: Boundary.cpp:135
double xmax() const
Returns maximum x-coordinate.
Definition: Boundary.cpp:123
static const GeoConvHelper & getFinal()
the coordinate transformation for writing the location element and for tracking the original coordina...
const Boundary & getConvBoundary() const
Returns the converted boundary.
static const double INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
Definition: GeomHelper.h:50
static double naviDegree(const double angle)
Definition: GeomHelper.cpp:192
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
Definition: GeomHelper.cpp:179
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
Definition: GeomHelper.cpp:173
const double myRange
Definition: Helper.h:69
const int myDomain
Definition: Helper.h:70
std::set< const Named * > & myObjects
The container.
Definition: Helper.h:67
void add(const MSLane *const l) const
Adds the given object to the container.
Definition: Helper.cpp:66
const PositionVector & myShape
Definition: Helper.h:68
The base class for microscopic and mesoscopic vehicles.
Definition: MSBaseVehicle.h:51
double getLength() const
Returns the vehicle's length.
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
A road/street connecting two junctions.
Definition: MSEdge.h:77
bool isCrossing() const
return whether this edge is a pedestrian crossing
Definition: MSEdge.h:267
SVCPermissions getPermissions() const
Returns the combined permissions of all lanes of this edge.
Definition: MSEdge.h:605
const MSEdgeVector & getPredecessors() const
Definition: MSEdge.h:392
const MSEdge * getInternalFollowingEdge(const MSEdge *followerAfterInternal) const
Definition: MSEdge.cpp:761
bool isWalkingArea() const
return whether this edge is walking area
Definition: MSEdge.h:281
const MSEdge * getOppositeEdge() const
Returns the opposite direction edge if on exists else a nullptr.
Definition: MSEdge.cpp:1163
bool isNormal() const
return whether this edge is an internal edge
Definition: MSEdge.h:257
std::vector< MSTransportable * > getSortedPersons(SUMOTime timestep, bool includeRiding=false) const
Returns this edge's persons sorted by pos.
Definition: MSEdge.cpp:1032
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
Definition: MSEdge.h:168
const MSJunction * getFromJunction() const
Definition: MSEdge.h:397
double getLength() const
return the length of the edge
Definition: MSEdge.h:641
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:262
static bool dictionary(const std::string &id, MSEdge *edge)
Inserts edge into the static dictionary Returns true if the key id isn't already in the dictionary....
Definition: MSEdge.cpp:885
const MSJunction * getToJunction() const
Definition: MSEdge.h:401
const MSEdgeVector & getSuccessors(SUMOVehicleClass vClass=SVC_IGNORING) const
Returns the following edges, restricted by vClass.
Definition: MSEdge.cpp:1084
The base class for an intersection.
Definition: MSJunction.h:58
Representation of a lane in the micro simulation.
Definition: MSLane.h:82
MSLane * getParallelLane(int offset, bool includeOpposite=true) const
Returns the lane with the given offset parallel to this one or 0 if it does not exist.
Definition: MSLane.cpp:2300
void visit(const LaneStoringVisitor &cont) const
Callback for visiting the lane when traversing an RTree.
Definition: MSLane.h:1234
virtual const VehCont & getVehiclesSecure() const
Returns the vehicles container; locks it for microsimulation.
Definition: MSLane.h:428
static void fill(RTREE &into)
Fills the given RTree with lane instances.
Definition: MSLane.cpp:2031
std::pair< MSVehicle *const, double > getFollower(const MSVehicle *ego, double egoPos, double dist, bool ignoreMinorLinks) const
Find follower vehicle for the given ego vehicle (which may be on the opposite direction lane)
Definition: MSLane.cpp:3822
std::set< MSVehicle * > getVehiclesInRange(const double a, const double b) const
Returns all vehicles on the lane overlapping with the interval [a,b].
Definition: MSLane.cpp:3723
const std::set< const MSBaseVehicle * > & getParkingVehicles() const
retrieve the parking vehicles (see GUIParkingArea)
Definition: MSLane.h:1153
double getLength() const
Returns the lane's length.
Definition: MSLane.h:541
std::vector< const MSLink * > getUpcomingLinks(double pos, double range, const std::vector< MSLane * > &contLanes) const
Returns all upcoming links within given range along the given (non-internal) continuation lanes measu...
Definition: MSLane.cpp:3754
std::set< MSVehicle * > getSurroundingVehicles(double startPos, double downstreamDist, double upstreamDist, std::shared_ptr< LaneCoverageInfo > checkedLanes) const
Returns all vehicles closer than downstreamDist along the road network starting on the given position...
Definition: MSLane.cpp:3671
bool allowsVehicleClass(SUMOVehicleClass vclass) const
Definition: MSLane.h:814
int getIndex() const
Returns the lane's index.
Definition: MSLane.h:563
double getOppositePos(double pos) const
return the corresponding position on the opposite lane
Definition: MSLane.cpp:3817
MSLane * getLogicalPredecessorLane() const
get the most likely precedecessor lane (sorted using by_connections_to_sorter). The result is cached ...
Definition: MSLane.cpp:2637
bool isInternal() const
Definition: MSLane.cpp:2122
MSEdge & getEdge() const
Returns the lane's edge.
Definition: MSLane.h:674
const PositionVector & getShape() const
Returns this lane's shape.
Definition: MSLane.h:478
double interpolateGeometryPosToLanePos(double geometryPos) const
Definition: MSLane.h:511
std::pair< MSVehicle *const, double > getLeader(const MSVehicle *veh, const double vehPos, const std::vector< MSLane * > &bestLaneConts, double dist=-1, bool checkTmpVehicles=false) const
Returns the immediate leader of veh and the distance to veh starting on this lane.
Definition: MSLane.cpp:2368
virtual void releaseVehicles() const
Allows to use the container for microsimulation again.
Definition: MSLane.h:458
MSLane * getParallelOpposite() const
return the opposite direction lane of this lanes edge or nullptr
Definition: MSLane.cpp:3811
VehicleState
Definition of a vehicle state.
Definition: MSNet.h:594
@ ARRIVED
The vehicle arrived at his destination (is deleted)
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:174
MSTLLogicControl & getTLSControl()
Returns the tls logics control.
Definition: MSNet.h:449
SUMOAbstractRouter< MSEdge, SUMOVehicle > & getRouterTT(const int rngIndex, const MSEdgeVector &prohibited=MSEdgeVector()) const
Definition: MSNet.cpp:1352
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
Definition: MSNet.h:376
static bool hasInstance()
Returns whether the network was already constructed.
Definition: MSNet.h:154
MSStoppingPlace * getStoppingPlace(const std::string &id, const SumoXMLTag category) const
Returns the named stopping place of the given category.
Definition: MSNet.cpp:1250
void addVehicleStateListener(VehicleStateListener *listener)
Adds a vehicle states listener.
Definition: MSNet.cpp:1127
void addTransportableStateListener(TransportableStateListener *listener)
Adds a transportable states listener.
Definition: MSNet.cpp:1155
TransportableState
Definition of a transportable state.
Definition: MSNet.h:671
virtual MSTransportableControl & getPersonControl()
Returns the person control.
Definition: MSNet.cpp:1059
void setRemoteControlled(Position xyPos, MSLane *l, double pos, double posLat, double angle, int edgeOffset, const ConstMSEdgeVector &route, SUMOTime t)
Definition: MSPerson.cpp:561
Influencer & getInfluencer()
Returns the velocity/lane influencer.
Definition: MSPerson.cpp:536
double getDistanceBetween(double fromPos, double toPos, const MSEdge *fromEdge, const MSEdge *toEdge, bool includeInternal=true, int routePosition=0) const
Compute the distance between 2 given edges on this route, including the length of internal lanes....
Definition: MSRoute.cpp:291
A lane area vehicles can halt at.
double getBeginLanePosition() const
Returns the begin position of this stop.
double getEndLanePosition() const
Returns the end position of this stop.
const MSLane & getLane() const
Returns the lane this stop is located at.
Storage for all programs of a single tls.
TLSLogicVariants & get(const std::string &id) const
Returns the variants of a named tls.
MSTransportable * get(const std::string &id) const
Returns the named transportable, if existing.
void setRemoteControlled(Position xyPos, MSLane *l, double pos, double posLat, double angle, int edgeOffset, const ConstMSEdgeVector &route, SUMOTime t)
Definition: MSVehicle.cpp:789
SUMOVehicle * getVehicle(const std::string &id) const
Returns the vehicle with the given id.
Representation of a vehicle in the micro simulation.
Definition: MSVehicle.h:75
const std::vector< const MSLane * > getUpcomingLanesUntil(double distance) const
Returns the upcoming (best followed by default 0) sequence of lanes to continue the route starting at...
Definition: MSVehicle.cpp:5525
bool isOnRoad() const
Returns the information whether the vehicle is on a road (is simulated)
Definition: MSVehicle.h:574
MSAbstractLaneChangeModel & getLaneChangeModel()
Definition: MSVehicle.cpp:5051
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:5497
const std::vector< const MSLane * > getPastLanesUntil(double distance) const
Returns the sequence of past lanes (right-most on edge) based on the route starting at the current la...
Definition: MSVehicle.cpp:5594
Influencer & getInfluencer()
Definition: MSVehicle.cpp:6389
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
Definition: MSVehicle.h:411
double getPositionOnLane() const
Get the vehicle's position along the lane.
Definition: MSVehicle.h:372
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:552
The car-following model and parameter.
Definition: MSVehicleType.h:62
SUMOVehicleClass getVehicleClass() const
Get this vehicle type's vehicle class.
const std::string & getID() const
Returns the name of the vehicle type.
Definition: MSVehicleType.h:90
double getLength() const
Get vehicle's length [m].
Allows to store the object; used as context while traveling the rtree in TraCI.
Definition: Named.h:90
Base class for objects which have an id.
Definition: Named.h:54
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
Definition: Named.h:67
const std::string & getID() const
Returns the id.
Definition: Named.h:74
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:37
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:252
double x() const
Returns the x-position.
Definition: Position.h:55
double z() const
Returns the z-position.
Definition: Position.h:65
double angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position
Definition: Position.h:262
double y() const
Returns the y-position.
Definition: Position.h:60
A list of positions.
void append(const PositionVector &v, double sameThreshold=2.0)
double length() const
Returns the length.
double distance2D(const Position &p, bool perpendicular=false) const
closest 2D-distance to point p (or -1 if perpendicular is true and the point is beyond this vector)
void prepend(const PositionVector &v, double sameThreshold=2.0)
double nearest_offset_to_point2D(const Position &p, bool perpendicular=true) const
return the nearest offest to point 2D
std::pair< PositionVector, PositionVector > splitAt(double where, bool use2D=false) const
Returns the two lists made when this list vector is splitted at the given point.
void move2side(double amount, double maxExtension=100)
move position vector to side using certain ammount
Boundary getBoxBoundary() const
Returns a boundary enclosing this list of lines.
void extrapolate2D(const double val, const bool onlyFirst=false)
extrapolate position vector in two dimensions (Z is ignored)
double nearest_offset_to_point25D(const Position &p, bool perpendicular=true) const
return the nearest offest to point 2D projected onto the 3D geometry
unsigned char red() const
Returns the red-amount of the color.
Definition: RGBColor.cpp:74
unsigned char alpha() const
Returns the alpha-amount of the color.
Definition: RGBColor.cpp:92
unsigned char green() const
Returns the green-amount of the color.
Definition: RGBColor.cpp:80
unsigned char blue() const
Returns the blue-amount of the color.
Definition: RGBColor.cpp:86
virtual bool compute(const E *from, const E *to, const V *const vehicle, SUMOTime msTime, std::vector< const E * > &into, bool silent=false)=0
Builds the route between the given edges using the minimum effort at the given time The definition of...
Representation of a vehicle, person, or container.
virtual bool isVehicle() const
Whether it is a vehicle.
virtual Position getPosition(const double offset=0) const =0
Return current position (x/y, cartesian)
virtual const MSLane * getLane() const =0
Returns the lane the object is currently at.
virtual const MSEdge * getEdge() const =0
Returns the edge the object is currently at.
Representation of a vehicle.
Definition: SUMOVehicle.h:60
virtual double getAngle() const =0
Get the vehicle's angle.
Definition of vehicle stop (position and duration)
SUMOTime started
the time at which this stop was reached
std::string edge
The edge to stop at (used only in NETEDIT)
std::string lane
The lane to stop at.
double speed
the speed at which this stop counts as reached (waypoint mode)
std::string parkingarea
(Optional) parking area if one is assigned to the stop
std::string split
the id of the vehicle (train portion) that splits of upon reaching this stop
double startPos
The stopping position start.
std::string line
the new line id of the trip within a cyclical public transport route
std::string chargingStation
(Optional) charging station if one is assigned to the stop
std::string overheadWireSegment
(Optional) overhead line segment if one is assigned to the stop
int parametersSet
Information for the output which parameter were set.
int index
at which position in the stops list
std::string join
the id of the vehicle (train portion) to which this vehicle shall be joined
SUMOTime until
The time at which the vehicle may continue its journey.
std::string actType
act Type (only used by Persons) (used by NETEDIT)
bool triggered
whether an arriving person lets the vehicle continue
SUMOTime ended
the time at which this stop was ended
double endPos
The stopping position end.
bool parking
whether the vehicle is removed from the net while stopping
std::string busstop
(Optional) bus stop if one is assigned to the stop
std::string tripId
id of the trip within a cyclical public transport route
std::string containerstop
(Optional) container stop if one is assigned to the stop
bool containerTriggered
whether an arriving container lets the vehicle continue
SUMOTime arrival
The (expected) time at which the vehicle reaches the stop.
SUMOTime duration
The stopping duration.
const MSEdge * nextEdge
Definition: Helper.h:255
const MSEdge * prevEdge
Definition: Helper.h:254
void setContext(const std::string &refID)
Definition: Helper.cpp:1776
bool wrapDouble(const std::string &objID, const int variable, const double value)
Definition: Helper.cpp:1790
bool wrapPositionVector(const std::string &objID, const int variable, const TraCIPositionVector &value)
Definition: Helper.cpp:1827
bool wrapColor(const std::string &objID, const int variable, const TraCIColor &value)
Definition: Helper.cpp:1834
bool wrapInt(const std::string &objID, const int variable, const int value)
Definition: Helper.cpp:1797
bool wrapStringList(const std::string &objID, const int variable, const std::vector< std::string > &value)
Definition: Helper.cpp:1811
bool wrapPosition(const std::string &objID, const int variable, const TraCIPosition &value)
Definition: Helper.cpp:1820
bool wrapString(const std::string &objID, const int variable, const std::string &value)
Definition: Helper.cpp:1804
bool wrapStringPair(const std::string &objID, const int variable, const std::pair< std::string, std::string > &value)
Definition: Helper.cpp:1848
bool wrapStringDoublePair(const std::string &objID, const int variable, const std::pair< std::string, double > &value)
Definition: Helper.cpp:1841
SubscriptionWrapper(VariableWrapper::SubscriptionHandler handler, SubscriptionResults &into, ContextSubscriptionResults &context)
Definition: Helper.cpp:1769
void transportableStateChanged(const MSTransportable *const transportable, MSNet::TransportableState to, const std::string &info="")
Called if a transportable changes its state.
Definition: Helper.cpp:1864
std::map< MSNet::TransportableState, std::vector< std::string > > myTransportableStateChanges
Changes in the states of simulated transportables.
Definition: Helper.h:306
std::map< MSNet::VehicleState, std::vector< std::string > > myVehicleStateChanges
Changes in the states of simulated vehicles.
Definition: Helper.h:299
void vehicleStateChanged(const SUMOVehicle *const vehicle, MSNet::VehicleState to, const std::string &info="")
Called if a vehicle changes its state.
Definition: Helper.cpp:1858
static Position makePosition(const TraCIPosition &position)
Definition: Helper.cpp:388
static MSEdge * getEdge(const std::string &edgeID)
Definition: Helper.cpp:394
static void postProcessRemoteControl()
Definition: Helper.cpp:1348
static void cleanup()
Definition: Helper.cpp:674
static void registerTransportableStateListener()
Definition: Helper.cpp:711
static double getDrivingDistance(std::pair< const MSLane *, double > &roadPos1, std::pair< const MSLane *, double > &roadPos2)
Definition: Helper.cpp:453
static void collectObjectsInRange(int domain, const PositionVector &shape, double range, std::set< const Named * > &into)
Definition: Helper.cpp:776
static TraCIPosition makeTraCIPosition(const Position &position, const bool includeZ=false)
Definition: Helper.cpp:378
static LANE_RTREE_QUAL * myLaneTree
A storage of lanes.
Definition: Helper.h:325
static void applySubscriptionFilterTurn(const Subscription &s, std::set< const SUMOTrafficObject * > &vehs)
Apply the subscription filter "turn": Gather upcoming junctions and vialanes within downstream distan...
Definition: Helper.cpp:1119
static void findObjectShape(int domain, const std::string &id, PositionVector &shape)
Definition: Helper.cpp:733
static PositionVector makePositionVector(const TraCIPositionVector &vector)
Definition: Helper.cpp:348
static void fuseLaneCoverage(std::shared_ptr< LaneCoverageInfo > aggregatedLaneCoverage, const std::shared_ptr< LaneCoverageInfo > newLaneCoverage)
Adds lane coverage information from newLaneCoverage into aggregatedLaneCoverage.
Definition: Helper.cpp:320
static bool moveToXYMap_matchingRoutePosition(const Position &pos, const std::string &origID, const ConstMSEdgeVector &currentRoute, int routeIndex, SUMOVehicleClass vClass, bool setLateralPos, double &bestDistance, MSLane **lane, double &lanePos, int &routeOffset)
Definition: Helper.cpp:1659
static void debugPrint(const SUMOTrafficObject *veh)
Definition: Helper.cpp:130
static MSPerson * getPerson(const std::string &id)
Definition: Helper.cpp:489
static void subscribe(const int commandId, const std::string &id, const std::vector< int > &variables, const double beginTime, const double endTime, const libsumo::TraCIResults &params, const int contextDomain=0, const double range=0.)
Definition: Helper.cpp:142
static TraCIPositionVector makeTraCIPositionVector(const PositionVector &positionVector)
helper functions
Definition: Helper.cpp:338
static void registerVehicleStateListener()
Definition: Helper.cpp:689
static const std::vector< std::string > & getTransportableStateChanges(const MSNet::TransportableState state)
Definition: Helper.cpp:719
static std::map< int, std::shared_ptr< VariableWrapper > > myWrapper
Map of commandIds -> their executors; applicable if the executor applies to the method footprint.
Definition: Helper.h:316
static void clearVehicleStates()
Definition: Helper.cpp:703
static void clearSubscriptions()
Definition: Helper.cpp:233
static MSBaseVehicle * getVehicle(const std::string &id)
Definition: Helper.cpp:475
static void applySubscriptionFilterLateralDistanceSinglePass(const Subscription &s, std::set< std::string > &objIDs, std::set< const SUMOTrafficObject * > &vehs, const std::vector< const MSLane * > &lanes, double posOnLane, double posLat, bool isDownstream)
Definition: Helper.cpp:1257
static void clearTransportableStates()
Definition: Helper.cpp:725
static TraCIColor makeTraCIColor(const RGBColor &color)
Definition: Helper.cpp:361
static void applySubscriptionFilterFieldOfVision(const Subscription &s, std::set< std::string > &objIDs)
Definition: Helper.cpp:1184
static Subscription * myLastContextSubscription
The last context subscription.
Definition: Helper.h:313
static TraCINextStopData buildStopData(const SUMOVehicleParameter::Stop &stopPar)
Definition: Helper.cpp:625
static TransportableStateListener myTransportableStateListener
Changes in the states of simulated transportables.
Definition: Helper.h:322
static void setRemoteControlled(MSVehicle *v, Position xyPos, MSLane *l, double pos, double posLat, double angle, int edgeOffset, ConstMSEdgeVector route, SUMOTime t)
Definition: Helper.cpp:1333
static void applySubscriptionFilters(const Subscription &s, std::set< std::string > &objIDs)
Filter the given ID-Set (which was obtained from an R-Tree search) according to the filters set by th...
Definition: Helper.cpp:814
static std::map< std::string, MSVehicle * > myRemoteControlledVehicles
Definition: Helper.h:327
static const MSVehicleType & getVehicleType(const std::string &vehicleID)
Definition: Helper.cpp:510
static bool moveToXYMap(const Position &pos, double maxRouteDistance, bool mayLeaveNetwork, const std::string &origID, const double angle, double speed, const ConstMSEdgeVector &currentRoute, const int routePosition, const MSLane *currentLane, double currentLanePos, bool onRoad, SUMOVehicleClass vClass, bool setLateralPos, double &bestDistance, MSLane **lane, double &lanePos, int &routeOffset, ConstMSEdgeVector &edges)
Definition: Helper.cpp:1369
static std::pair< MSLane *, double > convertCartesianToRoadMap(const Position &pos, const SUMOVehicleClass vClass)
Definition: Helper.cpp:421
static MSTLLogicControl::TLSLogicVariants & getTLS(const std::string &id)
Definition: Helper.cpp:516
static SUMOTrafficObject * getTrafficObject(int domain, const std::string &id)
Definition: Helper.cpp:499
static VehicleStateListener myVehicleStateListener
Changes in the states of simulated vehicles.
Definition: Helper.h:319
static std::vector< Subscription > mySubscriptions
The list of known, still valid subscriptions.
Definition: Helper.h:310
static SUMOVehicleParameter::Stop buildStopParameters(const std::string &edgeOrStoppingPlaceID, double pos, int laneIndex, double startPos, int flags, double duration, double until)
Definition: Helper.cpp:525
static void handleSingleSubscription(const Subscription &s)
Definition: Helper.cpp:251
static void applySubscriptionFilterLateralDistance(const Subscription &s, std::set< const SUMOTrafficObject * > &vehs, double downstreamDist, double upstreamDist, double lateralDist)
Apply the subscription filter "lateral distance": Only return vehicles within the given lateral dista...
Definition: Helper.cpp:1223
static const std::vector< std::string > & getVehicleStateChanges(const MSNet::VehicleState state)
Definition: Helper.cpp:697
static void collectObjectIDsInRange(int domain, const PositionVector &shape, double range, std::set< std::string > &into)
Definition: Helper.cpp:766
static void handleSubscriptions(const SUMOTime t)
Definition: Helper.cpp:183
static Subscription * addSubscriptionFilter(SubscriptionFilterType filter)
Definition: Helper.cpp:240
static const MSLane * getLaneChecking(const std::string &edgeID, int laneIndex, double pos)
Definition: Helper.cpp:404
static RGBColor makeRGBColor(const TraCIColor &color)
Definition: Helper.cpp:372
static void applySubscriptionFilterLanes(const Subscription &s, std::set< const SUMOTrafficObject * > &vehs, std::vector< int > &filterLanes, double downstreamDist, double upstreamDist, bool disregardOppositeDirection)
Apply the subscription filter "lanes": Only return vehicles on list of lanes relative to ego vehicle....
Definition: Helper.cpp:1021
static std::map< std::string, MSPerson * > myRemoteControlledPersons
Definition: Helper.h:328
static bool needNewSubscription(libsumo::Subscription &s, std::vector< Subscription > &subscriptions, libsumo::Subscription *&modifiedSubscription)
Definition: Helper.cpp:208
static bool findCloserLane(const MSEdge *edge, const Position &pos, SUMOVehicleClass vClass, double &bestDistance, MSLane **lane)
Definition: Helper.cpp:1628
static std::shared_ptr< tcpip::Storage > toStorage(const TraCIResult &v)
Definition: StorageHelper.h:33
Representation of a subscription.
Definition: Subscription.h:69
double filterUpstreamDist
Upstream distance specified by the upstream distance filter.
Definition: Subscription.h:136
int commandId
commandIdArg The command id of the subscription
Definition: Subscription.h:113
std::set< std::string > filterVTypes
vTypes specified by the vTypes filter
Definition: Subscription.h:140
double filterFieldOfVisionOpeningAngle
Opening angle (in deg) specified by the field of vision filter.
Definition: Subscription.h:144
std::vector< int > filterLanes
lanes specified by the lanes filter
Definition: Subscription.h:132
std::string id
The id of the object that is subscribed.
Definition: Subscription.h:115
int filterVClasses
vClasses specified by the vClasses filter,
Definition: Subscription.h:142
SUMOTime endTime
The end time of the subscription.
Definition: Subscription.h:123
int contextDomain
The domain ID of the context.
Definition: Subscription.h:125
double filterFoeDistToJunction
Foe distance to junction specified by the turn filter.
Definition: Subscription.h:138
bool isVehicleToVehicleContextSubscription() const
Definition: Subscription.h:104
SUMOTime beginTime
The begin time of the subscription.
Definition: Subscription.h:121
std::vector< int > variables
The subscribed variables.
Definition: Subscription.h:117
bool isVehicleToPersonContextSubscription() const
Definition: Subscription.h:108
double filterDownstreamDist
Downstream distance specified by the downstream distance filter.
Definition: Subscription.h:134
double filterLateralDist
Lateral distance specified by the lateral distance filter.
Definition: Subscription.h:146
int activeFilters
Active filters for the subscription (bitset,.
Definition: Subscription.h:130
double range
The range of the context.
Definition: Subscription.h:127
std::vector< std::shared_ptr< tcpip::Storage > > parameters
The parameters for the subscribed variables.
Definition: Subscription.h:119
An error which allows to continue.
Definition: TraCIDefs.h:130
bool(* SubscriptionHandler)(const std::string &objID, const int variable, VariableWrapper *wrapper, tcpip::Storage *paramData)
Definition of a method to be called for serving an associated commandID.
Definition: Subscription.h:152
virtual void setContext(const std::string &)
Definition: Subscription.h:155
TRACI_CONST double INVALID_DOUBLE_VALUE
TRACI_CONST int LAST_STEP_VEHICLE_NUMBER
TRACI_CONST int CMD_SUBSCRIBE_EDGE_CONTEXT
TRACI_CONST int CMD_SUBSCRIBE_LANE_CONTEXT
TRACI_CONST int TRACI_ID_LIST
TRACI_CONST int CMD_GET_POI_VARIABLE
TRACI_CONST int CMD_GET_TL_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_VEHICLE_CONTEXT
std::map< std::string, libsumo::SubscriptionResults > ContextSubscriptionResults
Definition: TraCIDefs.h:279
TRACI_CONST int VAR_ROAD_ID
TRACI_CONST int CMD_GET_VEHICLE_VARIABLE
TRACI_CONST int CMD_GET_EDGE_VARIABLE
TRACI_CONST int CMD_GET_PERSON_VARIABLE
TRACI_CONST int CMD_GET_LANEAREA_VARIABLE
TRACI_CONST int CMD_GET_ROUTE_VARIABLE
TRACI_CONST int CMD_GET_JUNCTION_VARIABLE
std::map< std::string, libsumo::TraCIResults > SubscriptionResults
{object->{variable->value}}
Definition: TraCIDefs.h:278
TRACI_CONST int VAR_LANEPOSITION
TRACI_CONST int CMD_GET_SIM_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_PERSON_CONTEXT
TRACI_CONST int CMD_GET_VEHICLETYPE_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_POLYGON_CONTEXT
TRACI_CONST int CMD_GET_LANE_VARIABLE
TRACI_CONST int CMD_GET_POLYGON_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_VEHICLE_VARIABLE
TRACI_CONST int CMD_GET_MULTIENTRYEXIT_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_INDUCTIONLOOP_CONTEXT
SubscriptionFilterType
Filter types for context subscriptions.
Definition: Subscription.h:36
@ SUBS_FILTER_LEAD_FOLLOW
Definition: Subscription.h:48
@ SUBS_FILTER_UPSTREAM_DIST
Definition: Subscription.h:46
@ SUBS_FILTER_VTYPE
Definition: Subscription.h:54
@ SUBS_FILTER_NO_RTREE
Definition: Subscription.h:61
@ SUBS_FILTER_LANES
Definition: Subscription.h:40
@ SUBS_FILTER_NOOPPOSITE
Definition: Subscription.h:42
@ SUBS_FILTER_DOWNSTREAM_DIST
Definition: Subscription.h:44
@ SUBS_FILTER_LATERAL_DIST
Definition: Subscription.h:59
@ SUBS_FILTER_TURN
Definition: Subscription.h:50
@ SUBS_FILTER_VCLASS
Definition: Subscription.h:52
@ SUBS_FILTER_MANEUVER
Definition: Subscription.h:63
@ SUBS_FILTER_FIELD_OF_VISION
Definition: Subscription.h:57
TRACI_CONST int CMD_SUBSCRIBE_POI_CONTEXT
TRACI_CONST int CMD_SUBSCRIBE_PERSON_VARIABLE
TRACI_CONST int CMD_GET_INDUCTIONLOOP_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_JUNCTION_CONTEXT
std::map< int, std::shared_ptr< libsumo::TraCIResult > > TraCIResults
{variable->value}
Definition: TraCIDefs.h:276
A 3D-position.
Definition: TraCIDefs.h:164
A list of positions.
Definition: TraCIDefs.h:207
std::vector< TraCIPosition > value
Definition: TraCIDefs.h:217