1
0
Fork 0

- 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.
This commit is contained in:
durk 2007-08-08 06:09:58 +00:00
parent 15defbb233
commit 1c8f2e3c5b
14 changed files with 671 additions and 215 deletions

View file

@ -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. // 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 // This waypoint marks the fact that the aircraft has passed the initial taxi
// departure waypoint, so it can release the parking. // departure waypoint, so it can release the parking.
if (prev->name == "park2") { if (prev->name == "PushBackPoint") {
dep->getDynamics()->releaseParking(fp->getGate()); 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 // This is the last taxi waypoint, and marks the the end of the flight plan

View file

@ -118,12 +118,13 @@ private:
double leadInAngle; double leadInAngle;
time_t start_time; time_t start_time;
int leg; int leg;
int gateId; int gateId, lastNodeVisited;
string activeRunway; string activeRunway;
FGAirRoute airRoute; FGAirRoute airRoute;
FGTaxiRoute *taxiRoute; FGTaxiRoute *taxiRoute;
void createPushBack(bool, FGAirport*, double, double, double, const string&, const string&, const string&); 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 createTaxi(bool, int, FGAirport *, double, double, double, const string&, const string&, const string&);
void createTakeOff(bool, FGAirport *, double, const string&); void createTakeOff(bool, FGAirport *, double, const string&);
void createClimb(bool, FGAirport *, double, double, const string&); void createClimb(bool, FGAirport *, double, double, const string&);

View file

@ -83,100 +83,10 @@ void FGAIFlightPlan::create(FGAirport *dep, FGAirport *arr, int legNr,
leg++; 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. * createCreate Taxi.
@ -259,11 +169,29 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction,
//FGTaxiRoute route; //FGTaxiRoute route;
delete taxiRoute; delete taxiRoute;
taxiRoute = new FGTaxiRoute; taxiRoute = new FGTaxiRoute;
if (gateId >= 0)
*taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(gateId, // Determine which node to start from.
runwayId); int node;
// Find out which node to start from
FGParking *park = apt->getDynamics()->getParking(gateId);
if (park)
node = park->getPushBackPoint();
else else
*taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(0, runwayId); 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; intVecIterator i;
if (taxiRoute->empty()) { if (taxiRoute->empty()) {
@ -299,28 +227,25 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction,
} else { } else {
int node; int node;
taxiRoute->first(); taxiRoute->first();
bool isPushBackPoint = false; //bool isPushBackPoint = false;
if (firstFlight) { if (firstFlight) {
// If this is called during initialization, randomly // If this is called during initialization, randomly
// skip a number of waypoints to get a more realistic // skip a number of waypoints to get a more realistic
// taxi situation. // taxi situation.
isPushBackPoint = true; //isPushBackPoint = true;
int nrWaypoints = taxiRoute->size(); int nrWaypoints = taxiRoute->size();
nrWaypointsToSkip = rand() % nrWaypoints; nrWaypointsToSkip = rand() % nrWaypoints;
// but make sure we always keep two active waypoints // but make sure we always keep two active waypoints
// to prevent a segmentation fault // to prevent a segmentation fault
for (int i = 0; i < nrWaypointsToSkip-2; i++) { for (int i = 0; i < nrWaypointsToSkip-2; i++) {
isPushBackPoint = false; //isPushBackPoint = false;
taxiRoute->next(&node); taxiRoute->next(&node);
} }
apt->getDynamics()->releaseParking(gateId);
} else { } else {
//chop off the first two waypoints, because if (taxiRoute->size() > 1) {
// those have already been created taxiRoute->next(&node); // chop off the first waypoint, because that is already the last of the pushback route
// by create pushback
int size = taxiRoute->size();
if (size > 2) {
taxiRoute->next(&node);
taxiRoute->next(&node);
} }
} }
int route; int route;
@ -338,13 +263,7 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction,
// Elevation is currently disregarded when on_ground is true // Elevation is currently disregarded when on_ground is true
// because the AIModel obtains a periodic ground elevation estimate. // because the AIModel obtains a periodic ground elevation estimate.
wpt->altitude = apt->getElevation(); wpt->altitude = apt->getElevation();
if (isPushBackPoint) {
wpt->speed = -10;
isPushBackPoint = false;
}
else {
wpt->speed = 15; wpt->speed = 15;
}
wpt->crossat = -10000; wpt->crossat = -10000;
wpt->gear_down = true; wpt->gear_down = true;
wpt->flaps_down= true; wpt->flaps_down= true;
@ -353,6 +272,7 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction,
wpt->routeIndex = route; wpt->routeIndex = route;
waypoints.push_back(wpt); waypoints.push_back(wpt);
} }
/*
//cerr << endl; //cerr << endl;
// finally, rewind the taxiRoute object to the point where we started // finally, rewind the taxiRoute object to the point where we started
// generating the Flightplan, for AI use. // generating the Flightplan, for AI use.
@ -369,7 +289,7 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction,
//taxiRoute->next(&node); //taxiRoute->next(&node);
//taxiRoute->next(&node); //taxiRoute->next(&node);
} }
} }*/
} // taxiRoute not empty } // taxiRoute not empty
} }
else else
@ -1076,4 +996,5 @@ string FGAIFlightPlan::getRunwayClassFromTrafficType(string fltType)
if ((fltType == "mil-fighter") || (fltType == "mil-transport")) { if ((fltType == "mil-fighter") || (fltType == "mil-transport")) {
return string("mil"); return string("mil");
} }
return string("com");
} }

