1 // groundnet.cxx - Implimentation of the FlightGear airport ground handling code
3 // Written by Durk Talsma, started June 2005.
5 // Copyright (C) 2004 Durk Talsma.
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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
31 #include <boost/foreach.hpp>
34 #include <osg/Geometry>
35 #include <osg/MatrixTransform>
38 #include <simgear/debug/logstream.hxx>
39 #include <simgear/scene/material/EffectGeode.hxx>
40 #include <simgear/scene/material/matlib.hxx>
41 #include <simgear/scene/material/mat.hxx>
42 #include <simgear/scene/util/OsgMath.hxx>
43 #include <simgear/structure/exception.hxx>
44 #include <simgear/timing/timestamp.hxx>
46 #include <Airports/simple.hxx>
47 #include <Airports/dynamics.hxx>
48 #include <Airports/runways.hxx>
50 #include <AIModel/AIAircraft.hxx>
51 #include <AIModel/performancedata.hxx>
52 #include <AIModel/AIFlightPlan.hxx>
53 #include <Navaids/NavDataCache.hxx>
55 #include <ATC/atc_mgr.hxx>
57 #include <Scenery/scenery.hxx>
59 #include "groundnetwork.hxx"
62 using flightgear::NavDataCache;
64 /***************************************************************************
66 **************************************************************************/
68 FGTaxiSegment::FGTaxiSegment(PositionedID aStart, PositionedID aEnd) :
77 SGGeod FGTaxiSegment::getCenter() const
79 FGTaxiNode* start(getStart()), *end(getEnd());
80 double heading, length, az2;
81 SGGeodesy::inverse(start->geod(), end->geod(), heading, az2, length);
82 return SGGeodesy::direct(start->geod(), heading, length * 0.5);
85 FGTaxiNode* FGTaxiSegment::getEnd() const
87 return static_cast<FGTaxiNode*>(NavDataCache::instance()->loadById(endNode));
90 FGTaxiNode* FGTaxiSegment::getStart() const
92 return static_cast<FGTaxiNode*>(NavDataCache::instance()->loadById(startNode));
95 double FGTaxiSegment::getLength() const
97 return dist(getStart()->cart(), getEnd()->cart());
100 double FGTaxiSegment::getHeading() const
102 return SGGeodesy::courseDeg(getStart()->geod(), getEnd()->geod());
106 void FGTaxiSegment::block(int id, time_t blockTime, time_t now)
108 BlockListIterator i = blockTimes.begin();
109 while (i != blockTimes.end()) {
110 if (i->getId() == id)
114 if (i == blockTimes.end()) {
115 blockTimes.push_back(Block(id, blockTime, now));
116 sort(blockTimes.begin(), blockTimes.end());
118 i->updateTimeStamps(blockTime, now);
122 // The segment has a block if any of the block times listed in the block list is
123 // smaller than the current time.
124 bool FGTaxiSegment::hasBlock(time_t now)
126 for (BlockListIterator i = blockTimes.begin(); i != blockTimes.end(); i++) {
127 if (i->getBlockTime() < now)
133 void FGTaxiSegment::unblock(time_t now)
135 if (blockTimes.size()) {
136 BlockListIterator i = blockTimes.begin();
137 if (i->getTimeStamp() < (now - 30)) {
145 /***************************************************************************
147 **************************************************************************/
148 bool FGTaxiRoute::next(PositionedID *nde)
150 if (currNode == nodes.end())
159 /***************************************************************************
161 **************************************************************************/
163 bool compare_trafficrecords(FGTrafficRecord a, FGTrafficRecord b)
165 return (a.getIntentions().size() < b.getIntentions().size());
168 FGGroundNetwork::FGGroundNetwork() :
176 currTraffic = activeTraffic.begin();
179 networkInitialized = false;
183 FGGroundNetwork::~FGGroundNetwork()
185 // JMT 2012-09-8 - disabling the groundnet-caching as part of enabling the
186 // navcache. The problem isn't the NavCache - it's that for the past few years,
187 // we have not being running destructors on FGPositioned classes, and hence,
188 // not running this code.
189 // When I fix FGPositioned lifetimes (unloading-at-runtime support), this
190 // will need to be re-visited so it can run safely during shutdown.
192 saveElevationCache();
194 BOOST_FOREACH(FGTaxiSegment* seg, segments) {
199 void FGGroundNetwork::saveElevationCache()
202 bool saveData = false;
204 if (fgGetBool("/sim/ai/groundnet-cache")) {
205 SGPath cacheData(globals->get_fg_home());
206 cacheData.append("ai");
207 string airport = parent->getId();
209 if ((airport) != "") {
211 ::snprintf(buffer, 128, "%c/%c/%c/",
212 airport[0], airport[1], airport[2]);
213 cacheData.append(buffer);
214 if (!cacheData.exists()) {
215 cacheData.create_dir(0777);
217 cacheData.append(airport + "-groundnet-cache.txt");
218 cachefile.open(cacheData.str().c_str());
222 cachefile << "[GroundNetcachedata:ref:2011:09:04]" << endl;
223 for (IndexTaxiNodeMap::iterator node = nodes.begin();
224 node != nodes.end(); node++) {
226 cachefile << node->second->getIndex () << " "
227 << node->second->getElevationM (parent->getElevation()*SG_FEET_TO_METER) << " "
237 void FGGroundNetwork::init(FGAirport* pr)
239 if (networkInitialized) {
240 FGATCController::init();
241 //cerr << "FGground network already initialized" << endl;
253 // establish pairing of segments
254 BOOST_FOREACH(FGTaxiSegment* segment, segments) {
255 segment->setIndex(index++);
257 if (segment->oppositeDirection) {
258 continue; // already establish
261 FGTaxiSegment* opp = findSegment(segment->endNode, segment->startNode);
263 assert(opp->oppositeDirection == NULL);
264 segment->oppositeDirection = opp;
265 opp->oppositeDirection = segment;
269 if (fgGetBool("/sim/ai/groundnet-cache")) {
273 networkInitialized = true;
276 void FGGroundNetwork::loadSegments()
278 flightgear::NavDataCache* cache = flightgear::NavDataCache::instance();
279 // iterate over all ground-net nodes in this airport
280 BOOST_FOREACH(PositionedID node, cache->groundNetNodes(parent->guid(), false)) {
281 // find all segments leaving the node
282 BOOST_FOREACH(PositionedID end, cache->groundNetEdgesFrom(node, false)) {
283 segments.push_back(new FGTaxiSegment(node, end));
288 void FGGroundNetwork::parseCache()
290 SGPath cacheData(globals->get_fg_home());
291 cacheData.append("ai");
292 string airport = parent->getId();
294 if (airport.empty()) {
299 ::snprintf(buffer, 128, "%c/%c/%c/",
300 airport[0], airport[1], airport[2]);
301 cacheData.append(buffer);
302 if (!cacheData.exists()) {
303 cacheData.create_dir(0777);
307 cacheData.append(airport + "-groundnet-cache.txt");
308 if (cacheData.exists()) {
309 ifstream data(cacheData.c_str());
312 if (revisionStr != "[GroundNetcachedata:ref:2011:09:04]") {
313 SG_LOG(SG_GENERAL, SG_ALERT,"GroundNetwork Warning: discarding outdated cachefile " <<
314 cacheData.c_str() << " for Airport " << airport);
316 for (IndexTaxiNodeMap::iterator i = nodes.begin();
319 i->second->setElevation(parent->elevation() * SG_FEET_TO_METER);
320 data >> index >> elev;
323 if (index != i->second->getIndex()) {
324 SG_LOG(SG_GENERAL, SG_ALERT, "Index read from ground network cache at airport " << airport << " does not match index in the network itself");
326 i->second->setElevation(elev);
334 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod) const
336 const bool onRunway = false;
337 return NavDataCache::instance()->findGroundNetNode(parent->guid(), aGeod, onRunway);
340 int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGRunway* aRunway) const
342 const bool onRunway = true;
343 return NavDataCache::instance()->findGroundNetNode(parent->guid(), aGeod, onRunway, aRunway);
346 FGTaxiNode* FGGroundNetwork::findNode(PositionedID idx) const
349 return static_cast<FGTaxiNode*>(NavDataCache::instance()->loadById(idx));
352 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx) const
354 if ((idx > 0) && (idx <= segments.size()))
355 return segments[idx - 1];
357 //cerr << "Alert: trying to find invalid segment " << idx << endl;
362 FGTaxiSegment* FGGroundNetwork::findSegment(PositionedID from, PositionedID to) const
368 // completely boring linear search of segments. Can be improved if/when
369 // this ever becomes a hot-spot
370 BOOST_FOREACH(FGTaxiSegment* seg, segments) {
371 if (seg->startNode != from) {
375 if ((to == 0) || (seg->endNode == to)) {
380 return NULL; // not found
383 static int edgePenalty(FGTaxiNode* tn)
385 return (tn->type() == FGPositioned::PARKING ? 10000 : 0) +
386 (tn->getIsOnRunway() ? 1000 : 0);
389 class ShortestPathData
397 FGTaxiNode_ptr previousNode;
400 FGTaxiRoute FGGroundNetwork::findShortestRoute(PositionedID start, PositionedID end,
403 //implements Dijkstra's algorithm to find shortest distance route from start to end
404 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
405 FGTaxiNodeVector unvisited;
406 flightgear::NavDataCache* cache = flightgear::NavDataCache::instance();
407 std::map<FGTaxiNode*, ShortestPathData> searchData;
409 BOOST_FOREACH(PositionedID n, cache->groundNetNodes(parent->guid(), !fullSearch)) {
410 unvisited.push_back(findNode(n));
413 FGTaxiNode *firstNode = findNode(start);
416 SG_LOG(SG_GENERAL, SG_ALERT,
417 "Error in ground network. Failed to find first waypoint: " << start
418 << " at " << ((parent) ? parent->getId() : "<unknown>"));
419 return FGTaxiRoute();
421 searchData[firstNode].score = 0.0;
423 FGTaxiNode *lastNode = findNode(end);
426 SG_LOG(SG_GENERAL, SG_ALERT,
427 "Error in ground network. Failed to find last waypoint: " << end
428 << " at " << ((parent) ? parent->getId() : "<unknown>"));
429 return FGTaxiRoute();
432 while (!unvisited.empty()) {
433 FGTaxiNode *best = unvisited.front();
434 BOOST_FOREACH(FGTaxiNode* i, unvisited) {
435 if (searchData[i].score < searchData[best].score) {
440 // remove 'best' from the unvisited set
441 FGTaxiNodeVectorIterator newend =
442 remove(unvisited.begin(), unvisited.end(), best);
443 unvisited.erase(newend, unvisited.end());
445 if (best == lastNode) { // found route or best not connected
449 BOOST_FOREACH(PositionedID targetId, cache->groundNetEdgesFrom(best->guid(), !fullSearch)) {
450 FGTaxiNode* tgt = (FGTaxiNode*) cache->loadById(targetId);
451 double edgeLength = dist(best->cart(), tgt->cart());
452 double alt = searchData[best].score + edgeLength + edgePenalty(tgt);
453 if (alt < searchData[tgt].score) { // Relax (u,v)
454 searchData[tgt].score = alt;
455 searchData[tgt].previousNode = best;
457 } // of outgoing arcs/segments from current best node iteration
458 } // of unvisited nodes remaining
460 if (searchData[lastNode].score == HUGE_VAL) {
461 // no valid route found
463 SG_LOG(SG_GENERAL, SG_ALERT,
464 "Failed to find route from waypoint " << start << " to "
465 << end << " at " << parent->getId());
468 return FGTaxiRoute();
471 // assemble route from backtrace information
472 PositionedIDVec nodes;
473 FGTaxiNode *bt = lastNode;
474 while (searchData[bt].previousNode != 0) {
475 nodes.push_back(bt->guid());
476 bt = searchData[bt].previousNode;
478 nodes.push_back(start);
479 reverse(nodes.begin(), nodes.end());
480 return FGTaxiRoute(nodes, searchData[lastNode].score, 0);
483 /* ATC Related Functions */
485 void FGGroundNetwork::announcePosition(int id,
486 FGAIFlightPlan * intendedRoute,
487 int currentPosition, double lat,
488 double lon, double heading,
489 double speed, double alt,
490 double radius, int leg,
491 FGAIAircraft * aircraft)
495 TrafficVectorIterator i = activeTraffic.begin();
496 // Search search if the current id alread has an entry
497 // This might be faster using a map instead of a vector, but let's start by taking a safe route
498 if (activeTraffic.size()) {
499 //while ((i->getId() != id) && i != activeTraffic.end()) {
500 while (i != activeTraffic.end()) {
501 if (i->getId() == id) {
507 // Add a new TrafficRecord if no one exsists for this aircraft.
508 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
512 rec.setPositionAndIntentions(currentPosition, intendedRoute);
513 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
514 rec.setRadius(radius); // only need to do this when creating the record.
515 rec.setAircraft(aircraft);
517 activeTraffic.push_front(rec);
519 activeTraffic.push_back(rec);
523 i->setPositionAndIntentions(currentPosition, intendedRoute);
524 i->setPositionAndHeading(lat, lon, heading, speed, alt);
529 void FGGroundNetwork::signOff(int id)
531 TrafficVectorIterator i = activeTraffic.begin();
532 // Search search if the current id alread has an entry
533 // This might be faster using a map instead of a vector, but let's start by taking a safe route
534 if (activeTraffic.size()) {
535 //while ((i->getId() != id) && i != activeTraffic.end()) {
536 while (i != activeTraffic.end()) {
537 if (i->getId() == id) {
543 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
544 SG_LOG(SG_GENERAL, SG_ALERT,
545 "AI error: Aircraft without traffic record is signing off at " << SG_ORIGIN);
547 i = activeTraffic.erase(i);
551 * The ground network can deal with the following states:
552 * 0 = Normal; no action required
553 * 1 = "Acknowledge "Hold position
554 * 2 = "Acknowledge "Resume taxi".
555 * 3 = "Issue TaxiClearance"
556 * 4 = Acknowledge Taxi Clearance"
557 * 5 = Post acknowlegde taxiclearance: Start taxiing
559 * 7 = Acknowledge report runway
560 * 8 = Switch tower frequency
561 * 9 = Acknowledge switch tower frequency
562 *************************************************************************************************************************/
563 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
566 int state = i->getState();
567 if ((state >= minState) && (state <= maxState) && available) {
568 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
569 //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
570 static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
571 int n = trans_num->getIntValue();
573 trans_num->setIntValue(-1);
575 //cerr << "Selected transmission message " << n << endl;
576 //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
577 FGATCDialogNew::instance()->removeEntry(1);
579 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
580 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, false);
584 transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, true);
586 lastTransmission = now;
593 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
594 double heading, double speed, double alt,
597 time_t currentTime = time(NULL);
598 if (nextSave < currentTime) {
599 saveElevationCache();
600 nextSave = currentTime + 100 + rand() % 200;
602 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
603 // Transmit air-to-ground "Ready to taxi request:
604 // Transmit ground to air approval / hold
605 // Transmit confirmation ...
606 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
609 TrafficVectorIterator i = activeTraffic.begin();
610 // Search search if the current id has an entry
611 // This might be faster using a map instead of a vector, but let's start by taking a safe route
612 TrafficVectorIterator current, closest;
613 if (activeTraffic.size()) {
614 //while ((i->getId() != id) && i != activeTraffic.end()) {
615 while (i != activeTraffic.end()) {
616 if (i->getId() == id) {
622 // update position of the current aircraft
623 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
624 SG_LOG(SG_GENERAL, SG_ALERT,
625 "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
627 i->setPositionAndHeading(lat, lon, heading, speed, alt);
633 // Update every three secs, but add some randomness
634 // to prevent all IA objects doing this in synchrony
635 //if (getDt() < (3.0) + (rand() % 10))
639 current->clearResolveCircularWait();
640 current->setWaitsForId(0);
641 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
642 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
643 if (!needsTaxiClearance) {
644 checkHoldPosition(id, lat, lon, heading, speed, alt);
645 //if (checkForCircularWaits(id)) {
646 // i->setResolveCircularWait();
649 current->setHoldPosition(true);
650 int state = current->getState();
651 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
652 if ((now - lastTransmission) > 15) {
655 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
656 current->setState(3);
658 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
659 current->setState(4);
661 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
662 current->setState(5);
664 if ((state == 5) && available) {
665 current->setState(0);
666 current->getAircraft()->setTaxiClearanceRequest(false);
667 current->setHoldPosition(false);
675 Scan for a speed adjustment change. Find the nearest aircraft that is in front
676 and adjust speed when we get too close. Only do this when current position and/or
677 intentions of the current aircraft match current taxiroute position of the proximate
678 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
679 instruction. See below for the hold position instruction.
681 Note that there currently still is one flaw in the logic that needs to be addressed.
682 There can be situations where one aircraft is in front of the current aircraft, on a separate
683 route, but really close after an intersection coming off the current route. This
684 aircraft is still close enough to block the current aircraft. This situation is currently
685 not addressed yet, but should be.
688 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
689 double lon, double heading,
690 double speed, double alt)
693 TrafficVectorIterator current, closest, closestOnNetwork;
694 TrafficVectorIterator i = activeTraffic.begin();
695 bool otherReasonToSlowDown = false;
696 // bool previousInstruction;
697 if (activeTraffic.size()) {
698 //while ((i->getId() != id) && (i != activeTraffic.end()))
699 while (i != activeTraffic.end()) {
700 if (i->getId() == id) {
708 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
709 SG_LOG(SG_GENERAL, SG_ALERT,
710 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment at " << SG_ORIGIN);
715 // previousInstruction = current->getSpeedAdjustment();
716 double mindist = HUGE_VAL;
717 if (activeTraffic.size()) {
718 double course, dist, bearing, az2; // minbearing,
719 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
720 //TrafficVector iterator closest;
722 closestOnNetwork = current;
723 for (TrafficVectorIterator i = activeTraffic.begin();
724 i != activeTraffic.end(); i++) {
729 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
732 SGGeodesy::inverse(curr, other, course, az2, dist);
733 bearing = fabs(heading - course);
735 bearing = 360 - bearing;
736 if ((dist < mindist) && (bearing < 60.0)) {
739 closestOnNetwork = i;
740 // minbearing = bearing;
744 //Check traffic at the tower controller
745 if (towerController->hasActiveTraffic()) {
746 for (TrafficVectorIterator i =
747 towerController->getActiveTraffic().begin();
748 i != towerController->getActiveTraffic().end(); i++) {
749 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
750 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
753 SGGeodesy::inverse(curr, other, course, az2, dist);
754 bearing = fabs(heading - course);
756 bearing = 360 - bearing;
757 if ((dist < mindist) && (bearing < 60.0)) {
758 //cerr << "Current aircraft " << current->getAircraft()->getTrafficRef()->getCallSign()
759 // << " is closest to " << i->getAircraft()->getTrafficRef()->getCallSign()
760 // << ", which has status " << i->getAircraft()->isScheduledForTakeoff()
764 // minbearing = bearing;
765 otherReasonToSlowDown = true;
769 // Finally, check UserPosition
770 // Note, as of 2011-08-01, this should no longer be necessecary.
772 double userLatitude = fgGetDouble("/position/latitude-deg");
773 double userLongitude = fgGetDouble("/position/longitude-deg");
774 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
775 SGGeodesy::inverse(curr, user, course, az2, dist);
777 bearing = fabs(heading - course);
779 bearing = 360 - bearing;
780 if ((dist < mindist) && (bearing < 60.0)) {
783 minbearing = bearing;
784 otherReasonToSlowDown = true;
787 current->clearSpeedAdjustment();
788 bool needBraking = false;
789 if (current->checkPositionAndIntentions(*closest)
790 || otherReasonToSlowDown) {
791 double maxAllowableDistance =
792 (1.1 * current->getRadius()) +
793 (1.1 * closest->getRadius());
794 if (mindist < 2 * maxAllowableDistance) {
795 if (current->getId() == closest->getWaitsForId())
798 current->setWaitsForId(closest->getId());
799 if (closest->getId() != current->getId()) {
800 current->setSpeedAdjustment(closest->getSpeed() *
805 // closest->getAircraft()->getTakeOffStatus() &&
806 // (current->getAircraft()->getTrafficRef()->getDepartureAirport() == closest->getAircraft()->getTrafficRef()->getDepartureAirport()) &&
807 // (current->getAircraft()->GetFlightPlan()->getRunway() == closest->getAircraft()->GetFlightPlan()->getRunway())
809 // current->getAircraft()->scheduleForATCTowerDepartureControl(1);
811 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
813 if (mindist < maxAllowableDistance) {
814 //double newSpeed = (maxAllowableDistance-mindist);
815 //current->setSpeedAdjustment(newSpeed);
816 //if (mindist < 0.5* maxAllowableDistance)
818 current->setSpeedAdjustment(0);
823 if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) {
824 swap(current, closest);
830 Check for "Hold position instruction".
831 The hold position should be issued under the following conditions:
832 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
833 2) For taxiing aircraft that use one taxiway in opposite directions
834 3) For crossing or merging taxiroutes.
837 void FGGroundNetwork::checkHoldPosition(int id, double lat,
838 double lon, double heading,
839 double speed, double alt)
841 TrafficVectorIterator current;
842 TrafficVectorIterator i = activeTraffic.begin();
843 if (activeTraffic.size()) {
844 //while ((i->getId() != id) && i != activeTraffic.end())
845 while (i != activeTraffic.end()) {
846 if (i->getId() == id) {
854 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
855 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
856 SG_LOG(SG_GENERAL, SG_ALERT,
857 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
861 if (current->getAircraft()->getTakeOffStatus() == 1) {
862 current->setHoldPosition(true);
865 if (current->getAircraft()->getTakeOffStatus() == 2) {
866 //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl;
867 current->setHoldPosition(false);
868 current->clearSpeedAdjustment();
871 bool origStatus = current->hasHoldPosition();
872 current->setHoldPosition(false);
873 //SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
874 int currentRoute = i->getCurrentPosition();
876 if (i->getIntentions().size()) {
877 nextRoute = (*(i->getIntentions().begin()));
881 if (currentRoute > 0) {
882 FGTaxiSegment *tx = findSegment(currentRoute);
885 nx = findSegment(nextRoute);
889 //if (tx->hasBlock(now) || nx->hasBlock(now) ) {
890 // current->setHoldPosition(true);
892 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
893 SGGeod end (nx->getStart()->geod());
895 double distance = SGGeodesy::distanceM(start, end);
896 if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) {
897 current->setHoldPosition(true);
899 intVecIterator ivi = i->getIntentions().begin();
900 while (ivi != i->getIntentions().end()) {
902 distance += segments[(*ivi)-1]->getLength();
903 if ((segments[(*ivi)-1]->hasBlock(now)) && (distance < i->getRadius() * 4)) {
904 current->setHoldPosition(true);
912 bool currStatus = current->hasHoldPosition();
913 current->setHoldPosition(origStatus);
914 // Either a Hold Position or a resume taxi transmission has been issued
915 if ((now - lastTransmission) > 2) {
918 if (current->getState() == 0) {
919 if ((origStatus != currStatus) && available) {
920 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
921 if (currStatus == true) { // No has a hold short instruction
922 transmit(&(*current), &(*parent->getDynamics()), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
923 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
924 current->setState(1);
926 transmit(&(*current), &(*parent->getDynamics()), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
927 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
928 current->setState(2);
930 lastTransmission = now;
932 // Don't act on the changed instruction until the transmission is confirmed
933 // So set back to original status
934 //cerr << "Current state " << current->getState() << endl;
939 // 7 = Acknowledge report runway
940 // 8 = Switch tower frequency
941 //9 = Acknowledge switch tower frequency
943 //int state = current->getState();
944 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
945 current->setState(0);
946 current->setHoldPosition(true);
948 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
949 current->setState(0);
950 current->setHoldPosition(false);
952 if (current->getAircraft()->getTakeOffStatus() && (current->getState() == 0)) {
953 //cerr << "Scheduling " << current->getAircraft()->getCallSign() << " for hold short" << endl;
954 current->setState(6);
956 if (checkTransmissionState(6,6, current, now, MSG_REPORT_RUNWAY_HOLD_SHORT, ATC_AIR_TO_GROUND)) {
958 if (checkTransmissionState(7,7, current, now, MSG_ACKNOWLEDGE_REPORT_RUNWAY_HOLD_SHORT, ATC_GROUND_TO_AIR)) {
960 if (checkTransmissionState(8,8, current, now, MSG_SWITCH_TOWER_FREQUENCY, ATC_GROUND_TO_AIR)) {
962 if (checkTransmissionState(9,9, current, now, MSG_ACKNOWLEDGE_SWITCH_TOWER_FREQUENCY, ATC_AIR_TO_GROUND)) {
967 //current->setState(0);
971 * Check whether situations occur where the current aircraft is waiting for itself
972 * due to higher order interactions.
973 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
974 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
975 * through this list of waiting aircraft, we can check if we'd eventually end back
976 * at the current aircraft.
978 * Note that we should consider the situation where we are actually checking aircraft
979 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
980 * the looping aircraft. If we don't check for that, this function will get stuck into
984 bool FGGroundNetwork::checkForCircularWaits(int id)
986 //cerr << "Performing Wait check " << id << endl;
988 TrafficVectorIterator current, other;
989 TrafficVectorIterator i = activeTraffic.begin();
990 int trafficSize = activeTraffic.size();
992 while (i != activeTraffic.end()) {
993 if (i->getId() == id) {
1001 if (i == activeTraffic.end() || (trafficSize == 0)) {
1002 SG_LOG(SG_GENERAL, SG_ALERT,
1003 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits at " << SG_ORIGIN);
1007 target = current->getWaitsForId();
1008 //bool printed = false; // Note that this variable is for debugging purposes only.
1012 //cerr << "aircraft waits for user" << endl;
1017 while ((target > 0) && (target != id) && counter++ < trafficSize) {
1019 TrafficVectorIterator i = activeTraffic.begin();
1021 //while ((i->getId() != id) && i != activeTraffic.end())
1022 while (i != activeTraffic.end()) {
1023 if (i->getId() == target) {
1031 if (i == activeTraffic.end() || (trafficSize == 0)) {
1032 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
1033 // The target id is not found on the current network, which means it's at the tower
1034 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1038 target = other->getWaitsForId();
1040 // actually this trap isn't as impossible as it first seemed:
1041 // the setWaitsForID(id) is set to current when the aircraft
1042 // is waiting for the user controlled aircraft.
1043 //if (current->getId() == other->getId()) {
1044 // cerr << "Caught the impossible trap" << endl;
1045 // cerr << "Current = " << current->getId() << endl;
1046 // cerr << "Other = " << other ->getId() << endl;
1047 // for (TrafficVectorIterator at = activeTraffic.begin();
1048 // at != activeTraffic.end();
1050 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1053 if (current->getId() == other->getId())
1056 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1057 // << " (" << other->getId() << "); " << endl;;
1067 // cerr << "[done] " << endl << endl;;
1069 SG_LOG(SG_GENERAL, SG_WARN,
1070 "Detected circular wait condition: Id = " << id <<
1071 "target = " << target);
1078 // Note that this function is probably obsolete...
1079 bool FGGroundNetwork::hasInstruction(int id)
1081 TrafficVectorIterator i = activeTraffic.begin();
1082 // Search search if the current id has an entry
1083 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1084 if (activeTraffic.size()) {
1085 //while ((i->getId() != id) && i != activeTraffic.end()) {
1086 while (i != activeTraffic.end()) {
1087 if (i->getId() == id) {
1093 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1094 SG_LOG(SG_GENERAL, SG_ALERT,
1095 "AI error: checking ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1097 return i->hasInstruction();
1102 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1104 TrafficVectorIterator i = activeTraffic.begin();
1105 // Search search if the current id has an entry
1106 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1107 if (activeTraffic.size()) {
1108 //while ((i->getId() != id) && i != activeTraffic.end()) {
1109 while (i != activeTraffic.end()) {
1110 if (i->getId() == id) {
1116 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1117 SG_LOG(SG_GENERAL, SG_ALERT,
1118 "AI error: requesting ATC instruction for aircraft without traffic record at " << SG_ORIGIN);
1120 return i->getInstruction();
1122 return FGATCInstruction();
1125 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1126 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1127 double lon, double elev, double hdg, double slope)
1129 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1130 obj_pos = makeZUpFrame(geod);
1131 // hdg is not a compass heading, but a counter-clockwise rotation
1132 // around the Z axis
1133 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1135 obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
1142 void FGGroundNetwork::render(bool visible)
1145 SGMaterialLib *matlib = globals->get_matlib();
1148 globals->get_scenery()->get_scene_graph()->removeChild(group);
1149 //while (group->getNumChildren()) {
1150 // cerr << "Number of children: " << group->getNumChildren() << endl;
1151 //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1152 //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1153 //geode->releaseGLObjects();
1154 //group->removeChild(geode);
1159 group = new osg::Group;
1160 FGScenery * local_scenery = globals->get_scenery();
1161 // double elevation_meters = 0.0;
1162 // double elevation_feet = 0.0;
1163 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1164 //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1166 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1167 // Handle start point
1168 int pos = i->getCurrentPosition() - 1;
1171 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1172 SGGeod end (segments[pos]->getEnd()->geod());
1174 double length = SGGeodesy::distanceM(start, end);
1175 //heading = SGGeodesy::headingDeg(start->geod(), end->geod());
1177 double az2, heading; //, distanceM;
1178 SGGeodesy::inverse(start, end, heading, az2, length);
1179 double coveredDistance = length * 0.5;
1181 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1182 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1183 ///////////////////////////////////////////////////////////////////////////////
1184 // Make a helper function out of this
1185 osg::Matrix obj_pos;
1186 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1187 obj_trans->setDataVariance(osg::Object::STATIC);
1188 // Experimental: Calculate slope here, based on length, and the individual elevations
1189 double elevationStart;
1190 if (isUserAircraft((i)->getAircraft())) {
1191 elevationStart = fgGetDouble("/position/ground-elev-m");
1193 elevationStart = ((i)->getAircraft()->_getAltitude());
1195 double elevationEnd = segments[pos]->getEnd()->getElevationM();
1196 //cerr << "Using elevation " << elevationEnd << endl;
1198 if ((elevationEnd == 0) || (elevationEnd = parent->getElevation())) {
1199 SGGeod center2 = end;
1200 center2.setElevationM(SG_MAX_ELEVATION_M);
1201 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1202 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1203 //elevation_meters += 0.5;
1206 elevationEnd = parent->getElevation();
1208 segments[pos]->getEnd()->setElevation(elevationEnd);
1210 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1211 double elevDiff = elevationEnd - elevationStart;
1213 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1215 //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
1217 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
1219 obj_trans->setMatrix( obj_pos );
1220 //osg::Vec3 center(0, 0, 0)
1222 float width = length /2.0;
1223 osg::Vec3 corner(-width, 0, 0.25f);
1224 osg::Vec3 widthVec(2*width + 1, 0, 0);
1225 osg::Vec3 heightVec(0, 1, 0);
1226 osg::Geometry* geometry;
1227 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1228 simgear::EffectGeode* geode = new simgear::EffectGeode;
1229 geode->setName("test");
1230 geode->addDrawable(geometry);
1231 //osg::Node *custom_obj;
1233 if (segments[pos]->hasBlock(now)) {
1234 mat = matlib->find("UnidirectionalTaperRed");
1236 mat = matlib->find("UnidirectionalTaperGreen");
1239 geode->setEffect(mat->get_effect());
1240 obj_trans->addChild(geode);
1241 // wire as much of the scene graph together as we can
1242 //->addChild( obj_trans );
1243 group->addChild( obj_trans );
1244 /////////////////////////////////////////////////////////////////////
1246 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1248 for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1249 osg::Matrix obj_pos;
1252 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1253 obj_trans->setDataVariance(osg::Object::STATIC);
1255 // Experimental: Calculate slope here, based on length, and the individual elevations
1256 double elevationStart = segments[k]->getStart()->getElevationM();
1257 double elevationEnd = segments[k]->getEnd ()->getElevationM();
1258 if ((elevationStart == 0) || (elevationStart == parent->getElevation())) {
1259 SGGeod center2 = segments[k]->getStart()->geod();
1260 center2.setElevationM(SG_MAX_ELEVATION_M);
1261 if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
1262 // elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
1263 //elevation_meters += 0.5;
1266 elevationStart = parent->getElevation();
1268 segments[k]->getStart()->setElevation(elevationStart);
1270 if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
1271 SGGeod center2 = segments[k]->getEnd()->geod();
1272 center2.setElevationM(SG_MAX_ELEVATION_M);
1273 if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
1274 // elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
1275 //elevation_meters += 0.5;
1278 elevationEnd = parent->getElevation();
1280 segments[k]->getEnd()->setElevation(elevationEnd);
1283 double elevationMean = (elevationStart + elevationEnd) / 2.0;
1284 double elevDiff = elevationEnd - elevationStart;
1285 double length = segments[k]->getLength();
1286 double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
1288 // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
1290 SGGeod segCenter = segments[k]->getCenter();
1291 WorldCoordinate( obj_pos, segCenter.getLatitudeDeg(), segCenter.getLongitudeDeg(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope );
1293 obj_trans->setMatrix( obj_pos );
1294 //osg::Vec3 center(0, 0, 0)
1296 float width = segments[k]->getLength() /2.0;
1297 osg::Vec3 corner(-width, 0, 0.25f);
1298 osg::Vec3 widthVec(2*width + 1, 0, 0);
1299 osg::Vec3 heightVec(0, 1, 0);
1300 osg::Geometry* geometry;
1301 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1302 simgear::EffectGeode* geode = new simgear::EffectGeode;
1303 geode->setName("test");
1304 geode->addDrawable(geometry);
1305 //osg::Node *custom_obj;
1307 if (segments[k]->hasBlock(now)) {
1308 mat = matlib->find("UnidirectionalTaperRed");
1310 mat = matlib->find("UnidirectionalTaperGreen");
1313 geode->setEffect(mat->get_effect());
1314 obj_trans->addChild(geode);
1315 // wire as much of the scene graph together as we can
1316 //->addChild( obj_trans );
1317 group->addChild( obj_trans );
1322 globals->get_scenery()->get_scene_graph()->addChild(group);
1326 string FGGroundNetwork::getName() {
1327 return string(parent->getId() + "-ground");
1330 void FGGroundNetwork::update(double dt)
1332 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
1333 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1334 (*tsi)->unblock(now);
1337 //sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords);
1338 // Handle traffic that is under ground control first; this way we'll prevent clutter at the gate areas.
1339 // Don't allow an aircraft to pushback when a taxiing aircraft is currently using part of the intended route.
1340 for (TrafficVectorIterator i = parent->getDynamics()->getStartupController()->getActiveTraffic().begin();
1341 i != parent->getDynamics()->getStartupController()->getActiveTraffic().end(); i++) {
1343 i->setPriority(priority++);
1344 // in meters per second;
1345 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1346 if (i->isActive(0)) {
1348 // Check for all active aircraft whether it's current pos segment is
1349 // an opposite of one of the departing aircraft's intentions
1350 for (TrafficVectorIterator j = activeTraffic.begin(); j != activeTraffic.end(); j++) {
1351 int pos = j->getCurrentPosition();
1353 FGTaxiSegment *seg = segments[pos-1]->opposite();
1355 int posReverse = seg->getIndex();
1356 for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) {
1357 if ((*k) == posReverse) {
1359 segments[posReverse-1]->block(i->getId(), now, now);
1365 // if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways.
1366 if (i->pushBackAllowed()) {
1368 int pos = i->getCurrentPosition();
1370 FGTaxiSegment *seg = segments[pos-1];
1371 FGTaxiNode *node = seg->getEnd();
1372 length = seg->getLength();
1373 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1374 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1375 (*tsi)->block(i->getId(), now, now);
1379 for (intVecIterator j = i->getIntentions().begin(); j != i->getIntentions().end(); j++) {
1382 FGTaxiSegment *seg = segments[pos-1];
1383 FGTaxiNode *node = seg->getEnd();
1384 length += seg->getLength();
1385 time_t blockTime = now + (length / vTaxi);
1386 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1387 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1388 (*tsi)->block(i->getId(), blockTime-30, now);
1396 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1398 double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
1399 i->setPriority(priority++);
1400 int pos = i->getCurrentPosition();
1402 length = segments[pos-1]->getLength();
1403 if (segments[pos-1]->hasBlock(now)) {
1404 //SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
1409 for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) {
1410 int segIndex = (*ivi);
1412 if (segments[segIndex-1]->hasBlock(now))
1416 //after this, ivi points just behind the last valid unblocked taxi segment.
1417 for (intVecIterator j = i->getIntentions().begin(); j != ivi; j++) {
1420 FGTaxiSegment *seg = segments[pos-1];
1421 FGTaxiNode *node = seg->getEnd();
1422 length += seg->getLength();
1423 for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
1424 if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
1425 time_t blockTime = now + (length / vTaxi);
1426 (*tsi)->block(i->getId(), blockTime - 30, now);