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());
346 if (!commandDistanceRequest(server, inputStorage, tempMsg, v)) {
364 std::string warning =
"";
386 const bool shouldExist = variable !=
ADD;
388 if (sumoVehicle == 0) {
394 if (v == 0 && shouldExist) {
402 int compoundSize = inputStorage.
readInt();
403 if (compoundSize != 4 && compoundSize != 5) {
425 bool parking =
false;
426 bool triggered =
false;
427 if (compoundSize == 5) {
432 parking = ((stopFlags & 1) != 0);
433 triggered = ((stopFlags & 2) != 0);
444 const std::vector<MSLane*>& allLanes = road->
getLanes();
445 if ((laneIndex < 0) || laneIndex >= (
int)(allLanes.size())) {
449 if (!v->
addTraciStop(allLanes[laneIndex], pos, 0, waitTime, parking, triggered)) {
459 if (inputStorage.
readInt() != 0) {
463 if (!static_cast<MSVehicle*>(v)->resumeFromStopping()) {
465 std::ostringstream strs;
466 strs <<
"reached: " << sto.
reached;
467 strs <<
", duration:" << sto.
duration;
468 strs <<
", edge:" << (*sto.
edge)->getID();
469 strs <<
", startPos: " << sto.
startPos;
470 std::string posStr = strs.str();
480 if (inputStorage.
readInt() != 2) {
493 if ((laneIndex < 0) || (laneIndex >= (
int)(v->
getEdge()->
getLanes().size()))) {
497 std::vector<std::pair<SUMOTime, unsigned int> > laneTimeLine;
498 laneTimeLine.push_back(std::make_pair(
MSNet::getInstance()->getCurrentTimeStep(), laneIndex));
499 laneTimeLine.push_back(std::make_pair(
MSNet::getInstance()->getCurrentTimeStep() + stickyTime, laneIndex));
510 if (inputStorage.
readInt() != 2) {
527 std::vector<std::pair<SUMOTime, SUMOReal> > speedTimeLine;
529 speedTimeLine.push_back(std::make_pair(
MSNet::getInstance()->getCurrentTimeStep() + duration, newSpeed));
568 std::vector<std::string> edgeIDs;
572 std::vector<const MSEdge*> edges;
583 int parameterCount = inputStorage.
readInt();
584 if (parameterCount == 4) {
610 }
else if (parameterCount == 2) {
630 }
else if (parameterCount == 1) {
653 int parameterCount = inputStorage.
readInt();
654 if (parameterCount == 4) {
680 }
else if (parameterCount == 2) {
700 }
else if (parameterCount == 1) {
723 if (inputStorage.
readInt() != 0) {
733 if (inputStorage.
readInt() != 0) {
752 if (inputStorage.
readInt() != 2) {
771 if (!v->
willPass(&destinationEdge)) {
776 while (v->
getEdge() != &destinationEdge) {
792 std::vector<std::pair<SUMOTime, SUMOReal> > speedTimeLine;
794 speedTimeLine.push_back(std::make_pair(
MSNet::getInstance()->getCurrentTimeStep(), speed));
795 speedTimeLine.push_back(std::make_pair(
SUMOTime_MAX, speed));
826 if (inputStorage.
readInt() != 6) {
830 vehicleParams.
id = id;
853 if (vehicleParams.
depart < 0) {
854 const int proc =
static_cast<int>(-vehicleParams.
depart);
867 const int proc =
static_cast<int>(-vehicleParams.
departPos);
882 const int proc =
static_cast<int>(-vehicleParams.
departSpeed);
896 const int proc =
static_cast<int>(-vehicleParams.
departLane);
906 *params = vehicleParams;
958 if (inputStorage.
readInt() != 4) {
984 std::string origID = edgeID +
" " +
toString(laneNum);
986 edgeID =
'-' + edgeID;
995 std::cout << std::endl <<
"begin vehicle " << v->
getID() <<
" vehPos:" << vehPos <<
" lane:" << v->
getLane()->
getID() << std::endl;
998 std::cout <<
" want pos:" << pos <<
" edge:" << edgeID <<
" laneNum:" << laneNum << std::endl;
1002 MSLane* laneA, *laneB, *laneC;
1003 laneA = laneB = laneC = 0;
1004 SUMOReal lanePosA, lanePosB, lanePosC;
1005 SUMOReal bestDistanceA, bestDistanceB, bestDistanceC;
1006 bestDistanceA = bestDistanceB = bestDistanceC = 1000.;
1007 int routeOffsetA, routeOffsetB, routeOffsetC;
1008 routeOffsetA = routeOffsetB = routeOffsetC = 0;
1010 bool aFound = vtdMap_matchingEdgeLane(pos, origID, *v, server.
vtdDebug(), bestDistanceA, &laneA, lanePosA, routeOffsetA, edgesA);
1012 bool bFound = vtdMap_matchingRoutePosition(pos, origID, *v, server.
vtdDebug(), bestDistanceB, &laneB, lanePosB, routeOffsetB, edgesB);
1014 bool cFound = vtdMap_matchingNearest(pos, origID, *v, server, server.
vtdDebug(), bestDistanceC, &laneC, lanePosC, routeOffsetC, edgesC);
1017 if (cFound && (bestDistanceA > maxRouteDistance && bestDistanceC > maxRouteDistance)) {
1024 }
else if (aFound) {
1026 }
else if (cFound) {
1052 const std::map<std::string, std::vector<MSLane*> >& vtdMap = getOrBuildVTDMap();
1053 if (vtdMap.find(origID) == vtdMap.end()) {
1055 std::cout <<
" a failed - lane not in map" << std::endl;
1059 const std::vector<MSLane*>& lanes = vtdMap.find(origID)->second;
1060 for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end() && bestDistance >
POSITION_EPS; ++i) {
1064 std::cout <<
" a at lane " << l->
getID() <<
" dist:" << dist <<
" best:" << bestDistance << std::endl;
1066 if (dist < bestDistance) {
1067 bestDistance = dist;
1078 std::cout <<
" a failed - no incoming lane" << std::endl;
1083 MSEdgeVector::const_iterator p = std::find(tedges.begin() + v.
getRoutePosition(), tedges.end(), &pni->
getEdge());
1084 if (p != tedges.end()) {
1088 std::cout <<
" a ok lane:" << (*lane)->getID() <<
" lanePos:" << lanePos <<
" routeOffset:" << routeOffset << std::endl;
1093 std::cout <<
" a failed - route position beyond route length" << std::endl;
1103 int lastBestRouteEdge = 0;
1104 int lastRouteEdge = 0;
1105 MSLane* bestRouteLane = 0;
1107 for (std::vector<MSLane*>::const_iterator i = bestLaneConts.begin(); i != bestLaneConts.end() && bestDistance >
POSITION_EPS; ++i) {
1108 MSEdge& e = (*i)->getEdge();
1112 const std::vector<MSLane*>& lanes = e.
getLanes();
1113 for (std::vector<MSLane*>::const_iterator k = lanes.begin(); k != lanes.end() && bestDistance >
POSITION_EPS; ++k) {
1117 std::cout <<
" b at lane " << cl->
getID() <<
" dist:" << dist <<
" best:" << bestDistance << std::endl;
1119 if (dist < bestDistance) {
1120 bestDistance = dist;
1122 lastBestRouteEdge = lastRouteEdge;
1126 bestRouteLane = *lane;
1131 if (bestRouteLane == 0) {
1133 std::cout <<
" b failed - no best route lane" << std::endl;
1138 routeOffset = lastBestRouteEdge;
1140 std::cout <<
" b ok lane " << bestRouteLane->
getID() <<
" lanePos:" << lanePos <<
" best:" << lastBestRouteEdge << std::endl;
1152 MSLane* nameMatchingLane = 0;
1153 SUMOReal minDistNameMatchingLane = 1 << (11);
1154 for (; minDistLane == 0 && r < 10 && nameMatchingLane == 0; ++r) {
1155 std::set<std::string> into;
1159 for (std::set<std::string>::const_iterator j = into.begin(); j != into.end(); ++j) {
1161 const std::vector<MSLane*>& lanes = e->
getLanes();
1162 for (std::vector<MSLane*>::const_iterator k = lanes.begin(); k != lanes.end(); ++k) {
1167 if (dist < minDistNameMatchingLane) {
1168 minDistNameMatchingLane = dist;
1169 nameMatchingLane = lane;
1173 if (dist < minDist) {
1180 *lane = nameMatchingLane != 0 ? nameMatchingLane : minDistLane;
1183 std::cout <<
" c failed - no matching lane" << std::endl;
1191 std::cout <<
" c ok, on same lane" << std::endl;
1195 MSEdge& destinationEdge = (*lane)->getEdge();
1196 MSEdge* routePos = &destinationEdge;
1198 routePos = &routePos->
getLanes()[0]->getLogicalPredecessorLane()->getEdge();
1204 unsigned int rindex = 0;
1206 while (!found && ((
int)(c - r) >= 0 || c + r < l)) {
1207 if ((
int)(c - r) >= 0 && route[c - r] == routePos) {
1211 if (c + r < l && route[c + r] == routePos) {
1221 std::cout <<
" c ok, on a different edge of same route" << std::endl;
1226 MSLane* firstLane = *lane;
1228 edges.push_back(&destinationEdge);
1231 edges.push_back(&firstLane->
getEdge());
1234 if (lc.size() != 0 && lc[0]->getLane() != 0) {
1235 edges.push_back(&lc[0]->getLane()->getEdge());
1238 std::cout <<
" c ok, on a different route" << std::endl;
1250 if (inputStorage.
readInt() != 2) {
1255 std::pair<const MSLane*, SUMOReal> roadPos;
1262 std::string roadID = inputStorage.
readString();
1265 pos = roadPos.first->getShape().positionAtOffset(roadPos.second);
1272 const double p1x = inputStorage.
readDouble();
1273 const double p1y = inputStorage.
readDouble();
1292 v->
getEdge(), &roadPos.first->getEdge());
1323 std::string newID = oType.
getID().find(
'@') == std::string::npos ? oType.
getID() +
"@" + veh->
getID() : oType.
getID();
1325 static_cast<MSVehicle*
>(veh)->replaceVehicleType(type);
1332 const std::map<std::string, std::vector<MSLane*> >&
1334 if (gVTDMap.size() == 0) {
1336 for (std::vector<MSEdge*>::const_iterator i = edges.begin(); i != edges.end(); ++i) {
1337 const std::vector<MSLane*>& lanes = (*i)->getLanes();
1338 for (std::vector<MSLane*>::const_iterator j = lanes.begin(); j != lanes.end(); ++j) {
1339 if ((*j)->knowsParameter(
"origId")) {
1340 std::string origID = (*j)->getParameter(
"origId",
"");
1341 if (gVTDMap.find(origID) == gVTDMap.end()) {
1342 gVTDMap[origID] = std::vector<MSLane*>();
1344 gVTDMap[origID].push_back(*j);
1348 if (gVTDMap.size() == 0) {
1349 gVTDMap[
"unknown"] = std::vector<MSLane*>();