View file

@ -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 <simgear/math/sg_geodesy.hxx>
#include <Airports/runways.hxx>
#include <Environment/environment_mgr.hxx>
#include <Environment/environment.hxx>
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);
}

View file

@ -10,7 +10,9 @@ libAIModel_a_SOURCES = submodel.cxx submodel.hxx \
AIStorm.hxx AIStorm.cxx \ AIStorm.hxx AIStorm.cxx \
AIThermal.hxx AIThermal.cxx \ AIThermal.hxx AIThermal.cxx \
AIFlightPlan.hxx AIFlightPlan.cxx \ AIFlightPlan.hxx AIFlightPlan.cxx \
AIFlightPlanCreate.cxx AIFlightPlanCreateCruise.cxx \ AIFlightPlanCreate.cxx \
AIFlightPlanCreatePushBack.cxx \
AIFlightPlanCreateCruise.cxx \
AICarrier.hxx AICarrier.cxx \ AICarrier.hxx AICarrier.cxx \
AIStatic.hxx AIStatic.cxx \ AIStatic.hxx AIStatic.cxx \
AITanker.cxx AITanker.hxx \ AITanker.cxx AITanker.hxx \

View file

@ -38,8 +38,11 @@ void FGAirportDynamicsXMLLoader::startElement (const char * name, const XMLAttr
string value; string value;
string gateName; string gateName;
string gateNumber; string gateNumber;
string attval;
string lat; string lat;
string lon; string lon;
int holdPointType;
if (name == string("Parking")) if (name == string("Parking"))
{ {
for (int i = 0; i < atts.size(); i++) 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)); taxiNode.setLatitude(atts.getValue(i));
if (attname == string("lon")) if (attname == string("lon"))
taxiNode.setLongitude(atts.getValue(i)); 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); _dynamics->getGroundNetwork()->addNode(taxiNode);
} }
@ -97,6 +116,8 @@ void FGAirportDynamicsXMLLoader::startElement (const char * name, const XMLAttr
taxiSegment.setStartNodeRef(atoi(atts.getValue(i))); taxiSegment.setStartNodeRef(atoi(atts.getValue(i)));
if (attname == string("end")) if (attname == string("end"))
taxiSegment.setEndNodeRef(atoi(atts.getValue(i))); taxiSegment.setEndNodeRef(atoi(atts.getValue(i)));
if (attname == string("isPushBackRoute"))
taxiSegment.setPushBackType((bool) atoi(atts.getValue(i)));
} }
_dynamics->getGroundNetwork()->addSegment(taxiSegment); _dynamics->getGroundNetwork()->addSegment(taxiSegment);
} }

View file

