2 // groundnet.cxx - Implimentation of the FlightGear airport ground handling code
4 // Written by Durk Talsma, started June 2005.
6 // Copyright (C) 2004 Durk Talsma.
8 // This program is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU General Public License as
10 // published by the Free Software Foundation; either version 2 of the
11 // License, or (at your option) any later version.
13 // This program is distributed in the hope that it will be useful, but
14 // WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // General Public License for more details.
18 // You should have received a copy of the GNU General Public License
19 // along with this program; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
33 #include <osg/Geometry>
34 #include <osg/MatrixTransform>
37 #include <simgear/debug/logstream.hxx>
38 #include <simgear/route/waypoint.hxx>
39 #include <simgear/scene/material/EffectGeode.hxx>
40 #include <simgear/scene/material/matlib.hxx>
41 #include <simgear/scene/material/mat.hxx>
43 #include <Airports/simple.hxx>
44 #include <Airports/dynamics.hxx>
46 #include <AIModel/AIAircraft.hxx>
47 #include <AIModel/AIFlightPlan.hxx>
49 #include <Scenery/scenery.hxx>
51 #include "groundnetwork.hxx"
53 /***************************************************************************
55 **************************************************************************/
57 void FGTaxiSegment::setStart(FGTaxiNodeVector * nodes)
59 FGTaxiNodeVectorIterator i = nodes->begin();
60 while (i != nodes->end()) {
61 //cerr << "Scanning start node index" << (*i)->getIndex() << endl;
62 if ((*i)->getIndex() == startNode) {
63 start = (*i)->getAddress();
64 (*i)->addSegment(this);
69 SG_LOG(SG_GENERAL, SG_ALERT,
70 "Could not find start node " << startNode << endl);
73 void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes)
75 FGTaxiNodeVectorIterator i = nodes->begin();
76 while (i != nodes->end()) {
77 //cerr << "Scanning end node index" << (*i)->getIndex() << endl;
78 if ((*i)->getIndex() == endNode) {
79 end = (*i)->getAddress();
84 SG_LOG(SG_GENERAL, SG_ALERT,
85 "Could not find end node " << endNode << endl);
90 // There is probably a computationally cheaper way of
92 void FGTaxiSegment::setDimensions(double elevation)
94 length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
95 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
97 double az2; //, distanceM;
98 SGGeodesy::inverse(start->getGeod(), end->getGeod(), heading, az2, length);
99 double coveredDistance = length * 0.5;
100 SGGeodesy::direct(start->getGeod(), heading, coveredDistance, center, az2);
101 //cerr << "Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
105 //void FGTaxiSegment::setCourseDiff(double crse)
107 // headingDiff = fabs(course - crse);
109 // if (headingDiff > 180)
110 // headingDiff = fabs(headingDiff - 360);
114 /***************************************************************************
116 **************************************************************************/
117 bool FGTaxiRoute::next(int *nde)
119 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
120 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
121 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
122 //if (currNode != nodes.end())
123 // cerr << "true" << endl;
125 // cerr << "false" << endl;
126 //if (nodes.size() != (routes.size()) +1)
127 // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
129 if (currNode == nodes.end())
132 if (currNode != nodes.begin()) // make sure route corresponds to the end node
138 bool FGTaxiRoute::next(int *nde, int *rte)
140 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
141 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
142 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
143 //if (currNode != nodes.end())
144 // cerr << "true" << endl;
146 // cerr << "false" << endl;
147 if (nodes.size() != (routes.size()) + 1) {
148 SG_LOG(SG_GENERAL, SG_ALERT,
149 "ALERT: Misconfigured TaxiRoute : " << nodes.
150 size() << " " << routes.size());
153 if (currNode == nodes.end())
156 //*rte = *(currRoute);
157 if (currNode != nodes.begin()) // Make sure route corresponds to the end node
162 // If currNode points to the first node, this means the aircraft is not on the taxi node
163 // yet. Make sure to return a unique identifyer in this situation though, because otherwise
164 // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
165 // taxi route or not. the negative of the preceding route seems a logical choice, as it is
166 // unique for any starting location.
167 // Note that this is probably just a temporary fix until I get Parking / tower control working.
168 *rte = -1 * *(currRoute);
174 void FGTaxiRoute::rewind(int route)
180 if (!(next(&currPoint, &currRoute))) {
181 SG_LOG(SG_GENERAL, SG_ALERT,
182 "Error in rewinding TaxiRoute: current" << currRoute <<
185 } while (currRoute != route);
191 /***************************************************************************
193 **************************************************************************/
194 bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b)
199 bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b)
204 FGGroundNetwork::FGGroundNetwork()
212 currTraffic = activeTraffic.begin();
217 FGGroundNetwork::~FGGroundNetwork()
219 for (FGTaxiNodeVectorIterator node = nodes.begin();
220 node != nodes.end(); node++) {
224 pushBackNodes.clear();
225 for (FGTaxiSegmentVectorIterator seg = segments.begin();
226 seg != segments.end(); seg++) {
232 void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
234 segments.push_back(new FGTaxiSegment(seg));
237 void FGGroundNetwork::addNode(const FGTaxiNode & node)
239 nodes.push_back(new FGTaxiNode(node));
242 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
245 FGParkingVecIterator i = parkings->begin();
246 while (i != parkings->end()) {
247 n.setIndex(i->getIndex());
248 n.setLatitude(i->getLatitude());
249 n.setLongitude(i->getLongitude());
250 n.setElevation(parent->getElevation());
251 nodes.push_back(new FGTaxiNode(n));
259 void FGGroundNetwork::init()
263 sort(nodes.begin(), nodes.end(), compare_nodes);
264 //sort(segments.begin(), segments.end(), compare_segments());
265 FGTaxiSegmentVectorIterator i = segments.begin();
266 while (i != segments.end()) {
267 (*i)->setStart(&nodes);
268 (*i)->setEnd(&nodes);
269 (*i)->setDimensions(parent->getElevation());
270 (*i)->setIndex(index);
271 if ((*i)->isPushBack()) {
272 pushBackNodes.push_back((*i)->getEnd());
274 //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl);
275 //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl);
276 //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to "
277 // << (*i)->getEnd()->getIndex() << endl);
282 i = segments.begin();
283 while (i != segments.end()) {
284 FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
285 while (j != (*i)->getEnd()->getEndRoute()) {
286 if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) {
287 // int start1 = (*i)->getStart()->getIndex();
288 // int end1 = (*i)->getEnd() ->getIndex();
289 // int start2 = (*j)->getStart()->getIndex();
290 // int end2 = (*j)->getEnd()->getIndex();
291 // int oppIndex = (*j)->getIndex();
292 //cerr << "Opposite of " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
293 // << "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl;
294 (*i)->setOpposite(*j);
301 //FGTaxiNodeVectorIterator j = nodes.begin();
302 //while (j != nodes.end()) {
303 // if ((*j)->getHoldPointType() == 3) {
304 // pushBackNodes.push_back((*j));
308 //cerr << "Done initializing ground network" << endl;
312 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
314 double minDist = HUGE_VAL;
317 for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
319 double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
322 index = (*itr)->getIndex();
323 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
330 int FGGroundNetwork::findNearestNode(double lat, double lon)
332 return findNearestNode(SGGeod::fromDeg(lon, lat));
335 FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
337 for (FGTaxiNodeVectorIterator
339 itr != nodes.end(); itr++)
341 if (itr->getIndex() == idx)
342 return itr->getAddress();
345 if ((idx >= 0) && (idx < nodes.size()))
346 return nodes[idx]->getAddress();
351 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
353 for (FGTaxiSegmentVectorIterator
354 itr = segments.begin();
355 itr != segments.end(); itr++)
357 if (itr->getIndex() == idx)
358 return itr->getAddress();
361 if ((idx > 0) && (idx <= segments.size()))
362 return segments[idx - 1]->getAddress();
364 //cerr << "Alert: trying to find invalid segment " << idx << endl;
370 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
373 //implements Dijkstra's algorithm to find shortest distance route from start to end
374 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
376 //double INFINITE = 100000000000.0;
377 // initialize scoring values
378 int nParkings = parent->getDynamics()->getNrOfParkings();
379 FGTaxiNodeVector *currNodesSet;
381 currNodesSet = &nodes;
383 currNodesSet = &pushBackNodes;
386 for (FGTaxiNodeVectorIterator
387 itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
388 (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
389 (*itr)->setPreviousNode(0); //
390 (*itr)->setPreviousSeg(0); //
393 FGTaxiNode *firstNode = findNode(start);
394 firstNode->setPathScore(0);
396 FGTaxiNode *lastNode = findNode(end);
398 FGTaxiNodeVector unvisited(*currNodesSet); // working copy
400 while (!unvisited.empty()) {
401 FGTaxiNode *best = *(unvisited.begin());
402 for (FGTaxiNodeVectorIterator
403 itr = unvisited.begin(); itr != unvisited.end(); itr++) {
404 if ((*itr)->getPathScore() < best->getPathScore())
408 FGTaxiNodeVectorIterator newend =
409 remove(unvisited.begin(), unvisited.end(), best);
410 unvisited.erase(newend, unvisited.end());
412 if (best == lastNode) { // found route or best not connected
415 for (FGTaxiSegmentVectorIterator
416 seg = best->getBeginRoute();
417 seg != best->getEndRoute(); seg++) {
418 if (fullSearch || (*seg)->isPushBack()) {
419 FGTaxiNode *tgt = (*seg)->getEnd();
421 best->getPathScore() + (*seg)->getLength() +
422 (*seg)->getPenalty(nParkings);
423 if (alt < tgt->getPathScore()) { // Relax (u,v)
424 tgt->setPathScore(alt);
425 tgt->setPreviousNode(best);
426 tgt->setPreviousSeg(*seg); //
429 // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
435 if (lastNode->getPathScore() == HUGE_VAL) {
436 // no valid route found
438 SG_LOG(SG_GENERAL, SG_ALERT,
439 "Failed to find route from waypoint " << start << " to "
440 << end << " at " << parent->getId());
444 //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
446 // assemble route from backtrace information
447 intVec nodes, routes;
448 FGTaxiNode *bt = lastNode;
449 while (bt->getPreviousNode() != 0) {
450 nodes.push_back(bt->getIndex());
451 routes.push_back(bt->getPreviousSegment()->getIndex());
452 bt = bt->getPreviousNode();
454 nodes.push_back(start);
455 reverse(nodes.begin(), nodes.end());
456 reverse(routes.begin(), routes.end());
458 return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
462 int FGTaxiSegment::getPenalty(int nGates)
465 if (end->getIndex() < nGates) {
468 if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
474 /* ATC Related Functions */
476 void FGGroundNetwork::announcePosition(int id,
477 FGAIFlightPlan * intendedRoute,
478 int currentPosition, double lat,
479 double lon, double heading,
480 double speed, double alt,
481 double radius, int leg,
482 FGAIAircraft * aircraft)
485 TrafficVectorIterator i = activeTraffic.begin();
486 // Search search if the current id alread has an entry
487 // This might be faster using a map instead of a vector, but let's start by taking a safe route
488 if (activeTraffic.size()) {
489 //while ((i->getId() != id) && i != activeTraffic.end()) {
490 while (i != activeTraffic.end()) {
491 if (i->getId() == id) {
497 // Add a new TrafficRecord if no one exsists for this aircraft.
498 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
502 rec.setPositionAndIntentions(currentPosition, intendedRoute);
503 rec.setPositionAndHeading(lat, lon, heading, speed, alt);
504 rec.setRadius(radius); // only need to do this when creating the record.
505 rec.setAircraft(aircraft);
506 activeTraffic.push_back(rec);
508 i->setPositionAndIntentions(currentPosition, intendedRoute);
509 i->setPositionAndHeading(lat, lon, heading, speed, alt);
513 void FGGroundNetwork::signOff(int id)
515 TrafficVectorIterator i = activeTraffic.begin();
516 // Search search if the current id alread has an entry
517 // This might be faster using a map instead of a vector, but let's start by taking a safe route
518 if (activeTraffic.size()) {
519 //while ((i->getId() != id) && i != activeTraffic.end()) {
520 while (i != activeTraffic.end()) {
521 if (i->getId() == id) {
527 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
528 SG_LOG(SG_GENERAL, SG_ALERT,
529 "AI error: Aircraft without traffic record is signing off");
531 i = activeTraffic.erase(i);
535 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
538 int state = i->getState();
539 if ((state >= minState) && (state <= maxState) && available) {
540 if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
542 //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
543 static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
544 int n = trans_num->getIntValue();
546 trans_num->setIntValue(-1);
548 cerr << "Selected transmission message " << n << endl;
550 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
551 transmit(&(*i), msgId, msgDir, false);
555 //cerr << "Transmitting startup msg" << endl;
556 transmit(&(*i), msgId, msgDir, true);
558 lastTransmission = now;
565 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
566 double heading, double speed, double alt,
569 // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
570 // Transmit air-to-ground "Ready to taxi request:
571 // Transmit ground to air approval / hold
572 // Transmit confirmation ...
573 // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
576 TrafficVectorIterator i = activeTraffic.begin();
577 // Search search if the current id has an entry
578 // This might be faster using a map instead of a vector, but let's start by taking a safe route
579 TrafficVectorIterator current, closest;
580 if (activeTraffic.size()) {
581 //while ((i->getId() != id) && i != activeTraffic.end()) {
582 while (i != activeTraffic.end()) {
583 if (i->getId() == id) {
589 // update position of the current aircraft
590 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
591 SG_LOG(SG_GENERAL, SG_ALERT,
592 "AI error: updating aircraft without traffic record");
594 i->setPositionAndHeading(lat, lon, heading, speed, alt);
600 // Update every three secs, but add some randomness
601 // to prevent all IA objects doing this in synchrony
602 //if (getDt() < (3.0) + (rand() % 10))
606 current->clearResolveCircularWait();
607 current->setWaitsForId(0);
608 checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
609 bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
610 if (!needsTaxiClearance) {
611 checkHoldPosition(id, lat, lon, heading, speed, alt);
612 if (checkForCircularWaits(id)) {
613 i->setResolveCircularWait();
616 current->setHoldPosition(true);
617 int state = current->getState();
618 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
619 if ((now - lastTransmission) > 15) {
622 if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
623 current->setState(3);
625 if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
626 current->setState(4);
628 if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
629 current->setState(5);
631 if ((state == 5) && available) {
632 current->setState(0);
633 current->getAircraft()->setTaxiClearanceRequest(false);
634 current->setHoldPosition(true);
643 Scan for a speed adjustment change. Find the nearest aircraft that is in front
644 and adjust speed when we get too close. Only do this when current position and/or
645 intentions of the current aircraft match current taxiroute position of the proximate
646 aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
647 instruction. See below for the hold position instruction.
649 Note that there currently still is one flaw in the logic that needs to be addressed.
650 There can be situations where one aircraft is in front of the current aircraft, on a separate
651 route, but really close after an intersection coming off the current route. This
652 aircraft is still close enough to block the current aircraft. This situation is currently
653 not addressed yet, but should be.
656 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
657 double lon, double heading,
658 double speed, double alt)
661 TrafficVectorIterator current, closest;
662 TrafficVectorIterator i = activeTraffic.begin();
663 bool otherReasonToSlowDown = false;
664 bool previousInstruction;
665 if (activeTraffic.size()) {
666 //while ((i->getId() != id) && (i != activeTraffic.end()))
667 while (i != activeTraffic.end()) {
668 if (i->getId() == id) {
676 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
677 SG_LOG(SG_GENERAL, SG_ALERT,
678 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
683 previousInstruction = current->getSpeedAdjustment();
684 double mindist = HUGE_VAL;
685 if (activeTraffic.size()) {
686 double course, dist, bearing, minbearing, az2;
687 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
688 //TrafficVector iterator closest;
690 for (TrafficVectorIterator i = activeTraffic.begin();
691 i != activeTraffic.end(); i++) {
696 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
699 SGGeodesy::inverse(curr, other, course, az2, dist);
700 bearing = fabs(heading - course);
702 bearing = 360 - bearing;
703 if ((dist < mindist) && (bearing < 60.0)) {
706 minbearing = bearing;
709 //Check traffic at the tower controller
710 if (towerController->hasActiveTraffic()) {
711 for (TrafficVectorIterator i =
712 towerController->getActiveTraffic().begin();
713 i != towerController->getActiveTraffic().end(); i++) {
714 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
715 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
718 SGGeodesy::inverse(curr, other, course, az2, dist);
719 bearing = fabs(heading - course);
721 bearing = 360 - bearing;
722 if ((dist < mindist) && (bearing < 60.0)) {
725 minbearing = bearing;
726 otherReasonToSlowDown = true;
730 // Finally, check UserPosition
731 double userLatitude = fgGetDouble("/position/latitude-deg");
732 double userLongitude = fgGetDouble("/position/longitude-deg");
733 SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
734 SGGeodesy::inverse(curr, user, course, az2, dist);
736 bearing = fabs(heading - course);
738 bearing = 360 - bearing;
739 if ((dist < mindist) && (bearing < 60.0)) {
742 minbearing = bearing;
743 otherReasonToSlowDown = true;
746 current->clearSpeedAdjustment();
748 if (current->checkPositionAndIntentions(*closest)
749 || otherReasonToSlowDown) {
750 double maxAllowableDistance =
751 (1.1 * current->getRadius()) +
752 (1.1 * closest->getRadius());
753 if (mindist < 2 * maxAllowableDistance) {
754 if (current->getId() == closest->getWaitsForId())
757 current->setWaitsForId(closest->getId());
758 if (closest->getId() != current->getId())
759 current->setSpeedAdjustment(closest->getSpeed() *
762 current->setSpeedAdjustment(0); // This can only happen when the user aircraft is the one closest
763 if (mindist < maxAllowableDistance) {
764 //double newSpeed = (maxAllowableDistance-mindist);
765 //current->setSpeedAdjustment(newSpeed);
766 //if (mindist < 0.5* maxAllowableDistance)
768 current->setSpeedAdjustment(0);
777 Check for "Hold position instruction".
778 The hold position should be issued under the following conditions:
779 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
780 2) For taxiing aircraft that use one taxiway in opposite directions
781 3) For crossing or merging taxiroutes.
784 void FGGroundNetwork::checkHoldPosition(int id, double lat,
785 double lon, double heading,
786 double speed, double alt)
788 TrafficVectorIterator current;
789 TrafficVectorIterator i = activeTraffic.begin();
790 if (activeTraffic.size()) {
791 //while ((i->getId() != id) && i != activeTraffic.end())
792 while (i != activeTraffic.end()) {
793 if (i->getId() == id) {
801 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
802 SG_LOG(SG_GENERAL, SG_ALERT,
803 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
806 bool origStatus = current->hasHoldPosition();
807 current->setHoldPosition(false);
808 SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
810 for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
811 if (i->getId() != current->getId()) {
812 int node = current->crosses(this, *i);
814 FGTaxiNode *taxiNode = findNode(node);
816 // Determine whether it's save to continue or not.
817 // If we have a crossing route, there are two possibilities:
818 // 1) This is an interestion
819 // 2) This is oncoming two-way traffic, using the same taxiway.
820 //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
822 SGGeod other(SGGeod::
823 fromDegM(i->getLongitude(), i->getLatitude(),
827 if (current->isOpposing(this, *i, node)) {
830 //cerr << "Hold check 2 : " << node << " has opposing segment " << endl;
831 // issue a "Hold Position" as soon as we're close to the offending node
832 // For now, I'm doing this as long as the other aircraft doesn't
833 // have a hold instruction as soon as we're within a reasonable
834 // distance from the offending node.
835 // This may be a bit of a conservative estimate though, as it may
836 // be well possible that both aircraft can both continue to taxi
837 // without crashing into each other.
840 if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
843 //cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
847 //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
852 SGGeodesy::distanceM(curr, taxiNode->getGeod());
853 if (!(i->hasHoldPosition())) {
855 if ((dist < 200) && //2.5*current->getRadius()) &&
856 (needsToWait) && (i->onRoute(this, *current)) &&
857 //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
858 (!(current->getId() == i->getWaitsForId())))
859 //(!(i->getSpeedAdjustment()))) // &&
860 //(!(current->getSpeedAdjustment())))
863 current->setHoldPosition(true);
864 current->setWaitsForId(i->getId());
865 //cerr << "Hold check 5: " << current->getCallSign() <<" Setting Hold Position: distance to node (" << node << ") "
866 // << dist << " meters. Waiting for " << i->getCallSign();
868 //cerr <<" [opposing] " << endl;
870 // cerr << "[non-opposing] " << endl;
871 //if (i->hasSpeefAdjustment())
873 // cerr << " (which in turn waits for ) " << i->
875 //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl;
881 bool currStatus = current->hasHoldPosition();
883 // Either a Hold Position or a resume taxi transmission has been issued
884 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
885 if ((now - lastTransmission) > 2) {
888 if ((origStatus != currStatus) && available) {
889 //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
890 if (currStatus == true) { // No has a hold short instruction
891 transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
892 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
893 current->setState(1);
895 transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
896 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
897 current->setState(2);
899 lastTransmission = now;
901 // Don't act on the changed instruction until the transmission is confirmed
902 // So set back to original status
903 current->setHoldPosition(origStatus);
904 //cerr << "Current state " << current->getState() << endl;
907 //int state = current->getState();
908 if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
909 current->setState(0);
910 current->setHoldPosition(true);
912 if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
913 current->setState(0);
914 current->setHoldPosition(false);
917 /*if ((state == 1) && (available)) {
918 //cerr << "ACKNOWLEDGE HOLD" << endl;
919 transmit(&(*current), MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND, true);
920 current->setState(0);
921 current->setHoldPosition(true);
922 lastTransmission = now;
926 if ((state == 2) && (available)) {
927 //cerr << "ACKNOWLEDGE RESUME" << endl;
928 transmit(&(*current), MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND, true);
929 current->setState(0);
930 current->setHoldPosition(false);
931 lastTransmission = now;
937 * Check whether situations occur where the current aircraft is waiting for itself
938 * due to higher order interactions.
939 * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
940 * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
941 * through this list of waiting aircraft, we can check if we'd eventually end back
942 * at the current aircraft.
944 * Note that we should consider the situation where we are actually checking aircraft
945 * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
946 * the looping aircraft. If we don't check for that, this function will get stuck into
950 bool FGGroundNetwork::checkForCircularWaits(int id)
952 //cerr << "Performing Wait check " << id << endl;
954 TrafficVectorIterator current, other;
955 TrafficVectorIterator i = activeTraffic.begin();
956 int trafficSize = activeTraffic.size();
958 while (i != activeTraffic.end()) {
959 if (i->getId() == id) {
967 if (i == activeTraffic.end() || (trafficSize == 0)) {
968 SG_LOG(SG_GENERAL, SG_ALERT,
969 "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
973 target = current->getWaitsForId();
974 //bool printed = false; // Note that this variable is for debugging purposes only.
978 //cerr << "aircraft waits for user" << endl;
983 while ((target > 0) && (target != id) && counter++ < trafficSize) {
985 TrafficVectorIterator i = activeTraffic.begin();
987 //while ((i->getId() != id) && i != activeTraffic.end())
988 while (i != activeTraffic.end()) {
989 if (i->getId() == target) {
997 if (i == activeTraffic.end() || (trafficSize == 0)) {
998 //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
999 // The target id is not found on the current network, which means it's at the tower
1000 //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
1004 target = other->getWaitsForId();
1006 // actually this trap isn't as impossible as it first seemed:
1007 // the setWaitsForID(id) is set to current when the aircraft
1008 // is waiting for the user controlled aircraft.
1009 //if (current->getId() == other->getId()) {
1010 // cerr << "Caught the impossible trap" << endl;
1011 // cerr << "Current = " << current->getId() << endl;
1012 // cerr << "Other = " << other ->getId() << endl;
1013 // for (TrafficVectorIterator at = activeTraffic.begin();
1014 // at != activeTraffic.end();
1016 // cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1019 if (current->getId() == other->getId())
1022 //cerr << current->getCallSign() << " (" << current->getId() << ") " << " -> " << other->getCallSign()
1023 // << " (" << other->getId() << "); " << endl;;
1033 // cerr << "[done] " << endl << endl;;
1035 SG_LOG(SG_GENERAL, SG_WARN,
1036 "Detected circular wait condition: Id = " << id <<
1037 "target = " << target);
1044 // Note that this function is probably obsolete...
1045 bool FGGroundNetwork::hasInstruction(int id)
1047 TrafficVectorIterator i = activeTraffic.begin();
1048 // Search search if the current id has an entry
1049 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1050 if (activeTraffic.size()) {
1051 //while ((i->getId() != id) && i != activeTraffic.end()) {
1052 while (i != activeTraffic.end()) {
1053 if (i->getId() == id) {
1059 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1060 SG_LOG(SG_GENERAL, SG_ALERT,
1061 "AI error: checking ATC instruction for aircraft without traffic record");
1063 return i->hasInstruction();
1068 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1070 TrafficVectorIterator i = activeTraffic.begin();
1071 // Search search if the current id has an entry
1072 // This might be faster using a map instead of a vector, but let's start by taking a safe route
1073 if (activeTraffic.size()) {
1074 //while ((i->getId() != id) && i != activeTraffic.end()) {
1075 while (i != activeTraffic.end()) {
1076 if (i->getId() == id) {
1082 if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1083 SG_LOG(SG_GENERAL, SG_ALERT,
1084 "AI error: requesting ATC instruction for aircraft without traffic record");
1086 return i->getInstruction();
1088 return FGATCInstruction();
1091 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1092 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1093 double lon, double elev, double hdg)
1095 SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1096 obj_pos = geod.makeZUpFrame();
1097 // hdg is not a compass heading, but a counter-clockwise rotation
1098 // around the Z axis
1099 obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1106 void FGGroundNetwork::render(bool visible)
1109 SGMaterialLib *matlib = globals->get_matlib();
1112 globals->get_scenery()->get_scene_graph()->removeChild(group);
1113 //while (group->getNumChildren()) {
1114 // cerr << "Number of children: " << group->getNumChildren() << endl;
1115 simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1116 //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1117 //geode->releaseGLObjects();
1118 //group->removeChild(geode);
1123 group = new osg::Group;
1125 //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1127 for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1128 // Handle start point
1129 int pos = i->getCurrentPosition() - 1;
1132 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1133 SGGeod end (SGGeod::fromDeg(segments[pos]->getEnd()->getLongitude(), segments[pos]->getEnd()->getLatitude()));
1135 double length = SGGeodesy::distanceM(start, end);
1136 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
1138 double az2, heading; //, distanceM;
1139 SGGeodesy::inverse(start, end, heading, az2, length);
1140 double coveredDistance = length * 0.5;
1142 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1143 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1144 ///////////////////////////////////////////////////////////////////////////////
1145 // Make a helper function out of this
1146 osg::Matrix obj_pos;
1147 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1148 obj_trans->setDataVariance(osg::Object::STATIC);
1150 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), parent->elevation()+8+dx, -(heading) );
1152 obj_trans->setMatrix( obj_pos );
1153 //osg::Vec3 center(0, 0, 0)
1155 float width = length /2.0;
1156 osg::Vec3 corner(-width, 0, 0.25f);
1157 osg::Vec3 widthVec(2*width + 1, 0, 0);
1158 osg::Vec3 heightVec(0, 1, 0);
1159 osg::Geometry* geometry;
1160 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1161 simgear::EffectGeode* geode = new simgear::EffectGeode;
1162 geode->setName("test");
1163 geode->addDrawable(geometry);
1164 //osg::Node *custom_obj;
1165 SGMaterial *mat = matlib->find("UnidirectionalTaper");
1167 geode->setEffect(mat->get_effect());
1168 obj_trans->addChild(geode);
1169 // wire as much of the scene graph together as we can
1170 //->addChild( obj_trans );
1171 group->addChild( obj_trans );
1172 /////////////////////////////////////////////////////////////////////
1174 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1176 for(intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1177 osg::Matrix obj_pos;
1180 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1181 obj_trans->setDataVariance(osg::Object::STATIC);
1183 WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), parent->elevation()+8+dx, -(segments[k]->getHeading()) );
1185 obj_trans->setMatrix( obj_pos );
1186 //osg::Vec3 center(0, 0, 0)
1188 float width = segments[k]->getLength() /2.0;
1189 osg::Vec3 corner(-width, 0, 0.25f);
1190 osg::Vec3 widthVec(2*width + 1, 0, 0);
1191 osg::Vec3 heightVec(0, 1, 0);
1192 osg::Geometry* geometry;
1193 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1194 simgear::EffectGeode* geode = new simgear::EffectGeode;
1195 geode->setName("test");
1196 geode->addDrawable(geometry);
1197 //osg::Node *custom_obj;
1198 SGMaterial *mat = matlib->find("UnidirectionalTaper");
1200 geode->setEffect(mat->get_effect());
1201 obj_trans->addChild(geode);
1202 // wire as much of the scene graph together as we can
1203 //->addChild( obj_trans );
1204 group->addChild( obj_trans );
1209 globals->get_scenery()->get_scene_graph()->addChild(group);