// FGGround - a class to provide ground control at larger airports. // // Written by David Luff, started March 2002. // // Copyright (C) 2002 David C. Luff - david.luff@nottingham.ac.uk // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License as // published by the Free Software Foundation; either version 2 of the // License, or (at your option) any later version. // // This program is distributed in the hope that it will be useful, but // WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. #include #include #include #include #include #include
#include #include STL_FSTREAM #include "ground.hxx" #include "ATCutils.hxx" SG_USING_STD(ifstream); SG_USING_STD(cout); node::node() { } node::~node() { for(unsigned int i=0; i < arcs.size(); ++i) { delete arcs[i]; } } // Make sure that a_path.cost += distance is safe from the moment it's created. a_path::a_path() { cost = 0; } FGGround::FGGround() { display = false; networkLoadOK = false; } FGGround::~FGGround() { } void FGGround::ParseRwyExits(node* np, char* es) { char* token; char estr[20]; strcpy(estr, es); const char delimiters[] = "-"; token = strtok(estr, delimiters); while(token != NULL) { int i = atoi(token); //cout << "token = " << token << endl; //cout << "rwy number = " << i << endl; //runways[(atoi(token))].exits.push_back(np); runways[i].exits.push_back(np); //cout << "token = " << token << '\n'; token = strtok(NULL, delimiters); } } // Load the ground logical network of the current instances airport // Return true if successfull. // TODO - currently the file is assumed to reside in the base/ATC directory. // This might change to something more thought out in the future. // NOTE - currently it is assumed that all nodes are loaded before any arcs. // It won't work ATM if this doesn't hold true. bool FGGround::LoadNetwork() { node* np; arc* ap; Gate* gp; int gateCount = 0; // This is used to allocate gateID's from zero upwards // This may well change in the future - probably to reading in the real-world // gate numbers from file. ifstream fin; SGPath path = globals->get_fg_root(); //string taxiPath = "ATC/" + ident + ".taxi"; string taxiPath = "ATC/KEMT.taxi"; // FIXME - HARDWIRED FOR TESTING path.append(taxiPath); SG_LOG(SG_GENERAL, SG_INFO, "Trying to read taxiway data for " << ident << "..."); //cout << "Trying to read taxiway data for " << ident << "..." << endl; fin.open(path.c_str(), ios::in); if(!fin) { SG_LOG(SG_GENERAL, SG_ALERT, "Unable to open taxiway data input file " << path.c_str()); //cout << "Unable to open taxiway data input file " << path.c_str() << endl; return(false); } char ch; char buf[30]; while(!fin.eof()) { fin >> buf; // Node, arc, or [End]? //cout << "Read in ground network element type = " << buf << endl; if(!strcmp(buf, "[End]")) { // TODO - maybe make this more robust to spelling errors by just looking for '[' SG_LOG(SG_GENERAL, SG_INFO, "Done reading " << path.c_str() << endl); break; } else if(!strcmp(buf, "N")) { // Node np = new node; np->struct_type = NODE; fin >> buf; np->nodeID = atoi(buf); fin >> buf; np->pos.setlon(atof(buf)); fin >> buf; np->pos.setlat(atof(buf)); fin >> buf; np->pos.setelev(atof(buf)); fin >> buf; // node type if(!strcmp(buf, "J")) { np->type = JUNCTION; } else if(!strcmp(buf, "T")) { np->type = TJUNCTION; } else if(!strcmp(buf, "H")) { np->type = HOLD; } else { cout << "**** ERROR ***** Unknown node type in taxi network...\n"; delete np; return(false); } fin >> buf; // rwy exit information - gets parsed later - FRAGILE - will break if buf is reused. // Now the name np->name = ""; while(1) { fin.unsetf(ios::skipws); fin >> ch; np->name += ch; if((ch == '"') || (ch == 0x0A)) { break; } // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the " } fin.setf(ios::skipws); network.push_back(np); // FIXME - fragile - replies on buf not getting modified from exits read to here // see if we also need to push it onto the runway exit list cout << "strlen(buf) = " << strlen(buf) << endl; if(strlen(buf) > 2) { cout << "Calling ParseRwyExits for " << buf << endl; ParseRwyExits(np, buf); } } else if(!strcmp(buf, "A")) { ap = new arc; ap->struct_type = ARC; fin >> buf; ap->n1 = atoi(buf); fin >> buf; ap->n2 = atoi(buf); fin >> buf; if(!strcmp(buf, "R")) { ap->type = RUNWAY; } else if(!strcmp(buf, "T")) { ap->type = TAXIWAY; } else { cout << "**** ERROR ***** Unknown arc type in taxi network...\n"; delete ap; return(false); } // directed? fin >> buf; if(!strcmp(buf, "Y")) { ap->directed = true; } else if(!strcmp(buf, "N")) { ap->directed = false; } else { cout << "**** ERROR ***** Unknown arc directed value in taxi network - should be Y/N !!!\n"; delete ap; return(false); } // Now the name ap->name = ""; while(1) { fin.unsetf(ios::skipws); fin >> ch; ap->name += ch; if((ch == '"') || (ch == 0x0A)) { break; } // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the " } fin.setf(ios::skipws); ap->distance = (int)dclGetHorizontalSeparation(network[ap->n1]->pos, network[ap->n2]->pos); cout << "Distance = " << ap->distance << '\n'; network[ap->n1]->arcs.push_back(ap); network[ap->n2]->arcs.push_back(ap); } else if(!strcmp(buf, "G")) { gp = new Gate; gp->struct_type = NODE; gp->type = GATE; fin >> buf; gp->nodeID = atoi(buf); fin >> buf; gp->pos.setlon(atof(buf)); fin >> buf; gp->pos.setlat(atof(buf)); fin >> buf; gp->pos.setelev(atof(buf)); fin >> buf; // gate type - ignore this for now fin >> buf; // gate heading gp->heading = atoi(buf); // Now the name gp->name = ""; while(1) { fin.unsetf(ios::skipws); fin >> ch; gp->name += ch; if((ch == '"') || (ch == 0x0A)) { break; } // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the " } fin.setf(ios::skipws); gp->id = gateCount; // Warning - this will likely change in the future. gp->used = false; network.push_back(gp); gates[gateCount] = gp; gateCount++; } else { // Something has gone seriously pear-shaped cout << "********* ERROR - unknown ground network element type... aborting read of " << path.c_str() << '\n'; return(false); } fin >> skipeol; } return(true); } void FGGround::Init() { display = false; // For now we'll hardwire the threshold end Point3D P010(-118.037483, 34.081358, 296 * SG_FEET_TO_METER); double hdg = 25.32; ortho.Init(P010, hdg); networkLoadOK = LoadNetwork(); } void FGGround::Update() { // Each time step, what do we need to do? // We need to go through the list of outstanding requests and acknowedgements // and process at least one of them. // We need to go through the list of planes under our control and check if // any need to be addressed. // We need to check for planes not under our control coming within our // control area and address if necessary. // Lets take the example of a plane which has just contacted ground // following landing - presumably requesting where to go? // First we need to establish the position of the plane within the logical network. // Next we need to decide where its going. } // Return a random gate ID of an unused gate. // Two error values may be returned and must be checked for by the calling function: // -2 signifies that no gates exist at this airport. // -1 signifies that all gates are currently full. int FGGround::GetRandomGateID() { // Check that this airport actually has some gates!! if(!gates.size()) { return(-2); } gate_vec_type gateVec; int num = 0; int thenum; int ID; gatesItr = gates.begin(); while(gatesItr != gates.end()) { if((gatesItr->second)->used == false) { gateVec.push_back(gatesItr->second); num++; } ++gatesItr; } // Check that there are some unused gates! if(!gateVec.size()) { return(-1); } // Randomly select one from the list sg_srandom_time(); thenum = (int)(sg_random() * gateVec.size()); ID = gateVec[thenum]->id; return(ID); } // Return a pointer to an unused gate node Gate* FGGround::GetGateNode() { int id = GetRandomGateID(); if(id < 0) { return(NULL); } else { return(gates[id]); } } // Get a path from a point on a runway to a gate // TODO !! // Get a path from a node to another node // Eventually we will need complex algorithms for this taking other traffic, // shortest path and suitable paths into accout. // For now we'll just call the shortest path algorithm. ground_network_path_type FGGround::GetPath(node* A, node* B) { return(GetShortestPath(A, B)); }; // A shortest path algorithm from memory (ie. I can't find the bl&*dy book again!) // I'm sure there must be enchancements that we can make to this, such as biasing the // order in which the nodes are searched out from in favour of those geographically // closer to the destination. // Note that we are working with the master set of nodes and arcs so we mustn't change // or delete them - we only delete the paths that we create during the algorithm. ground_network_path_type FGGround::GetShortestPath(node* A, node* B) { a_path* pathPtr; shortest_path_map_type pathMap; node_array_type nodesLeft; // Debugging check int pathsCreated = 0; // Initialise the algorithm nodesLeft.push_back(A); pathPtr = new a_path; pathsCreated++; pathPtr->path.push_back(A); pathPtr->cost = 0; pathMap[A->nodeID] = pathPtr; bool solution_found = false; // Flag to indicate that at least one candidate path has been found int solution_cost = -1; // Cost of current best cost solution. -1 indicates no solution found yet. a_path solution_path; node* nPtr; // nPtr is used to point to the node we are currently working with while(nodesLeft.size()) { //cout << "\n*****nodesLeft*****\n"; //for(unsigned int i=0; inodeID << '\n'; //} //cout << "*******************\n\n"; nPtr = *nodesLeft.begin(); // Thought - definate optimization possibilities here in the choice of which nodes we process first. nodesLeft.erase(nodesLeft.begin()); //cout << "Walking out from node " << nPtr->nodeID << '\n'; for(unsigned int i=0; iarcs.size(); ++i) { //cout << "ARC TO " << ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1) << '\n'; } if((solution_found) && (solution_cost <= pathMap[nPtr->nodeID]->cost)) { // Do nothing - we've already found a solution and this partial path is already more expensive } else { // This path could still be better than the current solution - check it out for(unsigned int i=0; i<(nPtr->arcs.size()); i++) { // Map the new path against the end node, ie. *not* the one we just started with. unsigned int end_nodeID = ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1); //cout << "end_nodeID = " << end_nodeID << '\n'; //cout << "pathMap size is " << pathMap.size() << '\n'; if(end_nodeID == nPtr->nodeID) { //cout << "Circular arc!\n"; // Then its a circular arc - don't bother!! //nPtr->arcs.erase(nPtr->arcs.begin() + i); } else { // see if the end node is already in the map or not if(pathMap.find(end_nodeID) == pathMap.end()) { //cout << "Not in the map" << endl;; // Not in the map - easy! pathPtr = new a_path; pathsCreated++; *pathPtr = *pathMap[nPtr->nodeID]; // *copy* the path pathPtr->path.push_back(nPtr->arcs[i]); pathPtr->path.push_back(network[end_nodeID]); pathPtr->cost += nPtr->arcs[i]->distance; pathMap[end_nodeID] = pathPtr; nodesLeft.push_back(network[end_nodeID]); // By definition this can't be in the list already, or // it would also have been in the map and hence OR'd with this one. if(end_nodeID == B->nodeID) { //cout << "Solution found!!!" << endl; // Since this node wasn't in the map this is by definition the first solution solution_cost = pathPtr->cost; solution_path = *pathPtr; solution_found = true; } } else { //cout << "Already in the map" << endl; // In the map - not so easy - need to get rid of an arc from the higher cost one. //cout << "Current cost of node " << end_nodeID << " is " << pathMap[end_nodeID]->cost << endl; int newCost = pathMap[nPtr->nodeID]->cost + nPtr->arcs[i]->distance; //cout << "New cost is of node " << nPtr->nodeID << " is " << newCost << endl; if(newCost >= pathMap[end_nodeID]->cost) { // No need to do anything. //cout << "Not doing anything!" << endl; } else { delete pathMap[end_nodeID]; pathsCreated--; pathPtr = new a_path; pathsCreated++; *pathPtr = *pathMap[nPtr->nodeID]; // *copy* the path pathPtr->path.push_back(nPtr->arcs[i]); pathPtr->path.push_back(network[end_nodeID]); pathPtr->cost += nPtr->arcs[i]->distance; pathMap[end_nodeID] = pathPtr; // We need to add this node to the list-to-do again to force a recalculation // onwards from this node with the new lower cost to node cost. nodesLeft.push_back(network[end_nodeID]); if(end_nodeID == B->nodeID) { //cout << "Solution found!!!" << endl; // Need to check if there is a previous better solution if((solution_cost < 0) || (pathPtr->cost < solution_cost)) { solution_cost = pathPtr->cost; solution_path = *pathPtr; solution_found = true; } } } } } } } } // delete all the paths before returning shortest_path_map_iterator spItr = pathMap.begin(); while(spItr != pathMap.end()) { if(spItr->second != NULL) { delete spItr->second; --pathsCreated; } ++spItr; } //cout << "pathsCreated = " << pathsCreated << '\n'; if(pathsCreated > 0) { SG_LOG(SG_GENERAL, SG_ALERT, "WARNING - Possible memory leak in FGGround::GetShortestPath\n\ Please report to flightgear-devel@flightgear.org\n"); } //cout << (solution_found ? "Result: solution found\n" : "Result: no solution found\n"); return(solution_path.path); // TODO - we really ought to have a fallback position incase a solution isn't found. } // Randomly or otherwise populate some of the gates with parked planes // (might eventually be done by the AIMgr if and when lots of AI traffic is generated) // Return a list of exits from a given runway // It is up to the calling function to check for non-zero size of returned array before use node_array_type FGGround::GetExits(int rwyID) { return(runways[rwyID].exits); } #if 0 void FGGround::NewArrival(plane_rec plane) { // What are we going to do here? // We need to start a new ground_rec and add the plane_rec to it // We need to decide what gate we are going to clear it to. // Then we need to add clearing it to that gate to the pending transmissions queue? - or simply transmit? // Probably simply transmit for now and think about a transmission queue later if we need one. // We might need one though in order to add a little delay for response time. ground_rec* g = new ground_rec; g->plane_rec = plane; g->current_pos = ConvertWGS84ToXY(plane.pos); g->node = GetNode(g->current_pos); // TODO - might need to sort out node/arc here AssignGate(g); g->cleared = false; ground_traffic.push_back(g); NextClearance(g); } void FGGround::NewContact(plane_rec plane) { // This is a bit of a convienience function at the moment and is likely to change. if(at a gate or apron) NewDeparture(plane); else NewArrival(plane); } void FGGround::NextClearance(ground_rec &g) { // Need to work out where we can clear g to. // Assume the pilot doesn't need progressive instructions // We *should* already have a gate or holding point assigned by the time we get here // but it wouldn't do any harm to check. // For now though we will hardwire it to clear to the final destination. } void FGGround::AssignGate(ground_rec &g) { // We'll cheat for now - since we only have the user's aircraft and a couple of airports implemented // we'll hardwire the gate! // In the long run the logic of which gate or area to send the plane to could be somewhat non-trivial. } #endif //0