1 // FGGround - a class to provide ground control at larger airports.
3 // Written by David Luff, started March 2002.
5 // Copyright (C) 2002 David C. Luff - david.luff@nottingham.ac.uk
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.
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.
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.
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>
33 #include "ATCutils.hxx"
34 #include "ATCdisplay.hxx"
35 #include "AILocalTraffic.hxx"
37 SG_USING_STD(ifstream);
44 for(unsigned int i=0; i < arcs.size(); ++i) {
49 // Make sure that a_path.cost += distance is safe from the moment it's created.
54 FGGround::FGGround() {
56 networkLoadOK = false;
57 ground_traffic.erase(ground_traffic.begin(), ground_traffic.end());
58 ground_traffic_itr = ground_traffic.begin();
61 FGGround::FGGround(string id) {
63 networkLoadOK = false;
64 ground_traffic.erase(ground_traffic.begin(), ground_traffic.end());
65 ground_traffic_itr = ground_traffic.begin();
69 FGGround::~FGGround() {
72 void FGGround::ParseRwyExits(node* np, char* es) {
76 const char delimiters[] = "-";
77 token = strtok(estr, delimiters);
78 while(token != NULL) {
80 //cout << "token = " << token << endl;
81 //cout << "rwy number = " << i << endl;
82 //runways[(atoi(token))].exits.push_back(np);
83 runways[i].exits.push_back(np);
84 //cout << "token = " << token << '\n';
85 token = strtok(NULL, delimiters);
90 // Load the ground logical network of the current instances airport
91 // Return true if successfull.
92 // TODO - currently the file is assumed to reside in the base/ATC directory.
93 // This might change to something more thought out in the future.
94 // NOTE - currently it is assumed that all nodes are loaded before any arcs.
95 // It won't work ATM if this doesn't hold true.
96 bool FGGround::LoadNetwork() {
101 int gateCount = 0; // This is used to allocate gateID's from zero upwards
102 // This may well change in the future - probably to reading in the real-world
103 // gate numbers from file.
106 SGPath path = globals->get_fg_root();
107 //string taxiPath = "ATC/" + ident + ".taxi";
108 string taxiPath = "ATC/KEMT.taxi"; // FIXME - HARDWIRED FOR TESTING
109 path.append(taxiPath);
111 SG_LOG(SG_ATC, SG_INFO, "Trying to read taxiway data for " << ident << "...");
112 //cout << "Trying to read taxiway data for " << ident << "..." << endl;
113 fin.open(path.c_str(), ios::in);
115 SG_LOG(SG_ATC, SG_ALERT, "Unable to open taxiway data input file " << path.c_str());
116 //cout << "Unable to open taxiway data input file " << path.c_str() << endl;
124 // Node, arc, or [End]?
125 //cout << "Read in ground network element type = " << buf << endl;
126 if(!strcmp(buf, "[End]")) { // TODO - maybe make this more robust to spelling errors by just looking for '['
127 SG_LOG(SG_ATC, SG_INFO, "Done reading " << path.c_str() << endl);
129 } else if(!strcmp(buf, "N")) {
132 np->struct_type = NODE;
134 np->nodeID = atoi(buf);
136 np->pos.setlon(atof(buf));
138 np->pos.setlat(atof(buf));
140 np->pos.setelev(atof(buf));
141 fin >> buf; // node type
142 if(!strcmp(buf, "J")) {
144 } else if(!strcmp(buf, "T")) {
145 np->type = TJUNCTION;
146 } else if(!strcmp(buf, "H")) {
149 SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown node type in taxi network...\n");
153 fin >> buf; // rwy exit information - gets parsed later - FRAGILE - will break if buf is reused.
155 fin >> ch; // strip the leading " off
158 fin.unsetf(ios::skipws);
160 if((ch == '"') || (ch == 0x0A)) {
162 } // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
165 fin.setf(ios::skipws);
166 network.push_back(np);
167 // FIXME - fragile - replies on buf not getting modified from exits read to here
168 // see if we also need to push it onto the runway exit list
169 //cout << "strlen(buf) = " << strlen(buf) << endl;
170 if(strlen(buf) > 2) {
171 //cout << "Calling ParseRwyExits for " << buf << endl;
172 ParseRwyExits(np, buf);
174 } else if(!strcmp(buf, "A")) {
176 ap->struct_type = ARC;
182 if(!strcmp(buf, "R")) {
184 } else if(!strcmp(buf, "T")) {
187 SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown arc type in taxi network...\n");
193 if(!strcmp(buf, "Y")) {
195 } else if(!strcmp(buf, "N")) {
196 ap->directed = false;
198 SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown arc directed value in taxi network - should be Y/N !!!\n");
205 fin.unsetf(ios::skipws);
208 if((ch == '"') || (ch == 0x0A)) {
210 } // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
212 fin.setf(ios::skipws);
213 ap->distance = (int)dclGetHorizontalSeparation(network[ap->n1]->pos, network[ap->n2]->pos);
214 //cout << "Distance = " << ap->distance << '\n';
215 network[ap->n1]->arcs.push_back(ap);
216 network[ap->n2]->arcs.push_back(ap);
217 } else if(!strcmp(buf, "G")) {
219 gp->struct_type = NODE;
222 gp->nodeID = atoi(buf);
224 gp->pos.setlon(atof(buf));
226 gp->pos.setlat(atof(buf));
228 gp->pos.setelev(atof(buf));
229 fin >> buf; // gate type - ignore this for now
230 fin >> buf; // gate heading
231 gp->heading = atoi(buf);
235 fin.unsetf(ios::skipws);
238 if((ch == '"') || (ch == 0x0A)) {
240 } // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
242 fin.setf(ios::skipws);
243 gp->id = gateCount; // Warning - this will likely change in the future.
245 network.push_back(gp);
246 gates[gateCount] = gp;
249 // Something has gone seriously pear-shaped
250 SG_LOG(SG_ATC, SG_ALERT, "********* ERROR - unknown ground network element type... aborting read of " << path.c_str() << '\n');
259 void FGGround::Init() {
262 //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
263 // For now we'll hardwire the threshold end FIXME FIXME FIXME - use actual active rwy
264 Point3D P010(-118.037483, 34.081358, 296 * SG_FEET_TO_METER);
266 ortho.Init(P010, hdg);
267 // FIXME TODO FIXME TODO
268 // TODO FIXME TODO FIXME
269 //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
271 networkLoadOK = LoadNetwork();
274 void FGGround::Update(double dt) {
275 // Each time step, what do we need to do?
276 // We need to go through the list of outstanding requests and acknowedgements
277 // and process at least one of them.
278 // We need to go through the list of planes under our control and check if
279 // any need to be addressed.
280 // We need to check for planes not under our control coming within our
281 // control area and address if necessary.
283 // Lets take the example of a plane which has just contacted ground
284 // following landing - presumably requesting where to go?
285 // First we need to establish the position of the plane within the logical network.
286 // Next we need to decide where its going.
288 if(ground_traffic.size()) {
289 if(ground_traffic_itr == ground_traffic.end()) {
290 ground_traffic_itr = ground_traffic.begin();
293 //Process(*ground_traffic_itr);
294 GroundRec* g = *ground_traffic_itr;
295 if(g->taxiRequestOutstanding) {
296 double responseTime = 10.0; // seconds - this should get more sophisticated at some point
297 if(g->clearanceCounter > responseTime) {
299 // TODO - move the mechanics of making up the transmission out of the main Update(...) routine.
301 trns += g->plane.callsign;
302 trns += " taxi holding point runway "; // TODO - add the holding point name
303 // eg " taxi holding point G2 runway "
306 globals->get_ATC_display()->RegisterSingleMessage(trns, 0);
308 g->planePtr->RegisterTransmission(1); // cleared to taxi
309 g->clearanceCounter = 0.0;
310 g->taxiRequestOutstanding = false;
312 g->clearanceCounter += (dt * ground_traffic.size());
314 } else if(((FGAILocalTraffic*)(g->planePtr))->AtHoldShort()) { // That's a hack - eventually we should monitor actual position
315 // HACK ALERT - the automatic cast to AILocalTraffic has to go once we have other sorts working!!!!! FIXME TODO
316 // NOTE - we don't need to do the contact tower bit unless we have separate tower and ground
317 string trns = g->plane.callsign;
318 trns += " contact Tower ";
319 double f = globals->get_ATC_mgr()->GetFrequency(ident, TOWER);
321 sprintf(buf, "%f", f);
324 globals->get_ATC_display()->RegisterSingleMessage(trns, 0);
326 g->planePtr->RegisterTransmission(2); // contact tower
327 delete *ground_traffic_itr;
328 ground_traffic.erase(ground_traffic_itr);
329 ground_traffic_itr = ground_traffic.begin();
331 ++ground_traffic_itr;
335 // Return a random gate ID of an unused gate.
336 // Two error values may be returned and must be checked for by the calling function:
337 // -2 signifies that no gates exist at this airport.
338 // -1 signifies that all gates are currently full.
339 int FGGround::GetRandomGateID() {
340 // Check that this airport actually has some gates!!
345 gate_vec_type gateVec;
350 gatesItr = gates.begin();
351 while(gatesItr != gates.end()) {
352 if((gatesItr->second)->used == false) {
353 gateVec.push_back(gatesItr->second);
359 // Check that there are some unused gates!
360 if(!gateVec.size()) {
364 // Randomly select one from the list
366 thenum = (int)(sg_random() * gateVec.size());
367 ID = gateVec[thenum]->id;
372 // Return a pointer to an unused gate node
373 Gate* FGGround::GetGateNode() {
374 int id = GetRandomGateID();
383 // WARNING - This is hardwired to my prototype logical network format
384 // and will almost certainly change when Bernie's stuff comes on-line.
385 node* FGGround::GetThresholdNode(string rwyID) {
386 // For now go through all the nodes and parse their names
387 // Maybe in the future we'll map threshold nodes by ID
388 //cout << "Size of network is " << network.size() << '\n';
389 for(unsigned int i=0; i<network.size(); ++i) {
390 //cout << "Name = " << network[i]->name << '\n';
391 if(network[i]->name.size()) {
392 string s = network[i]->name;
393 // Warning - the next bit is fragile and dependent on my current naming scheme
394 //cout << "substr = " << s.substr(0,3) << '\n';
395 //cout << "size of s = " << s.size() << '\n';
396 if(s.substr(0,3) == "rwy") {
397 //cout << "subsubstr = " << s.substr(4, s.size() - 4) << '\n';
398 if(s.substr(4, s.size() - 4) == rwyID) {
408 // Get a path from a point on a runway to a gate
411 // Get a path from a node to another node
412 // Eventually we will need complex algorithms for this taking other traffic,
413 // shortest path and suitable paths into accout.
414 // For now we'll just call the shortest path algorithm.
415 ground_network_path_type FGGround::GetPath(node* A, node* B) {
416 return(GetShortestPath(A, B));
419 // Get a path from a node to a runway threshold
420 ground_network_path_type FGGround::GetPath(node* A, string rwyID) {
421 node* b = GetThresholdNode(rwyID);
423 SG_LOG(SG_ATC, SG_ALERT, "ERROR - unable to find path to runway theshold in ground.cxx\n");
424 ground_network_path_type emptyPath;
425 emptyPath.erase(emptyPath.begin(), emptyPath.end());
428 return GetShortestPath(A, b);
431 // Get a path from a node to a runway hold short point
432 // Bit of a hack this at the moment!
433 ground_network_path_type FGGround::GetPathToHoldShort(node* A, string rwyID) {
434 ground_network_path_type path = GetPath(A, rwyID);
435 path.pop_back(); // That should be the threshold stripped of
436 path.pop_back(); // and that should be the arc from hold short to threshold
437 // This isn't robust though - TODO - implement properly!
441 // A shortest path algorithm from memory (ie. I can't find the bl&*dy book again!)
442 // I'm sure there must be enchancements that we can make to this, such as biasing the
443 // order in which the nodes are searched out from in favour of those geographically
444 // closer to the destination.
445 // Note that we are working with the master set of nodes and arcs so we mustn't change
446 // or delete them - we only delete the paths that we create during the algorithm.
447 ground_network_path_type FGGround::GetShortestPath(node* A, node* B) {
449 shortest_path_map_type pathMap;
450 node_array_type nodesLeft;
453 int pathsCreated = 0;
455 // Initialise the algorithm
456 nodesLeft.push_back(A);
457 pathPtr = new a_path;
459 pathPtr->path.push_back(A);
461 pathMap[A->nodeID] = pathPtr;
462 bool solution_found = false; // Flag to indicate that at least one candidate path has been found
463 int solution_cost = -1; // Cost of current best cost solution. -1 indicates no solution found yet.
464 a_path solution_path;
466 node* nPtr; // nPtr is used to point to the node we are currently working with
468 while(nodesLeft.size()) {
469 //cout << "\n*****nodesLeft*****\n";
470 //for(unsigned int i=0; i<nodesLeft.size(); ++i) {
471 //cout << nodesLeft[i]->nodeID << '\n';
473 //cout << "*******************\n\n";
474 nPtr = *nodesLeft.begin(); // Thought - definate optimization possibilities here in the choice of which nodes we process first.
475 nodesLeft.erase(nodesLeft.begin());
476 //cout << "Walking out from node " << nPtr->nodeID << '\n';
477 for(unsigned int i=0; i<nPtr->arcs.size(); ++i) {
478 //cout << "ARC TO " << ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1) << '\n';
480 if((solution_found) && (solution_cost <= pathMap[nPtr->nodeID]->cost)) {
481 // Do nothing - we've already found a solution and this partial path is already more expensive
483 // This path could still be better than the current solution - check it out
484 for(unsigned int i=0; i<(nPtr->arcs.size()); i++) {
485 // Map the new path against the end node, ie. *not* the one we just started with.
486 unsigned int end_nodeID = ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1);
487 //cout << "end_nodeID = " << end_nodeID << '\n';
488 //cout << "pathMap size is " << pathMap.size() << '\n';
489 if(end_nodeID == nPtr->nodeID) {
490 //cout << "Circular arc!\n";
491 // Then its a circular arc - don't bother!!
492 //nPtr->arcs.erase(nPtr->arcs.begin() + i);
494 // see if the end node is already in the map or not
495 if(pathMap.find(end_nodeID) == pathMap.end()) {
496 //cout << "Not in the map" << endl;;
497 // Not in the map - easy!
498 pathPtr = new a_path;
500 *pathPtr = *pathMap[nPtr->nodeID]; // *copy* the path
501 pathPtr->path.push_back(nPtr->arcs[i]);
502 pathPtr->path.push_back(network[end_nodeID]);
503 pathPtr->cost += nPtr->arcs[i]->distance;
504 pathMap[end_nodeID] = pathPtr;
505 nodesLeft.push_back(network[end_nodeID]); // By definition this can't be in the list already, or
506 // it would also have been in the map and hence OR'd with this one.
507 if(end_nodeID == B->nodeID) {
508 //cout << "Solution found!!!" << endl;
509 // Since this node wasn't in the map this is by definition the first solution
510 solution_cost = pathPtr->cost;
511 solution_path = *pathPtr;
512 solution_found = true;
515 //cout << "Already in the map" << endl;
516 // In the map - not so easy - need to get rid of an arc from the higher cost one.
517 //cout << "Current cost of node " << end_nodeID << " is " << pathMap[end_nodeID]->cost << endl;
518 int newCost = pathMap[nPtr->nodeID]->cost + nPtr->arcs[i]->distance;
519 //cout << "New cost is of node " << nPtr->nodeID << " is " << newCost << endl;
520 if(newCost >= pathMap[end_nodeID]->cost) {
521 // No need to do anything.
522 //cout << "Not doing anything!" << endl;
524 delete pathMap[end_nodeID];
527 pathPtr = new a_path;
529 *pathPtr = *pathMap[nPtr->nodeID]; // *copy* the path
530 pathPtr->path.push_back(nPtr->arcs[i]);
531 pathPtr->path.push_back(network[end_nodeID]);
532 pathPtr->cost += nPtr->arcs[i]->distance;
533 pathMap[end_nodeID] = pathPtr;
535 // We need to add this node to the list-to-do again to force a recalculation
536 // onwards from this node with the new lower cost to node cost.
537 nodesLeft.push_back(network[end_nodeID]);
539 if(end_nodeID == B->nodeID) {
540 //cout << "Solution found!!!" << endl;
541 // Need to check if there is a previous better solution
542 if((solution_cost < 0) || (pathPtr->cost < solution_cost)) {
543 solution_cost = pathPtr->cost;
544 solution_path = *pathPtr;
545 solution_found = true;
555 // delete all the paths before returning
556 shortest_path_map_iterator spItr = pathMap.begin();
557 while(spItr != pathMap.end()) {
558 if(spItr->second != NULL) {
559 delete spItr->second;
565 //cout << "pathsCreated = " << pathsCreated << '\n';
566 if(pathsCreated > 0) {
567 SG_LOG(SG_ATC, SG_ALERT, "WARNING - Possible memory leak in FGGround::GetShortestPath\n\
568 Please report to flightgear-devel@flightgear.org\n");
571 //cout << (solution_found ? "Result: solution found\n" : "Result: no solution found\n");
572 return(solution_path.path); // TODO - we really ought to have a fallback position incase a solution isn't found.
577 // Randomly or otherwise populate some of the gates with parked planes
578 // (might eventually be done by the AIMgr if and when lots of AI traffic is generated)
580 // Return a list of exits from a given runway
581 // It is up to the calling function to check for non-zero size of returned array before use
582 node_array_type FGGround::GetExits(string rwyID) {
583 // FIXME - get a 07L or similar in here and we're stuffed!!!
584 return(runways[atoi(rwyID.c_str())].exits);
587 void FGGround::RequestDeparture(PlaneRec plane, FGAIEntity* requestee) {
588 // For now we'll just automatically clear all planes to the runway hold.
589 // This communication needs to be delayed 20 sec or so from receiving the request.
590 // Even if display=false we still need to start the timer in case display=true when communication starts.
591 // We also need to bear in mind we also might have other outstanding communications, although for now we'll punt that issue!
592 // FIXME - sort the above!
594 // HACK - assume that anything requesting departure is new for now - FIXME LATER
595 GroundRec* g = new GroundRec;
597 g->planePtr = requestee;
598 g->taxiRequestOutstanding = true;
599 g->clearanceCounter = 0;
602 // TODO - need to handle the next 3 as well
603 //Point3D current_pos;
605 //node* last_clearance;
607 ground_traffic.push_back(g);
611 void FGGround::NewArrival(plane_rec plane) {
612 // What are we going to do here?
613 // We need to start a new ground_rec and add the plane_rec to it
614 // We need to decide what gate we are going to clear it to.
615 // Then we need to add clearing it to that gate to the pending transmissions queue? - or simply transmit?
616 // Probably simply transmit for now and think about a transmission queue later if we need one.
617 // We might need one though in order to add a little delay for response time.
618 ground_rec* g = new ground_rec;
619 g->plane_rec = plane;
620 g->current_pos = ConvertWGS84ToXY(plane.pos);
621 g->node = GetNode(g->current_pos); // TODO - might need to sort out node/arc here
624 ground_traffic.push_back(g);
628 void FGGround::NewContact(plane_rec plane) {
629 // This is a bit of a convienience function at the moment and is likely to change.
630 if(at a gate or apron)
636 void FGGround::NextClearance(ground_rec &g) {
637 // Need to work out where we can clear g to.
638 // Assume the pilot doesn't need progressive instructions
639 // We *should* already have a gate or holding point assigned by the time we get here
640 // but it wouldn't do any harm to check.
642 // For now though we will hardwire it to clear to the final destination.
645 void FGGround::AssignGate(ground_rec &g) {
646 // We'll cheat for now - since we only have the user's aircraft and a couple of airports implemented
647 // we'll hardwire the gate!
648 // In the long run the logic of which gate or area to send the plane to could be somewhat non-trivial.