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.
32 #include <osg/Geometry>
33 #include <osg/MatrixTransform>
36 #include <simgear/debug/logstream.hxx>
37 #include <simgear/route/waypoint.hxx>
38 #include <simgear/scene/material/EffectGeode.hxx>
40 #include <Airports/simple.hxx>
41 #include <Airports/dynamics.hxx>
43 #include <AIModel/AIAircraft.hxx>
44 #include <AIModel/AIFlightPlan.hxx>
46 #include "groundnetwork.hxx"
48 /***************************************************************************
50 **************************************************************************/
52 void FGTaxiSegment::setStart(FGTaxiNodeVector * nodes)
54 FGTaxiNodeVectorIterator i = nodes->begin();
55 while (i != nodes->end()) {
56 //cerr << "Scanning start node index" << (*i)->getIndex() << endl;
57 if ((*i)->getIndex() == startNode) {
58 start = (*i)->getAddress();
59 (*i)->addSegment(this);
64 SG_LOG(SG_GENERAL, SG_ALERT,
65 "Could not find start node " << startNode << endl);
68 void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes)
70 FGTaxiNodeVectorIterator i = nodes->begin();
71 while (i != nodes->end()) {
72 //cerr << "Scanning end node index" << (*i)->getIndex() << endl;
73 if ((*i)->getIndex() == endNode) {
74 end = (*i)->getAddress();
79 SG_LOG(SG_GENERAL, SG_ALERT,
80 "Could not find end node " << endNode << endl);
85 // There is probably a computationally cheaper way of
87 void FGTaxiSegment::setDimensions(double elevation)
89 length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
90 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
92 double az2; //, distanceM;
93 SGGeodesy::inverse(start->getGeod(), end->getGeod(), heading, az2, length);
94 double coveredDistance = length * 0.5;
95 SGGeodesy::direct(start->getGeod(), heading, coveredDistance, center, az2);
96 cerr << "Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
100 //void FGTaxiSegment::setCourseDiff(double crse)
102 // headingDiff = fabs(course - crse);
104 // if (headingDiff > 180)
105 // headingDiff = fabs(headingDiff - 360);
109 /***************************************************************************
111 **************************************************************************/
112 bool FGTaxiRoute::next(int *nde)
114 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
115 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
116 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
117 //if (currNode != nodes.end())
118 // cerr << "true" << endl;
120 // cerr << "false" << endl;
121 //if (nodes.size() != (routes.size()) +1)
122 // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
124 if (currNode == nodes.end())
127 if (currNode != nodes.begin()) // make sure route corresponds to the end node
133 bool FGTaxiRoute::next(int *nde, int *rte)
135 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
136 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
137 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
138 //if (currNode != nodes.end())
139 // cerr << "true" << endl;
141 // cerr << "false" << endl;
142 if (nodes.size() != (routes.size()) + 1) {
143 SG_LOG(SG_GENERAL, SG_ALERT,
144 "ALERT: Misconfigured TaxiRoute : " << nodes.
145 size() << " " << routes.size());
148 if (currNode == nodes.end())
151 //*rte = *(currRoute);
152 if (currNode != nodes.begin()) // Make sure route corresponds to the end node
157 // If currNode points to the first node, this means the aircraft is not on the taxi node
158 // yet. Make sure to return a unique identifyer in this situation though, because otherwise
159 // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
160 // taxi route or not. the negative of the preceding route seems a logical choice, as it is
161 // unique for any starting location.
162 // Note that this is probably just a temporary fix until I get Parking / tower control working.
163 *rte = -1 * *(currRoute);
169 void FGTaxiRoute::rewind(int route)
175 if (!(next(&currPoint, &currRoute))) {
176 SG_LOG(SG_GENERAL, SG_ALERT,
177 "Error in rewinding TaxiRoute: current" << currRoute <<
180 } while (currRoute != route);
186 /***************************************************************************
188 **************************************************************************/
189 bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b)
194 bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b)
199 FGGroundNetwork::FGGroundNetwork()
207 currTraffic = activeTraffic.begin();
211 FGGroundNetwork::~FGGroundNetwork()
213 for (FGTaxiNodeVectorIterator node = nodes.begin();
214 node != nodes.end(); node++) {
218 pushBackNodes.clear();
219 for (FGTaxiSegmentVectorIterator seg = segments.begin();
220 seg != segments.end(); seg++) {
226 void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
228 segments.push_back(new FGTaxiSegment(seg));
231 void FGGroundNetwork::addNode(const FGTaxiNode & node)
233 nodes.push_back(new FGTaxiNode(node));
236 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
239 FGParkingVecIterator i = parkings->begin();
240 while (i != parkings->end()) {
241 n.setIndex(i->getIndex());
242 n.setLatitude(i->getLatitude());
243 n.setLongitude(i->getLongitude());
244 n.setElevation(parent->getElevation());
245 nodes.push_back(new FGTaxiNode(n));
253 void FGGroundNetwork::init()
257 sort(nodes.begin(), nodes.end(), compare_nodes);
258 //sort(segments.begin(), segments.end(), compare_segments());
259 FGTaxiSegmentVectorIterator i = segments.begin();
260 while (i != segments.end()) {
261 (*i)->setStart(&nodes);
262 (*i)->setEnd(&nodes);
263 (*i)->setDimensions(parent->getElevation());
264 (*i)->setIndex(index);
265 if ((*i)->isPushBack()) {
266 pushBackNodes.push_back((*i)->getEnd());
268 //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl);
269 //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl);
270 //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to "
271 // << (*i)->getEnd()->getIndex() << endl);
276 i = segments.begin();
277 while (i != segments.end()) {
278 FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
279 while (j != (*i)->getEnd()->getEndRoute()) {
280 if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) {
281 // int start1 = (*i)->getStart()->getIndex();
282 // int end1 = (*i)->getEnd() ->getIndex();
283 // int start2 = (*j)->getStart()->getIndex();
284 // int end2 = (*j)->getEnd()->getIndex();
285 // int oppIndex = (*j)->getIndex();
286 //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
287 // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl;
288 (*i)->setOpposite(*j);
295 //FGTaxiNodeVectorIterator j = nodes.begin();
296 //while (j != nodes.end()) {
297 // if ((*j)->getHoldPointType() == 3) {
298 // pushBackNodes.push_back((*j));
302 //cerr << "Done initializing ground network" << endl;
306 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
308 double minDist = HUGE_VAL;
311 for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
313 double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
316 index = (*itr)->getIndex();
317 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
324 int FGGroundNetwork::findNearestNode(double lat, double lon)
326 return findNearestNode(SGGeod::fromDeg(lon, lat));
329 FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
331 for (FGTaxiNodeVectorIterator
333 itr != nodes.end(); itr++)
335 if (itr->getIndex() == idx)
336 return itr->getAddress();
339 if ((idx >= 0) && (idx < nodes.size()))
340 return nodes[idx]->getAddress();
345 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
347 for (FGTaxiSegmentVectorIterator
348 itr = segments.begin();
349 itr != segments.end(); itr++)
351 if (itr->getIndex() == idx)
352 return itr->getAddress();
355 if ((idx > 0) && (idx <= segments.size()))
356 return segments[idx - 1]->getAddress();
358 //cerr << "Alert: trying to find invalid segment " << idx << endl;
364 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
367 //implements Dijkstra's algorithm to find shortest distance route from start to end
368 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
370 //double INFINITE = 100000000000.0;
371 // initialize scoring values
372 int nParkings = parent->getDynamics()->getNrOfParkings();
373 FGTaxiNodeVector *currNodesSet;
375 currNodesSet = &nodes;
377 currNodesSet = &pushBackNodes;
380 for (FGTaxiNodeVectorIterator
381 itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
382 (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
383 (*itr)->setPreviousNode(0); //
384 (*itr)->setPreviousSeg(0); //
387 FGTaxiNode *firstNode = findNode(start);
388 firstNode->setPathScore(0);
390 FGTaxiNode *lastNode = findNode(end);
392 FGTaxiNodeVector unvisited(*currNodesSet); // working copy
394 while (!unvisited.empty()) {
395 FGTaxiNode *best = *(unvisited.begin());
396 for (FGTaxiNodeVectorIterator
397 itr = unvisited.begin(); itr != unvisited.end(); itr++) {
398 if ((*itr)->getPathScore() < best->getPathScore())
402 FGTaxiNodeVectorIterator newend =
403 remove(unvisited.begin(), unvisited.end(), best);
404 unvisited.erase(newend, unvisited.end());
406 if (best == lastNode) { // found route or best not connected
409 for (FGTaxiSegmentVectorIterator
410 seg = best->getBeginRoute();
411 seg != best->getEndRoute(); seg++) {
412 if (fullSearch || (*seg)->isPushBack()) {
413 FGTaxiNode *tgt = (*seg)->getEnd();
415 best->getPathScore() + (*seg)->getLength() +
416 (*seg)->getPenalty(nParkings);
417 if (alt < tgt->getPathScore()) { // Relax (u,v)
418 tgt->setPathScore(alt);
419 tgt->setPreviousNode(best);
420 tgt->setPreviousSeg(*seg); //
423 // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
429 if (lastNode->getPathScore() == HUGE_VAL) {
430 // no valid route found
432 SG_LOG(SG_GENERAL, SG_ALERT,
433 "Failed to find route from waypoint " << start << " to "
434 << end << " at " << parent->getId());
438 //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
440 // assemble route from backtrace information
441 intVec nodes, routes;
442 FGTaxiNode *bt = lastNode;
443 while (bt->getPreviousNode() != 0) {
444 nodes.push_back(bt->getIndex());
445 routes.push_back(bt->getPreviousSegment()->getIndex());
446 bt = bt->getPreviousNode();
448 nodes.push_back(start);
449 reverse(nodes.begin(), nodes.end());
450 reverse(routes.begin(), routes.end());
452 return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
456 int FGTaxiSegment::getPenalty(int nGates)
459 if (end->getIndex() < nGates) {
462 if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
468 /* ATC Related Functions */
470 void FGGroundNetwork::announcePosition(int id,
471 FGAIFlightPlan * intendedRoute,
472 int currentPosition, double lat,
473 double lon, double heading,
474 double speed, double alt,
475 double radius, int leg,
476 FGAIAircraft * aircraft)
479 TrafficVectorIterator i = activeTraffic.begin();
480 // Search search if the current id alread has an entry
481 // This might be faster using a map instead of a vector, but let's start by taking a safe route
482 if (activeTraffic.size()) {
483 //while ((i->getId() != id) && i != activeTraffic.end()) {
484 while (i != activeTraffic.end()) {
485 if (i->getId() == id) {
491 // Add a new TrafficRecord if no one exsists for this aircraft.
492 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
496 rec.setPositionAndIntentions(currentPosition, intendedRoute);
497 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
498 rec.setRadius(radius); // only need to do this when creating the record.
499 rec.setAircraft(aircraft);
500 activeTraffic.push_back(rec);
502 i->setPositionAndIntentions(currentPosition, intendedRoute);
503 i->setPositionAndHeading(lat, lon, heading, speed, alt);
507 void FGGroundNetwork::signOff(int id)
509 TrafficVectorIterator i = activeTraffic.begin();
510 // Search search if the current id alread has an entry
511 // This might be faster using a map instead of a vector, but let's start by taking a safe route
512 if (activeTraffic.size()) {
513 //while ((i->getId() != id) && i != activeTraffic.end()) {
514 while (i != activeTraffic.end()) {
515 if (i->getId() == id) {
521 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
522 SG_LOG(SG_GENERAL, SG_ALERT,
523 "AI error: Aircraft without traffic record is signing off");
525 i = activeTraffic.erase(i);
529 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
532 int state = i->getState();
533 if ((state >= minState) && (state <= maxState) && available) {
534 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
536 cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
537 static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
538 int n = trans_num->getIntValue();
540 trans_num->setIntValue(-1);
542 cerr << "Selected transmission message" << n << endl;
544 cerr << "creading message for " << i->getAircraft()->getCallSign() << endl;
545 transmit(&(*i), msgId, msgDir, false);
549 //cerr << "Transmitting startup msg" << endl;
550 transmit(&(*i), msgId, msgDir, true);
552 lastTransmission = now;
559 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
560 double heading, double speed, double alt,
563 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
564 // Transmit air-to-ground "Ready to taxi request:
565 // Transmit ground to air approval / hold
566 // Transmit confirmation ...
567 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
570 TrafficVectorIterator i = activeTraffic.begin();
571 // Search search if the current id has an entry
572 // This might be faster using a map instead of a vector, but let's start by taking a safe route
573 TrafficVectorIterator current, closest;
574 if (activeTraffic.size()) {
575 //while ((i->getId() != id) && i != activeTraffic.end()) {
576 while (i != activeTraffic.end()) {
577 if (i->getId() == id) {
583 // update position of the current aircraft
584 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
585 SG_LOG(SG_GENERAL, SG_ALERT,
586 "AI error: updating aircraft without traffic record");
588 i->setPositionAndHeading(lat, lon, heading, speed, alt);
594 // Update every three secs, but add some randomness
595 // to prevent all IA objects doing this in synchrony
596 //if (getDt() < (3.0) + (rand() % 10))
600 current->clearResolveCircularWait();
601 current->setWaitsForId(0);
602 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
603 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
604 if (!needsTaxiClearance) {
605 checkHoldPosition(id, lat, lon, heading, speed, alt);
606 if (checkForCircularWaits(id)) {
607 i->setResolveCircularWait();
610 current->setHoldPosition(true);
611 int state = current->getState();
612 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
613 if ((now - lastTransmission) > 15) {
616 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
617 current->setState(3);
619 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
620 current->setState(4);
622 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
623 current->setState(5);
625 if ((state == 5) && available) {
626 current->setState(0);
627 current->getAircraft()->setTaxiClearanceRequest(false);
628 current->setHoldPosition(true);
637 Scan for a speed adjustment change. Find the nearest aircraft that is in front
638 and adjust speed when we get too close. Only do this when current position and/or
639 intentions of the current aircraft match current taxiroute position of the proximate
640 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
641 instruction. See below for the hold position instruction.
643 Note that there currently still is one flaw in the logic that needs to be addressed.
644 There can be situations where one aircraft is in front of the current aircraft, on a separate
645 route, but really close after an intersection coming off the current route. This
646 aircraft is still close enough to block the current aircraft. This situation is currently
647 not addressed yet, but should be.
650 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
651 double lon, double heading,
652 double speed, double alt)
655 TrafficVectorIterator current, closest;
656 TrafficVectorIterator i = activeTraffic.begin();
657 bool otherReasonToSlowDown = false;
658 bool previousInstruction;
659 if (activeTraffic.size()) {
660 //while ((i->getId() != id) && (i != activeTraffic.end()))
661 while (i != activeTraffic.end()) {
662 if (i->getId() == id) {
670 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
671 SG_LOG(SG_GENERAL, SG_ALERT,
672 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
677 previousInstruction = current->getSpeedAdjustment();
678 double mindist = HUGE_VAL;
679 if (activeTraffic.size()) {
680 double course, dist, bearing, minbearing, az2;
681 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
682 //TrafficVector iterator closest;
684 for (TrafficVectorIterator i = activeTraffic.begin();
685 i != activeTraffic.end(); i++) {
690 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
693 SGGeodesy::inverse(curr, other, course, az2, dist);
694 bearing = fabs(heading - course);
696 bearing = 360 - bearing;
697 if ((dist < mindist) && (bearing < 60.0)) {
700 minbearing = bearing;
703 //Check traffic at the tower controller
704 if (towerController->hasActiveTraffic()) {
705 for (TrafficVectorIterator i =
706 towerController->getActiveTraffic().begin();
707 i != towerController->getActiveTraffic().end(); i++) {
708 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
709 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
712 SGGeodesy::inverse(curr, other, course, az2, dist);
713 bearing = fabs(heading - course);
715 bearing = 360 - bearing;
716 if ((dist < mindist) && (bearing < 60.0)) {
719 minbearing = bearing;
720 otherReasonToSlowDown = true;
724 // Finally, check UserPosition
725 double userLatitude = fgGetDouble("/position/latitude-deg");
726 double userLongitude = fgGetDouble("/position/longitude-deg");
727 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
728 SGGeodesy::inverse(curr, user, course, az2, dist);
730 bearing = fabs(heading - course);
732 bearing = 360 - bearing;
733 if ((dist < mindist) && (bearing < 60.0)) {
736 minbearing = bearing;
737 otherReasonToSlowDown = true;
740 current->clearSpeedAdjustment();
742 if (current->checkPositionAndIntentions(*closest)
743 || otherReasonToSlowDown) {
744 double maxAllowableDistance =
745 (1.1 * current->getRadius()) +
746 (1.1 * closest->getRadius());
747 if (mindist < 2 * maxAllowableDistance) {
748 if (current->getId() == closest->getWaitsForId())
751 current->setWaitsForId(closest->getId());
752 if (closest->getId() != current->getId())
753 current->setSpeedAdjustment(closest->getSpeed() *
756 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
757 if (mindist < maxAllowableDistance) {
758 //double newSpeed = (maxAllowableDistance-mindist);
759 //current->setSpeedAdjustment(newSpeed);
760 //if (mindist < 0.5* maxAllowableDistance)
762 current->setSpeedAdjustment(0);
771 Check for "Hold position instruction".
772 The hold position should be issued under the following conditions:
773 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
774 2) For taxiing aircraft that use one taxiway in opposite directions
775 3) For crossing or merging taxiroutes.
778 void FGGroundNetwork::checkHoldPosition(int id, double lat,
779 double lon, double heading,
780 double speed, double alt)
782 TrafficVectorIterator current;
783 TrafficVectorIterator i = activeTraffic.begin();
784 if (activeTraffic.size()) {
785 //while ((i->getId() != id) && i != activeTraffic.end())
786 while (i != activeTraffic.end()) {
787 if (i->getId() == id) {
795 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
796 SG_LOG(SG_GENERAL, SG_ALERT,
797 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
800 bool origStatus = current->hasHoldPosition();
801 current->setHoldPosition(false);
802 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
804 for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
805 if (i->getId() != current->getId()) {
806 int node = current->crosses(this, *i);
808 FGTaxiNode *taxiNode = findNode(node);
810 // Determine whether it's save to continue or not.
811 // If we have a crossing route, there are two possibilities:
812 // 1) This is an interestion
813 // 2) This is oncoming two-way traffic, using the same taxiway.
814 //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
816 SGGeod other(SGGeod::
817 fromDegM(i->getLongitude(), i->getLatitude(),
821 if (current->isOpposing(this, *i, node)) {
824 //cerr << "Hold check 2 : " << node << " has opposing segment " << endl;
825 // issue a "Hold Position" as soon as we're close to the offending node
826 // For now, I'm doing this as long as the other aircraft doesn't
827 // have a hold instruction as soon as we're within a reasonable
828 // distance from the offending node.
829 // This may be a bit of a conservative estimate though, as it may
830 // be well possible that both aircraft can both continue to taxi
831 // without crashing into each other.
834 if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
837 //cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
841 //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
846 SGGeodesy::distanceM(curr, taxiNode->getGeod());
847 if (!(i->hasHoldPosition())) {
849 if ((dist < 200) && //2.5*current->getRadius()) &&
850 (needsToWait) && (i->onRoute(this, *current)) &&
851 //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
852 (!(current->getId() == i->getWaitsForId())))
853 //(!(i->getSpeedAdjustment()))) // &&
854 //(!(current->getSpeedAdjustment())))
857 current->setHoldPosition(true);
858 current->setWaitsForId(i->getId());
859 //cerr << "Hold check 5: " << current->getCallSign() <<" Setting Hold Position: distance to node (" << node << ") "
860 // << dist << " meters. Waiting for " << i->getCallSign();
862 //cerr <<" [opposing] " << endl;
864 // cerr << "[non-opposing] " << endl;
865 //if (i->hasSpeefAdjustment())
867 // cerr << " (which in turn waits for ) " << i->
869 //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl;
875 bool currStatus = current->hasHoldPosition();
877 // Either a Hold Position or a resume taxi transmission has been issued
878 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
879 if ((now - lastTransmission) > 2) {
882 if ((origStatus != currStatus) && available) {
883 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
884 if (currStatus == true) { // No has a hold short instruction
885 transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
886 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
887 current->setState(1);
889 transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
890 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
891 current->setState(2);
893 lastTransmission = now;
895 // Don't act on the changed instruction until the transmission is confirmed
896 // So set back to original status
897 current->setHoldPosition(origStatus);
898 //cerr << "Current state " << current->getState() << endl;
901 //int state = current->getState();
902 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
903 current->setState(0);
904 current->setHoldPosition(true);
906 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
907 current->setState(0);
908 current->setHoldPosition(false);
911 /*if ((state == 1) && (available)) {
912 //cerr << "ACKNOWLEDGE HOLD" << endl;
913 transmit(&(*current), MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND, true);
914 current->setState(0);
915 current->setHoldPosition(true);
916 lastTransmission = now;
920 if ((state == 2) && (available)) {
921 //cerr << "ACKNOWLEDGE RESUME" << endl;
922 transmit(&(*current), MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND, true);
923 current->setState(0);
924 current->setHoldPosition(false);
925 lastTransmission = now;
931 * Check whether situations occur where the current aircraft is waiting for itself
932 * due to higher order interactions.
933 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
934 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
935 * through this list of waiting aircraft, we can check if we'd eventually end back
936 * at the current aircraft.
938 * Note that we should consider the situation where we are actually checking aircraft
939 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
940 * the looping aircraft. If we don't check for that, this function will get stuck into
944 bool FGGroundNetwork::checkForCircularWaits(int id)
946 //cerr << "Performing Wait check " << id << endl;
948 TrafficVectorIterator current, other;
949 TrafficVectorIterator i = activeTraffic.begin();
950 int trafficSize = activeTraffic.size();
952 while (i != activeTraffic.end()) {
953 if (i->getId() == id) {
961 if (i == activeTraffic.end() || (trafficSize == 0)) {
962 SG_LOG(SG_GENERAL, SG_ALERT,
963 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
967 target = current->getWaitsForId();
968 //bool printed = false; // Note that this variable is for debugging purposes only.
972 //cerr << "aircraft waits for user" << endl;
977 while ((target > 0) && (target != id) && counter++ < trafficSize) {
979 TrafficVectorIterator i = activeTraffic.begin();
981 //while ((i->getId() != id) && i != activeTraffic.end())
982 while (i != activeTraffic.end()) {
983 if (i->getId() == target) {
991 if (i == activeTraffic.end() || (trafficSize == 0)) {
992 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
993 // The target id is not found on the current network, which means it's at the tower
994 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
998 target = other->getWaitsForId();
1000 // actually this trap isn't as impossible as it first seemed:
1001 // the setWaitsForID(id) is set to current when the aircraft
1002 // is waiting for the user controlled aircraft.
1003 //if (current->getId() == other->getId()) {
1004 // cerr << "Caught the impossible trap" << endl;
1005 // cerr << "Current = " << current->getId() << endl;
1006 // cerr << "Other = " << other ->getId() << endl;
1007 // for (TrafficVectorIterator at = activeTraffic.begin();
1008 // at != activeTraffic.end();
1010 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1013 if (current->getId() == other->getId())
1016 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1017 // << " (" << other->getId() << "); " << endl;;
1027 // cerr << "[done] " << endl << endl;;
1029 SG_LOG(SG_GENERAL, SG_WARN,
1030 "Detected circular wait condition: Id = " << id <<
1031 "target = " << target);
1038 // Note that this function is probably obsolete...
1039 bool FGGroundNetwork::hasInstruction(int id)
1041 TrafficVectorIterator i = activeTraffic.begin();
1042 // Search search if the current id has an entry
1043 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1044 if (activeTraffic.size()) {
1045 //while ((i->getId() != id) && i != activeTraffic.end()) {
1046 while (i != activeTraffic.end()) {
1047 if (i->getId() == id) {
1053 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1054 SG_LOG(SG_GENERAL, SG_ALERT,
1055 "AI error: checking ATC instruction for aircraft without traffic record");
1057 return i->hasInstruction();
1062 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1064 TrafficVectorIterator i = activeTraffic.begin();
1065 // Search search if the current id has an entry
1066 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1067 if (activeTraffic.size()) {
1068 //while ((i->getId() != id) && i != activeTraffic.end()) {
1069 while (i != activeTraffic.end()) {
1070 if (i->getId() == id) {
1076 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1077 SG_LOG(SG_GENERAL, SG_ALERT,
1078 "AI error: requesting ATC instruction for aircraft without traffic record");
1080 return i->getInstruction();
1082 return FGATCInstruction();
1085 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1086 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1087 double lon, double elev, double hdg)
1089 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1090 obj_pos = geod.makeZUpFrame();
1091 // hdg is not a compass heading, but a counter-clockwise rotation
1092 // around the Z axis
1093 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1099 osg::Node* FGGroundNetwork::getRenderNode()
1101 osg::Group* group = new osg::Group;
1103 for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1104 osg::Matrix obj_pos;
1105 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1106 obj_trans->setDataVariance(osg::Object::STATIC);
1107 WorldCoordinate( obj_pos, (*i)->getLatitude(), (*i)->getLongitude(), parent->elevation()+10, -((*i)->getHeading()) );
1109 obj_trans->setMatrix( obj_pos );
1110 //osg::Vec3 center(0, 0, 0)
1112 float width = (*i)->getLength() /2.0;
1113 osg::Vec3 corner(-width, 0, 0.25f);
1114 osg::Vec3 widthVec(2*width + 1, 0, 0);
1115 osg::Vec3 heightVec(0, 0, 1);
1116 osg::Geometry* geometry;
1117 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1118 simgear::EffectGeode* geode = new simgear::EffectGeode;
1119 geode->setName("test");
1120 geode->addDrawable(geometry);
1121 //osg::Node *custom_obj;
1123 obj_trans->addChild(geode);
1124 // wire as much of the scene graph together as we can
1125 //->addChild( obj_trans );
1126 group->addChild( obj_trans );