src/ATC/ground.cxx

   1 // FGGround - a class to provide ground control at larger airports.
   2 //
   3 // Written by David Luff, started March 2002.
   4 //
   5 // Copyright (C) 2002  David C. Luff - david.luff@nottingham.ac.uk
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  20
  21 #include <simgear/misc/sg_path.hxx>
  22 #include <simgear/math/sg_random.h>
  23 #include <simgear/debug/logstream.hxx>
  24 #include <simgear/misc/sgstream.hxx>
  25 #include <simgear/constants.h>
  26 #include <Main/globals.hxx>
  27
  28 #include <stdlib.h>
  29 #include STL_FSTREAM
  30
  31 #include "ground.hxx"
  32 #include "ATCutils.hxx"
  33
  34 SG_USING_STD(ifstream);
  35 SG_USING_STD(cout);
  36
  37 node::node() {
  38 }
  39
  40 node::~node() {
  41         for(unsigned int i=0; i < arcs.size(); ++i) {
  42                 delete arcs[i];
  43         }
  44 }
  45
  46 // Make sure that a_path.cost += distance is safe from the moment it's created.
  47 a_path::a_path() {
  48         cost = 0;
  49 }
  50
  51 FGGround::FGGround() {
  52         display = false;
  53         networkLoadOK = false;
  54 }
  55
  56 FGGround::FGGround(string id) {
  57         display = false;
  58         networkLoadOK = false;
  59         ident = id;
  60 }
  61
  62 FGGround::~FGGround() {
  63 }
  64
  65 void FGGround::ParseRwyExits(node* np, char* es) {
  66         char* token;
  67         char estr[20];
  68         strcpy(estr, es);
  69         const char delimiters[] = "-";
  70         token = strtok(estr, delimiters);
  71         while(token != NULL) {
  72                 int i = atoi(token);
  73                 //cout << "token = " << token << endl;
  74                 //cout << "rwy number = " << i << endl;
  75                 //runways[(atoi(token))].exits.push_back(np);
  76                 runways[i].exits.push_back(np);
  77                 //cout << "token = " << token << '\n';
  78                 token = strtok(NULL, delimiters);
  79         }
  80 }
  81
  82
  83 // Load the ground logical network of the current instances airport
  84 // Return true if successfull.
  85 // TODO - currently the file is assumed to reside in the base/ATC directory.
  86 // This might change to something more thought out in the future.
  87 // NOTE - currently it is assumed that all nodes are loaded before any arcs.
  88 // It won't work ATM if this doesn't hold true.
  89 bool FGGround::LoadNetwork() {
  90         node* np;
  91         arc* ap;
  92         Gate* gp;
  93
  94         int gateCount = 0;      // This is used to allocate gateID's from zero upwards
  95         // This may well change in the future - probably to reading in the real-world
  96         // gate numbers from file.
  97
  98         ifstream fin;
  99         SGPath path = globals->get_fg_root();
 100         //string taxiPath = "ATC/" + ident + ".taxi";
 101         string taxiPath = "ATC/KEMT.taxi";      // FIXME - HARDWIRED FOR TESTING
 102         path.append(taxiPath);
 103
 104         SG_LOG(SG_GENERAL, SG_INFO, "Trying to read taxiway data for " << ident << "...");
 105         //cout << "Trying to read taxiway data for " << ident << "..." << endl;
 106         fin.open(path.c_str(), ios::in);
 107         if(!fin) {
 108                 SG_LOG(SG_GENERAL, SG_ALERT, "Unable to open taxiway data input file " << path.c_str());
 109                 //cout << "Unable to open taxiway data input file " << path.c_str() << endl;
 110                 return(false);
 111         }
 112
 113         char ch;
 114         char buf[30];
 115         while(!fin.eof()) {
 116                 fin >> buf;
 117                 // Node, arc, or [End]?
 118                 //cout << "Read in ground network element type = " << buf << endl;
 119                 if(!strcmp(buf, "[End]")) {             // TODO - maybe make this more robust to spelling errors by just looking for '['
 120                         SG_LOG(SG_GENERAL, SG_INFO, "Done reading " << path.c_str() << endl);
 121                         break;
 122                 } else if(!strcmp(buf, "N")) {
 123                         // Node
 124                         np = new node;
 125                         np->struct_type = NODE;
 126                         fin >> buf;
 127                         np->nodeID = atoi(buf);
 128                         fin >> buf;
 129                         np->pos.setlon(atof(buf));
 130                         fin >> buf;
 131                         np->pos.setlat(atof(buf));
 132                         fin >> buf;
 133                         np->pos.setelev(atof(buf));
 134                         fin >> buf;             // node type
 135                         if(!strcmp(buf, "J")) {
 136                                 np->type = JUNCTION;
 137                         } else if(!strcmp(buf, "T")) {
 138                                 np->type = TJUNCTION;
 139                         } else if(!strcmp(buf, "H")) {
 140                                 np->type = HOLD;
 141                         } else {
 142                                 SG_LOG(SG_GENERAL, SG_ALERT, "**** ERROR ***** Unknown node type in taxi network...\n");
 143                                 delete np;
 144                                 return(false);
 145                         }
 146                         fin >> buf;             // rwy exit information - gets parsed later - FRAGILE - will break if buf is reused.
 147                         // Now the name
 148                         fin >> ch;      // strip the leading " off
 149                         np->name = "";
 150                         while(1) {
 151                                 fin.unsetf(ios::skipws);
 152                                 fin >> ch;
 153                                 if((ch == '"') || (ch == 0x0A)) {
 154                                         break;
 155                                 }   // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
 156                                 np->name += ch;
 157                         }
 158                         fin.setf(ios::skipws);
 159                         network.push_back(np);
 160                         // FIXME - fragile - replies on buf not getting modified from exits read to here
 161                         // see if we also need to push it onto the runway exit list
 162                         //cout << "strlen(buf) = " << strlen(buf) << endl;
 163                         if(strlen(buf) > 2) {
 164                                 //cout << "Calling ParseRwyExits for " << buf << endl;
 165                                 ParseRwyExits(np, buf);
 166                         }
 167                 } else if(!strcmp(buf, "A")) {
 168                         ap = new arc;
 169                         ap->struct_type = ARC;
 170                         fin >> buf;
 171                         ap->n1 = atoi(buf);
 172                         fin >> buf;
 173                         ap->n2 = atoi(buf);
 174                         fin >> buf;
 175                         if(!strcmp(buf, "R")) {
 176                                 ap->type = RUNWAY;
 177                         } else if(!strcmp(buf, "T")) {
 178                                 ap->type = TAXIWAY;
 179                         } else {
 180                                 SG_LOG(SG_GENERAL, SG_ALERT, "**** ERROR ***** Unknown arc type in taxi network...\n");
 181                                 delete ap;
 182                                 return(false);
 183                         }
 184                         // directed?
 185                         fin >> buf;
 186                         if(!strcmp(buf, "Y")) {
 187                                 ap->directed = true;
 188                         } else if(!strcmp(buf, "N")) {
 189                                 ap->directed = false;
 190                         } else {
 191                                 SG_LOG(SG_GENERAL, SG_ALERT, "**** ERROR ***** Unknown arc directed value in taxi network - should be Y/N !!!\n");
 192                                 delete ap;
 193                                 return(false);
 194                         }
 195                         // Now the name
 196                         ap->name = "";
 197                         while(1) {
 198                                 fin.unsetf(ios::skipws);
 199                                 fin >> ch;
 200                                 ap->name += ch;
 201                                 if((ch == '"') || (ch == 0x0A)) {
 202                                         break;
 203                                 }   // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
 204                         }
 205                         fin.setf(ios::skipws);
 206                         ap->distance = (int)dclGetHorizontalSeparation(network[ap->n1]->pos, network[ap->n2]->pos);
 207                         //cout << "Distance  = " << ap->distance << '\n';
 208                         network[ap->n1]->arcs.push_back(ap);
 209                         network[ap->n2]->arcs.push_back(ap);
 210                 } else if(!strcmp(buf, "G")) {
 211                         gp = new Gate;
 212                         gp->struct_type = NODE;
 213                         gp->type = GATE;
 214                         fin >> buf;
 215                         gp->nodeID = atoi(buf);
 216                         fin >> buf;
 217                         gp->pos.setlon(atof(buf));
 218                         fin >> buf;
 219                         gp->pos.setlat(atof(buf));
 220                         fin >> buf;
 221                         gp->pos.setelev(atof(buf));
 222                         fin >> buf;             // gate type - ignore this for now
 223                         fin >> buf;             // gate heading
 224                         gp->heading = atoi(buf);
 225                         // Now the name
 226                         gp->name = "";
 227                         while(1) {
 228                                 fin.unsetf(ios::skipws);
 229                                 fin >> ch;
 230                                 gp->name += ch;
 231                                 if((ch == '"') || (ch == 0x0A)) {
 232                                         break;
 233                                 }   // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
 234                         }
 235                         fin.setf(ios::skipws);
 236                         gp->id = gateCount;             // Warning - this will likely change in the future.
 237                         gp->used = false;
 238                         network.push_back(gp);
 239                         gates[gateCount] = gp;
 240                         gateCount++;
 241                 } else {
 242                         // Something has gone seriously pear-shaped
 243                         SG_LOG(SG_GENERAL, SG_ALERT, "********* ERROR - unknown ground network element type... aborting read of " << path.c_str() << '\n');
 244                         return(false);
 245                 }
 246
 247                 fin >> skipeol;
 248         }
 249         return(true);
 250 }
 251
 252 void FGGround::Init() {
 253         display = false;
 254
 255         // For now we'll hardwire the threshold end
 256         Point3D P010(-118.037483, 34.081358, 296 * SG_FEET_TO_METER);
 257         double hdg = 25.32;
 258         ortho.Init(P010, hdg);
 259
 260         networkLoadOK = LoadNetwork();
 261 }
 262
 263 void FGGround::Update() {
 264         // Each time step, what do we need to do?
 265         // We need to go through the list of outstanding requests and acknowedgements
 266         // and process at least one of them.
 267         // We need to go through the list of planes under our control and check if
 268         // any need to be addressed.
 269         // We need to check for planes not under our control coming within our
 270         // control area and address if necessary.
 271
 272         // Lets take the example of a plane which has just contacted ground
 273         // following landing - presumably requesting where to go?
 274         // First we need to establish the position of the plane within the logical network.
 275         // Next we need to decide where its going.
 276 }
 277
 278 // Return a random gate ID of an unused gate.
 279 // Two error values may be returned and must be checked for by the calling function:
 280 // -2 signifies that no gates exist at this airport.
 281 // -1 signifies that all gates are currently full.
 282 int FGGround::GetRandomGateID() {
 283         // Check that this airport actually has some gates!!
 284         if(!gates.size()) {
 285                 return(-2);
 286         }
 287
 288         gate_vec_type gateVec;
 289         int num = 0;
 290         int thenum;
 291         int ID;
 292
 293         gatesItr = gates.begin();
 294         while(gatesItr != gates.end()) {
 295                 if((gatesItr->second)->used == false) {
 296                         gateVec.push_back(gatesItr->second);
 297                         num++;
 298                 }
 299                 ++gatesItr;
 300         }
 301
 302         // Check that there are some unused gates!
 303         if(!gateVec.size()) {
 304                 return(-1);
 305         }
 306
 307         // Randomly select one from the list
 308         sg_srandom_time();
 309         thenum = (int)(sg_random() * gateVec.size());
 310         ID = gateVec[thenum]->id;
 311
 312         return(ID);
 313 }
 314
 315 // Return a pointer to an unused gate node
 316 Gate* FGGround::GetGateNode() {
 317         int id = GetRandomGateID();
 318         if(id < 0) {
 319                 return(NULL);
 320         } else {
 321                 return(gates[id]);
 322         }
 323 }
 324
 325
 326 // WARNING - This is hardwired to my prototype logical network format
 327 // and will almost certainly change when Bernie's stuff comes on-line.
 328 node* FGGround::GetThresholdNode(string rwyID) {
 329         // For now go through all the nodes and parse their names
 330         // Maybe in the future we'll map threshold nodes by ID
 331         //cout << "Size of network is " << network.size() << '\n';
 332         for(unsigned int i=0; i<network.size(); ++i) {
 333                 //cout << "Name = " << network[i]->name << '\n';
 334                 if(network[i]->name.size()) {
 335                         string s = network[i]->name;
 336                         // Warning - the next bit is fragile and dependent on my current naming scheme
 337                         //cout << "substr = " << s.substr(0,3) << '\n';
 338                         //cout << "size of s = " << s.size() << '\n';
 339                         if(s.substr(0,3) == "rwy") {
 340                                 //cout << "subsubstr = " << s.substr(4, s.size() - 4) << '\n';
 341                                 if(s.substr(4, s.size() - 4) == rwyID) {
 342                                         return network[i];
 343                                 }
 344                         }
 345                 }
 346         }
 347         return NULL;
 348 }
 349
 350 // Get a path from a point on a runway to a gate
 351 // TODO !!
 352
 353 // Get a path from a node to another node
 354 // Eventually we will need complex algorithms for this taking other traffic,
 355 // shortest path and suitable paths into accout.
 356 // For now we'll just call the shortest path algorithm.
 357 ground_network_path_type FGGround::GetPath(node* A, node* B) {
 358         return(GetShortestPath(A, B));
 359 };
 360
 361 // Get a path from a node to a runway threshold
 362 ground_network_path_type FGGround::GetPath(node* A, string rwyID) {
 363         node* b = GetThresholdNode(rwyID);
 364         if(b == NULL) {
 365                 SG_LOG(SG_GENERAL, SG_ALERT, "ERROR - unable to find path to runway theshold in ground.cxx\n");
 366                 ground_network_path_type emptyPath;
 367                 emptyPath.erase(emptyPath.begin(), emptyPath.end());
 368                 return(emptyPath);
 369         }
 370         return GetShortestPath(A, b);
 371 }
 372
 373 // A shortest path algorithm from memory (ie. I can't find the bl&*dy book again!)
 374 // I'm sure there must be enchancements that we can make to this, such as biasing the
 375 // order in which the nodes are searched out from in favour of those geographically
 376 // closer to the destination.
 377 // Note that we are working with the master set of nodes and arcs so we mustn't change
 378 // or delete them -  we only delete the paths that we create during the algorithm.
 379 ground_network_path_type FGGround::GetShortestPath(node* A, node* B) {
 380         a_path* pathPtr;
 381         shortest_path_map_type pathMap;
 382         node_array_type nodesLeft;
 383
 384         // Debugging check
 385         int pathsCreated = 0;
 386
 387         // Initialise the algorithm
 388         nodesLeft.push_back(A);
 389         pathPtr = new a_path;
 390         pathsCreated++;
 391         pathPtr->path.push_back(A);
 392         pathPtr->cost = 0;
 393         pathMap[A->nodeID] = pathPtr;
 394         bool solution_found = false;    // Flag to indicate that at least one candidate path has been found
 395         int solution_cost = -1;                 // Cost of current best cost solution.  -1 indicates no solution found yet.
 396         a_path solution_path;
 397
 398         node* nPtr;     // nPtr is used to point to the node we are currently working with
 399
 400         while(nodesLeft.size()) {
 401                 //cout << "\n*****nodesLeft*****\n";
 402                 //for(unsigned int i=0; i<nodesLeft.size(); ++i) {
 403                         //cout << nodesLeft[i]->nodeID << '\n';
 404                 //}
 405                 //cout << "*******************\n\n";
 406                 nPtr = *nodesLeft.begin();              // Thought - definate optimization possibilities here in the choice of which nodes we process first.
 407                 nodesLeft.erase(nodesLeft.begin());
 408                 //cout << "Walking out from node " << nPtr->nodeID << '\n';
 409                 for(unsigned int i=0; i<nPtr->arcs.size(); ++i) {
 410                         //cout << "ARC TO " << ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1) << '\n';
 411                 }
 412                 if((solution_found) && (solution_cost <= pathMap[nPtr->nodeID]->cost)) {
 413                         // Do nothing - we've already found a solution and this partial path is already more expensive
 414                 } else {
 415                         // This path could still be better than the current solution - check it out
 416                         for(unsigned int i=0; i<(nPtr->arcs.size()); i++) {
 417                                 // Map the new path against the end node, ie. *not* the one we just started with.
 418                                 unsigned int end_nodeID = ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1);
 419                                 //cout << "end_nodeID = " << end_nodeID << '\n';
 420                                 //cout << "pathMap size is " << pathMap.size() << '\n';
 421                                 if(end_nodeID == nPtr->nodeID) {
 422                                         //cout << "Circular arc!\n";
 423                                         // Then its a circular arc - don't bother!!
 424                                         //nPtr->arcs.erase(nPtr->arcs.begin() + i);
 425                                 } else {
 426                                         // see if the end node is already in the map or not
 427                                         if(pathMap.find(end_nodeID) == pathMap.end()) {
 428                                                 //cout << "Not in the map" << endl;;
 429                                                 // Not in the map - easy!
 430                                                 pathPtr = new a_path;
 431                                                 pathsCreated++;
 432                                                 *pathPtr = *pathMap[nPtr->nodeID];      // *copy* the path
 433                                                 pathPtr->path.push_back(nPtr->arcs[i]);
 434                                                 pathPtr->path.push_back(network[end_nodeID]);
 435                                                 pathPtr->cost += nPtr->arcs[i]->distance;
 436                                                 pathMap[end_nodeID] = pathPtr;
 437                                                 nodesLeft.push_back(network[end_nodeID]);       // By definition this can't be in the list already, or
 438                                                 // it would also have been in the map and hence OR'd with this one.
 439                                                 if(end_nodeID == B->nodeID) {
 440                                                         //cout << "Solution found!!!" << endl;
 441                                                         // Since this node wasn't in the map this is by definition the first solution
 442                                                         solution_cost = pathPtr->cost;
 443                                                         solution_path = *pathPtr;
 444                                                         solution_found = true;
 445                                                 }
 446                                         } else {
 447                                                 //cout << "Already in the map" << endl;
 448                                                 // In the map - not so easy - need to get rid of an arc from the higher cost one.
 449                                                 //cout << "Current cost of node " << end_nodeID << " is " << pathMap[end_nodeID]->cost << endl;
 450                                                 int newCost = pathMap[nPtr->nodeID]->cost + nPtr->arcs[i]->distance;
 451                                                 //cout << "New cost is of node " << nPtr->nodeID << " is " << newCost << endl;
 452                                                 if(newCost >= pathMap[end_nodeID]->cost) {
 453                                                         // No need to do anything.
 454                                                         //cout << "Not doing anything!" << endl;
 455                                                 } else {
 456                                                         delete pathMap[end_nodeID];
 457                                                         pathsCreated--;
 458
 459                                                         pathPtr = new a_path;
 460                                                         pathsCreated++;
 461                                                         *pathPtr = *pathMap[nPtr->nodeID];      // *copy* the path
 462                                                         pathPtr->path.push_back(nPtr->arcs[i]);
 463                                                         pathPtr->path.push_back(network[end_nodeID]);
 464                                                         pathPtr->cost += nPtr->arcs[i]->distance;
 465                                                         pathMap[end_nodeID] = pathPtr;
 466
 467                                                         // We need to add this node to the list-to-do again to force a recalculation
 468                                                         // onwards from this node with the new lower cost to node cost.
 469                                                         nodesLeft.push_back(network[end_nodeID]);
 470
 471                                                         if(end_nodeID == B->nodeID) {
 472                                                                 //cout << "Solution found!!!" << endl;
 473                                                                 // Need to check if there is a previous better solution
 474                                                                 if((solution_cost < 0) || (pathPtr->cost < solution_cost)) {
 475                                                                         solution_cost = pathPtr->cost;
 476                                                                         solution_path = *pathPtr;
 477                                                                         solution_found = true;
 478                                                                 }
 479                                                         }
 480                                                 }
 481                                         }
 482                                 }
 483                         }
 484                 }
 485         }
 486
 487         // delete all the paths before returning
 488         shortest_path_map_iterator spItr = pathMap.begin();
 489         while(spItr != pathMap.end()) {
 490                 if(spItr->second != NULL) {
 491                         delete spItr->second;
 492                         --pathsCreated;
 493                 }
 494                 ++spItr;
 495         }
 496
 497         //cout << "pathsCreated = " << pathsCreated << '\n';
 498         if(pathsCreated > 0) {
 499                 SG_LOG(SG_GENERAL, SG_ALERT, "WARNING - Possible memory leak in FGGround::GetShortestPath\n\
 500                                                                           Please report to flightgear-devel@flightgear.org\n");
 501         }
 502
 503         //cout << (solution_found ? "Result: solution found\n" : "Result: no solution found\n");
 504         return(solution_path.path);             // TODO - we really ought to have a fallback position incase a solution isn't found.
 505 }
 506
 507
 508
 509 // Randomly or otherwise populate some of the gates with parked planes
 510 // (might eventually be done by the AIMgr if and when lots of AI traffic is generated)
 511
 512 // Return a list of exits from a given runway
 513 // It is up to the calling function to check for non-zero size of returned array before use
 514 node_array_type FGGround::GetExits(int rwyID) {
 515         return(runways[rwyID].exits);
 516 }
 517
 518 #if 0
 519 void FGGround::NewArrival(plane_rec plane) {
 520         // What are we going to do here?
 521         // We need to start a new ground_rec and add the plane_rec to it
 522         // We need to decide what gate we are going to clear it to.
 523         // Then we need to add clearing it to that gate to the pending transmissions queue? - or simply transmit?
 524         // Probably simply transmit for now and think about a transmission queue later if we need one.
 525         // We might need one though in order to add a little delay for response time.
 526         ground_rec* g = new ground_rec;
 527         g->plane_rec = plane;
 528         g->current_pos = ConvertWGS84ToXY(plane.pos);
 529         g->node = GetNode(g->current_pos);  // TODO - might need to sort out node/arc here
 530         AssignGate(g);
 531         g->cleared = false;
 532         ground_traffic.push_back(g);
 533         NextClearance(g);
 534 }
 535
 536 void FGGround::NewContact(plane_rec plane) {
 537         // This is a bit of a convienience function at the moment and is likely to change.
 538         if(at a gate or apron)
 539                 NewDeparture(plane);
 540         else
 541                 NewArrival(plane);
 542 }
 543
 544 void FGGround::NextClearance(ground_rec &g) {
 545         // Need to work out where we can clear g to.
 546         // Assume the pilot doesn't need progressive instructions
 547         // We *should* already have a gate or holding point assigned by the time we get here
 548         // but it wouldn't do any harm to check.
 549
 550         // For now though we will hardwire it to clear to the final destination.
 551 }
 552
 553 void FGGround::AssignGate(ground_rec &g) {
 554         // We'll cheat for now - since we only have the user's aircraft and a couple of airports implemented
 555         // we'll hardwire the gate!
 556         // In the long run the logic of which gate or area to send the plane to could be somewhat non-trivial.
 557 }
 558 #endif //0
 559