55 #ifdef CHECK_MEMORY_LEAKS
57 #endif // CHECK_MEMORY_LEAKS
63 using namespace traci;
106 std::vector<std::string> ids;
109 if ((*i).second->isOnRoad()) {
110 ids.push_back((*i).first);
122 if (sumoVehicle == 0) {
160 tempMsg.
writeInt((
int)std::distance(lanes.begin(), std::find(lanes.begin(), lanes.end(), v->
getLane())));
220 if (inputStorage.
readInt() != 2) {
252 if (inputStorage.
readInt() != 2) {
302 unsigned int cnt = 0;
304 const std::vector<MSVehicle::LaneQ>& bestLanes = onRoad ? v->
getBestLanes() : std::vector<MSVehicle::LaneQ>();
305 tempContent.
writeInt((
int) bestLanes.size());
307 for (std::vector<MSVehicle::LaneQ>::const_iterator i = bestLanes.begin(); i != bestLanes.end(); ++i) {
324 std::vector<std::string> bestContIDs;
326 bestContIDs.push_back((*j)->getID());
337 if (!commandDistanceRequest(server, inputStorage, tempMsg, v)) {
355 std::string warning =
"";
377 const bool shouldExist = variable !=
ADD;
379 if (sumoVehicle == 0) {
385 if (v == 0 && shouldExist) {
393 if (inputStorage.
readInt() != 4) {
423 const std::vector<MSLane*>& allLanes = road->
getLanes();
424 if ((laneIndex < 0) || laneIndex >= (
int)(allLanes.size())) {
428 if (!v->
addTraciStop(allLanes[laneIndex], pos, 0, waitTime)) {
437 if (inputStorage.
readInt() != 2) {
450 if ((laneIndex < 0) || (laneIndex >= (
int)(v->
getEdge()->
getLanes().size()))) {
454 std::vector<std::pair<SUMOTime, unsigned int> > laneTimeLine;
455 laneTimeLine.push_back(std::make_pair(
MSNet::getInstance()->getCurrentTimeStep(), laneIndex));
456 laneTimeLine.push_back(std::make_pair(
MSNet::getInstance()->getCurrentTimeStep() + stickyTime, laneIndex));
467 if (inputStorage.
readInt() != 2) {
484 std::vector<std::pair<SUMOTime, SUMOReal> > speedTimeLine;
486 speedTimeLine.push_back(std::make_pair(
MSNet::getInstance()->getCurrentTimeStep() + duration, newSpeed));
525 std::vector<std::string> edgeIDs;
529 std::vector<const MSEdge*> edges;
540 int parameterCount = inputStorage.
readInt();
541 if (parameterCount == 4) {
567 }
else if (parameterCount == 2) {
587 }
else if (parameterCount == 1) {
610 int parameterCount = inputStorage.
readInt();
611 if (parameterCount == 4) {
637 }
else if (parameterCount == 2) {
657 }
else if (parameterCount == 1) {
680 if (inputStorage.
readInt() != 0) {
690 if (inputStorage.
readInt() != 0) {
709 if (inputStorage.
readInt() != 2) {
728 if (!v->
willPass(&destinationEdge)) {
733 while (v->
getEdge() != &destinationEdge) {
749 std::vector<std::pair<SUMOTime, SUMOReal> > speedTimeLine;
751 speedTimeLine.push_back(std::make_pair(
MSNet::getInstance()->getCurrentTimeStep(), speed));
752 speedTimeLine.push_back(std::make_pair(
SUMOTime_MAX, speed));
783 if (inputStorage.
readInt() != 6) {
787 vehicleParams.
id = id;
810 if (vehicleParams.
depart < 0) {
811 const int proc =
static_cast<int>(-vehicleParams.
depart);
824 const int proc =
static_cast<int>(-vehicleParams.
departPos);
839 const int proc =
static_cast<int>(-vehicleParams.
departSpeed);
853 const int proc =
static_cast<int>(-vehicleParams.
departLane);
863 *params = vehicleParams;
911 if (inputStorage.
readInt() != 4) {
937 std::string origID = edgeID +
" " +
toString(laneNum);
939 edgeID =
'-' + edgeID;
948 std::cout << std::endl <<
"begin vehicle " << v->
getID() <<
" vehPos:" << vehPos <<
" lane:" << v->
getLane()->
getID() << std::endl;
951 std::cout <<
" want pos:" << pos <<
" edge:" << edgeID <<
" laneNum:" << laneNum << std::endl;
955 MSLane* laneA, *laneB, *laneC;
956 laneA = laneB = laneC = 0;
957 SUMOReal lanePosA, lanePosB, lanePosC;
958 SUMOReal bestDistanceA, bestDistanceB, bestDistanceC;
959 bestDistanceA = bestDistanceB = bestDistanceC = 1000.;
960 int routeOffsetA, routeOffsetB, routeOffsetC;
961 routeOffsetA = routeOffsetB = routeOffsetC = 0;
963 bool aFound = vtdMap_matchingEdgeLane(pos, origID, *v, server.
vtdDebug(), bestDistanceA, &laneA, lanePosA, routeOffsetA, edgesA);
965 bool bFound = vtdMap_matchingRoutePosition(pos, origID, *v, server.
vtdDebug(), bestDistanceB, &laneB, lanePosB, routeOffsetB, edgesB);
967 bool cFound = vtdMap_matchingNearest(pos, origID, *v, server, server.
vtdDebug(), bestDistanceC, &laneC, lanePosC, routeOffsetC, edgesC);
970 if (cFound && (bestDistanceA > maxRouteDistance && bestDistanceC > maxRouteDistance)) {
1005 const std::map<std::string, std::vector<MSLane*> >& vtdMap = getOrBuildVTDMap();
1006 if (vtdMap.find(origID) == vtdMap.end()) {
1008 std::cout <<
" a failed - lane not in map" << std::endl;
1012 const std::vector<MSLane*>& lanes = vtdMap.find(origID)->second;
1013 for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end() && bestDistance >
POSITION_EPS; ++i) {
1017 std::cout <<
" a at lane " << l->
getID() <<
" dist:" << dist <<
" best:" << bestDistance << std::endl;
1019 if (dist < bestDistance) {
1020 bestDistance = dist;
1031 std::cout <<
" a failed - no incoming lane" << std::endl;
1036 MSEdgeVector::const_iterator p = std::find(tedges.begin() + v.
getRoutePosition(), tedges.end(), &pni->
getEdge());
1037 if (p != tedges.end()) {
1041 std::cout <<
" a ok lane:" << (*lane)->getID() <<
" lanePos:" << lanePos <<
" routeOffset:" << routeOffset << std::endl;
1046 std::cout <<
" a failed - route position beyond route length" << std::endl;
1056 int lastBestRouteEdge = 0;
1057 int lastRouteEdge = 0;
1058 MSLane* bestRouteLane = 0;
1060 for (std::vector<MSLane*>::const_iterator i = bestLaneConts.begin(); i != bestLaneConts.end() && bestDistance >
POSITION_EPS; ++i) {
1061 MSEdge& e = (*i)->getEdge();
1065 const std::vector<MSLane*>& lanes = e.
getLanes();
1066 for (std::vector<MSLane*>::const_iterator k = lanes.begin(); k != lanes.end() && bestDistance >
POSITION_EPS; ++k) {
1070 std::cout <<
" b at lane " << cl->
getID() <<
" dist:" << dist <<
" best:" << bestDistance << std::endl;
1072 if (dist < bestDistance) {
1073 bestDistance = dist;
1075 lastBestRouteEdge = lastRouteEdge;
1079 bestRouteLane = *lane;
1084 if (bestRouteLane == 0) {
1086 std::cout <<
" b failed - no best route lane" << std::endl;
1091 routeOffset = lastBestRouteEdge;
1093 std::cout <<
" b ok lane " << bestRouteLane->
getID() <<
" lanePos:" << lanePos <<
" best:" << lastBestRouteEdge << std::endl;
1105 MSLane* nameMatchingLane = 0;
1106 SUMOReal minDistNameMatchingLane = 1 << (11);
1107 for (; minDistLane == 0 && r < 10 && nameMatchingLane == 0; ++r) {
1108 std::set<std::string> into;
1112 for (std::set<std::string>::const_iterator j = into.begin(); j != into.end(); ++j) {
1114 const std::vector<MSLane*>& lanes = e->
getLanes();
1115 for (std::vector<MSLane*>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
1120 if (dist < minDistNameMatchingLane) {
1121 minDistNameMatchingLane = dist;
1122 nameMatchingLane = lane;
1126 if (dist < minDist) {
1133 *lane = nameMatchingLane != 0 ? nameMatchingLane : minDistLane;
1136 std::cout <<
" c failed - no matching lane" << std::endl;
1144 std::cout <<
" c ok, on same lane" << std::endl;
1148 MSEdge& destinationEdge = (*lane)->getEdge();
1149 MSEdge* routePos = &destinationEdge;
1151 routePos = &routePos->
getLanes()[0]->getLogicalPredecessorLane()->getEdge();
1157 unsigned int rindex = 0;
1159 while (!found && ((
int)(c - r) >= 0 || c + r < l)) {
1160 if ((
int)(c - r) >= 0 && route[c - r] == routePos) {
1164 if (c + r < l && route[c + r] == routePos) {
1174 std::cout <<
" c ok, on a different edge of same route" << std::endl;
1179 MSLane* firstLane = *lane;
1181 edges.push_back(&destinationEdge);
1184 edges.push_back(&firstLane->
getEdge());
1187 if (lc.size() != 0 && lc[0]->getLane() != 0) {
1188 edges.push_back(&lc[0]->getLane()->getEdge());
1191 std::cout <<
" c ok, on a different route" << std::endl;
1203 if (inputStorage.
readInt() != 2) {
1208 std::pair<const MSLane*, SUMOReal> roadPos;
1215 std::string roadID = inputStorage.
readString();
1218 pos = roadPos.first->getShape().positionAtOffset(roadPos.second);
1225 const double p1x = inputStorage.
readDouble();
1226 const double p1y = inputStorage.
readDouble();
1245 v->
getEdge(), &roadPos.first->getEdge());
1276 std::string newID = oType.
getID().find(
'@') == std::string::npos ? oType.
getID() +
"@" + veh->
getID() : oType.
getID();
1278 static_cast<MSVehicle*
>(veh)->replaceVehicleType(type);
1285 const std::map<std::string, std::vector<MSLane*> >&
1287 if (gVTDMap.size() == 0) {
1289 for (std::vector<MSEdge*>::const_iterator i = edges.begin(); i != edges.end(); ++i) {
1290 const std::vector<MSLane*>& lanes = (*i)->getLanes();
1291 for (std::vector<MSLane*>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1292 if ((*j)->knowsParameter(
"origId")) {
1293 std::string origID = (*j)->getParameter(
"origId",
"");
1294 if (gVTDMap.find(origID) == gVTDMap.end()) {
1295 gVTDMap[origID] = std::vector<MSLane*>();
1297 gVTDMap[origID].push_back(*j);
1301 if (gVTDMap.size() == 0) {
1302 gVTDMap[
"unknown"] = std::vector<MSLane*>();