55 #define FAR_AWAY 1000.0
81 for (
auto p : persons) {
126 if (veh !=
nullptr) {
128 std::cout <<
" '" << veh->
getID() <<
"' on lane '" << ((
SUMOVehicle*)veh)->getLane()->getID() <<
"'\n";
130 std::cout <<
" '" << veh->
getID() <<
"' on edge '" << veh->
getEdge()->
getID() <<
"'\n";
137 Helper::subscribe(
const int commandId,
const std::string&
id,
const std::vector<int>& variables,
139 const int contextDomain,
const double range) {
141 if (variables.empty()) {
143 if (j->id ==
id && j->commandId == commandId && j->contextDomain == contextDomain) {
151 std::vector<std::vector<unsigned char> > parameters(variables.size());
171 std::vector<unsigned char> dest(
sizeof(param));
172 std::memcpy(dest.data(), ¶m,
sizeof(param));
180 std::vector<unsigned char> dest(param.size());
181 std::memcpy(dest.data(), ¶m, param.size());
190 wrapper.second->clear();
198 if (s.
endTime < t || isArrivedVehicle || isArrivedPerson) {
205 if (s.beginTime <= t) {
218 std::vector<std::vector<unsigned char> >::const_iterator k = s.
parameters.begin();
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);
227 modifiedSubscription = &o;
231 subscriptions.push_back(s);
232 modifiedSubscription = &subscriptions.back();
249 WRITE_WARNING(
"addSubscriptionFilter: No previous vehicle context subscription exists to apply the context filter.");
258 std::set<std::string> objIDs;
285 auto wrapper =
myWrapper.find(getCommandId);
289 std::shared_ptr<VariableWrapper> handler = wrapper->second;
293 if (containerWrapper ==
myWrapper.end()) {
296 container = containerWrapper->second.get();
301 for (
const std::string& objID : objIDs) {
303 std::vector<std::vector<unsigned char> >::const_iterator k = s.
parameters.begin();
308 handler->handle(objID, variable, container);
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()) {
335 (*aggregatedLaneCoverage)[lane] = (*newLaneCoverage)[lane];
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;
350 for (
int i = 0; i < (int)positionVector.size(); ++i) {
361 if (std::isnan(pos.x) || std::isnan(pos.y)) {
364 pv.push_back(
Position(pos.x, pos.y));
383 return RGBColor((
unsigned char)c.
r, (
unsigned char)c.
g, (
unsigned char)c.
b, (
unsigned char)c.
a);
406 if (edge ==
nullptr) {
407 throw TraCIException(
"Referenced edge '" + edgeID +
"' is not known.");
416 if (edge ==
nullptr) {
419 if (laneIndex < 0 || laneIndex >= (
int)edge->
getLanes().size()) {
423 if (pos < 0 || pos > lane->
getLength()) {
430 std::pair<MSLane*, double>
433 std::pair<MSLane*, double> result;
434 double range = 1000.;
437 while (range < maxRange) {
438 std::set<const Named*> lanes;
440 double minDistance = std::numeric_limits<double>::max();
441 for (
const Named* named : lanes) {
446 if (newDistance < minDistance) {
447 minDistance = newDistance;
452 if (minDistance < std::numeric_limits<double>::max()) {
464 if (roadPos1.first == roadPos2.first && roadPos1.second <= roadPos2.second) {
466 return roadPos2.second - roadPos1.second;
468 double distance = 0.0;
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();
476 if (newRoute.empty()) {
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());
487 if (sumoVehicle ==
nullptr) {
492 throw TraCIException(
"Vehicle '" +
id +
"' is not a proper vehicle.");
530 InductionLoop::cleanup();
545 const std::vector<std::string>&
563 InductionLoop::storeShape(
id, shape);
566 Lane::storeShape(
id, shape);
569 Vehicle::storeShape(
id, shape);
572 Person::storeShape(
id, shape);
581 Junction::storeShape(
id, shape);
584 Edge::storeShape(
id, shape);
594 std::set<const Named*> objects;
596 for (
const Named* obj : objects) {
597 into.insert(obj->getID());
605 const float cmin[2] = {(float) b.
xmin(), (float) b.
ymin()};
606 const float cmax[2] = {(float) b.
xmax(), (float) b.
ymax()};
610 InductionLoop::getTree()->Search(cmin, cmax, sv);
619 Junction::getTree()->Search(cmin, cmax, sv);
642 #ifdef DEBUG_SURROUNDING
644 std::cout <<
SIMTIME <<
" applySubscriptionFilters for vehicle '" << _veh->
getID() <<
"' on lane '" << _veh->
getLane()->
getID() <<
"'"
646 <<
"objIDs = " <<
toString(objIDs) << std::endl;
660 WRITE_WARNING(
"Ignoring no-opposite subscription filter for geographic range object collection. Consider using the 'lanes' filter.")
663 WRITE_WARNING(
"Ignoring field of vision subscription filter due to incompatibility with other filter(s).")
668 std::set<const SUMOTrafficObject*> vehs;
671 double downstreamDist = s.
range, upstreamDist = s.
range, lateralDist = s.
range;
686 throw TraCIException(
"Subscription filter not yet implemented for meso vehicle");
692 if (vehLane ==
nullptr) {
696 std::vector<int> filterLanes;
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;
718 for (
int offset : filterLanes) {
720 if (lane !=
nullptr) {
724 vehs.insert(vehs.end(), leader);
725 vehs.insert(vehs.end(), follower);
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;
733 }
else if (!disregardOppositeDirection && offset > 0) {
736 if (opposite ==
nullptr) {
737 #ifdef DEBUG_SURROUNDING
738 std::cout <<
"No lane at index " << offset << std::endl;
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;
752 lane = opposite->
getLanes()[ix_opposite];
757 vehs.insert(vehs.end(), lane->
getFollower(v, posOnOpposite, downstreamDist,
true).first);
759 vehs.insert(vehs.end(), lane->
getLeader(v, posOnOpposite - v->
getLength(), std::vector<MSLane*>()).first);
768 #ifdef DEBUG_SURROUNDING
769 std::cout <<
"Applying turn filter for vehicle '" << v->
getID() <<
"'\n Gathering foes ..." << std::endl;
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;
776 for (
auto& foeLane : l->getFoeLanes()) {
778 const MSLink* foeLink = foeLane->getEntryLink();
780 if (vi.second.dist <= upstreamDist) {
781 #ifdef DEBUG_SURROUNDING
782 std::cout <<
" Approaching from foe-lane '" << vi.first->getID() <<
"'" << std::endl;
784 vehs.insert(vehs.end(),
dynamic_cast<const MSVehicle*
>(vi.first));
788 for (
const MSVehicle* foe : foeLane->getVehiclesSecure()) {
789 vehs.insert(vehs.end(), foe);
791 foeLane->releaseVehicles();
795 #ifdef DEBUG_SURROUNDING
796 std::cout <<
SIMTIME <<
" applySubscriptionFilters() for veh '" << v->
getID() <<
"'. Found the following vehicles:\n";
797 for (
auto veh : vehs) {
802 assert(vehs.size() == 0);
803 assert(objIDs.size() == 0);
808 double range =
MAX3(downstreamDist, upstreamDist, lateralDist);
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;
818 #ifdef DEBUG_SURROUNDING
819 std::cout <<
"FILTER_LATERAL_DIST: myLane is '" << v->
getLane()->
getID() <<
"'" << std::endl;
832 assert(filterLanes.size() > 0);
834 auto checkedLanesInDrivingDir = std::make_shared<LaneCoverageInfo>();
835 for (
int offset : filterLanes) {
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;
844 std::shared_ptr<LaneCoverageInfo> checkedLanes = std::make_shared<LaneCoverageInfo>();
846 vehs.insert(new_vehs.begin(), new_vehs.end());
848 }
else if (!disregardOppositeDirection && offset > 0) {
852 if (opposite ==
nullptr) {
853 #ifdef DEBUG_SURROUNDING
854 std::cout <<
"No opposite edge, thus no lane at index " << offset << std::endl;
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;
868 lane = opposite->
getLanes()[ix_opposite];
871 vehs.insert(new_vehs.begin(), new_vehs.end());
873 #ifdef DEBUG_SURROUNDING
875 std::cout <<
"No lane at index " << offset << std::endl;
879 if (!disregardOppositeDirection) {
886 const int nOpp =
MAX2(0, (*std::max_element(filterLanes.begin(), filterLanes.end())) - ((
int)vehEdge->
getLanes().size() - 1 - vehLane->
getIndex()));
889 for (
auto& laneCov : *checkedLanesInDrivingDir) {
890 const MSLane*
const l = laneCov.first;
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) {
903 const MSLane* oppositeLane = *oppositeLaneIt;
905 vehs.insert(new_vehs.begin(), new_vehs.end());
910 #ifdef DEBUG_SURROUNDING
911 std::cout <<
SIMTIME <<
" applySubscriptionFilters() for veh '" << v->
getID() <<
"'. Found the following vehicles:\n";
912 for (
auto veh : vehs) {
921 auto i = vehs.begin();
922 while (i != vehs.end()) {
923 if (((*i)->getVehicleType().getVehicleClass() & s.
filterVClasses) == 0) {
932 auto i = vehs.begin();
933 while (i != vehs.end()) {
944 if (veh !=
nullptr) {
945 objIDs.insert(objIDs.end(), veh->getID());
951 auto i = objIDs.begin();
952 while (i != objIDs.end()) {
963 auto i = objIDs.begin();
964 while (i != objIDs.end()) {
991 #ifdef DEBUG_SURROUNDING
992 std::cout <<
"FOVFILTER: ego direction = " <<
toString(
RAD2DEG(egoVehicle->
getAngle())) <<
" (deg)" << std::endl;
995 auto i = objIDs.begin();
996 while (i != objIDs.end()) {
997 if (s.
id.compare(*i) == 0) {
1005 #ifdef DEBUG_SURROUNDING
1008 std::cout <<
"FOVFILTER: " << objType <<
" '" << *i <<
"' alpha = " <<
toString(
RAD2DEG(alpha)) <<
" (deg)" << std::endl;
1011 if (abs(alpha) > openingAngle * 0.5) {
1012 i = objIDs.erase(i);
1021 std::set<const SUMOTrafficObject*>& vehs,
1022 const std::vector<const MSLane*>& lanes,
double posOnLane,
double posLat,
bool isDownstream) {
1024 double distRemaining = streamDist;
1025 bool isFirstLane =
true;
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;
1034 isFirstLane =
false;
1035 if (posOnLane == 0) {
1036 if (!isDownstream) {
1040 double geometryPos = lane->interpolateLanePosToGeometryPos(posOnLane);
1041 if (geometryPos >= laneShape.
length()) {
1044 auto pair = laneShape.
splitAt(geometryPos,
false);
1045 laneShape = isDownstream ? pair.second : pair.first;
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;
1055 distRemaining -= laneLength;
1059 WRITE_WARNING(
"addSubscriptionFilterLateralDistance could not determine shape of lane '" + lane->getID() +
"' with lateral shift of " +
toString(posLat));
1061 #ifdef DEBUG_SURROUNDING
1062 std::cout <<
" posLat=" << posLat <<
" laneShape=" << laneShape <<
"\n";
1064 combinedShape.
append(laneShape);
1065 if (distRemaining <= 0) {
1070 auto i = objIDs.begin();
1071 while (i != objIDs.end()) {
1074 #ifdef DEBUG_SURROUNDING
1075 std::cout <<
" obj " << obj->
getID() <<
" dist=" << minPerpendicularDist <<
" filterDist=" << s.
filterLateralDist <<
"\n";
1079 i = objIDs.erase(i);
1105 controlled.second->getInfluencer().postProcessRemoteControl(controlled.second);
1107 WRITE_WARNING(
"Vehicle '" + controlled.first +
"' was removed though being controlled by TraCI");
1113 controlled.second->getInfluencer().postProcessRemoteControl(controlled.second);
1115 WRITE_WARNING(
"Person '" + controlled.first +
"' was removed though being controlled by TraCI");
1124 double speed,
const ConstMSEdgeVector& currentRoute,
const int routePosition,
const MSLane* currentLane,
double currentLanePos,
bool onRoad,
1129 std::cout <<
SIMTIME <<
" moveToXYMap pos=" << pos <<
" angle=" << angle <<
" vClass=" <<
toString(vClass) <<
"\n";
1131 const MSEdge*
const currentRouteEdge = currentRoute[routePosition];
1132 std::set<const Named*> into;
1134 shape.push_back(pos);
1137 std::map<MSLane*, LaneUtility> lane2utility;
1139 for (
const Named* namedEdge : into) {
1140 const MSEdge* e =
dynamic_cast<const MSEdge*
>(namedEdge);
1144 const MSEdge* prevEdge =
nullptr;
1145 const MSEdge* nextEdge =
nullptr;
1146 bool onRoute =
false;
1152 for (
int i = routePosition; i < (int)currentRoute.size(); i++) {
1153 const MSEdge* cand = currentRoute[i];
1156 if (i + 1 < (
int)currentRoute.size()) {
1158 nextEdge = currentRoute[i + 1];
1163 if (onRoute ==
false) {
1165 for (
int i = routePosition - 1; i >= 0; i--) {
1166 const MSEdge* cand = currentRoute[i];
1170 nextEdge = currentRoute[i + 1];
1175 if (prevEdge ==
nullptr) {
1181 if (e2 != nextEdge) {
1188 if (nextEdge ==
nullptr) {
1193 if (e2 != prevEdge) {
1200 #ifdef DEBUG_MOVEXY_ANGLE
1210 ConstMSEdgeVector::const_iterator searchStart = currentRoute.begin() + routePosition;
1214 ConstMSEdgeVector::const_iterator edgePos = std::find(searchStart, currentRoute.end(), e);
1215 onRoute = edgePos != currentRoute.end();
1216 if (edgePos == currentRoute.end() - 1 && currentRouteEdge == e) {
1222 nextEdge = !onRoute || edgePos == currentRoute.end() - 1 ? nullptr : *(edgePos + 1);
1223 #ifdef DEBUG_MOVEXY_ANGLE
1230 while (prevEdge !=
nullptr && prevEdge->
isInternal()) {
1233 prevEdge = l ==
nullptr ? nullptr : &l->
getEdge();
1236 ConstMSEdgeVector::const_iterator prevEdgePos = std::find(currentRoute.begin() + routePosition, currentRoute.end(), prevEdge);
1238 while (nextEdge !=
nullptr && nextEdge->
isInternal()) {
1241 if (prevEdgePos != currentRoute.end() && (prevEdgePos + 1) != currentRoute.end()) {
1242 onRoute = *(prevEdgePos + 1) == nextEdge;
1244 #ifdef DEBUG_MOVEXY_ANGLE
1251 const bool perpendicular =
false;
1253 if (!l->allowsVehicleClass(vClass)) {
1256 if (l->getShape().length() == 0) {
1260 double langle = 180.;
1262 double perpendicularDist =
FAR_AWAY;
1264 const double slack = POSITION_EPS;
1269 perpendicularDist = laneShape.
distance2D(pos,
true);
1271 off = l->getShape().nearest_offset_to_point2D(pos, perpendicular);
1273 dist = l->getShape().distance2D(pos, perpendicular);
1277 bool sameEdge = onRoad && e == ¤tLane->
getEdge() && currentRouteEdge->
getLength() > currentLanePos +
SPEED2DIST(speed) && !e->isWalkingArea();
1285 double dist2 = dist;
1286 if (mayLeaveNetwork && fabs(dist - perpendicularDist) > slack) {
1291 #ifdef DEBUG_MOVEXY_ANGLE
1293 <<
" candLane=" << l->getID() <<
" lAngle=" << langle <<
" lLength=" << l->getLength()
1294 <<
" angleDiff=" << angleDiff
1296 <<
" pDist=" << perpendicularDist
1298 <<
" dist2=" << dist2
1300 std::cout << l->getID() <<
" param=" << l->getParameter(
SUMO_PARAM_ORIGID,
"") <<
" origID='" << origID <<
"\n";
1304 if (origIDMatch && setLateralPos
1305 && perpendicularDist > l->getWidth() / 2) {
1306 origIDMatch =
false;
1309 dist2, perpendicularDist, off, angleDiff,
1311 onRoute, sameEdge, prevEdge, nextEdge));
1319 double bestValue = 0;
1320 MSLane* bestLane =
nullptr;
1321 for (
const auto& it : lane2utility) {
1322 MSLane*
const l = it.first;
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;
1329 double value = (distN * .5
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;
1338 if (value > bestValue || bestLane ==
nullptr) {
1348 if (bestLane ==
nullptr) {
1351 const LaneUtility& u = lane2utility.find(bestLane)->second;
1352 bestDistance = u.
dist;
1354 lanePos =
MAX2(0.,
MIN2(
double((*lane)->getLength() - POSITION_EPS),
1359 ConstMSEdgeVector::const_iterator prevEdgePos = std::find(currentRoute.begin(), currentRoute.end(), prevEdge);
1360 routeOffset = (int)std::distance(currentRoute.begin(), prevEdgePos);
1373 #ifdef DEBUG_MOVEXY_ANGLE
1383 if (edge ==
nullptr) {
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;
1399 std::cout <<
" b at lane " << candidateLane->
getID() <<
" dist:" << dist <<
" best:" << bestDistance << std::endl;
1401 if (dist < bestDistance) {
1403 bestDistance = dist;
1404 *lane = candidateLane;
1416 double& bestDistance,
MSLane** lane,
double& lanePos,
int& routeOffset) {
1418 std::cout <<
SIMTIME <<
" moveToXYMap_matchingRoutePosition pos=" << pos <<
" vClass=" <<
toString(vClass) <<
"\n";
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) {
1433 prev = internalCand;
1441 const MSEdge* next = currentRoute[routeIndex];
1442 for (
int i = routeIndex; i >= 0; --i) {
1443 const MSEdge* cand = currentRoute[i];
1446 while (prev !=
nullptr) {
1449 if (
findCloserLane(internalCand, pos, vClass, bestDistance, lane)) {
1452 prev = internalCand;
1461 std::map<const MSJunction*, int> routeJunctions;
1462 for (
int i = 0; i < (int)currentRoute.size() - 1; ++i) {
1463 routeJunctions[currentRoute[i]->getToJunction()] = i;
1465 std::set<const Named*> into;
1467 shape.push_back(pos);
1469 for (
const Named* named : into) {
1470 const MSLane* cand =
dynamic_cast<const MSLane*
>(named);
1482 if (lane ==
nullptr) {
1484 std::cout <<
" b failed - no best route lane" << std::endl;
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) {
1496 if (setLateralPos) {
1499 const double dist = (*i)->getShape().distance2D(pos);
1500 if (dist < (*i)->getWidth() / 2) {
1512 lanePos =
MAX2(0.,
MIN2(
double((*lane)->getLength() - POSITION_EPS),
1513 (*lane)->interpolateGeometryPosToLanePos(
1514 (*lane)->getShape().nearest_offset_to_point25D(pos,
false))));
1517 std::cout <<
" b ok lane " << (*lane)->getID() <<
" lanePos:" << lanePos << std::endl;
1524 :
VariableWrapper(handler), myResults(into), myContextResults(context), myActiveResults(&into) {
1531 myActiveResults = refID ==
"" ? &myResults : &myContextResults[refID];
1543 myActiveResults = &myResults;
1545 myContextResults.clear();
1551 (*myActiveResults)[objID][variable] = std::make_shared<TraCIDouble>(value);
1558 (*myActiveResults)[objID][variable] = std::make_shared<TraCIInt>(value);
1565 (*myActiveResults)[objID][variable] = std::make_shared<TraCIString>(value);
1572 auto sl = std::make_shared<TraCIStringList>();
1574 (*myActiveResults)[objID][variable] = sl;
1581 (*myActiveResults)[objID][variable] = std::make_shared<TraCIPosition>(value);
1588 (*myActiveResults)[objID][variable] = std::make_shared<TraCIColor>(value);
1595 (*myActiveResults)[objID][variable] = std::make_shared<TraCIRoadPosition>(value);
1602 myVehicleStateChanges[to].push_back(vehicle->
getID());
std::vector< const MSEdge * > ConstMSEdgeVector
#define WRITE_WARNINGF(...)
#define WRITE_WARNING(msg)
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
const double SUMO_const_laneWidth
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
A class that stores a 2D geometrical boundary.
double ymin() const
Returns minimum y-coordinate.
double xmin() const
Returns minimum x-coordinate.
Boundary & grow(double by)
extends the boundary by the given amount
double distanceTo2D(const Position &p) const
returns the euclidean distance in the x-y-plane
double getHeight() const
Returns the height of the boundary (y-axis)
double getWidth() const
Returns the width of the boudary (x-axis)
double ymax() const
Returns maximum y-coordinate.
double xmax() const
Returns maximum x-coordinate.
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()]
static double naviDegree(const double angle)
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
static double getMinAngleDiff(double angle1, double angle2)
Returns the minimum distance (clockwise/counter-clockwise) between both angles.
std::set< const Named * > & myObjects
The container.
void add(const MSLane *const l) const
Adds the given object to the container.
const PositionVector & myShape
The base class for microscopic and mesoscopic vehicles.
double getLength() const
Returns the vehicle's length.
const MSVehicleType & getVehicleType() const
Returns the vehicle's type definition.
A road/street connecting two junctions.
bool isCrossing() const
return whether this edge is a pedestrian crossing
SVCPermissions getPermissions() const
Returns the combined permissions of all lanes of this edge.
const MSEdgeVector & getPredecessors() const
const MSEdge * getInternalFollowingEdge(const MSEdge *followerAfterInternal) const
bool isWalkingArea() const
return whether this edge is walking area
const MSEdge * getOppositeEdge() const
Returns the opposite direction edge if on exists else a nullptr.
bool isNormal() const
return whether this edge is an internal edge
std::vector< MSTransportable * > getSortedPersons(SUMOTime timestep, bool includeRiding=false) const
Returns this edge's persons sorted by pos.
const std::vector< MSLane * > & getLanes() const
Returns this edge's lanes.
const MSJunction * getFromJunction() const
double getLength() const
return the length of the edge
bool isInternal() const
return whether this edge is an internal edge
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....
const MSJunction * getToJunction() const
const MSEdgeVector & getSuccessors(SUMOVehicleClass vClass=SVC_IGNORING) const
Returns the following edges, restricted by vClass.
The base class for an intersection.
Representation of a lane in the micro simulation.
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.
void visit(const LaneStoringVisitor &cont) const
Callback for visiting the lane when traversing an RTree.
virtual const VehCont & getVehiclesSecure() const
Returns the vehicles container; locks it for microsimulation.
static void fill(RTREE &into)
Fills the given RTree with lane instances.
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)
const std::set< const MSVehicle * > & getParkingVehicles() const
retrieve the parking vehicles (see GUIParkingArea)
std::set< MSVehicle * > getVehiclesInRange(const double a, const double b) const
Returns all vehicles on the lane overlapping with the interval [a,b].
double getLength() const
Returns the lane's length.
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...
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...
bool allowsVehicleClass(SUMOVehicleClass vclass) const
int getIndex() const
Returns the lane's index.
MSLane * getLogicalPredecessorLane() const
get the most likely precedecessor lane (sorted using by_connections_to_sorter). The result is cached ...
MSEdge & getEdge() const
Returns the lane's edge.
const PositionVector & getShape() const
Returns this lane's shape.
double interpolateGeometryPosToLanePos(double geometryPos) const
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.
virtual void releaseVehicles() const
Allows to use the container for microsimulation again.
ApproachingVehicleInformation getApproaching(const SUMOVehicle *veh) const
VehicleState
Definition of a vehicle state.
@ VEHICLE_STATE_ARRIVED
The vehicle arrived at his destination (is deleted)
static MSNet * getInstance()
Returns the pointer to the unique instance of MSNet (singleton).
SUMOAbstractRouter< MSEdge, SUMOVehicle > & getRouterTT(const int rngIndex, const MSEdgeVector &prohibited=MSEdgeVector()) const
MSVehicleControl & getVehicleControl()
Returns the vehicle control.
static bool hasInstance()
Returns whether the network was already constructed.
void addVehicleStateListener(VehicleStateListener *listener)
Adds a vehicle states listener.
virtual MSTransportableControl & getPersonControl()
Returns the person control.
void setRemoteControlled(Position xyPos, MSLane *l, double pos, double posLat, double angle, int edgeOffset, const ConstMSEdgeVector &route, SUMOTime t)
Influencer & getInfluencer()
Returns the velocity/lane influencer.
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....
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)
SUMOVehicle * getVehicle(const std::string &id) const
Returns the vehicle with the given id.
Representation of a vehicle in the micro simulation.
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...
bool isOnRoad() const
Returns the information whether the vehicle is on a road (is simulated)
const std::vector< MSLane * > & getBestLanesContinuation() const
Returns the best sequence of lanes to continue the route starting at myLane.
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...
Influencer & getInfluencer()
double getLateralPositionOnLane() const
Get the vehicle's lateral position on the lane.
double getPositionOnLane() const
Get the vehicle's position along the lane.
const MSLane * getLane() const
Returns the lane the vehicle is on.
The car-following model and parameter.
SUMOVehicleClass getVehicleClass() const
Get this vehicle type's vehicle class.
const std::string & getID() const
Returns the name of the vehicle type.
double getLength() const
Get vehicle's length [m].
Allows to store the object; used as context while traveling the rtree in TraCI.
Base class for objects which have an id.
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
const std::string & getID() const
Returns the id.
int Search(const float a_min[2], const float a_max[2], const Named::StoringVisitor &c) const
Find all within search rectangle.
A point in 2D or 3D with translation and scaling methods.
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
double x() const
Returns the x-position.
double z() const
Returns the z-position.
double angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position
double y() const
Returns the y-position.
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.
unsigned char alpha() const
Returns the alpha-amount of the color.
unsigned char green() const
Returns the green-amount of the color.
unsigned char blue() const
Returns the blue-amount of the color.
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.
virtual double getAngle() const =0
Get the vehicle's angle.
void setContext(const std::string &refID)
bool wrapDouble(const std::string &objID, const int variable, const double value)
bool wrapColor(const std::string &objID, const int variable, const TraCIColor &value)
bool wrapInt(const std::string &objID, const int variable, const int value)
bool wrapStringList(const std::string &objID, const int variable, const std::vector< std::string > &value)
bool wrapPosition(const std::string &objID, const int variable, const TraCIPosition &value)
bool wrapString(const std::string &objID, const int variable, const std::string &value)
void setParams(const std::vector< unsigned char > *params)
bool wrapRoadPosition(const std::string &objID, const int variable, const TraCIRoadPosition &value)
SubscriptionWrapper(VariableWrapper::SubscriptionHandler handler, SubscriptionResults &into, ContextSubscriptionResults &context)
std::map< MSNet::VehicleState, std::vector< std::string > > myVehicleStateChanges
Changes in the states of simulated vehicles.
void vehicleStateChanged(const SUMOVehicle *const vehicle, MSNet::VehicleState to, const std::string &info="")
Called if a vehicle changes its state.
static Position makePosition(const TraCIPosition &position)
static MSEdge * getEdge(const std::string &edgeID)
static void postProcessRemoteControl()
static double getDrivingDistance(std::pair< const MSLane *, double > &roadPos1, std::pair< const MSLane *, double > &roadPos2)
static void collectObjectsInRange(int domain, const PositionVector &shape, double range, std::set< const Named * > &into)
static TraCIPosition makeTraCIPosition(const Position &position, const bool includeZ=false)
static LANE_RTREE_QUAL * myLaneTree
A storage of lanes.
static void findObjectShape(int domain, const std::string &id, PositionVector &shape)
static PositionVector makePositionVector(const TraCIPositionVector &vector)
static void fuseLaneCoverage(std::shared_ptr< LaneCoverageInfo > aggregatedLaneCoverage, const std::shared_ptr< LaneCoverageInfo > newLaneCoverage)
Adds lane coverage information from newLaneCoverage into aggregatedLaneCoverage.
static bool moveToXYMap_matchingRoutePosition(const Position &pos, const std::string &origID, const ConstMSEdgeVector ¤tRoute, int routeIndex, SUMOVehicleClass vClass, bool setLateralPos, double &bestDistance, MSLane **lane, double &lanePos, int &routeOffset)
static void debugPrint(const SUMOTrafficObject *veh)
static MSPerson * getPerson(const std::string &id)
static void subscribe(const int commandId, const std::string &id, const std::vector< int > &variables, const double beginTime, const double endTime, const libsumo::TraCIResults ¶ms, const int contextDomain=0, const double range=0.)
static TraCIPositionVector makeTraCIPositionVector(const PositionVector &positionVector)
helper functions
static void registerVehicleStateListener()
static std::map< int, std::shared_ptr< VariableWrapper > > myWrapper
Map of commandIds -> their executors; applicable if the executor applies to the method footprint.
static void clearVehicleStates()
static void clearSubscriptions()
static MSBaseVehicle * getVehicle(const std::string &id)
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)
static TraCIColor makeTraCIColor(const RGBColor &color)
static void applySubscriptionFilterFieldOfVision(const Subscription &s, std::set< std::string > &objIDs)
static Subscription * myLastContextSubscription
The last context subscription.
static void setRemoteControlled(MSVehicle *v, Position xyPos, MSLane *l, double pos, double posLat, double angle, int edgeOffset, ConstMSEdgeVector route, SUMOTime t)
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...
static std::map< std::string, MSVehicle * > myRemoteControlledVehicles
static const MSVehicleType & getVehicleType(const std::string &vehicleID)
static bool moveToXYMap(const Position &pos, double maxRouteDistance, bool mayLeaveNetwork, const std::string &origID, const double angle, double speed, const ConstMSEdgeVector ¤tRoute, const int routePosition, const MSLane *currentLane, double currentLanePos, bool onRoad, SUMOVehicleClass vClass, bool setLateralPos, double &bestDistance, MSLane **lane, double &lanePos, int &routeOffset, ConstMSEdgeVector &edges)
static std::pair< MSLane *, double > convertCartesianToRoadMap(const Position &pos, const SUMOVehicleClass vClass)
static SUMOTrafficObject * getTrafficObject(int domain, const std::string &id)
static VehicleStateListener myVehicleStateListener
Changes in the states of simulated vehicles.
static std::vector< Subscription > mySubscriptions
The list of known, still valid subscriptions.
static void handleSingleSubscription(const Subscription &s)
static const std::vector< std::string > & getVehicleStateChanges(const MSNet::VehicleState state)
static void collectObjectIDsInRange(int domain, const PositionVector &shape, double range, std::set< std::string > &into)
static void handleSubscriptions(const SUMOTime t)
static void addSubscriptionParam(double param)
static Subscription * addSubscriptionFilter(SubscriptionFilterType filter)
static const MSLane * getLaneChecking(const std::string &edgeID, int laneIndex, double pos)
static RGBColor makeRGBColor(const TraCIColor &color)
static std::map< std::string, MSPerson * > myRemoteControlledPersons
static bool needNewSubscription(libsumo::Subscription &s, std::vector< Subscription > &subscriptions, libsumo::Subscription *&modifiedSubscription)
static bool findCloserLane(const MSEdge *edge, const Position &pos, SUMOVehicleClass vClass, double &bestDistance, MSLane **lane)
static NamedRTree * getTree()
Returns a tree filled with PoI instances.
static std::shared_ptr< VariableWrapper > makeWrapper()
static void storeShape(const std::string &id, PositionVector &shape)
Saves the shape of the requested object in the given container.
static NamedRTree * getTree()
Returns a tree filled with polygon instances.
static void storeShape(const std::string &id, PositionVector &shape)
Saves the shape of the requested object in the given container.
static std::shared_ptr< VariableWrapper > makeWrapper()
Representation of a subscription.
double filterUpstreamDist
Upstream distance specified by the upstream distance filter.
int commandId
commandIdArg The command id of the subscription
std::set< std::string > filterVTypes
vTypes specified by the vTypes filter
double filterFieldOfVisionOpeningAngle
Opening angle (in deg) specified by the field of vision filter.
std::vector< int > filterLanes
lanes specified by the lanes filter
std::string id
The id of the object that is subscribed.
int filterVClasses
vClasses specified by the vClasses filter,
SUMOTime endTime
The end time of the subscription.
int contextDomain
The domain ID of the context.
bool isVehicleToVehicleContextSubscription() const
SUMOTime beginTime
The begin time of the subscription.
std::vector< int > variables
The subscribed variables.
bool isVehicleToPersonContextSubscription() const
double filterDownstreamDist
Downstream distance specified by the downstream distance filter.
double filterLateralDist
Lateral distance specified by the lateral distance filter.
std::vector< std::vector< unsigned char > > parameters
The parameters for the subscribed variables.
int activeFilters
Active filters for the subscription (bitset,.
double range
The range of the context.
virtual void setParams(const std::vector< unsigned char > *)
bool(* SubscriptionHandler)(const std::string &objID, const int variable, VariableWrapper *wrapper)
Definition of a method to be called for serving an associated commandID.
virtual void setContext(const std::string &)
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}
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
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}}
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.
@ SUBS_FILTER_LEAD_FOLLOW
@ SUBS_FILTER_UPSTREAM_DIST
@ SUBS_FILTER_DOWNSTREAM_DIST
@ SUBS_FILTER_LATERAL_DIST
@ SUBS_FILTER_FIELD_OF_VISION
TRACI_CONST int CMD_SUBSCRIBE_POI_CONTEXT
std::map< std::string, SubscriptionResults > ContextSubscriptionResults
TRACI_CONST int CMD_SUBSCRIBE_PERSON_VARIABLE
TRACI_CONST int CMD_GET_INDUCTIONLOOP_VARIABLE
TRACI_CONST int CMD_SUBSCRIBE_JUNCTION_CONTEXT
An edgeId, position and laneIndex.