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-2020 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>
30 #include <microsim/MSEdgeControl.h>
32 #include <microsim/MSEdge.h>
33 #include <microsim/MSLane.h>
34 #include <microsim/MSVehicle.h>
37 #include <libsumo/TraCIDefs.h>
38 #include <libsumo/Edge.h>
39 #include <libsumo/InductionLoop.h>
40 #include <libsumo/Junction.h>
41 #include <libsumo/Lane.h>
42 #include <libsumo/LaneArea.h>
43 #include <libsumo/MultiEntryExit.h>
44 #include <libsumo/Person.h>
45 #include <libsumo/POI.h>
46 #include <libsumo/Polygon.h>
47 #include <libsumo/Route.h>
48 #include <libsumo/Simulation.h>
49 #include <libsumo/TrafficLight.h>
50 #include <libsumo/Vehicle.h>
51 #include <libsumo/VehicleType.h>
52 #include <libsumo/TraCIConstants.h>
53 #include "Helper.h"
54 
55 #define FAR_AWAY 1000.0
56 
57 //#define DEBUG_MOVEXY
58 //#define DEBUG_MOVEXY_ANGLE
59 //#define DEBUG_SURROUNDING
60 
61 void
62 LaneStoringVisitor::add(const MSLane* const l) const {
63  switch (myDomain) {
65  for (const MSVehicle* veh : l->getVehiclesSecure()) {
66  if (myShape.distance2D(veh->getPosition()) <= myRange) {
67  myObjects.insert(veh);
68  }
69  }
70  for (const MSVehicle* veh : l->getParkingVehicles()) {
71  if (myShape.distance2D(veh->getPosition()) <= myRange) {
72  myObjects.insert(veh);
73  }
74  }
75  l->releaseVehicles();
76  }
77  break;
79  l->getVehiclesSecure();
80  std::vector<MSTransportable*> persons = l->getEdge().getSortedPersons(MSNet::getInstance()->getCurrentTimeStep(), true);
81  for (auto p : persons) {
82  if (myShape.distance2D(p->getPosition()) <= myRange) {
83  myObjects.insert(p);
84  }
85  }
86  l->releaseVehicles();
87  }
88  break;
90  if (myShape.size() != 1 || l->getShape().distance2D(myShape[0]) <= myRange) {
91  myObjects.insert(&l->getEdge());
92  }
93  }
94  break;
96  if (myShape.size() != 1 || l->getShape().distance2D(myShape[0]) <= myRange) {
97  myObjects.insert(l);
98  }
99  }
100  break;
101  default:
102  break;
103 
104  }
105 }
106 
107 namespace libsumo {
108 // ===========================================================================
109 // static member initializations
110 // ===========================================================================
111 std::vector<Subscription> Helper::mySubscriptions;
112 Subscription* Helper::myLastContextSubscription = nullptr;
113 std::map<int, std::shared_ptr<VariableWrapper> > Helper::myWrapper;
114 Helper::VehicleStateListener Helper::myVehicleStateListener;
116 std::map<std::string, MSVehicle*> Helper::myRemoteControlledVehicles;
117 std::map<std::string, MSPerson*> Helper::myRemoteControlledPersons;
118 
119 
120 // ===========================================================================
121 // static member definitions
122 // ===========================================================================
123 
124 void
126  if (veh != nullptr) {
127  if (veh->isVehicle()) {
128  std::cout << " '" << veh->getID() << "' on lane '" << ((SUMOVehicle*)veh)->getLane()->getID() << "'\n";
129  } else {
130  std::cout << " '" << veh->getID() << "' on edge '" << veh->getEdge()->getID() << "'\n";
131  }
132  }
133 }
134 
135 
136 void
137 Helper::subscribe(const int commandId, const std::string& id, const std::vector<int>& variables,
138  const double beginTime, const double endTime, const libsumo::TraCIResults& /* params */,
139  const int contextDomain, const double range) {
140  myLastContextSubscription = nullptr;
141  if (variables.empty()) {
142  for (std::vector<libsumo::Subscription>::iterator j = mySubscriptions.begin(); j != mySubscriptions.end();) {
143  if (j->id == id && j->commandId == commandId && j->contextDomain == contextDomain) {
144  j = mySubscriptions.erase(j);
145  } else {
146  ++j;
147  }
148  }
149  return;
150  }
151  std::vector<std::vector<unsigned char> > parameters(variables.size());
152  const SUMOTime begin = beginTime == INVALID_DOUBLE_VALUE ? 0 : TIME2STEPS(beginTime);
153  const SUMOTime end = endTime == INVALID_DOUBLE_VALUE || endTime > STEPS2TIME(SUMOTime_MAX) ? SUMOTime_MAX : TIME2STEPS(endTime);
154  libsumo::Subscription s(commandId, id, variables, parameters, begin, end, contextDomain, range);
155  if (s.variables.size() == 1 && s.variables.front() == -1) {
156  s.variables.clear();
157  }
159  libsumo::Subscription* modifiedSubscription = nullptr;
160  needNewSubscription(s, mySubscriptions, modifiedSubscription);
161  if (modifiedSubscription->isVehicleToVehicleContextSubscription()
162  || modifiedSubscription->isVehicleToPersonContextSubscription()) {
163  // Set last modified vehicle context subscription active for filter modifications
164  myLastContextSubscription = modifiedSubscription;
165  }
166 }
167 
168 
169 void
171  std::vector<unsigned char> dest(sizeof(param));
172  std::memcpy(dest.data(), &param, sizeof(param));
173  mySubscriptions.back().parameters.pop_back();
174  mySubscriptions.back().parameters.emplace_back(dest);
175 }
176 
177 
178 void
179 Helper::addSubscriptionParam(const std::string& param) {
180  std::vector<unsigned char> dest(param.size());
181  std::memcpy(dest.data(), &param, param.size());
182  mySubscriptions.back().parameters.pop_back();
183  mySubscriptions.back().parameters.emplace_back(dest);
184 }
185 
186 
187 void
189  for (auto& wrapper : myWrapper) {
190  wrapper.second->clear();
191  }
192  for (std::vector<libsumo::Subscription>::iterator i = mySubscriptions.begin(); i != mySubscriptions.end();) {
193  const libsumo::Subscription& s = *i;
194  const bool isArrivedVehicle = (s.commandId == CMD_SUBSCRIBE_VEHICLE_VARIABLE || s.commandId == CMD_SUBSCRIBE_VEHICLE_CONTEXT)
197  && MSNet::getInstance()->getPersonControl().get(s.id) == nullptr;
198  if (s.endTime < t || isArrivedVehicle || isArrivedPerson) {
199  i = mySubscriptions.erase(i);
200  continue;
201  }
202  ++i;
203  }
204  for (const libsumo::Subscription& s : mySubscriptions) {
205  if (s.beginTime <= t) {
207  }
208  }
209 }
210 
211 
212 bool
213 Helper::needNewSubscription(libsumo::Subscription& s, std::vector<Subscription>& subscriptions, libsumo::Subscription*& modifiedSubscription) {
214  for (libsumo::Subscription& o : subscriptions) {
215  if (s.commandId == o.commandId && s.id == o.id &&
216  s.beginTime == o.beginTime && s.endTime == o.endTime &&
217  s.contextDomain == o.contextDomain && s.range == o.range) {
218  std::vector<std::vector<unsigned char> >::const_iterator k = s.parameters.begin();
219  for (const int v : s.variables) {
220  const int offset = (int)(std::find(o.variables.begin(), o.variables.end(), v) - o.variables.begin());
221  if (offset == (int)o.variables.size() || o.parameters[offset] != *k) {
222  o.variables.push_back(v);
223  o.parameters.push_back(*k);
224  }
225  ++k;
226  }
227  modifiedSubscription = &o;
228  return false;
229  }
230  }
231  subscriptions.push_back(s);
232  modifiedSubscription = &subscriptions.back();
233  return true;
234 }
235 
236 
237 void
239  mySubscriptions.clear();
240  myLastContextSubscription = nullptr;
241 }
242 
243 
246  if (myLastContextSubscription != nullptr) {
248  } else {
249  WRITE_WARNING("addSubscriptionFilter: No previous vehicle context subscription exists to apply the context filter.");
250  }
252 }
253 
254 
255 void
257  const int getCommandId = s.contextDomain > 0 ? s.contextDomain : s.commandId - 0x30;
258  std::set<std::string> objIDs;
259  if (s.contextDomain > 0) {
260  if ((s.activeFilters & SUBS_FILTER_NO_RTREE) == 0) {
261  PositionVector shape;
262  findObjectShape(s.commandId, s.id, shape);
263  collectObjectIDsInRange(s.contextDomain, shape, s.range, objIDs);
264  }
265  applySubscriptionFilters(s, objIDs);
266  } else {
267  objIDs.insert(s.id);
268  }
269  if (myWrapper.empty()) {
270  myWrapper[libsumo::CMD_GET_EDGE_VARIABLE] = Edge::makeWrapper();
271  myWrapper[libsumo::CMD_GET_INDUCTIONLOOP_VARIABLE] = InductionLoop::makeWrapper();
272  myWrapper[libsumo::CMD_GET_JUNCTION_VARIABLE] = Junction::makeWrapper();
273  myWrapper[libsumo::CMD_GET_LANE_VARIABLE] = Lane::makeWrapper();
274  myWrapper[libsumo::CMD_GET_LANEAREA_VARIABLE] = LaneArea::makeWrapper();
275  myWrapper[libsumo::CMD_GET_MULTIENTRYEXIT_VARIABLE] = MultiEntryExit::makeWrapper();
276  myWrapper[libsumo::CMD_GET_PERSON_VARIABLE] = Person::makeWrapper();
279  myWrapper[libsumo::CMD_GET_ROUTE_VARIABLE] = Route::makeWrapper();
280  myWrapper[libsumo::CMD_GET_SIM_VARIABLE] = Simulation::makeWrapper();
281  myWrapper[libsumo::CMD_GET_TL_VARIABLE] = TrafficLight::makeWrapper();
282  myWrapper[libsumo::CMD_GET_VEHICLE_VARIABLE] = Vehicle::makeWrapper();
283  myWrapper[libsumo::CMD_GET_VEHICLETYPE_VARIABLE] = VehicleType::makeWrapper();
284  }
285  auto wrapper = myWrapper.find(getCommandId);
286  if (wrapper == myWrapper.end()) {
287  throw TraCIException("Unsupported command specified");
288  }
289  std::shared_ptr<VariableWrapper> handler = wrapper->second;
290  VariableWrapper* container = handler.get();
291  if (s.contextDomain > 0) {
292  auto containerWrapper = myWrapper.find(s.commandId + 0x20);
293  if (containerWrapper == myWrapper.end()) {
294  throw TraCIException("Unsupported domain specified");
295  }
296  container = containerWrapper->second.get();
297  container->setContext(s.id);
298  } else {
299  container->setContext("");
300  }
301  for (const std::string& objID : objIDs) {
302  if (!s.variables.empty()) {
303  std::vector<std::vector<unsigned char> >::const_iterator k = s.parameters.begin();
304  for (const int variable : s.variables) {
305  if (!k->empty()) {
306  container->setParams(&*k);
307  }
308  handler->handle(objID, variable, container);
309  if (!k->empty()) {
310  container->setParams(nullptr);
311  }
312  ++k;
313  }
314  } else {
315  if (s.contextDomain == 0 && getCommandId == libsumo::CMD_GET_VEHICLE_VARIABLE) {
316  // default for vehicles is edge id and lane position
317  handler->handle(objID, VAR_ROAD_ID, container);
318  handler->handle(objID, VAR_LANEPOSITION, container);
319  } else if (s.contextDomain > 0 || !handler->handle(objID, libsumo::LAST_STEP_VEHICLE_NUMBER, container)) {
320  // default for detectors is vehicle number, for all others (and contexts) id list
321  handler->handle(objID, libsumo::TRACI_ID_LIST, container);
322  }
323  }
324  }
325 }
326 
327 
328 
329 void
330 Helper::fuseLaneCoverage(std::shared_ptr<LaneCoverageInfo> aggregatedLaneCoverage, const std::shared_ptr<LaneCoverageInfo> newLaneCoverage) {
331  for (auto& p : *newLaneCoverage) {
332  const MSLane* lane = p.first;
333  if (aggregatedLaneCoverage->find(lane) == aggregatedLaneCoverage->end()) {
334  // Lane has no coverage in aggregatedLaneCoverage, yet
335  (*aggregatedLaneCoverage)[lane] = (*newLaneCoverage)[lane];
336  } else {
337  // Lane is covered in aggregatedLaneCoverage as well
338  std::pair<double, double>& range1 = (*aggregatedLaneCoverage)[lane];
339  std::pair<double, double>& range2 = (*newLaneCoverage)[lane];
340  std::pair<double, double> hull = std::make_pair(MIN2(range1.first, range2.first), MAX2(range1.second, range2.second));
341  (*aggregatedLaneCoverage)[lane] = hull;
342  }
343  }
344 }
345 
346 
350  for (int i = 0; i < (int)positionVector.size(); ++i) {
351  tp.push_back(makeTraCIPosition(positionVector[i]));
352  }
353  return tp;
354 }
355 
356 
359  PositionVector pv;
360  for (const TraCIPosition& pos : vector) {
361  if (std::isnan(pos.x) || std::isnan(pos.y)) {
362  throw libsumo::TraCIException("NaN-Value in shape.");
363  }
364  pv.push_back(Position(pos.x, pos.y));
365  }
366  return pv;
367 }
368 
369 
372  TraCIColor tc;
373  tc.a = color.alpha();
374  tc.b = color.blue();
375  tc.g = color.green();
376  tc.r = color.red();
377  return tc;
378 }
379 
380 
381 RGBColor
383  return RGBColor((unsigned char)c.r, (unsigned char)c.g, (unsigned char)c.b, (unsigned char)c.a);
384 }
385 
386 
388 Helper::makeTraCIPosition(const Position& position, const bool includeZ) {
389  TraCIPosition p;
390  p.x = position.x();
391  p.y = position.y();
392  p.z = includeZ ? position.z() : INVALID_DOUBLE_VALUE;
393  return p;
394 }
395 
396 
397 Position
399  return Position(tpos.x, tpos.y, tpos.z);
400 }
401 
402 
403 MSEdge*
404 Helper::getEdge(const std::string& edgeID) {
405  MSEdge* edge = MSEdge::dictionary(edgeID);
406  if (edge == nullptr) {
407  throw TraCIException("Referenced edge '" + edgeID + "' is not known.");
408  }
409  return edge;
410 }
411 
412 
413 const MSLane*
414 Helper::getLaneChecking(const std::string& edgeID, int laneIndex, double pos) {
415  const MSEdge* edge = MSEdge::dictionary(edgeID);
416  if (edge == nullptr) {
417  throw TraCIException("Unknown edge " + edgeID);
418  }
419  if (laneIndex < 0 || laneIndex >= (int)edge->getLanes().size()) {
420  throw TraCIException("Invalid lane index for " + edgeID);
421  }
422  const MSLane* lane = edge->getLanes()[laneIndex];
423  if (pos < 0 || pos > lane->getLength()) {
424  throw TraCIException("Position on lane invalid");
425  }
426  return lane;
427 }
428 
429 
430 std::pair<MSLane*, double>
432  const PositionVector shape({ pos });
433  std::pair<MSLane*, double> result;
434  double range = 1000.;
435  const Boundary& netBounds = GeoConvHelper::getFinal().getConvBoundary();
436  const double maxRange = MAX2(1001., netBounds.getWidth() + netBounds.getHeight() + netBounds.distanceTo2D(pos));
437  while (range < maxRange) {
438  std::set<const Named*> lanes;
440  double minDistance = std::numeric_limits<double>::max();
441  for (const Named* named : lanes) {
442  MSLane* lane = const_cast<MSLane*>(dynamic_cast<const MSLane*>(named));
443  if (lane->allowsVehicleClass(vClass)) {
444  // @todo this may be a place where 3D is required but 2D is used
445  const double newDistance = lane->getShape().distance2D(pos);
446  if (newDistance < minDistance) {
447  minDistance = newDistance;
448  result.first = lane;
449  }
450  }
451  }
452  if (minDistance < std::numeric_limits<double>::max()) {
453  result.second = result.first->interpolateGeometryPosToLanePos(result.first->getShape().nearest_offset_to_point2D(pos, false));
454  break;
455  }
456  range *= 2;
457  }
458  return result;
459 }
460 
461 
462 double
463 Helper::getDrivingDistance(std::pair<const MSLane*, double>& roadPos1, std::pair<const MSLane*, double>& roadPos2) {
464  if (roadPos1.first == roadPos2.first && roadPos1.second <= roadPos2.second) {
465  // same edge
466  return roadPos2.second - roadPos1.second;
467  }
468  double distance = 0.0;
469  ConstMSEdgeVector newRoute;
470  while (roadPos2.first->isInternal() && roadPos2.first != roadPos1.first) {
471  distance += roadPos2.second;
472  roadPos2.first = roadPos2.first->getLogicalPredecessorLane();
473  roadPos2.second = roadPos2.first->getLength();
474  }
475  MSNet::getInstance()->getRouterTT(0).compute(&roadPos1.first->getEdge(), &roadPos2.first->getEdge(), nullptr, SIMSTEP, newRoute, true);
476  if (newRoute.empty()) {
478  }
479  MSRoute route("", newRoute, false, nullptr, std::vector<SUMOVehicleParameter::Stop>());
480  return distance + route.getDistanceBetween(roadPos1.second, roadPos2.second, &roadPos1.first->getEdge(), &roadPos2.first->getEdge());
481 }
482 
483 
485 Helper::getVehicle(const std::string& id) {
487  if (sumoVehicle == nullptr) {
488  throw TraCIException("Vehicle '" + id + "' is not known.");
489  }
490  MSBaseVehicle* v = dynamic_cast<MSBaseVehicle*>(sumoVehicle);
491  if (v == nullptr) {
492  throw TraCIException("Vehicle '" + id + "' is not a proper vehicle.");
493  }
494  return v;
495 }
496 
497 
498 MSPerson*
499 Helper::getPerson(const std::string& personID) {
501  MSPerson* p = dynamic_cast<MSPerson*>(c.get(personID));
502  if (p == nullptr) {
503  throw TraCIException("Person '" + personID + "' is not known");
504  }
505  return p;
506 }
507 
509 Helper::getTrafficObject(int domain, const std::string& id) {
510  if (domain == CMD_GET_VEHICLE_VARIABLE) {
511  return getVehicle(id);
512  } else if (domain == CMD_GET_PERSON_VARIABLE) {
513  return getPerson(id);
514  } else {
515  throw TraCIException("Cannot retrieve traffic object for domain " + toString(domain));
516  }
517 }
518 
519 const MSVehicleType&
520 Helper::getVehicleType(const std::string& vehicleID) {
521  return getVehicle(vehicleID)->getVehicleType();
522 }
523 
524 
525 void
527  // clean up NamedRTrees
529  POI::cleanup();
530  InductionLoop::cleanup();
531  Junction::cleanup();
532  delete myLaneTree;
533  myLaneTree = nullptr;
534 }
535 
536 
537 void
539  if (MSNet::hasInstance()) {
541  }
542 }
543 
544 
545 const std::vector<std::string>&
548 }
549 
550 
551 void
554  i.second.clear();
555  }
556 }
557 
558 
559 void
560 Helper::findObjectShape(int domain, const std::string& id, PositionVector& shape) {
561  switch (domain) {
563  InductionLoop::storeShape(id, shape);
564  break;
566  Lane::storeShape(id, shape);
567  break;
569  Vehicle::storeShape(id, shape);
570  break;
572  Person::storeShape(id, shape);
573  break;
575  POI::storeShape(id, shape);
576  break;
578  Polygon::storeShape(id, shape);
579  break;
581  Junction::storeShape(id, shape);
582  break;
584  Edge::storeShape(id, shape);
585  break;
586  default:
587  break;
588  }
589 }
590 
591 
592 void
593 Helper::collectObjectIDsInRange(int domain, const PositionVector& shape, double range, std::set<std::string>& into) {
594  std::set<const Named*> objects;
595  collectObjectsInRange(domain, shape, range, objects);
596  for (const Named* obj : objects) {
597  into.insert(obj->getID());
598  }
599 }
600 
601 
602 void
603 Helper::collectObjectsInRange(int domain, const PositionVector& shape, double range, std::set<const Named*>& into) {
604  const Boundary b = shape.getBoxBoundary().grow(range);
605  const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
606  const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
607  Named::StoringVisitor sv(into);
608  switch (domain) {
610  InductionLoop::getTree()->Search(cmin, cmax, sv);
611  break;
613  POI::getTree()->Search(cmin, cmax, sv);
614  break;
616  Polygon::getTree()->Search(cmin, cmax, sv);
617  break;
619  Junction::getTree()->Search(cmin, cmax, sv);
620  break;
625  if (myLaneTree == nullptr) {
628  }
629  LaneStoringVisitor lsv(into, shape, range, domain);
630  myLaneTree->Search(cmin, cmax, lsv);
631  }
632  break;
633  default:
634  break;
635  }
636 }
637 
638 
639 
640 void
641 Helper::applySubscriptionFilters(const Subscription& s, std::set<std::string>& objIDs) {
642 #ifdef DEBUG_SURROUNDING
643  MSBaseVehicle* _veh = getVehicle(s.id);
644  std::cout << SIMTIME << " applySubscriptionFilters for vehicle '" << _veh->getID() << "' on lane '" << _veh->getLane()->getID() << "'"
645  << "\n on edge '" << _veh->getLane()->getEdge().getID() << "' (" << toString(_veh->getLane()->getEdge().getLanes()) << ")\n"
646  << "objIDs = " << toString(objIDs) << std::endl;
647 #endif
648 
649  if (s.activeFilters == 0) {
650  // No filters set
651  return;
652  }
653 
654  // Whether vehicles on opposite lanes shall be taken into account
655  const bool disregardOppositeDirection = (s.activeFilters & SUBS_FILTER_NOOPPOSITE) != 0;
656 
657  // Check filter specification consistency
658  // TODO: Warn only once
659  if (disregardOppositeDirection && (s.activeFilters & SUBS_FILTER_NO_RTREE) == 0) {
660  WRITE_WARNING("Ignoring no-opposite subscription filter for geographic range object collection. Consider using the 'lanes' filter.")
661  }
663  WRITE_WARNING("Ignoring field of vision subscription filter due to incompatibility with other filter(s).")
664  }
665 
666  // TODO: Treat case, where ego vehicle is currently on opposite lane
667 
668  std::set<const SUMOTrafficObject*> vehs;
670  // Set defaults for upstream/downstream/lateral distances
671  double downstreamDist = s.range, upstreamDist = s.range, lateralDist = s.range;
673  // Specifies maximal downstream distance for vehicles in context subscription result
674  downstreamDist = s.filterDownstreamDist;
675  }
677  // Specifies maximal downstream distance for vehicles in context subscription result
678  upstreamDist = s.filterUpstreamDist;
679  }
681  // Specifies maximal lateral distance for vehicles in context subscription result
682  lateralDist = s.filterLateralDist;
683  }
684  MSVehicle* v = dynamic_cast<MSVehicle*>(getVehicle(s.id));
685  if (v == nullptr) {
686  throw TraCIException("Subscription filter not yet implemented for meso vehicle");
687  }
688  if (!v->isOnRoad()) {
689  return;
690  }
691  const MSLane* vehLane = v->getLane();
692  if (vehLane == nullptr) {
693  return;
694  }
695  MSEdge* vehEdge = &vehLane->getEdge();
696  std::vector<int> filterLanes;
697  if ((s.activeFilters & SUBS_FILTER_LANES) == 0) {
698  // No lane indices are specified (but downstream and/or upstream distance)
699  // -> use only vehicle's current lane as origin for the searches
700  filterLanes = {0};
701  // Lane indices must be specified when leader/follower information is requested.
702  assert((s.activeFilters & SUBS_FILTER_LEAD_FOLLOW) == 0);
703  } else {
704  filterLanes = s.filterLanes;
705  }
706 
707 #ifdef DEBUG_SURROUNDING
708  std::cout << "Filter lanes: " << toString(filterLanes) << std::endl;
709  std::cout << "Downstream distance: " << downstreamDist << std::endl;
710  std::cout << "Upstream distance: " << upstreamDist << std::endl;
711  std::cout << "Lateral distance: " << lateralDist << std::endl;
712 #endif
713 
714  if ((s.activeFilters & SUBS_FILTER_MANEUVER) != 0) {
715  // Maneuver filters disables road net search for all surrounding vehicles
716  if ((s.activeFilters & SUBS_FILTER_LEAD_FOLLOW) != 0) {
717  // Return leader and follower on the specified lanes in context subscription result.
718  for (int offset : filterLanes) {
719  MSLane* lane = v->getLane()->getParallelLane(offset, false);
720  if (lane != nullptr) {
721  // this is a non-opposite lane
722  MSVehicle* leader = lane->getLeader(v, v->getPositionOnLane(), v->getBestLanesContinuation(lane), downstreamDist).first;
723  MSVehicle* follower = lane->getFollower(v, v->getPositionOnLane(), upstreamDist, false).first;
724  vehs.insert(vehs.end(), leader);
725  vehs.insert(vehs.end(), follower);
726 
727 #ifdef DEBUG_SURROUNDING
728  std::cout << "Lane at index " << offset << ": '" << lane->getID() << std::endl;
729  std::cout << "Leader: '" << (leader != nullptr ? leader->getID() : "NULL") << "'" << std::endl;
730  std::cout << "Follower: '" << (follower != nullptr ? follower->getID() : "NULL") << "'" << std::endl;
731 #endif
732 
733  } else if (!disregardOppositeDirection && offset > 0) { // TODO: offset<0 may indicate opposite query when vehicle is on opposite itself
734  // check whether ix points to an opposite lane
735  const MSEdge* opposite = vehEdge->getOppositeEdge();
736  if (opposite == nullptr) {
737 #ifdef DEBUG_SURROUNDING
738  std::cout << "No lane at index " << offset << std::endl;
739 #endif
740  // no opposite edge
741  continue;
742  }
743  // Index of opposite lane at relative offset
744  const int ix_opposite = (int)opposite->getLanes().size() - 1 - (vehLane->getIndex() + offset - (int)vehEdge->getLanes().size());
745  if (ix_opposite < 0) {
746 #ifdef DEBUG_SURROUNDING
747  std::cout << "No lane on opposite at index " << ix_opposite << std::endl;
748 #endif
749  // no opposite edge
750  continue;
751  }
752  lane = opposite->getLanes()[ix_opposite];
753  // Search vehs along opposite lanes (swap upstream and downstream distance)
754  // XXX transformations for curved geometries
755  double posOnOpposite = MAX2(0., opposite->getLength() - v->getPositionOnLane());
756  // Get leader on opposite
757  vehs.insert(vehs.end(), lane->getFollower(v, posOnOpposite, downstreamDist, true).first);
758  // Get follower (no search on consecutive lanes
759  vehs.insert(vehs.end(), lane->getLeader(v, posOnOpposite - v->getLength(), std::vector<MSLane*>()).first);
760  }
761  }
762  }
763 
765  // Get upcoming junctions and vialanes within downstream distance, where foe links exist or at least the link direction is not straight
766  const MSLane* lane = v->getLane();
767  std::vector<const MSLink*> links = lane->getUpcomingLinks(v->getPositionOnLane(), downstreamDist, v->getBestLanesContinuation());
768 #ifdef DEBUG_SURROUNDING
769  std::cout << "Applying turn filter for vehicle '" << v->getID() << "'\n Gathering foes ..." << std::endl;
770 #endif
771  // Iterate through junctions and find approaching foes within upstreamDist.
772  for (auto& l : links) {
773 #ifdef DEBUG_SURROUNDING
774  std::cout << " On junction '" << l->getJunction()->getID() << "' (no. foe links = " << l->getFoeLinks().size() << "):" << std::endl;
775 #endif
776  for (auto& foeLane : l->getFoeLanes()) {
777  // Check vehicles approaching the entry link corresponding to this lane
778  const MSLink* foeLink = foeLane->getEntryLink();
779  for (auto& vi : foeLink->getApproaching()) {
780  if (vi.second.dist <= upstreamDist) {
781 #ifdef DEBUG_SURROUNDING
782  std::cout << " Approaching from foe-lane '" << vi.first->getID() << "'" << std::endl;
783 #endif
784  vehs.insert(vehs.end(), dynamic_cast<const MSVehicle*>(vi.first));
785  }
786  }
787  // add vehicles currently on the junction
788  for (const MSVehicle* foe : foeLane->getVehiclesSecure()) {
789  vehs.insert(vehs.end(), foe);
790  }
791  foeLane->releaseVehicles();
792  }
793  }
794  }
795 #ifdef DEBUG_SURROUNDING
796  std::cout << SIMTIME << " applySubscriptionFilters() for veh '" << v->getID() << "'. Found the following vehicles:\n";
797  for (auto veh : vehs) {
798  debugPrint(veh);
799  }
800 #endif
801  } else if (s.activeFilters & SUBS_FILTER_LATERAL_DIST) {
802  assert(vehs.size() == 0);
803  assert(objIDs.size() == 0);
804 
805  // collect all vehicles within maximum range of interest to get an upper bound
806  PositionVector vehShape;
807  findObjectShape(s.commandId, s.id, vehShape);
808  double range = MAX3(downstreamDist, upstreamDist, lateralDist);
809  collectObjectIDsInRange(s.contextDomain, vehShape, range, objIDs);
810 
811 #ifdef DEBUG_SURROUNDING
812  std::cout << "FILTER_LATERAL_DIST: collected object IDs (range " << range << "):" << std::endl;
813  for (std::string i : objIDs) {
814  std::cout << i << std::endl;
815  }
816 #endif
817 
818 #ifdef DEBUG_SURROUNDING
819  std::cout << "FILTER_LATERAL_DIST: myLane is '" << v->getLane()->getID() << "'" << std::endl;
820 #endif
821  // 1st pass: downstream (make sure that the whole length of the vehicle is included in the match)
822  const double backPosOnLane = MAX2(0.0, v->getPositionOnLane() - v->getVehicleType().getLength());
824  backPosOnLane, v->getLateralPositionOnLane(), true);
825  // 2nd pass: upstream
827  v->getPositionOnLane(), v->getLateralPositionOnLane(), false);
828 
829  objIDs.clear();
830  } else {
831  // No maneuver or lateral distance filters requested, but only lanes filter (directly, or indirectly by specifying downstream or upstream distance)
832  assert(filterLanes.size() > 0);
833  // This is to remember the lanes checked in the driving direction of the vehicle (their opposites can be added in a second pass)
834  auto checkedLanesInDrivingDir = std::make_shared<LaneCoverageInfo>();
835  for (int offset : filterLanes) {
836  MSLane* lane = vehLane->getParallelLane(offset, false);
837  if (lane != nullptr) {
838 #ifdef DEBUG_SURROUNDING
839  std::cout << "Checking for surrounding vehicles starting on lane '" << lane->getID() << "' at index " << offset << std::endl;
840 #endif
841  // Search vehs along this lane
842  // (Coverage info is collected per origin lane since lanes reached from neighboring lanes may have different distances
843  // and aborting at previously scanned when coming from a closer origin may prevent scanning of parts that should be included.)
844  std::shared_ptr<LaneCoverageInfo> checkedLanes = std::make_shared<LaneCoverageInfo>();
845  const std::set<MSVehicle*> new_vehs = lane->getSurroundingVehicles(v->getPositionOnLane(), downstreamDist, upstreamDist + v->getLength(), checkedLanes);
846  vehs.insert(new_vehs.begin(), new_vehs.end());
847  fuseLaneCoverage(checkedLanesInDrivingDir, checkedLanes);
848  } else if (!disregardOppositeDirection && offset > 0) {
849  // Check opposite edge, too
850  assert(vehLane->getIndex() + offset >= (int)vehEdge->getLanes().size()); // index points beyond this edge
851  const MSEdge* opposite = vehEdge->getOppositeEdge();
852  if (opposite == nullptr) {
853 #ifdef DEBUG_SURROUNDING
854  std::cout << "No opposite edge, thus no lane at index " << offset << std::endl;
855 #endif
856  // no opposite edge
857  continue;
858  }
859  // Index of opposite lane at relative offset
860  const int ix_opposite = (int)opposite->getLanes().size() - 1 - (vehLane->getIndex() + offset - (int)vehEdge->getLanes().size());
861  if (ix_opposite < 0) {
862 #ifdef DEBUG_SURROUNDING
863  std::cout << "No lane on opposite at index " << ix_opposite << std::endl;
864 #endif
865  // no opposite edge
866  continue;
867  }
868  lane = opposite->getLanes()[ix_opposite];
869  // Search vehs along opposite lanes (swap upstream and downstream distance)
870  const std::set<MSVehicle*> new_vehs = lane->getSurroundingVehicles(lane->getLength() - v->getPositionOnLane(), upstreamDist + v->getLength(), downstreamDist, std::make_shared<LaneCoverageInfo>());
871  vehs.insert(new_vehs.begin(), new_vehs.end());
872  }
873 #ifdef DEBUG_SURROUNDING
874  else {
875  std::cout << "No lane at index " << offset << std::endl;
876  }
877 #endif
878 
879  if (!disregardOppositeDirection) {
880  // If opposite should be checked, do this for each lane of the search tree in checkedLanesInDrivingDir
881  // (For instance, some opposite lanes of these would not be obtained if the ego lane does not have an opposite.)
882 
883  // Number of opposite lanes to be checked (assumes filterLanes.size()>0, see assertion above) determined as hypothetical offset
884  // overlap into opposing edge from the vehicle's current lane.
885  // TODO: offset<0 may indicate opposite query when vehicle is on opposite itself (-> use min_element(filterLanes...) instead, etc)
886  const int nOpp = MAX2(0, (*std::max_element(filterLanes.begin(), filterLanes.end())) - ((int)vehEdge->getLanes().size() - 1 - vehLane->getIndex()));
887  // Collect vehicles from opposite lanes
888  if (nOpp > 0) {
889  for (auto& laneCov : *checkedLanesInDrivingDir) {
890  const MSLane* const l = laneCov.first;
891  if (l == nullptr || l->getEdge().getOppositeEdge() == nullptr) {
892  continue;
893  }
894  const MSEdge* opposite = l->getEdge().getOppositeEdge();
895  const std::pair<double, double>& range = laneCov.second;
896  auto leftMostOppositeLaneIt = opposite->getLanes().rbegin();
897  for (auto oppositeLaneIt = leftMostOppositeLaneIt;
898  oppositeLaneIt != opposite->getLanes().rend(); ++oppositeLaneIt) {
899  if ((int)(oppositeLaneIt - leftMostOppositeLaneIt) == nOpp) {
900  break;
901  }
902  // Add vehicles from corresponding range on opposite direction
903  const MSLane* oppositeLane = *oppositeLaneIt;
904  auto new_vehs = oppositeLane->getVehiclesInRange(l->getLength() - range.second, l->getLength() - range.first);
905  vehs.insert(new_vehs.begin(), new_vehs.end());
906  }
907  }
908  }
909  }
910 #ifdef DEBUG_SURROUNDING
911  std::cout << SIMTIME << " applySubscriptionFilters() for veh '" << v->getID() << "'. Found the following vehicles:\n";
912  for (auto veh : vehs) {
913  debugPrint(veh);
914  }
915 #endif
916  }
917 
918  // filter vehicles in vehs by class and/or type if requested
920  // Only return vehicles of the given vClass in context subscription result
921  auto i = vehs.begin();
922  while (i != vehs.end()) {
923  if (((*i)->getVehicleType().getVehicleClass() & s.filterVClasses) == 0) {
924  i = vehs.erase(i);
925  } else {
926  ++i;
927  }
928  }
929  }
931  // Only return vehicles of the given vType in context subscription result
932  auto i = vehs.begin();
933  while (i != vehs.end()) {
934  if (s.filterVTypes.find((*i)->getVehicleType().getID()) == s.filterVTypes.end()) {
935  i = vehs.erase(i);
936  } else {
937  ++i;
938  }
939  }
940  }
941  }
942  // Write vehs IDs in objIDs
943  for (const SUMOTrafficObject* veh : vehs) {
944  if (veh != nullptr) {
945  objIDs.insert(objIDs.end(), veh->getID());
946  }
947  }
948  } else { // apply rTree-based filters
950  // Only return vehicles of the given vClass in context subscription result
951  auto i = objIDs.begin();
952  while (i != objIDs.end()) {
953  MSBaseVehicle* veh = getVehicle(*i);
954  if ((veh->getVehicleType().getVehicleClass() & s.filterVClasses) == 0) {
955  i = objIDs.erase(i);
956  } else {
957  ++i;
958  }
959  }
960  }
962  // Only return vehicles of the given vType in context subscription result
963  auto i = objIDs.begin();
964  while (i != objIDs.end()) {
965  MSBaseVehicle* veh = getVehicle(*i);
966  if (s.filterVTypes.find(veh->getVehicleType().getID()) == s.filterVTypes.end()) {
967  i = objIDs.erase(i);
968  } else {
969  ++i;
970  }
971  }
972  }
974  // Only return vehicles within field of vision in context subscription result
976  }
977  }
978 }
979 
980 void
981 Helper::applySubscriptionFilterFieldOfVision(const Subscription& s, std::set<std::string>& objIDs) {
983  WRITE_WARNINGF("Field of vision opening angle ('%') should be within interval (0, 360), ignoring filter...", s.filterFieldOfVisionOpeningAngle);
984  return;
985  }
986 
987  MSBaseVehicle* egoVehicle = getVehicle(s.id);
988  Position egoPosition = egoVehicle->getPosition();
989  double openingAngle = DEG2RAD(s.filterFieldOfVisionOpeningAngle);
990 
991 #ifdef DEBUG_SURROUNDING
992  std::cout << "FOVFILTER: ego direction = " << toString(RAD2DEG(egoVehicle->getAngle())) << " (deg)" << std::endl;
993 #endif
994 
995  auto i = objIDs.begin();
996  while (i != objIDs.end()) {
997  if (s.id.compare(*i) == 0) { // skip if this is the ego vehicle
998  ++i;
999  continue;
1000  }
1002  double angleEgoToVeh = egoPosition.angleTo2D(obj->getPosition());
1003  double alpha = GeomHelper::angleDiff(egoVehicle->getAngle(), angleEgoToVeh);
1004 
1005 #ifdef DEBUG_SURROUNDING
1006  const std::string objType = s.isVehicleToPersonContextSubscription() ? "person" : "veh";
1007  std::cout << "FOVFILTER: " << objType << " '" << *i << "' dist = " << toString(egoPosition.distanceTo2D(obj->getPosition())) << std::endl;
1008  std::cout << "FOVFILTER: " << objType << " '" << *i << "' alpha = " << toString(RAD2DEG(alpha)) << " (deg)" << std::endl;
1009 #endif
1010 
1011  if (abs(alpha) > openingAngle * 0.5) {
1012  i = objIDs.erase(i);
1013  } else {
1014  ++i;
1015  }
1016  }
1017 }
1018 
1019 void
1021  std::set<const SUMOTrafficObject*>& vehs,
1022  const std::vector<const MSLane*>& lanes, double posOnLane, double posLat, bool isDownstream) {
1023  const double streamDist = isDownstream ? s.filterDownstreamDist : s.filterUpstreamDist;
1024  double distRemaining = streamDist;
1025  bool isFirstLane = true;
1026  PositionVector combinedShape;
1027  for (const MSLane* lane : lanes) {
1028 #ifdef DEBUG_SURROUNDING
1029  std::cout << "FILTER_LATERAL_DIST: current lane " << (isDownstream ? "down" : "up") << " is '" << lane->getID() << "', length " << lane->getLength()
1030  << ", pos " << posOnLane << ", distRemaining " << distRemaining << std::endl;
1031 #endif
1032  PositionVector laneShape = lane->getShape();
1033  if (isFirstLane) {
1034  isFirstLane = false;
1035  if (posOnLane == 0) {
1036  if (!isDownstream) {
1037  continue;
1038  }
1039  } else {
1040  double geometryPos = lane->interpolateLanePosToGeometryPos(posOnLane);
1041  if (geometryPos >= laneShape.length()) {
1042  laneShape = isDownstream ? PositionVector() : laneShape;
1043  } else {
1044  auto pair = laneShape.splitAt(geometryPos, false);
1045  laneShape = isDownstream ? pair.second : pair.first;
1046  }
1047  }
1048  }
1049  double laneLength = lane->interpolateGeometryPosToLanePos(laneShape.length());
1050  if (distRemaining - laneLength < 0.) {
1051  double geometryPos = lane->interpolateLanePosToGeometryPos(isDownstream ? distRemaining : laneLength - distRemaining);
1052  auto pair = laneShape.splitAt(geometryPos, false);
1053  laneShape = isDownstream ? pair.first : pair.second;
1054  }
1055  distRemaining -= laneLength;
1056  try {
1057  laneShape.move2side(-posLat);
1058  } catch (ProcessError&) {
1059  WRITE_WARNING("addSubscriptionFilterLateralDistance could not determine shape of lane '" + lane->getID() + "' with lateral shift of " + toString(posLat));
1060  }
1061 #ifdef DEBUG_SURROUNDING
1062  std::cout << " posLat=" << posLat << " laneShape=" << laneShape << "\n";
1063 #endif
1064  combinedShape.append(laneShape);
1065  if (distRemaining <= 0) {
1066  break;
1067  }
1068  }
1069  // check remaining objects' distances to the combined shape
1070  auto i = objIDs.begin();
1071  while (i != objIDs.end()) {
1073  double minPerpendicularDist = combinedShape.distance2D(obj->getPosition(), true);
1074 #ifdef DEBUG_SURROUNDING
1075  std::cout << " obj " << obj->getID() << " dist=" << minPerpendicularDist << " filterDist=" << s.filterLateralDist << "\n";
1076 #endif
1077  if ((minPerpendicularDist != GeomHelper::INVALID_OFFSET) && (minPerpendicularDist <= s.filterLateralDist)) {
1078  vehs.insert(obj);
1079  i = objIDs.erase(i);
1080  } else {
1081  ++i;
1082  }
1083  }
1084 }
1085 
1086 void
1087 Helper::setRemoteControlled(MSVehicle* v, Position xyPos, MSLane* l, double pos, double posLat, double angle,
1088  int edgeOffset, ConstMSEdgeVector route, SUMOTime t) {
1090  v->getInfluencer().setRemoteControlled(xyPos, l, pos, posLat, angle, edgeOffset, route, t);
1091 }
1092 
1093 void
1094 Helper::setRemoteControlled(MSPerson* p, Position xyPos, MSLane* l, double pos, double posLat, double angle,
1095  int edgeOffset, ConstMSEdgeVector route, SUMOTime t) {
1096  myRemoteControlledPersons[p->getID()] = p;
1097  p->getInfluencer().setRemoteControlled(xyPos, l, pos, posLat, angle, edgeOffset, route, t);
1098 }
1099 
1100 
1101 void
1103  for (auto& controlled : myRemoteControlledVehicles) {
1104  if (MSNet::getInstance()->getVehicleControl().getVehicle(controlled.first) != nullptr) {
1105  controlled.second->getInfluencer().postProcessRemoteControl(controlled.second);
1106  } else {
1107  WRITE_WARNING("Vehicle '" + controlled.first + "' was removed though being controlled by TraCI");
1108  }
1109  }
1111  for (auto& controlled : myRemoteControlledPersons) {
1112  if (MSNet::getInstance()->getPersonControl().get(controlled.first) != nullptr) {
1113  controlled.second->getInfluencer().postProcessRemoteControl(controlled.second);
1114  } else {
1115  WRITE_WARNING("Person '" + controlled.first + "' was removed though being controlled by TraCI");
1116  }
1117  }
1118  myRemoteControlledPersons.clear();
1119 }
1120 
1121 
1122 bool
1123 Helper::moveToXYMap(const Position& pos, double maxRouteDistance, bool mayLeaveNetwork, const std::string& origID, const double angle,
1124  double speed, const ConstMSEdgeVector& currentRoute, const int routePosition, const MSLane* currentLane, double currentLanePos, bool onRoad,
1125  SUMOVehicleClass vClass, bool setLateralPos,
1126  double& bestDistance, MSLane** lane, double& lanePos, int& routeOffset, ConstMSEdgeVector& edges) {
1127  // collect edges around the vehicle/person
1128 #ifdef DEBUG_MOVEXY
1129  std::cout << SIMTIME << " moveToXYMap pos=" << pos << " angle=" << angle << " vClass=" << toString(vClass) << "\n";
1130 #endif
1131  const MSEdge* const currentRouteEdge = currentRoute[routePosition];
1132  std::set<const Named*> into;
1133  PositionVector shape;
1134  shape.push_back(pos);
1135  collectObjectsInRange(libsumo::CMD_GET_EDGE_VARIABLE, shape, maxRouteDistance, into);
1136  double maxDist = 0;
1137  std::map<MSLane*, LaneUtility> lane2utility;
1138  // compute utility for all candidate edges
1139  for (const Named* namedEdge : into) {
1140  const MSEdge* e = dynamic_cast<const MSEdge*>(namedEdge);
1141  if ((e->getPermissions() & vClass) != vClass) {
1142  continue;
1143  }
1144  const MSEdge* prevEdge = nullptr;
1145  const MSEdge* nextEdge = nullptr;
1146  bool onRoute = false;
1147  // the next if/the clause sets "onRoute", "prevEdge", and "nextEdge", depending on
1148  // whether the currently seen edge is an internal one or a normal one
1149  if (e->isWalkingArea() || e->isCrossing()) {
1150  // find current intersection along the route
1151  const MSJunction* junction = e->getFromJunction();
1152  for (int i = routePosition; i < (int)currentRoute.size(); i++) {
1153  const MSEdge* cand = currentRoute[i];
1154  if (cand->getToJunction() == junction) {
1155  prevEdge = cand;
1156  if (i + 1 < (int)currentRoute.size()) {
1157  onRoute = true;
1158  nextEdge = currentRoute[i + 1];
1159  }
1160  break;
1161  }
1162  }
1163  if (onRoute == false) {
1164  // search backward
1165  for (int i = routePosition - 1; i >= 0; i--) {
1166  const MSEdge* cand = currentRoute[i];
1167  if (cand->getToJunction() == junction) {
1168  onRoute = true;
1169  prevEdge = cand;
1170  nextEdge = currentRoute[i + 1];
1171  break;
1172  }
1173  }
1174  }
1175  if (prevEdge == nullptr) {
1176  // use arbitrary predecessor
1177  if (e->getPredecessors().size() > 0) {
1178  prevEdge = e->getPredecessors().front();
1179  } else if (e->getSuccessors().size() > 1) {
1180  for (MSEdge* e2 : e->getSuccessors()) {
1181  if (e2 != nextEdge) {
1182  prevEdge = e2;
1183  break;
1184  }
1185  }
1186  }
1187  }
1188  if (nextEdge == nullptr) {
1189  if (e->getSuccessors().size() > 0) {
1190  nextEdge = e->getSuccessors().front();
1191  } else if (e->getPredecessors().size() > 1) {
1192  for (MSEdge* e2 : e->getPredecessors()) {
1193  if (e2 != prevEdge) {
1194  nextEdge = e2;
1195  break;
1196  }
1197  }
1198  }
1199  }
1200 #ifdef DEBUG_MOVEXY_ANGLE
1201  std::cout << "walkingarea/crossing:" << e->getID() << " prev:" << Named::getIDSecure(prevEdge) << " next:" << Named::getIDSecure(nextEdge)
1202  << " pred=" << toString(e->getPredecessors()) << " succ=" << toString(e->getSuccessors())
1203  << "\n";
1204 #endif
1205  } else if (e->isNormal()) {
1206  // a normal edge
1207  //
1208  // check whether the currently seen edge is in the vehicle's route
1209  // - either the one it's on or one of the next edges
1210  ConstMSEdgeVector::const_iterator searchStart = currentRoute.begin() + routePosition;
1211  if (onRoad && currentLane->getEdge().isInternal()) {
1212  ++searchStart;
1213  }
1214  ConstMSEdgeVector::const_iterator edgePos = std::find(searchStart, currentRoute.end(), e);
1215  onRoute = edgePos != currentRoute.end(); // no? -> onRoute is false
1216  if (edgePos == currentRoute.end() - 1 && currentRouteEdge == e) {
1217  // onRoute is false as well if the vehicle is beyond the edge
1218  onRoute &= currentRouteEdge->getLength() > currentLanePos + SPEED2DIST(speed);
1219  }
1220  // save prior and next edges
1221  prevEdge = e;
1222  nextEdge = !onRoute || edgePos == currentRoute.end() - 1 ? nullptr : *(edgePos + 1);
1223 #ifdef DEBUG_MOVEXY_ANGLE
1224  std::cout << "normal:" << e->getID() << " prev:" << Named::getIDSecure(prevEdge) << " next:" << Named::getIDSecure(nextEdge) << "\n";
1225 #endif
1226  } else if (e->isInternal()) {
1227  // an internal edge
1228  // get the previous edge
1229  prevEdge = e;
1230  while (prevEdge != nullptr && prevEdge->isInternal()) {
1231  MSLane* l = prevEdge->getLanes()[0];
1232  l = l->getLogicalPredecessorLane();
1233  prevEdge = l == nullptr ? nullptr : &l->getEdge();
1234  }
1235  // check whether the previous edge is on the route (was on the route)
1236  ConstMSEdgeVector::const_iterator prevEdgePos = std::find(currentRoute.begin() + routePosition, currentRoute.end(), prevEdge);
1237  nextEdge = e;
1238  while (nextEdge != nullptr && nextEdge->isInternal()) {
1239  nextEdge = nextEdge->getSuccessors()[0]; // should be only one for an internal edge
1240  }
1241  if (prevEdgePos != currentRoute.end() && (prevEdgePos + 1) != currentRoute.end()) {
1242  onRoute = *(prevEdgePos + 1) == nextEdge;
1243  }
1244 #ifdef DEBUG_MOVEXY_ANGLE
1245  std::cout << "internal:" << e->getID() << " prev:" << Named::getIDSecure(prevEdge) << " next:" << Named::getIDSecure(nextEdge) << "\n";
1246 #endif
1247  }
1248 
1249 
1250  // weight the lanes...
1251  const bool perpendicular = false;
1252  for (MSLane* const l : e->getLanes()) {
1253  if (!l->allowsVehicleClass(vClass)) {
1254  continue;
1255  }
1256  if (l->getShape().length() == 0) {
1257  // mapping to shapeless lanes is a bad idea
1258  continue;
1259  }
1260  double langle = 180.;
1261  double dist = FAR_AWAY;
1262  double perpendicularDist = FAR_AWAY;
1263  // add some slack to avoid issues from tiny gaps between consecutive lanes
1264  const double slack = POSITION_EPS;
1265  PositionVector laneShape = l->getShape();
1266  laneShape.extrapolate2D(slack);
1267  double off = laneShape.nearest_offset_to_point2D(pos, true);
1268  if (off != GeomHelper::INVALID_OFFSET) {
1269  perpendicularDist = laneShape.distance2D(pos, true);
1270  }
1271  off = l->getShape().nearest_offset_to_point2D(pos, perpendicular);
1272  if (off != GeomHelper::INVALID_OFFSET) {
1273  dist = l->getShape().distance2D(pos, perpendicular);
1274  langle = GeomHelper::naviDegree(l->getShape().rotationAtOffset(off));
1275  }
1276  // cannot trust lanePos on walkingArea
1277  bool sameEdge = onRoad && e == &currentLane->getEdge() && currentRouteEdge->getLength() > currentLanePos + SPEED2DIST(speed) && !e->isWalkingArea();
1278  /*
1279  const MSEdge* rNextEdge = nextEdge;
1280  while(rNextEdge==0&&lane->getEdge().getPurpose()==MSEdge::EDGEFUNCTION_INTERNAL) {
1281  MSLane* next = lane->getLinkCont()[0]->getLane();
1282  rNextEdge = next == 0 ? 0 : &next->getEdge();
1283  }
1284  */
1285  double dist2 = dist;
1286  if (mayLeaveNetwork && fabs(dist - perpendicularDist) > slack) {
1287  // ambiguous mapping. Don't trust this
1288  dist2 = FAR_AWAY;
1289  }
1290  const double angleDiff = (angle == INVALID_DOUBLE_VALUE ? 0 : GeomHelper::getMinAngleDiff(angle, langle));
1291 #ifdef DEBUG_MOVEXY_ANGLE
1292  std::cout << std::setprecision(gPrecision)
1293  << " candLane=" << l->getID() << " lAngle=" << langle << " lLength=" << l->getLength()
1294  << " angleDiff=" << angleDiff
1295  << " off=" << off
1296  << " pDist=" << perpendicularDist
1297  << " dist=" << dist
1298  << " dist2=" << dist2
1299  << "\n";
1300  std::cout << l->getID() << " param=" << l->getParameter(SUMO_PARAM_ORIGID, "") << " origID='" << origID << "\n";
1301 #endif
1302 
1303  bool origIDMatch = l->getParameter(SUMO_PARAM_ORIGID, l->getID()) == origID;
1304  if (origIDMatch && setLateralPos
1305  && perpendicularDist > l->getWidth() / 2) {
1306  origIDMatch = false;
1307  }
1308  lane2utility.emplace(l, LaneUtility(
1309  dist2, perpendicularDist, off, angleDiff,
1310  origIDMatch,
1311  onRoute, sameEdge, prevEdge, nextEdge));
1312  // update scaling value
1313  maxDist = MAX2(maxDist, MIN2(dist, SUMO_const_laneWidth));
1314 
1315  }
1316  }
1317 
1318  // get the best lane given the previously computed values
1319  double bestValue = 0;
1320  MSLane* bestLane = nullptr;
1321  for (const auto& it : lane2utility) {
1322  MSLane* const l = it.first;
1323  const LaneUtility& u = it.second;
1324  double distN = u.dist > 999 ? -10 : 1. - (u.dist / maxDist);
1325  double angleDiffN = 1. - (u.angleDiff / 180.);
1326  double idN = u.ID ? 1 : 0;
1327  double onRouteN = u.onRoute ? 1 : 0;
1328  double sameEdgeN = u.sameEdge ? MIN2(currentRouteEdge->getLength() / speed, (double)1.) : 0;
1329  double value = (distN * .5 // distance is more important than angle because the vehicle might be driving in the opposite direction
1330  + angleDiffN * 0.35 /*.5 */
1331  + idN * 1
1332  + onRouteN * 0.1
1333  + sameEdgeN * 0.1);
1334 #ifdef DEBUG_MOVEXY
1335  std::cout << " x; l:" << l->getID() << " d:" << u.dist << " dN:" << distN << " aD:" << angleDiffN <<
1336  " ID:" << idN << " oRN:" << onRouteN << " sEN:" << sameEdgeN << " value:" << value << std::endl;
1337 #endif
1338  if (value > bestValue || bestLane == nullptr) {
1339  bestValue = value;
1340  if (u.dist == FAR_AWAY) {
1341  bestLane = nullptr;
1342  } else {
1343  bestLane = l;
1344  }
1345  }
1346  }
1347  // no best lane found, return
1348  if (bestLane == nullptr) {
1349  return false;
1350  }
1351  const LaneUtility& u = lane2utility.find(bestLane)->second;
1352  bestDistance = u.dist;
1353  *lane = bestLane;
1354  lanePos = MAX2(0., MIN2(double((*lane)->getLength() - POSITION_EPS),
1356  bestLane->getShape().nearest_offset_to_point25D(pos, false))));
1357  const MSEdge* prevEdge = u.prevEdge;
1358  if (u.onRoute) {
1359  ConstMSEdgeVector::const_iterator prevEdgePos = std::find(currentRoute.begin(), currentRoute.end(), prevEdge);
1360  routeOffset = (int)std::distance(currentRoute.begin(), prevEdgePos);
1361  //std::cout << SIMTIME << "moveToXYMap vehicle=" << veh.getID() << " currLane=" << veh.getLane()->getID() << " routeOffset=" << routeOffset << " edges=" << toString(ev) << " bestLane=" << bestLane->getID() << " prevEdge=" << prevEdge->getID() << "\n";
1362  } else {
1363  edges.push_back(u.prevEdge);
1364  /*
1365  if(bestLane->getEdge().getPurpose()!=MSEdge::EDGEFUNCTION_INTERNAL) {
1366  edges.push_back(&bestLane->getEdge());
1367  }
1368  */
1369  if (u.nextEdge != nullptr) {
1370  edges.push_back(u.nextEdge);
1371  }
1372  routeOffset = 0;
1373 #ifdef DEBUG_MOVEXY_ANGLE
1374  std::cout << SIMTIME << " internal2: lane=" << bestLane->getID() << " prev=" << Named::getIDSecure(u.prevEdge) << " next=" << Named::getIDSecure(u.nextEdge) << "\n";
1375 #endif
1376  }
1377  return true;
1378 }
1379 
1380 
1381 bool
1382 Helper::findCloserLane(const MSEdge* edge, const Position& pos, SUMOVehicleClass vClass, double& bestDistance, MSLane** lane) {
1383  if (edge == nullptr) {
1384  return false;
1385  }
1386  const std::vector<MSLane*>& lanes = edge->getLanes();
1387  bool newBest = false;
1388  for (std::vector<MSLane*>::const_iterator k = lanes.begin(); k != lanes.end() && bestDistance > POSITION_EPS; ++k) {
1389  MSLane* candidateLane = *k;
1390  if (!candidateLane->allowsVehicleClass(vClass)) {
1391  continue;
1392  }
1393  if (candidateLane->getShape().length() == 0) {
1394  // mapping to shapeless lanes is a bad idea
1395  continue;
1396  }
1397  const double dist = candidateLane->getShape().distance2D(pos); // get distance
1398 #ifdef DEBUG_MOVEXY
1399  std::cout << " b at lane " << candidateLane->getID() << " dist:" << dist << " best:" << bestDistance << std::endl;
1400 #endif
1401  if (dist < bestDistance) {
1402  // is the new distance the best one? keep then...
1403  bestDistance = dist;
1404  *lane = candidateLane;
1405  newBest = true;
1406  }
1407  }
1408  return newBest;
1409 }
1410 
1411 
1412 bool
1413 Helper::moveToXYMap_matchingRoutePosition(const Position& pos, const std::string& origID,
1414  const ConstMSEdgeVector& currentRoute, int routeIndex,
1415  SUMOVehicleClass vClass, bool setLateralPos,
1416  double& bestDistance, MSLane** lane, double& lanePos, int& routeOffset) {
1417 #ifdef DEBUG_MOVEXY
1418  std::cout << SIMTIME << " moveToXYMap_matchingRoutePosition pos=" << pos << " vClass=" << toString(vClass) << "\n";
1419 #endif
1420  //std::cout << "moveToXYMap_matchingRoutePosition pos=" << pos << "\n";
1421  routeOffset = 0;
1422  // routes may be looped which makes routeOffset ambiguous. We first try to
1423  // find the closest upcoming edge on the route and then look for closer passed edges
1424 
1425  // look forward along the route
1426  const MSEdge* prev = nullptr;
1427  for (int i = routeIndex; i < (int)currentRoute.size(); ++i) {
1428  const MSEdge* cand = currentRoute[i];
1429  while (prev != nullptr) {
1430  // check internal edge(s)
1431  const MSEdge* internalCand = prev->getInternalFollowingEdge(cand);
1432  findCloserLane(internalCand, pos, vClass, bestDistance, lane);
1433  prev = internalCand;
1434  }
1435  if (findCloserLane(cand, pos, vClass, bestDistance, lane)) {
1436  routeOffset = i;
1437  }
1438  prev = cand;
1439  }
1440  // look backward along the route
1441  const MSEdge* next = currentRoute[routeIndex];
1442  for (int i = routeIndex; i >= 0; --i) {
1443  const MSEdge* cand = currentRoute[i];
1444  //std::cout << " next=" << next->getID() << " cand=" << cand->getID() << " i=" << i << " routeIndex=" << routeIndex << "\n";
1445  prev = cand;
1446  while (prev != nullptr) {
1447  // check internal edge(s)
1448  const MSEdge* internalCand = prev->getInternalFollowingEdge(next);
1449  if (findCloserLane(internalCand, pos, vClass, bestDistance, lane)) {
1450  routeOffset = i;
1451  }
1452  prev = internalCand;
1453  }
1454  if (findCloserLane(cand, pos, vClass, bestDistance, lane)) {
1455  routeOffset = i;
1456  }
1457  next = cand;
1458  }
1459  if (vClass == SVC_PEDESTRIAN) {
1460  // consider all crossings and walkingareas along the route
1461  std::map<const MSJunction*, int> routeJunctions;
1462  for (int i = 0; i < (int)currentRoute.size() - 1; ++i) {
1463  routeJunctions[currentRoute[i]->getToJunction()] = i;
1464  }
1465  std::set<const Named*> into;
1466  PositionVector shape;
1467  shape.push_back(pos);
1469  for (const Named* named : into) {
1470  const MSLane* cand = dynamic_cast<const MSLane*>(named);
1471  if ((cand->getEdge().isWalkingArea() || cand->getEdge().isCrossing())
1472  && routeJunctions.count(cand->getEdge().getToJunction()) != 0) {
1473  if (findCloserLane(&cand->getEdge(), pos, vClass, bestDistance, lane)) {
1474  routeOffset = routeJunctions[cand->getEdge().getToJunction()];
1475  }
1476  }
1477  }
1478  }
1479 
1480  assert(lane != 0);
1481  // quit if no solution was found, reporting a failure
1482  if (lane == nullptr) {
1483 #ifdef DEBUG_MOVEXY
1484  std::cout << " b failed - no best route lane" << std::endl;
1485 #endif
1486  return false;
1487  }
1488 
1489 
1490  // position may be inaccurate; let's checkt the given index, too
1491  // @note: this is enabled for non-internal lanes only, as otherwise the position information may ambiguous
1492  if (!(*lane)->getEdge().isInternal()) {
1493  const std::vector<MSLane*>& lanes = (*lane)->getEdge().getLanes();
1494  for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
1495  if ((*i)->getParameter(SUMO_PARAM_ORIGID, (*i)->getID()) == origID) {
1496  if (setLateralPos) {
1497  // vehicle might end up on top of another lane with a big
1498  // lateral offset to the lane with origID.
1499  const double dist = (*i)->getShape().distance2D(pos); // get distance
1500  if (dist < (*i)->getWidth() / 2) {
1501  *lane = *i;
1502  break;
1503  }
1504  } else {
1505  *lane = *i;
1506  break;
1507  }
1508  }
1509  }
1510  }
1511  // check position, stuff, we should have the best lane along the route
1512  lanePos = MAX2(0., MIN2(double((*lane)->getLength() - POSITION_EPS),
1513  (*lane)->interpolateGeometryPosToLanePos(
1514  (*lane)->getShape().nearest_offset_to_point25D(pos, false))));
1515  //std::cout << SIMTIME << " moveToXYMap_matchingRoutePosition vehicle=" << veh.getID() << " currLane=" << veh.getLane()->getID() << " routeOffset=" << routeOffset << " edges=" << toString(edges) << " lane=" << (*lane)->getID() << "\n";
1516 #ifdef DEBUG_MOVEXY
1517  std::cout << " b ok lane " << (*lane)->getID() << " lanePos:" << lanePos << std::endl;
1518 #endif
1519  return true;
1520 }
1521 
1522 
1524  : VariableWrapper(handler), myResults(into), myContextResults(context), myActiveResults(&into) {
1525 
1526 }
1527 
1528 
1529 void
1530 Helper::SubscriptionWrapper::setContext(const std::string& refID) {
1531  myActiveResults = refID == "" ? &myResults : &myContextResults[refID];
1532 }
1533 
1534 
1535 void
1536 Helper::SubscriptionWrapper::setParams(const std::vector<unsigned char>* params) {
1537  myParams = params;
1538 }
1539 
1540 
1541 void
1543  myActiveResults = &myResults;
1544  myResults.clear();
1545  myContextResults.clear();
1546 }
1547 
1548 
1549 bool
1550 Helper::SubscriptionWrapper::wrapDouble(const std::string& objID, const int variable, const double value) {
1551  (*myActiveResults)[objID][variable] = std::make_shared<TraCIDouble>(value);
1552  return true;
1553 }
1554 
1555 
1556 bool
1557 Helper::SubscriptionWrapper::wrapInt(const std::string& objID, const int variable, const int value) {
1558  (*myActiveResults)[objID][variable] = std::make_shared<TraCIInt>(value);
1559  return true;
1560 }
1561 
1562 
1563 bool
1564 Helper::SubscriptionWrapper::wrapString(const std::string& objID, const int variable, const std::string& value) {
1565  (*myActiveResults)[objID][variable] = std::make_shared<TraCIString>(value);
1566  return true;
1567 }
1568 
1569 
1570 bool
1571 Helper::SubscriptionWrapper::wrapStringList(const std::string& objID, const int variable, const std::vector<std::string>& value) {
1572  auto sl = std::make_shared<TraCIStringList>();
1573  sl->value = value;
1574  (*myActiveResults)[objID][variable] = sl;
1575  return true;
1576 }
1577 
1578 
1579 bool
1580 Helper::SubscriptionWrapper::wrapPosition(const std::string& objID, const int variable, const TraCIPosition& value) {
1581  (*myActiveResults)[objID][variable] = std::make_shared<TraCIPosition>(value);
1582  return true;
1583 }
1584 
1585 
1586 bool
1587 Helper::SubscriptionWrapper::wrapColor(const std::string& objID, const int variable, const TraCIColor& value) {
1588  (*myActiveResults)[objID][variable] = std::make_shared<TraCIColor>(value);
1589  return true;
1590 }
1591 
1592 
1593 bool
1594 Helper::SubscriptionWrapper::wrapRoadPosition(const std::string& objID, const int variable, const TraCIRoadPosition& value) {
1595  (*myActiveResults)[objID][variable] = std::make_shared<TraCIRoadPosition>(value);
1596  return true;
1597 }
1598 
1599 
1600 void
1601 Helper::VehicleStateListener::vehicleStateChanged(const SUMOVehicle* const vehicle, MSNet::VehicleState to, const std::string& /*info*/) {
1602  myVehicleStateChanges[to].push_back(vehicle->getID());
1603 }
1604 
1605 
1606 }
1607 
1608 
1609 /****************************************************************************/
#define DEG2RAD(x)
Definition: GeomHelper.h:35
#define RAD2DEG(x)
Definition: GeomHelper.h:36
#define FAR_AWAY
Definition: Helper.cpp:55
#define LANE_RTREE_QUAL
Definition: Helper.h:80
std::vector< const MSEdge * > ConstMSEdgeVector
Definition: MSEdge.h:74
#define WRITE_WARNINGF(...)
Definition: MsgHandler.h:277
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:276
#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:32
#define SIMTIME
Definition: SUMOTime.h:60
#define TIME2STEPS(x)
Definition: SUMOTime.h:55
long long int SUMOTime
Definition: SUMOTime.h:31
SUMOVehicleClass
Definition of vehicle classes to differ between different lane usage and authority types.
@ SVC_PEDESTRIAN
pedestrian
const std::string SUMO_PARAM_ORIGID
int gPrecision
the precision for floating point outputs
Definition: StdDefs.cpp:25
const double SUMO_const_laneWidth
Definition: StdDefs.h:47
T MIN2(T a, T b)
Definition: StdDefs.h:73
T MAX2(T a, T b)
Definition: StdDefs.h:79
T MAX3(T a, T b, T c)
Definition: StdDefs.h:93
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:44
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:62
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:261
SVCPermissions getPermissions() const
Returns the combined permissions of all lanes of this edge.
Definition: MSEdge.h:594
const MSEdgeVector & getPredecessors() const
Definition: MSEdge.h:383
const MSEdge * getInternalFollowingEdge(const MSEdge *followerAfterInternal) const
Definition: MSEdge.cpp:690
bool isWalkingArea() const
return whether this edge is walking area
Definition: MSEdge.h:275
const MSEdge * getOppositeEdge() const
Returns the opposite direction edge if on exists else a nullptr.
Definition: MSEdge.cpp:1092
bool isNormal() const
return whether this edge is an internal edge
Definition: MSEdge.h:251
std::vector< MSTransportable * > getSortedPersons(SUMOTime timestep, bool includeRiding=false) const
Returns this edge's persons sorted by pos.
Definition: MSEdge.cpp:961
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
Definition: MSEdge.h:166
const MSJunction * getFromJunction() const
Definition: MSEdge.h:388
double getLength() const
return the length of the edge
Definition: MSEdge.h:630
bool isInternal() const
return whether this edge is an internal edge
Definition: MSEdge.h:256
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:814
const MSJunction * getToJunction() const
Definition: MSEdge.h:392
const MSEdgeVector & getSuccessors(SUMOVehicleClass vClass=SVC_IGNORING) const
Returns the following edges, restricted by vClass.
Definition: MSEdge.cpp:1013
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:2211
void visit(const LaneStoringVisitor &cont) const
Callback for visiting the lane when traversing an RTree.
Definition: MSLane.h:1219
virtual const VehCont & getVehiclesSecure() const
Returns the vehicles container; locks it for microsimulation.
Definition: MSLane.h:426
static void fill(RTREE &into)
Fills the given RTree with lane instances.
Definition: MSLane.cpp:1948
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:3633
const std::set< const MSVehicle * > & getParkingVehicles() const
retrieve the parking vehicles (see GUIParkingArea)
Definition: MSLane.h:1138
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:3533
double getLength() const
Returns the lane's length.
Definition: MSLane.h:539
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:3564
std::set< MSVehicle * > getSurroundingVehicles(double startPos, double downstreamDist, double upstreamDist, std::shared_ptr< LaneCoverageInfo > checkedLanes) const
Returns all vehicles closer than downstreamDist along the along the road network starting on the give...
Definition: MSLane.cpp:3481
bool allowsVehicleClass(SUMOVehicleClass vclass) const
Definition: MSLane.h:812
int getIndex() const
Returns the lane's index.
Definition: MSLane.h:562
MSLane * getLogicalPredecessorLane() const
get the most likely precedecessor lane (sorted using by_connections_to_sorter). The result is cached ...
Definition: MSLane.cpp:2542
MSEdge & getEdge() const
Returns the lane's edge.
Definition: MSLane.h:673
const PositionVector & getShape() const
Returns this lane's shape.
Definition: MSLane.h:476
double interpolateGeometryPosToLanePos(double geometryPos) const
Definition: MSLane.h:509
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:2278
virtual void releaseVehicles() const
Allows to use the container for microsimulation again.
Definition: MSLane.h:456
VehicleState
Definition of a vehicle state.
Definition: MSNet.h:587
@ VEHICLE_STATE_ARRIVED
The vehicle arrived at his destination (is deleted)
Definition: MSNet.h:597
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
Definition: MSNet.cpp:171
SUMOAbstractRouter< MSEdge, SUMOVehicle > & getRouterTT(const int rngIndex, const MSEdgeVector &prohibited=MSEdgeVector()) const
Definition: MSNet.cpp:1199
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
Definition: MSNet.h:371
static bool hasInstance()
Returns whether the network was already constructed.
Definition: MSNet.h:154
void addVehicleStateListener(VehicleStateListener *listener)
Adds a vehicle states listener.
Definition: MSNet.cpp:1054
virtual MSTransportableControl & getPersonControl()
Returns the person control.
Definition: MSNet.cpp:986
void setRemoteControlled(Position xyPos, MSLane *l, double pos, double posLat, double angle, int edgeOffset, const ConstMSEdgeVector &route, SUMOTime t)
Definition: MSPerson.cpp:530
Influencer & getInfluencer()
Returns the velocity/lane influencer.
Definition: MSPerson.cpp:505
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:299
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:783
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:77
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:5134
bool isOnRoad() const
Returns the information whether the vehicle is on a road (is simulated)
Definition: MSVehicle.h:580
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
Definition: MSVehicle.cpp:5106
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:5199
Influencer & getInfluencer()
Definition: MSVehicle.cpp:6059
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:374
const MSLane * getLane() const
Returns the lane the vehicle is on.
Definition: MSVehicle.h:550
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:89
Base class for objects which have an id.
Definition: Named.h:53
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:66
const std::string & getID() const
Returns the id.
Definition: Named.h:73
int Search(const float a_min[2], const float a_max[2], const Named::StoringVisitor &c) const
Find all within search rectangle.
Definition: NamedRTree.h:111
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:36
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:241
double x() const
Returns the x-position.
Definition: Position.h:54
double z() const
Returns the z-position.
Definition: Position.h:64
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:251
double y() const
Returns the y-position.
Definition: Position.h:59
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)
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.h:52
unsigned char alpha() const
Returns the alpha-amount of the color.
Definition: RGBColor.h:73
unsigned char green() const
Returns the green-amount of the color.
Definition: RGBColor.h:59
unsigned char blue() const
Returns the blue-amount of the color.
Definition: RGBColor.h:66
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:58
virtual double getAngle() const =0
Get the vehicle's angle.
const MSEdge * nextEdge
Definition: Helper.h:212
const MSEdge * prevEdge
Definition: Helper.h:211
void setContext(const std::string &refID)
Definition: Helper.cpp:1530
bool wrapDouble(const std::string &objID, const int variable, const double value)
Definition: Helper.cpp:1550
bool wrapColor(const std::string &objID, const int variable, const TraCIColor &value)
Definition: Helper.cpp:1587
bool wrapInt(const std::string &objID, const int variable, const int value)
Definition: Helper.cpp:1557
bool wrapStringList(const std::string &objID, const int variable, const std::vector< std::string > &value)
Definition: Helper.cpp:1571
bool wrapPosition(const std::string &objID, const int variable, const TraCIPosition &value)
Definition: Helper.cpp:1580
bool wrapString(const std::string &objID, const int variable, const std::string &value)
Definition: Helper.cpp:1564
void setParams(const std::vector< unsigned char > *params)
Definition: Helper.cpp:1536
bool wrapRoadPosition(const std::string &objID, const int variable, const TraCIRoadPosition &value)
Definition: Helper.cpp:1594
SubscriptionWrapper(VariableWrapper::SubscriptionHandler handler, SubscriptionResults &into, ContextSubscriptionResults &context)
Definition: Helper.cpp:1523
std::map< MSNet::VehicleState, std::vector< std::string > > myVehicleStateChanges
Changes in the states of simulated vehicles.
Definition: Helper.h:259
void vehicleStateChanged(const SUMOVehicle *const vehicle, MSNet::VehicleState to, const std::string &info="")
Called if a vehicle changes its state.
Definition: Helper.cpp:1601
static Position makePosition(const TraCIPosition &position)
Definition: Helper.cpp:398
static MSEdge * getEdge(const std::string &edgeID)
Definition: Helper.cpp:404
static void postProcessRemoteControl()
Definition: Helper.cpp:1102
static void cleanup()
Definition: Helper.cpp:526
static double getDrivingDistance(std::pair< const MSLane *, double > &roadPos1, std::pair< const MSLane *, double > &roadPos2)
Definition: Helper.cpp:463
static void collectObjectsInRange(int domain, const PositionVector &shape, double range, std::set< const Named * > &into)
Definition: Helper.cpp:603
static TraCIPosition makeTraCIPosition(const Position &position, const bool includeZ=false)
Definition: Helper.cpp:388
static LANE_RTREE_QUAL * myLaneTree
A storage of lanes.
Definition: Helper.h:275
static void findObjectShape(int domain, const std::string &id, PositionVector &shape)
Definition: Helper.cpp:560
static PositionVector makePositionVector(const TraCIPositionVector &vector)
Definition: Helper.cpp:358
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:330
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:1413
static void debugPrint(const SUMOTrafficObject *veh)
Definition: Helper.cpp:125
static MSPerson * getPerson(const std::string &id)
Definition: Helper.cpp:499
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:137
static TraCIPositionVector makeTraCIPositionVector(const PositionVector &positionVector)
helper functions
Definition: Helper.cpp:348
static void registerVehicleStateListener()
Definition: Helper.cpp:538
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:269
static void clearVehicleStates()
Definition: Helper.cpp:552
static void clearSubscriptions()
Definition: Helper.cpp:238
static MSBaseVehicle * getVehicle(const std::string &id)
Definition: Helper.cpp:485
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:1020
static TraCIColor makeTraCIColor(const RGBColor &color)
Definition: Helper.cpp:371
static void applySubscriptionFilterFieldOfVision(const Subscription &s, std::set< std::string > &objIDs)
Definition: Helper.cpp:981
static Subscription * myLastContextSubscription
The last context subscription.
Definition: Helper.h:266
static void setRemoteControlled(MSVehicle *v, Position xyPos, MSLane *l, double pos, double posLat, double angle, int edgeOffset, ConstMSEdgeVector route, SUMOTime t)
Definition: Helper.cpp:1087
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:641
static std::map< std::string, MSVehicle * > myRemoteControlledVehicles
Definition: Helper.h:277
static const MSVehicleType & getVehicleType(const std::string &vehicleID)
Definition: Helper.cpp:520
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:1123
static std::pair< MSLane *, double > convertCartesianToRoadMap(const Position &pos, const SUMOVehicleClass vClass)
Definition: Helper.cpp:431
static SUMOTrafficObject * getTrafficObject(int domain, const std::string &id)
Definition: Helper.cpp:509
static VehicleStateListener myVehicleStateListener
Changes in the states of simulated vehicles.
Definition: Helper.h:272
static std::vector< Subscription > mySubscriptions
The list of known, still valid subscriptions.
Definition: Helper.h:263
static void handleSingleSubscription(const Subscription &s)
Definition: Helper.cpp:256
static const std::vector< std::string > & getVehicleStateChanges(const MSNet::VehicleState state)
Definition: Helper.cpp:546
static void collectObjectIDsInRange(int domain, const PositionVector &shape, double range, std::set< std::string > &into)
Definition: Helper.cpp:593
static void handleSubscriptions(const SUMOTime t)
Definition: Helper.cpp:188
static void addSubscriptionParam(double param)
Definition: Helper.cpp:170
static Subscription * addSubscriptionFilter(SubscriptionFilterType filter)
Definition: Helper.cpp:245
static const MSLane * getLaneChecking(const std::string &edgeID, int laneIndex, double pos)
Definition: Helper.cpp:414
static RGBColor makeRGBColor(const TraCIColor &color)
Definition: Helper.cpp:382
static std::map< std::string, MSPerson * > myRemoteControlledPersons
Definition: Helper.h:278
static bool needNewSubscription(libsumo::Subscription &s, std::vector< Subscription > &subscriptions, libsumo::Subscription *&modifiedSubscription)
Definition: Helper.cpp:213
static bool findCloserLane(const MSEdge *edge, const Position &pos, SUMOVehicleClass vClass, double &bestDistance, MSLane **lane)
Definition: Helper.cpp:1382
static NamedRTree * getTree()
Returns a tree filled with PoI instances.
Definition: POI.cpp:268
static std::shared_ptr< VariableWrapper > makeWrapper()
Definition: POI.cpp:295
static void cleanup()
Definition: POI.cpp:282
static void storeShape(const std::string &id, PositionVector &shape)
Saves the shape of the requested object in the given container.
Definition: POI.cpp:289
static NamedRTree * getTree()
Returns a tree filled with polygon instances.
Definition: Polygon.cpp:266
static void storeShape(const std::string &id, PositionVector &shape)
Saves the shape of the requested object in the given container.
Definition: Polygon.cpp:287
static std::shared_ptr< VariableWrapper > makeWrapper()
Definition: Polygon.cpp:293
static void cleanup()
Definition: Polygon.cpp:281
Representation of a subscription.
Definition: Subscription.h:67
double filterUpstreamDist
Upstream distance specified by the upstream distance filter.
Definition: Subscription.h:133
int commandId
commandIdArg The command id of the subscription
Definition: Subscription.h:110
std::set< std::string > filterVTypes
vTypes specified by the vTypes filter
Definition: Subscription.h:135
double filterFieldOfVisionOpeningAngle
Opening angle (in deg) specified by the field of vision filter.
Definition: Subscription.h:139
std::vector< int > filterLanes
lanes specified by the lanes filter
Definition: Subscription.h:129
std::string id
The id of the object that is subscribed.
Definition: Subscription.h:112
int filterVClasses
vClasses specified by the vClasses filter,
Definition: Subscription.h:137
SUMOTime endTime
The end time of the subscription.
Definition: Subscription.h:120
int contextDomain
The domain ID of the context.
Definition: Subscription.h:122
bool isVehicleToVehicleContextSubscription() const
Definition: Subscription.h:101
SUMOTime beginTime
The begin time of the subscription.
Definition: Subscription.h:118
std::vector< int > variables
The subscribed variables.
Definition: Subscription.h:114
bool isVehicleToPersonContextSubscription() const
Definition: Subscription.h:105
double filterDownstreamDist
Downstream distance specified by the downstream distance filter.
Definition: Subscription.h:131
double filterLateralDist
Lateral distance specified by the lateral distance filter.
Definition: Subscription.h:141
std::vector< std::vector< unsigned char > > parameters
The parameters for the subscribed variables.
Definition: Subscription.h:116
int activeFilters
Active filters for the subscription (bitset,.
Definition: Subscription.h:127
double range
The range of the context.
Definition: Subscription.h:124
virtual void setParams(const std::vector< unsigned char > *)
Definition: Subscription.h:151
bool(* SubscriptionHandler)(const std::string &objID, const int variable, VariableWrapper *wrapper)
Definition of a method to be called for serving an associated commandID.
Definition: Subscription.h:147
virtual void setContext(const std::string &)
Definition: Subscription.h:150
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
std::map< int, std::shared_ptr< TraCIResult > > TraCIResults
{variable->value}
Definition: TraCIDefs.h:248
TRACI_CONST int CMD_SUBSCRIBE_VEHICLE_CONTEXT
TRACI_CONST int VAR_ROAD_ID
TRACI_CONST int CMD_GET_VEHICLE_VARIABLE
TRACI_CONST int CMD_GET_EDGE_VARIABLE
std::vector< TraCIPosition > TraCIPositionVector
Definition: TraCIDefs.h:196
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, TraCIResults > SubscriptionResults
{object->{variable->value}}
Definition: TraCIDefs.h:250
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:34
@ SUBS_FILTER_LEAD_FOLLOW
Definition: Subscription.h:46
@ SUBS_FILTER_UPSTREAM_DIST
Definition: Subscription.h:44
@ SUBS_FILTER_VTYPE
Definition: Subscription.h:52
@ SUBS_FILTER_NO_RTREE
Definition: Subscription.h:59
@ SUBS_FILTER_LANES
Definition: Subscription.h:38
@ SUBS_FILTER_NOOPPOSITE
Definition: Subscription.h:40
@ SUBS_FILTER_DOWNSTREAM_DIST
Definition: Subscription.h:42
@ SUBS_FILTER_LATERAL_DIST
Definition: Subscription.h:57
@ SUBS_FILTER_TURN
Definition: Subscription.h:48
@ SUBS_FILTER_VCLASS
Definition: Subscription.h:50
@ SUBS_FILTER_MANEUVER
Definition: Subscription.h:61
@ SUBS_FILTER_FIELD_OF_VISION
Definition: Subscription.h:55
TRACI_CONST int CMD_SUBSCRIBE_POI_CONTEXT
std::map< std::string, SubscriptionResults > ContextSubscriptionResults
Definition: TraCIDefs.h:251
TRACI_CONST int CMD_SUBSCRIBE_PERSON_VARIABLE
TRACI_CONST int CMD_GET_INDUCTIONLOOP_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_JUNCTION_CONTEXT
A list of positions.
A 3D-position.
Definition: TraCIDefs.h:141
An edgeId, position and laneIndex.
Definition: TraCIDefs.h:153