@ -305,7 +305,7 @@ void FGAirportDynamics::getParking (int id, double *lat, double* lon, double *he
FGParking *FGAirportDynamics::getParking(int i) FGParking *FGAirportDynamics::getParking(int i)
{ {
if (i < (int)parkings.size()) if (i < (int)parkings.size() && (i >= 0))
return &(parkings[i]); return &(parkings[i]);
else else
return 0; return 0;

View file

@ -86,6 +86,7 @@ public:
FGParking *getParking(int i); FGParking *getParking(int i);
void releaseParking(int id); void releaseParking(int id);
string getParkingName(int i); string getParkingName(int i);
int getNrOfParkings() { return parkings.size(); };
//FGAirport *getAddress() { return this; }; //FGAirport *getAddress() { return this; };
//const string &getName() const { return _name;}; //const string &getName() const { return _name;};
// Returns degrees // Returns degrees

View file

@ -42,9 +42,7 @@ bool sortByLength(FGTaxiSegment *a, FGTaxiSegment *b) {
/************************************************************************** /**************************************************************************
* FGTaxiNode * FGTaxiNode
*************************************************************************/ *************************************************************************/
FGTaxiNode::FGTaxiNode()
{
}
void FGTaxiNode::sortEndSegments(bool byLength) void FGTaxiNode::sortEndSegments(bool byLength)
{ {

View file

@ -37,23 +37,80 @@ private:
double lat; double lat;
double lon; double lon;
int index; int index;
bool isOnRunway;
int holdType;
FGTaxiSegmentVector next; // a vector of pointers to all the segments leaving from this node FGTaxiSegmentVector next; // a vector of pointers to all the segments leaving from this node
public: // used in way finding
FGTaxiNode(); double pathScore;
FGTaxiNode(double, double, int); FGTaxiNode* previousNode;
FGTaxiSegment* previousSeg;
void setIndex(int idx) { index = idx;}; public:
void setLatitude (double val) { lat = val;}; FGTaxiNode() :
void setLongitude(double val) { lon = val;}; 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 setLatitude (const string& val) { lat = processPosition(val); };
void setLongitude(const string& val) { lon = processPosition(val); }; void setLongitude(const string& val) { lon = processPosition(val); };
void addSegment(FGTaxiSegment *segment) { next.push_back(segment); }; 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 getLatitude() { return lat;};
double getLongitude(){ return lon;}; double getLongitude(){ return lon;};
int getIndex() { return index; }; int getIndex() { return index; };
int getHoldPointType() { return holdType; };
bool getIsOnRunway() { return isOnRunway; };
FGTaxiNode *getAddress() { return this;}; FGTaxiNode *getAddress() { return this;};
FGTaxiSegmentVectorIterator getBeginRoute() { return next.begin(); }; FGTaxiSegmentVectorIterator getBeginRoute() { return next.begin(); };
FGTaxiSegmentVectorIterator getEndRoute() { return next.end(); }; FGTaxiSegmentVectorIterator getEndRoute() { return next.end(); };
@ -61,11 +118,6 @@ public:
void sortEndSegments(bool); void sortEndSegments(bool);
// used in way finding
double pathscore;
FGTaxiNode* previousnode;
FGTaxiSegment* previousseg;
}; };
typedef vector<FGTaxiNode*> FGTaxiNodeVector; typedef vector<FGTaxiNode*> FGTaxiNodeVector;

View file

@ -50,17 +50,13 @@
/*************************************************************************** /***************************************************************************
* FGTaxiSegment * FGTaxiSegment
**************************************************************************/ **************************************************************************/
FGTaxiSegment::FGTaxiSegment()
{
oppositeDirection = 0;
isActive = true;
}
void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes) void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes)
{ {
FGTaxiNodeVectorIterator i = nodes->begin(); FGTaxiNodeVectorIterator i = nodes->begin();
while (i != nodes->end()) while (i != nodes->end())
{ {
//cerr << "Scanning start node index" << (*i)->getIndex() << endl;
if ((*i)->getIndex() == startNode) if ((*i)->getIndex() == startNode)
{ {
start = (*i)->getAddress(); start = (*i)->getAddress();
@ -69,6 +65,7 @@ void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes)
} }
i++; i++;
} }
SG_LOG(SG_GENERAL, SG_ALERT, "Could not find start node " << startNode << endl);
} }
void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes) void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes)
@ -76,6 +73,7 @@ void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes)
FGTaxiNodeVectorIterator i = nodes->begin(); FGTaxiNodeVectorIterator i = nodes->begin();
while (i != nodes->end()) while (i != nodes->end())
{ {
//cerr << "Scanning end node index" << (*i)->getIndex() << endl;
if ((*i)->getIndex() == endNode) if ((*i)->getIndex() == endNode)
{ {
end = (*i)->getAddress(); end = (*i)->getAddress();
@ -83,6 +81,7 @@ void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes)
} }
i++; i++;
} }
SG_LOG(SG_GENERAL, SG_ALERT, "Could not find end node " << endNode << endl);
} }
@ -219,6 +218,7 @@ FGGroundNetwork::~FGGroundNetwork()
delete (*node); delete (*node);
} }
nodes.clear(); nodes.clear();
pushBackNodes.clear();
for (FGTaxiSegmentVectorIterator seg = segments.begin(); for (FGTaxiSegmentVectorIterator seg = segments.begin();
seg != segments.end(); seg != segments.end();
seg++) seg++)
@ -263,13 +263,17 @@ void FGGroundNetwork::init()
//sort(segments.begin(), segments.end(), compare_segments()); //sort(segments.begin(), segments.end(), compare_segments());
FGTaxiSegmentVectorIterator i = segments.begin(); FGTaxiSegmentVectorIterator i = segments.begin();
while(i != segments.end()) { while(i != segments.end()) {
//cerr << "initializing node " << i->getIndex() << endl;
(*i)->setStart(&nodes); (*i)->setStart(&nodes);
(*i)->setEnd (&nodes); (*i)->setEnd (&nodes);
(*i)->setTrackDistance(); (*i)->setTrackDistance();
(*i)->setIndex(index); (*i)->setIndex(index);
//cerr << "Track distance = " << i->getLength() << endl; if ((*i)->isPushBack()) {
//cerr << "Track ends at" << i->getEnd()->getIndex() << endl; 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++; i++;
index++; index++;
} }
@ -295,6 +299,13 @@ void FGGroundNetwork::init()
} }
i++; i++;
} }
//FGTaxiNodeVectorIterator j = nodes.begin();
//while (j != nodes.end()) {
// if ((*j)->getHoldPointType() == 3) {
// pushBackNodes.push_back((*j));
// }
// j++;
//}
//cerr << "Done initializing ground network" << endl; //cerr << "Done initializing ground network" << endl;
//exit(1); //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 //implements Dijkstra's algorithm to find shortest distance route from start to end
//taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
//double INFINITE = 100000000000.0; //double INFINITE = 100000000000.0;
// initialize scoring values // initialize scoring values
int nParkings = parent->getDynamics()->getNrOfParkings();
FGTaxiNodeVector *currNodesSet;
if (fullSearch) {
currNodesSet = &nodes;
} else {
currNodesSet = &pushBackNodes;
}
for (FGTaxiNodeVectorIterator for (FGTaxiNodeVectorIterator
itr = nodes.begin(); itr = currNodesSet->begin();
itr != nodes.end(); itr++) { itr != currNodesSet->end(); itr++) {
(*itr)->pathscore = HUGE_VAL; //infinity by all practical means (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
(*itr)->previousnode = 0; // (*itr)->setPreviousNode(0); //
(*itr)->previousseg = 0; // (*itr)->setPreviousSeg (0); //
} }
FGTaxiNode *firstNode = findNode(start); FGTaxiNode *firstNode = findNode(start);
firstNode->pathscore = 0; firstNode->setPathScore(0);
FGTaxiNode *lastNode = findNode(end); FGTaxiNode *lastNode = findNode(end);
FGTaxiNodeVector unvisited(nodes); // working copy FGTaxiNodeVector unvisited(*currNodesSet); // working copy
while (!unvisited.empty()) { while (!unvisited.empty()) {
FGTaxiNode* best = *(unvisited.begin()); FGTaxiNode* best = *(unvisited.begin());
for (FGTaxiNodeVectorIterator for (FGTaxiNodeVectorIterator
itr = unvisited.begin(); itr = unvisited.begin();
itr != unvisited.end(); itr++) { itr != unvisited.end(); itr++) {
if ((*itr)->pathscore < best->pathscore) if ((*itr)->getPathScore() < best->getPathScore())
best = (*itr); best = (*itr);
} }
@ -405,39 +424,57 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
for (FGTaxiSegmentVectorIterator for (FGTaxiSegmentVectorIterator
seg = best->getBeginRoute(); seg = best->getBeginRoute();
seg != best->getEndRoute(); seg++) { seg != best->getEndRoute(); seg++) {
if (fullSearch || (*seg)->isPushBack()) {
FGTaxiNode* tgt = (*seg)->getEnd(); FGTaxiNode* tgt = (*seg)->getEnd();
double alt = best->pathscore + (*seg)->getLength(); double alt = best->getPathScore() + (*seg)->getLength() + (*seg)->getPenalty(nParkings);
if (alt < tgt->pathscore) { // Relax (u,v) if (alt < tgt->getPathScore()) { // Relax (u,v)
tgt->pathscore = alt; tgt->setPathScore(alt);
tgt->previousnode = best; tgt->setPreviousNode(best);
tgt->previousseg = *seg; // tgt->setPreviousSeg(*seg); //
}
} else {
// // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
} }
} }
} }
} }
if (lastNode->pathscore == HUGE_VAL) { if (lastNode->getPathScore() == HUGE_VAL) {
// no valid route found // no valid route found
if (fullSearch) {
SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find route from waypoint " << start << " to " << end << " at " << SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find route from waypoint " << start << " to " << end << " at " <<
parent->getId()); parent->getId());
exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's }
FGTaxiRoute empty;
return empty;
//exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
} else { } else {
// assemble route from backtrace information // assemble route from backtrace information
intVec nodes, routes; intVec nodes, routes;
FGTaxiNode* bt = lastNode; FGTaxiNode* bt = lastNode;
while (bt->previousnode != 0) { while (bt->getPreviousNode() != 0) {
nodes.push_back(bt->getIndex()); nodes.push_back(bt->getIndex());
routes.push_back(bt->previousseg->getIndex()); routes.push_back(bt->getPreviousSegment()->getIndex());
bt = bt->previousnode; bt = bt->getPreviousNode();
} }
nodes.push_back(start); nodes.push_back(start);
reverse(nodes.begin(), nodes.end()); reverse(nodes.begin(), nodes.end());
reverse(routes.begin(), routes.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) // void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance)
// { // {
@ -598,7 +635,7 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
totalDistance -= distance; totalDistance -= distance;
return; return;
}*/ }*/
/*
void FGGroundNetwork::printRoutingError(string mess) void FGGroundNetwork::printRoutingError(string mess)
{ {
SG_LOG(SG_GENERAL, SG_ALERT, "Error in ground network trace algorithm " << 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); //exit(1);
} }
*/
void FGGroundNetwork::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition, void FGGroundNetwork::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition,
double lat, double lon, double heading, double lat, double lon, double heading,
@ -1089,9 +1126,7 @@ bool FGGroundNetwork::checkForCircularWaits(int id)
//if (printed) //if (printed)
// cerr << "[done] " << endl << endl;; // cerr << "[done] " << endl << endl;;
if (id == target) { if (id == target) {
SG_LOG(SG_GENERAL, SG_ALERT, "Detected circular wait condition"); SG_LOG(SG_GENERAL, SG_WARN, "Detected circular wait condition: Id = " << id << "target = " << target);
cerr << "Id = " << id << endl;
cerr << "target = " << target << endl;
return true; return true;
} else { } else {
return false; return false;

View file

@ -59,6 +59,7 @@ private:
double course; double course;
double headingDiff; double headingDiff;
bool isActive; bool isActive;
bool isPushBackRoute;
FGTaxiNode *start; FGTaxiNode *start;
FGTaxiNode *end; FGTaxiNode *end;
int index; int index;
@ -67,8 +68,51 @@ private:
public: public:
FGTaxiSegment(); FGTaxiSegment() :
//FGTaxiSegment(FGTaxiNode *, FGTaxiNode *, int); 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 setIndex (int val) { index = val; };
void setStartNodeRef (int val) { startNode = val; }; void setStartNodeRef (int val) { startNode = val; };
@ -78,6 +122,7 @@ public:
void setStart(FGTaxiNodeVector *nodes); void setStart(FGTaxiNodeVector *nodes);
void setEnd (FGTaxiNodeVector *nodes); void setEnd (FGTaxiNodeVector *nodes);
void setPushBackType(bool val) { isPushBackRoute = val; };
void setTrackDistance(); void setTrackDistance();
FGTaxiNode * getEnd() { return end;}; FGTaxiNode * getEnd() { return end;};
@ -85,6 +130,10 @@ public:
double getLength() { return length; }; double getLength() { return length; };
int getIndex() { return index; }; 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 operator<(const FGTaxiSegment &other) const { return index < other.index; };
@ -170,10 +219,11 @@ private:
//int maxDepth; //int maxDepth;
int count; int count;
FGTaxiNodeVector nodes; FGTaxiNodeVector nodes;
FGTaxiNodeVector pushBackNodes;
FGTaxiSegmentVector segments; FGTaxiSegmentVector segments;
//intVec route; //intVec route;
intVec nodesStack; //intVec nodesStack;
intVec routesStack; //intVec routesStack;
TaxiRouteVector routes; TaxiRouteVector routes;
TrafficVector activeTraffic; TrafficVector activeTraffic;
TrafficVectorIterator currTraffic; TrafficVectorIterator currTraffic;
@ -185,7 +235,7 @@ private:
FGAirport *parent; FGAirport *parent;
void printRoutingError(string); //void printRoutingError(string);
void checkSpeedAdjustment(int id, double lat, double lon, void checkSpeedAdjustment(int id, double lat, double lon,
double heading, double speed, double alt); double heading, double speed, double alt);
@ -206,9 +256,11 @@ public:
int findNearestNode(double lat, double lon); int findNearestNode(double lat, double lon);
FGTaxiNode *findNode(int idx); FGTaxiNode *findNode(int idx);
FGTaxiSegment *findSegment(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); //void trace(FGTaxiNode *, int, int, double dist);
int getNrOfNodes() { return nodes.size(); };
void setParent(FGAirport *par) { parent = par; }; void setParent(FGAirport *par) { parent = par; };
virtual void announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentRoute, virtual void announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentRoute,

View file

@ -46,22 +46,26 @@
#include STL_STRING #include STL_STRING
#include "parking.hxx" #include "parking.hxx"
#include "groundnetwork.hxx"
/********************************************************************************* /*********************************************************************************
* FGParking * FGParking
********************************************************************************/ ********************************************************************************/
FGParking::FGParking(double lat, // FGParking::FGParking(double lat,
double lon, // double lon,
double hdg, // double hdg,
double rad, // double rad,
int idx, // int idx,
const string &name, // const string &name,
const string &tpe, // const string &tpe,
const string &codes) // const string &codes)
: FGTaxiNode(lat,lon,idx) // : FGTaxiNode(lat,lon,idx)
{ // {
heading = hdg; // heading = hdg;
parkingName = name; // parkingName = name;
type = tpe; // type = tpe;
airlineCodes = codes; // airlineCodes = codes;
// }
FGParking::~FGParking() {
delete pushBackRoute;
} }

View file

@ -39,6 +39,9 @@
SG_USING_STD(string); SG_USING_STD(string);
SG_USING_STD(vector); SG_USING_STD(vector);
class FGTaxiRoute;
class FGParking : public FGTaxiNode { class FGParking : public FGTaxiNode {
private: private:
double heading; double heading;
@ -48,20 +51,58 @@ private:
string airlineCodes; string airlineCodes;
bool available; bool available;
int pushBackPoint;
FGTaxiRoute *pushBackRoute;
public: public:
FGParking() { available = true;}; FGParking() :
//FGParking(FGParking &other); heading(0),
FGParking(double lat, radius(0),
double lon, //parkingName(0),
double hdg, //type(0),
double rad, //airlineCodes(0),
int idx, available(true),
const string& name, pushBackPoint(-1),
const string& tpe, pushBackRoute(0)
const string& codes); {
};
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 setHeading (double hdg) { heading = hdg; };
void setRadius (double rad) { radius = rad; }; void setRadius (double rad) { radius = rad; };
@ -70,6 +111,9 @@ public:
void setType (const string& tpe) { type = tpe; }; void setType (const string& tpe) { type = tpe; };
void setCodes (const string& codes){ airlineCodes= codes;}; void setCodes (const string& codes){ airlineCodes= codes;};
void setPushBackRoute(FGTaxiRoute *val) { pushBackRoute = val; };
void setPushBackPoint(int val) { pushBackPoint = val; };
bool isAvailable () { return available;}; bool isAvailable () { return available;};
void setAvailable(bool val) { available = val; }; void setAvailable(bool val) { available = val; };
@ -80,6 +124,10 @@ public:
string getCodes () { return airlineCodes;}; string getCodes () { return airlineCodes;};
string getName () { return parkingName; }; string getName () { return parkingName; };
FGTaxiRoute * getPushBackRoute () { return pushBackRoute; };
int getPushBackPoint () { return pushBackPoint; };
bool operator< (const FGParking &other) const { bool operator< (const FGParking &other) const {
return radius < other.radius; }; return radius < other.radius; };
}; };