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