From 1c8f2e3c5b7e5467aa858c735568d033bb309aef Mon Sep 17 00:00:00 2001 From: durk Date: Wed, 8 Aug 2007 06:09:58 +0000 Subject: [PATCH 1/1] - Ground network XML parsing code reads the new attributes "holdPointType" and "isOnRunway". - Added initial support for AI controlled pushback operations, making use of the current editing capabilities of TaxiDraw CVS / New_GUI_CODE. The current implementation is slightly more computationally intensive than strictly required, due to the currently inability of taxidraw to link one specific pushBack point to to a particular startup location. FlightGear now determines this dynamically, and once we have that functionality in TaxiDraw, the initialization part of createPushBack() can be further simplified. - Smoother transition from pushback to taxi. No more skipping of waypoints, and aircraft wait for two minutes at pushback point. - The classes FGTaxiNode, FGTaxiSegment, and FGParking, now have copy constructors, and assignment operators. - Removed declaration of undefined constructor FGTaxiNode(double, double, int) - Array boundry checks and cleanup. - Modified Dijkstra path search algoritm to solve partial problems. Currently limited to include pushback points and routes only, but can probably be extended to a more general approach. - Added initial support for giving certain routes in the network a penalty, in order to discourage the use of certain routes over others. --- src/AIModel/AIAircraft.cxx | 7 +- src/AIModel/AIFlightPlan.hxx | 3 +- src/AIModel/AIFlightPlanCreate.cxx | 157 +++------- src/AIModel/AIFlightPlanCreatePushBack.cxx | 318 +++++++++++++++++++++ src/AIModel/Makefile.am | 4 +- src/Airports/dynamicloader.cxx | 21 ++ src/Airports/dynamics.cxx | 2 +- src/Airports/dynamics.hxx | 1 + src/Airports/gnnode.cxx | 4 +- src/Airports/gnnode.hxx | 84 ++++-- src/Airports/groundnetwork.cxx | 107 ++++--- src/Airports/groundnetwork.hxx | 72 ++++- src/Airports/parking.cxx | 32 ++- src/Airports/parking.hxx | 72 ++++- 14 files changed, 670 insertions(+), 214 deletions(-) create mode 100644 src/AIModel/AIFlightPlanCreatePushBack.cxx diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index 05860b603..2228f83d9 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -1,4 +1,4 @@ -// FGAIAircraft - FGAIBase-derived class creates an AI airplane +// // FGAIAircraft - FGAIBase-derived class creates an AI airplane // // Written by David Culp, started October 2003. // @@ -656,8 +656,11 @@ bool FGAIAircraft::handleAirportEndPoints(FGAIFlightPlan::waypoint* prev, time_t // This waypoint marks the fact that the aircraft has passed the initial taxi // departure waypoint, so it can release the parking. - if (prev->name == "park2") { + if (prev->name == "PushBackPoint") { dep->getDynamics()->releaseParking(fp->getGate()); + time_t holdUntil = now + 120; + fp->setTime(holdUntil); + //cerr << _getCallsign() << "Holding at pushback point" << endl; } // This is the last taxi waypoint, and marks the the end of the flight plan diff --git a/src/AIModel/AIFlightPlan.hxx b/src/AIModel/AIFlightPlan.hxx index 96fc03002..f4b078590 100644 --- a/src/AIModel/AIFlightPlan.hxx +++ b/src/AIModel/AIFlightPlan.hxx @@ -118,12 +118,13 @@ private: double leadInAngle; time_t start_time; int leg; - int gateId; + int gateId, lastNodeVisited; string activeRunway; FGAirRoute airRoute; FGTaxiRoute *taxiRoute; void createPushBack(bool, FGAirport*, double, double, double, const string&, const string&, const string&); + void createPushBackFallBack(bool, FGAirport*, double, double, double, const string&, const string&, const string&); void createTaxi(bool, int, FGAirport *, double, double, double, const string&, const string&, const string&); void createTakeOff(bool, FGAirport *, double, const string&); void createClimb(bool, FGAirport *, double, double, const string&); diff --git a/src/AIModel/AIFlightPlanCreate.cxx b/src/AIModel/AIFlightPlanCreate.cxx index d615f4b54..98da9247c 100644 --- a/src/AIModel/AIFlightPlanCreate.cxx +++ b/src/AIModel/AIFlightPlanCreate.cxx @@ -83,100 +83,10 @@ void FGAIFlightPlan::create(FGAirport *dep, FGAirport *arr, int legNr, leg++; } -/******************************************************************* - * createPushBack - * initialize the Aircraft at the parking location - ******************************************************************/ -void FGAIFlightPlan::createPushBack(bool firstFlight, FGAirport *dep, - double latitude, - double longitude, - double radius, - const string& fltType, - const string& aircraftType, - const string& airline) -{ - double heading; - double lat; - double lon; - double lat2; - double lon2; - double az2; - - //int currWpt = wpt_iterator - waypoints.begin(); - // Erase all existing waypoints. - //resetWaypoints(); - - // We only need to get a valid parking if this is the first leg. - // Otherwise use the current aircraft position. - if (firstFlight) - { - if (!(dep->getDynamics()->getAvailableParking(&lat, &lon, - &heading, &gateId, - radius, fltType, - aircraftType, airline))) - { - SG_LOG(SG_INPUT, SG_ALERT, "Could not find parking for a " << - aircraftType << - " of flight type " << fltType << - " of airline " << airline << - " at airport " << dep->getId()); - //exit(1); - } - } - else - { - dep->getDynamics()->getParking(gateId, &lat, &lon, &heading); - } - heading += 180.0; - if (heading > 360) - heading -= 360; - waypoint *wpt = new waypoint; - wpt->name = "park"; - wpt->latitude = lat; - wpt->longitude = lon; - wpt->altitude = dep->getElevation(); - wpt->speed = -10; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - waypoints.push_back(wpt); - - geo_direct_wgs_84 ( 0, lat, lon, heading, - 10, - &lat2, &lon2, &az2 ); - wpt = new waypoint; - wpt->name = "park2"; - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = dep->getElevation(); - wpt->speed = -10; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; - waypoints.push_back(wpt); - geo_direct_wgs_84 ( 0, lat, lon, heading, - 2.2*radius, - &lat2, &lon2, &az2 ); - wpt = new waypoint; - wpt->name = "taxiStart"; - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = dep->getElevation(); - wpt->speed = 10; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; - waypoints.push_back(wpt); -} + + + /******************************************************************* * createCreate Taxi. @@ -259,11 +169,29 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, //FGTaxiRoute route; delete taxiRoute; taxiRoute = new FGTaxiRoute; - if (gateId >= 0) - *taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(gateId, - runwayId); - else - *taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(0, runwayId); + + // Determine which node to start from. + int node; + // Find out which node to start from + FGParking *park = apt->getDynamics()->getParking(gateId); + if (park) + node = park->getPushBackPoint(); + else + node = 0; + if (node == -1) + node = gateId; + + // HAndle case where parking doens't have a node + if ((node == 0) && park) { + if (firstFlight) { + node = gateId; + } else { + node = lastNodeVisited; + } + } + + //cerr << "Using node " << node << endl; + *taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(node, runwayId); intVecIterator i; if (taxiRoute->empty()) { @@ -299,30 +227,27 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, } else { int node; taxiRoute->first(); - bool isPushBackPoint = false; + //bool isPushBackPoint = false; if (firstFlight) { + // If this is called during initialization, randomly // skip a number of waypoints to get a more realistic // taxi situation. - isPushBackPoint = true; + //isPushBackPoint = true; int nrWaypoints = taxiRoute->size(); nrWaypointsToSkip = rand() % nrWaypoints; // but make sure we always keep two active waypoints // to prevent a segmentation fault for (int i = 0; i < nrWaypointsToSkip-2; i++) { - isPushBackPoint = false; + //isPushBackPoint = false; taxiRoute->next(&node); } + apt->getDynamics()->releaseParking(gateId); } else { - //chop off the first two waypoints, because - // those have already been created - // by create pushback - int size = taxiRoute->size(); - if (size > 2) { - taxiRoute->next(&node); - taxiRoute->next(&node); - } - } + if (taxiRoute->size() > 1) { + taxiRoute->next(&node); // chop off the first waypoint, because that is already the last of the pushback route + } + } int route; while(taxiRoute->next(&node, &route)) { @@ -338,13 +263,7 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, // Elevation is currently disregarded when on_ground is true // because the AIModel obtains a periodic ground elevation estimate. wpt->altitude = apt->getElevation(); - if (isPushBackPoint) { - wpt->speed = -10; - isPushBackPoint = false; - } - else { - wpt->speed = 15; - } + wpt->speed = 15; wpt->crossat = -10000; wpt->gear_down = true; wpt->flaps_down= true; @@ -353,6 +272,7 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, wpt->routeIndex = route; waypoints.push_back(wpt); } + /* //cerr << endl; // finally, rewind the taxiRoute object to the point where we started // generating the Flightplan, for AI use. @@ -369,7 +289,7 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, //taxiRoute->next(&node); //taxiRoute->next(&node); } - } + }*/ } // taxiRoute not empty } else @@ -1076,4 +996,5 @@ string FGAIFlightPlan::getRunwayClassFromTrafficType(string fltType) if ((fltType == "mil-fighter") || (fltType == "mil-transport")) { return string("mil"); } + return string("com"); } diff --git a/src/AIModel/AIFlightPlanCreatePushBack.cxx b/src/AIModel/AIFlightPlanCreatePushBack.cxx new file mode 100644 index 000000000..e83e0dcbd --- /dev/null +++ b/src/AIModel/AIFlightPlanCreatePushBack.cxx @@ -0,0 +1,318 @@ +/****************************************************************************** + * AIFlightPlanCreatePushBack.cxx + * Written by Durk Talsma, started August 1, 2007. + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License as + * published by the Free Software Foundation; either version 2 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * + **************************************************************************/ +#include "AIFlightPlan.hxx" +#include +#include + +#include +#include + + +void FGAIFlightPlan::createPushBack(bool firstFlight, FGAirport *dep, + double latitude, + double longitude, + double radius, + const string& fltType, + const string& aircraftType, + const string& airline) +{ + double lat, lon, heading; + FGTaxiRoute *pushBackRoute; + if (!(dep->getDynamics()->getGroundNetwork()->exists())) { + //cerr << "Push Back fallback" << endl; + createPushBackFallBack(firstFlight, dep, latitude, longitude, + radius, fltType, aircraftType, airline); + } else { + if (firstFlight) { + if (!(dep->getDynamics()->getAvailableParking(&lat, &lon, + &heading, &gateId, + radius, fltType, + aircraftType, airline))) { + SG_LOG(SG_INPUT, SG_WARN, "Warning: Could not find parking for a " << + aircraftType << + " of flight type " << fltType << + " of airline " << airline << + " at airport " << dep->getId()); + char buffer[10]; + snprintf (buffer, 10, "%d", gateId); + //FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(node); + waypoint *wpt; + wpt = new waypoint; + wpt->name = string(buffer); // fixme: should be the name of the taxiway + wpt->latitude = lat; + wpt->longitude = lon; + // Elevation is currently disregarded when on_ground is true + // because the AIModel obtains a periodic ground elevation estimate. + wpt->altitude = dep->getElevation(); + wpt->speed = -10; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = true; + wpt->routeIndex = -1; + waypoints.push_back(wpt); + } + } else { + //cerr << "Push Back follow-up Flight" << endl; + dep->getDynamics()->getParking(gateId, &lat, &lon, &heading); + } + if (gateId < 0) { + createPushBackFallBack(firstFlight, dep, latitude, longitude, + radius, fltType, aircraftType, airline); + return; + + } + //cerr << "getting parking " << gateId; + //cerr << " for a " << + // aircraftType << + // " of flight type " << fltType << + // " of airline " << airline << + // " at airport " << dep->getId() << endl; + FGParking *parking = dep->getDynamics()->getParking(gateId); + int pushBackNode = parking->getPushBackPoint(); + + // initialize the pushback route. Note that parts + // of this procedure should probably be done inside + // taxidraw. This code is likely to change once this + // this is fully implemented in taxidraw. Until that time, + // however, the full initialization procedure looks like this: + // 1) Build a list of all the nodes that are classified as + // pushBack hold points + // 2) For each hold point, use the dykstra algorithm to find a route + // between the gate and the pushBack hold nodes, however use only + // segments that are classified as "pushback" routes. + // 3) return the TaxiRoute class that is non empty. + // 4) store refer this route in the parking object, for future use + + if (pushBackNode < 0) { + //cerr << "Initializing PushBackRoute " << endl; + intVec pushBackNodes; + int nAINodes = dep->getDynamics()->getGroundNetwork()->getNrOfNodes(); + int hits = 0; + parking->setPushBackPoint(0); // default in case no network was found. + + // Collect all the nodes that are classified as having pushBack hold status + for (int i = 0; i < nAINodes; i++) { + if (dep->getDynamics()->getGroundNetwork()->findNode(i)->getHoldPointType() == 3) { + pushBackNodes.push_back(i); + } + } + + // For each node found in step 1, check if it can be reached + FGTaxiRoute route; + for (intVecIterator nodes = pushBackNodes.begin(); + nodes != pushBackNodes.end(); + nodes++) { + route = dep->getDynamics()->getGroundNetwork()->findShortestRoute(gateId, *nodes, false); + if (!(route.empty())) { + //cerr << "Found Pushback route of size " << route.size() << endl; + hits++; + parking->setPushBackRoute(new FGTaxiRoute(route)); + parking->setPushBackPoint(*nodes); + pushBackNode = *nodes; + } + } + if (hits == 0) { + SG_LOG(SG_GENERAL, SG_INFO, "No pushback route found for gate " << gateId << " at " << dep->getId()); + } + if (hits > 1) { + SG_LOG(SG_GENERAL, SG_WARN, hits << " pushback routes found for gate " << gateId << " at " << dep->getId()); + } + } + if (pushBackNode > 0) { + int node, rte; + //cerr << "Found valid pusback node " << pushBackNode << "for gate " << gateId << endl; + pushBackRoute = parking->getPushBackRoute(); + int size = pushBackRoute->size(); + if (size < 2) { + SG_LOG(SG_GENERAL, SG_WARN, "Push back route from gate " << gateId << " has only " << size << " nodes."); + } + pushBackRoute->first(); + waypoint *wpt; + while(pushBackRoute->next(&node, &rte)) + { + //FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findSegment(node)->getEnd(); + char buffer[10]; + snprintf (buffer, 10, "%d", node); + FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(node); + //ids.pop_back(); + wpt = new waypoint; + wpt->name = string(buffer); // fixme: should be the name of the taxiway + wpt->latitude = tn->getLatitude(); + wpt->longitude = tn->getLongitude(); + // Elevation is currently disregarded when on_ground is true + // because the AIModel obtains a periodic ground elevation estimate. + wpt->altitude = dep->getElevation(); + wpt->speed = -10; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = true; + wpt->routeIndex = rte; + waypoints.push_back(wpt); + } + // some special considerations for the last point: + wpt->name = string("PushBackPoint"); + wpt->speed = 15; + } else { + //cerr << "Creating direct forward departure route fragment" << endl; + double lat2, lon2, az2; + waypoint *wpt; + geo_direct_wgs_84 ( 0, lat, lon, heading, + 2, &lat2, &lon2, &az2 ); + wpt = new waypoint; + wpt->name = "park2"; + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = dep->getElevation(); + wpt->speed = 10; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = true; + wpt->routeIndex = 0; + waypoints.push_back(wpt); + + geo_direct_wgs_84 ( 0, lat, lon, heading, + 4, &lat2, &lon2, &az2 ); + wpt = new waypoint; + wpt->name = "name"; + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = dep->getElevation(); + wpt->speed = 10; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = true; + wpt->routeIndex = 0; + waypoints.push_back(wpt); + + //cerr << "Creating final push forward point for gate " << gateId << endl; + FGTaxiNode *tn = dep->getDynamics()->getGroundNetwork()->findNode(gateId); + FGTaxiSegmentVectorIterator ts = tn->getBeginRoute(); + FGTaxiSegmentVectorIterator te = tn->getEndRoute(); + if (ts == te) { + SG_LOG(SG_GENERAL, SG_ALERT, "Gate " << gateId << "doesn't seem to have routes associated with it."); + //exit(1); + } + tn = (*ts)->getEnd(); + lastNodeVisited = tn->getIndex(); + if (tn == NULL) { + SG_LOG(SG_GENERAL, SG_ALERT, "No valid taxinode found"); + exit(1); + } + wpt = new waypoint; + wpt->name = "PushBackPoint"; + wpt->latitude = tn->getLatitude(); + wpt->longitude = tn->getLongitude(); + wpt->altitude = dep->getElevation(); + wpt->speed = 10; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = true; + wpt->routeIndex = (*ts)->getIndex(); + waypoints.push_back(wpt); + + + } + + } +} +/******************************************************************* + * createPushBackFallBack + * This is the backup function for airports that don't have a + * network yet. + ******************************************************************/ +void FGAIFlightPlan::createPushBackFallBack(bool firstFlight, FGAirport *dep, + double latitude, + double longitude, + double radius, + const string& fltType, + const string& aircraftType, + const string& airline) +{ + double heading; + double lat; + double lon; + double lat2; + double lon2; + double az2; + + + + dep->getDynamics()->getParking(-1, &lat, &lon, &heading); + + heading += 180.0; + if (heading > 360) + heading -= 360; + waypoint *wpt = new waypoint; + wpt->name = "park"; + wpt->latitude = lat; + wpt->longitude = lon; + wpt->altitude = dep->getElevation(); + wpt->speed = -10; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = true; + + waypoints.push_back(wpt); + + geo_direct_wgs_84 ( 0, lat, lon, heading, + 10, + &lat2, &lon2, &az2 ); + wpt = new waypoint; + wpt->name = "park2"; + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = dep->getElevation(); + wpt->speed = -10; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = true; + wpt->routeIndex = 0; + waypoints.push_back(wpt); + geo_direct_wgs_84 ( 0, lat, lon, heading, + 2.2*radius, + &lat2, &lon2, &az2 ); + wpt = new waypoint; + wpt->name = "taxiStart"; + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = dep->getElevation(); + wpt->speed = 10; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = true; + wpt->routeIndex = 0; + waypoints.push_back(wpt); +} diff --git a/src/AIModel/Makefile.am b/src/AIModel/Makefile.am index 30ba83f4b..d4c395500 100644 --- a/src/AIModel/Makefile.am +++ b/src/AIModel/Makefile.am @@ -10,7 +10,9 @@ libAIModel_a_SOURCES = submodel.cxx submodel.hxx \ AIStorm.hxx AIStorm.cxx \ AIThermal.hxx AIThermal.cxx \ AIFlightPlan.hxx AIFlightPlan.cxx \ - AIFlightPlanCreate.cxx AIFlightPlanCreateCruise.cxx \ + AIFlightPlanCreate.cxx \ + AIFlightPlanCreatePushBack.cxx \ + AIFlightPlanCreateCruise.cxx \ AICarrier.hxx AICarrier.cxx \ AIStatic.hxx AIStatic.cxx \ AITanker.cxx AITanker.hxx \ diff --git a/src/Airports/dynamicloader.cxx b/src/Airports/dynamicloader.cxx index 5f111d1d8..53027f63d 100644 --- a/src/Airports/dynamicloader.cxx +++ b/src/Airports/dynamicloader.cxx @@ -38,8 +38,11 @@ void FGAirportDynamicsXMLLoader::startElement (const char * name, const XMLAttr string value; string gateName; string gateNumber; + string attval; string lat; string lon; + int holdPointType; + if (name == string("Parking")) { for (int i = 0; i < atts.size(); i++) @@ -84,6 +87,22 @@ void FGAirportDynamicsXMLLoader::startElement (const char * name, const XMLAttr taxiNode.setLatitude(atts.getValue(i)); if (attname == string("lon")) taxiNode.setLongitude(atts.getValue(i)); + if (attname == string("isOnRunway")) + taxiNode.setOnRunway((bool) atoi(atts.getValue(i))); + if (attname == string("holdPointType")) { + attval = atts.getValue(i); + if (attval==string("none")) { + holdPointType=0; + } else if (attval==string("normal")) { + holdPointType=1; + } else if (attval==string("CAT II/III")) { + holdPointType=3; + } else if (attval==string("PushBack")) { + holdPointType=3; + } + //cerr << "Setting Holding point to " << holdPointType << endl; + taxiNode.setHoldPointType(holdPointType); + } } _dynamics->getGroundNetwork()->addNode(taxiNode); } @@ -97,6 +116,8 @@ void FGAirportDynamicsXMLLoader::startElement (const char * name, const XMLAttr taxiSegment.setStartNodeRef(atoi(atts.getValue(i))); if (attname == string("end")) taxiSegment.setEndNodeRef(atoi(atts.getValue(i))); + if (attname == string("isPushBackRoute")) + taxiSegment.setPushBackType((bool) atoi(atts.getValue(i))); } _dynamics->getGroundNetwork()->addSegment(taxiSegment); } diff --git a/src/Airports/dynamics.cxx b/src/Airports/dynamics.cxx index a7d450ccf..0123013ca 100644 --- a/src/Airports/dynamics.cxx +++ b/src/Airports/dynamics.cxx @@ -305,7 +305,7 @@ void FGAirportDynamics::getParking (int id, double *lat, double* lon, double *he FGParking *FGAirportDynamics::getParking(int i) { - if (i < (int)parkings.size()) + if (i < (int)parkings.size() && (i >= 0)) return &(parkings[i]); else return 0; diff --git a/src/Airports/dynamics.hxx b/src/Airports/dynamics.hxx index 4d42b4865..0e5d6f438 100644 --- a/src/Airports/dynamics.hxx +++ b/src/Airports/dynamics.hxx @@ -86,6 +86,7 @@ public: FGParking *getParking(int i); void releaseParking(int id); string getParkingName(int i); + int getNrOfParkings() { return parkings.size(); }; //FGAirport *getAddress() { return this; }; //const string &getName() const { return _name;}; // Returns degrees diff --git a/src/Airports/gnnode.cxx b/src/Airports/gnnode.cxx index b3b34d960..d82ae28fb 100644 --- a/src/Airports/gnnode.cxx +++ b/src/Airports/gnnode.cxx @@ -42,9 +42,7 @@ bool sortByLength(FGTaxiSegment *a, FGTaxiSegment *b) { /************************************************************************** * FGTaxiNode *************************************************************************/ -FGTaxiNode::FGTaxiNode() -{ -} + void FGTaxiNode::sortEndSegments(bool byLength) { diff --git a/src/Airports/gnnode.hxx b/src/Airports/gnnode.hxx index 54dd66284..9912600e6 100644 --- a/src/Airports/gnnode.hxx +++ b/src/Airports/gnnode.hxx @@ -37,23 +37,80 @@ private: double lat; double lon; int index; + + bool isOnRunway; + int holdType; FGTaxiSegmentVector next; // a vector of pointers to all the segments leaving from this node - + + // used in way finding + double pathScore; + FGTaxiNode* previousNode; + FGTaxiSegment* previousSeg; + public: - FGTaxiNode(); - FGTaxiNode(double, double, int); - - void setIndex(int idx) { index = idx;}; - void setLatitude (double val) { lat = val;}; - void setLongitude(double val) { lon = val;}; - void setLatitude (const string& val) { lat = processPosition(val); }; - void setLongitude(const string& val) { lon = processPosition(val); }; - void addSegment(FGTaxiSegment *segment) { next.push_back(segment); }; - + FGTaxiNode() : + lat (0.0), + lon (0.0), + index(0), + isOnRunway(false), + holdType(0), + pathScore(0), + previousNode(0), + previousSeg(0) +{ +}; + + FGTaxiNode(const FGTaxiNode &other) : + lat(other.lat), + lon(other.lon), + index(other.index), + isOnRunway(other.isOnRunway), + holdType(other.holdType), + next(other.next), + pathScore(other.pathScore), + previousNode(other.previousNode), + previousSeg(other.previousSeg) +{ +}; + +FGTaxiNode &operator =(const FGTaxiNode &other) +{ + lat = other.lat; + lon = other.lon; + index = other.index; + isOnRunway = other.isOnRunway; + holdType = other.holdType; + next = other.next; + pathScore = other.pathScore; + previousNode = other.previousNode; + previousSeg = other.previousSeg; + return *this; +}; + + void setIndex(int idx) { index = idx; }; + void setLatitude (double val) { lat = val; }; + void setLongitude(double val) { lon = val; }; + void setLatitude (const string& val) { lat = processPosition(val); }; + void setLongitude(const string& val) { lon = processPosition(val); }; + void addSegment(FGTaxiSegment *segment) { next.push_back(segment); }; + void setHoldPointType(int val) { holdType = val; }; + void setOnRunway(bool val) { isOnRunway = val; }; + + void setPathScore (double val) { pathScore = val; }; + void setPreviousNode(FGTaxiNode *val) { previousNode = val; }; + void setPreviousSeg (FGTaxiSegment *val) { previousSeg = val; }; + + FGTaxiNode *getPreviousNode() { return previousNode; }; + FGTaxiSegment *getPreviousSegment() { return previousSeg; }; + + double getPathScore() { return pathScore; }; double getLatitude() { return lat;}; double getLongitude(){ return lon;}; int getIndex() { return index; }; + int getHoldPointType() { return holdType; }; + bool getIsOnRunway() { return isOnRunway; }; + FGTaxiNode *getAddress() { return this;}; FGTaxiSegmentVectorIterator getBeginRoute() { return next.begin(); }; FGTaxiSegmentVectorIterator getEndRoute() { return next.end(); }; @@ -61,11 +118,6 @@ public: void sortEndSegments(bool); - // used in way finding - double pathscore; - FGTaxiNode* previousnode; - FGTaxiSegment* previousseg; - }; typedef vector FGTaxiNodeVector; diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index 9e26374aa..925c6e068 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -50,17 +50,13 @@ /*************************************************************************** * FGTaxiSegment **************************************************************************/ -FGTaxiSegment::FGTaxiSegment() -{ - oppositeDirection = 0; - isActive = true; -} void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes) { FGTaxiNodeVectorIterator i = nodes->begin(); while (i != nodes->end()) { + //cerr << "Scanning start node index" << (*i)->getIndex() << endl; if ((*i)->getIndex() == startNode) { start = (*i)->getAddress(); @@ -69,6 +65,7 @@ void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes) } i++; } + SG_LOG(SG_GENERAL, SG_ALERT, "Could not find start node " << startNode << endl); } void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes) @@ -76,6 +73,7 @@ void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes) FGTaxiNodeVectorIterator i = nodes->begin(); while (i != nodes->end()) { + //cerr << "Scanning end node index" << (*i)->getIndex() << endl; if ((*i)->getIndex() == endNode) { end = (*i)->getAddress(); @@ -83,6 +81,7 @@ void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes) } i++; } + SG_LOG(SG_GENERAL, SG_ALERT, "Could not find end node " << endNode << endl); } @@ -219,6 +218,7 @@ FGGroundNetwork::~FGGroundNetwork() delete (*node); } nodes.clear(); + pushBackNodes.clear(); for (FGTaxiSegmentVectorIterator seg = segments.begin(); seg != segments.end(); seg++) @@ -263,13 +263,17 @@ void FGGroundNetwork::init() //sort(segments.begin(), segments.end(), compare_segments()); FGTaxiSegmentVectorIterator i = segments.begin(); while(i != segments.end()) { - //cerr << "initializing node " << i->getIndex() << endl; (*i)->setStart(&nodes); (*i)->setEnd (&nodes); (*i)->setTrackDistance(); (*i)->setIndex(index); - //cerr << "Track distance = " << i->getLength() << endl; - //cerr << "Track ends at" << i->getEnd()->getIndex() << endl; + if ((*i)->isPushBack()) { + pushBackNodes.push_back((*i)->getEnd()); + } + //SG_LOG(SG_GENERAL, SG_BULK, "initializing segment " << (*i)->getIndex() << endl); + //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = " << (*i)->getLength() << endl); + //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from " << (*i)->getStart()->getIndex() << " to " + // << (*i)->getEnd()->getIndex() << endl); i++; index++; } @@ -295,6 +299,13 @@ void FGGroundNetwork::init() } i++; } + //FGTaxiNodeVectorIterator j = nodes.begin(); + //while (j != nodes.end()) { + // if ((*j)->getHoldPointType() == 3) { + // pushBackNodes.push_back((*j)); + // } + // j++; + //} //cerr << "Done initializing ground network" << endl; //exit(1); } @@ -365,34 +376,42 @@ FGTaxiSegment *FGGroundNetwork::findSegment(int idx) } -FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end) +FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end, bool fullSearch) { //implements Dijkstra's algorithm to find shortest distance route from start to end //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm //double INFINITE = 100000000000.0; // initialize scoring values + int nParkings = parent->getDynamics()->getNrOfParkings(); + FGTaxiNodeVector *currNodesSet; + if (fullSearch) { + currNodesSet = &nodes; + } else { + currNodesSet = &pushBackNodes; + } + for (FGTaxiNodeVectorIterator - itr = nodes.begin(); - itr != nodes.end(); itr++) { - (*itr)->pathscore = HUGE_VAL; //infinity by all practical means - (*itr)->previousnode = 0; // - (*itr)->previousseg = 0; // + itr = currNodesSet->begin(); + itr != currNodesSet->end(); itr++) { + (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means + (*itr)->setPreviousNode(0); // + (*itr)->setPreviousSeg (0); // } FGTaxiNode *firstNode = findNode(start); - firstNode->pathscore = 0; + firstNode->setPathScore(0); FGTaxiNode *lastNode = findNode(end); - FGTaxiNodeVector unvisited(nodes); // working copy + FGTaxiNodeVector unvisited(*currNodesSet); // working copy while (!unvisited.empty()) { FGTaxiNode* best = *(unvisited.begin()); for (FGTaxiNodeVectorIterator itr = unvisited.begin(); itr != unvisited.end(); itr++) { - if ((*itr)->pathscore < best->pathscore) + if ((*itr)->getPathScore() < best->getPathScore()) best = (*itr); } @@ -405,39 +424,57 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end) for (FGTaxiSegmentVectorIterator seg = best->getBeginRoute(); seg != best->getEndRoute(); seg++) { - FGTaxiNode* tgt = (*seg)->getEnd(); - double alt = best->pathscore + (*seg)->getLength(); - if (alt < tgt->pathscore) { // Relax (u,v) - tgt->pathscore = alt; - tgt->previousnode = best; - tgt->previousseg = *seg; // + if (fullSearch || (*seg)->isPushBack()) { + FGTaxiNode* tgt = (*seg)->getEnd(); + double alt = best->getPathScore() + (*seg)->getLength() + (*seg)->getPenalty(nParkings); + if (alt < tgt->getPathScore()) { // Relax (u,v) + tgt->setPathScore(alt); + tgt->setPreviousNode(best); + tgt->setPreviousSeg(*seg); // + } + } else { + // // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl; } } } } - if (lastNode->pathscore == HUGE_VAL) { + if (lastNode->getPathScore() == HUGE_VAL) { // no valid route found - SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find route from waypoint " << start << " to " << end << " at " << - parent->getId()); - exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's + if (fullSearch) { + SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find route from waypoint " << start << " to " << end << " at " << + parent->getId()); + } + FGTaxiRoute empty; + return empty; + //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's } else { // assemble route from backtrace information intVec nodes, routes; FGTaxiNode* bt = lastNode; - while (bt->previousnode != 0) { + while (bt->getPreviousNode() != 0) { nodes.push_back(bt->getIndex()); - routes.push_back(bt->previousseg->getIndex()); - bt = bt->previousnode; + routes.push_back(bt->getPreviousSegment()->getIndex()); + bt = bt->getPreviousNode(); } nodes.push_back(start); reverse(nodes.begin(), nodes.end()); reverse(routes.begin(), routes.end()); - return FGTaxiRoute(nodes, routes, lastNode->pathscore, 0); + return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0); } } +int FGTaxiSegment::getPenalty(int nGates) { + int penalty = 0; + if (end->getIndex() < nGates) { + penalty += 10000; + } + if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active. + penalty += 1000; + } + return penalty; +} // void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance) // { @@ -598,7 +635,7 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end) totalDistance -= distance; return; }*/ - +/* void FGGroundNetwork::printRoutingError(string mess) { SG_LOG(SG_GENERAL, SG_ALERT, "Error in ground network trace algorithm " << mess); @@ -616,7 +653,7 @@ void FGGroundNetwork::printRoutingError(string mess) } //exit(1); } - +*/ void FGGroundNetwork::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition, double lat, double lon, double heading, @@ -1089,9 +1126,7 @@ bool FGGroundNetwork::checkForCircularWaits(int id) //if (printed) // cerr << "[done] " << endl << endl;; if (id == target) { - SG_LOG(SG_GENERAL, SG_ALERT, "Detected circular wait condition"); - cerr << "Id = " << id << endl; - cerr << "target = " << target << endl; + SG_LOG(SG_GENERAL, SG_WARN, "Detected circular wait condition: Id = " << id << "target = " << target); return true; } else { return false; diff --git a/src/Airports/groundnetwork.hxx b/src/Airports/groundnetwork.hxx index 3ed63e8c9..f27aff980 100644 --- a/src/Airports/groundnetwork.hxx +++ b/src/Airports/groundnetwork.hxx @@ -59,6 +59,7 @@ private: double course; double headingDiff; bool isActive; + bool isPushBackRoute; FGTaxiNode *start; FGTaxiNode *end; int index; @@ -67,8 +68,51 @@ private: public: - FGTaxiSegment(); - //FGTaxiSegment(FGTaxiNode *, FGTaxiNode *, int); + FGTaxiSegment() : + startNode(0), + endNode(0), + length(0), + course(0), + headingDiff(0), + isActive(0), + isPushBackRoute(0), + start(0), + end(0), + index(0), + oppositeDirection(0) + { + }; + + FGTaxiSegment (const FGTaxiSegment &other) : + startNode (other.startNode), + endNode (other.endNode), + length (other.length), + course (other.course), + headingDiff (other.headingDiff), + isActive (other.isActive), + isPushBackRoute (other.isPushBackRoute), + start (other.start), + end (other.end), + index (other.index), + oppositeDirection (other.oppositeDirection) + { + }; + + FGTaxiSegment& operator=(const FGTaxiSegment &other) + { + startNode = other.startNode; + endNode = other.endNode; + length = other.length; + course = other.course; + headingDiff = other.headingDiff; + isActive = other.isActive; + isPushBackRoute = other.isPushBackRoute; + start = other.start; + end = other.end; + index = other.index; + oppositeDirection = other.oppositeDirection; + return *this; + }; void setIndex (int val) { index = val; }; void setStartNodeRef (int val) { startNode = val; }; @@ -78,14 +122,19 @@ public: void setStart(FGTaxiNodeVector *nodes); void setEnd (FGTaxiNodeVector *nodes); + void setPushBackType(bool val) { isPushBackRoute = val; }; void setTrackDistance(); FGTaxiNode * getEnd() { return end;}; FGTaxiNode * getStart() { return start; }; double getLength() { return length; }; int getIndex() { return index; }; + + bool isPushBack() { return isPushBackRoute; }; + + int getPenalty(int nGates); - FGTaxiSegment *getAddress() { return this;}; + FGTaxiSegment *getAddress() { return this;}; bool operator<(const FGTaxiSegment &other) const { return index < other.index; }; bool hasSmallerHeadingDiff (const FGTaxiSegment &other) const { return headingDiff < other.headingDiff; }; @@ -170,28 +219,29 @@ private: //int maxDepth; int count; FGTaxiNodeVector nodes; + FGTaxiNodeVector pushBackNodes; FGTaxiSegmentVector segments; //intVec route; - intVec nodesStack; - intVec routesStack; + //intVec nodesStack; + //intVec routesStack; TaxiRouteVector routes; TrafficVector activeTraffic; TrafficVectorIterator currTraffic; SGWayPoint destination; - + bool foundRoute; double totalDistance, maxDistance; FGTowerController *towerController; FGAirport *parent; - - void printRoutingError(string); + + //void printRoutingError(string); void checkSpeedAdjustment(int id, double lat, double lon, double heading, double speed, double alt); void checkHoldPosition(int id, double lat, double lon, double heading, double speed, double alt); - + public: FGGroundNetwork(); ~FGGroundNetwork(); @@ -206,9 +256,11 @@ public: int findNearestNode(double lat, double lon); FGTaxiNode *findNode(int idx); FGTaxiSegment *findSegment(int idx); - FGTaxiRoute findShortestRoute(int start, int end); + FGTaxiRoute findShortestRoute(int start, int end, bool fullSearch=true); //void trace(FGTaxiNode *, int, int, double dist); + int getNrOfNodes() { return nodes.size(); }; + void setParent(FGAirport *par) { parent = par; }; virtual void announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentRoute, diff --git a/src/Airports/parking.cxx b/src/Airports/parking.cxx index ebc06210f..ce3181fa4 100644 --- a/src/Airports/parking.cxx +++ b/src/Airports/parking.cxx @@ -46,22 +46,26 @@ #include STL_STRING #include "parking.hxx" +#include "groundnetwork.hxx" /********************************************************************************* * FGParking ********************************************************************************/ -FGParking::FGParking(double lat, - double lon, - double hdg, - double rad, - int idx, - const string &name, - const string &tpe, - const string &codes) - : FGTaxiNode(lat,lon,idx) -{ - heading = hdg; - parkingName = name; - type = tpe; - airlineCodes = codes; +// FGParking::FGParking(double lat, +// double lon, +// double hdg, +// double rad, +// int idx, +// const string &name, +// const string &tpe, +// const string &codes) +// : FGTaxiNode(lat,lon,idx) +// { +// heading = hdg; +// parkingName = name; +// type = tpe; +// airlineCodes = codes; +// } +FGParking::~FGParking() { + delete pushBackRoute; } diff --git a/src/Airports/parking.hxx b/src/Airports/parking.hxx index c96ab1d24..3b50d5d2a 100644 --- a/src/Airports/parking.hxx +++ b/src/Airports/parking.hxx @@ -39,6 +39,9 @@ SG_USING_STD(string); SG_USING_STD(vector); +class FGTaxiRoute; + + class FGParking : public FGTaxiNode { private: double heading; @@ -48,20 +51,58 @@ private: string airlineCodes; bool available; - - + int pushBackPoint; + FGTaxiRoute *pushBackRoute; public: - FGParking() { available = true;}; - //FGParking(FGParking &other); - FGParking(double lat, - double lon, - double hdg, - double rad, - int idx, - const string& name, - const string& tpe, - const string& codes); + FGParking() : + heading(0), + radius(0), + //parkingName(0), + //type(0), + //airlineCodes(0), + available(true), + pushBackPoint(-1), + pushBackRoute(0) + { + }; + + FGParking(const FGParking &other) : + FGTaxiNode (other), + heading (other.heading), + radius (other.radius), + parkingName (other.parkingName), + type (other.type), + airlineCodes (other.airlineCodes), + available (other.available), + pushBackPoint(other.pushBackPoint), + pushBackRoute(other.pushBackRoute) + { + }; + + + FGParking& operator =(const FGParking &other) + { + FGTaxiNode::operator=(other); + heading = other.heading; + radius = other.radius; + parkingName = other.parkingName; + type = other.type; + airlineCodes = other.airlineCodes; + available = other.available; + pushBackPoint= other.pushBackPoint; + pushBackRoute= other.pushBackRoute; + return *this; + }; + ~FGParking(); +// FGParking(double lat, +// double lon, +// double hdg, +// double rad, +// int idx, +// const string& name, +// const string& tpe, +// const string& codes); void setHeading (double hdg) { heading = hdg; }; void setRadius (double rad) { radius = rad; }; @@ -70,6 +111,9 @@ public: void setType (const string& tpe) { type = tpe; }; void setCodes (const string& codes){ airlineCodes= codes;}; + void setPushBackRoute(FGTaxiRoute *val) { pushBackRoute = val; }; + void setPushBackPoint(int val) { pushBackPoint = val; }; + bool isAvailable () { return available;}; void setAvailable(bool val) { available = val; }; @@ -80,6 +124,10 @@ public: string getCodes () { return airlineCodes;}; string getName () { return parkingName; }; + FGTaxiRoute * getPushBackRoute () { return pushBackRoute; }; + + int getPushBackPoint () { return pushBackPoint; }; + bool operator< (const FGParking &other) const { return radius < other.radius; }; }; -- 2.39.2