fly/flightgear
1
0
Fork 0
Code Activity

- Ground network XML parsing code reads the new attributes "holdPointType"

Browse source 
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
Show stats Download patch file Download diff file Expand all files Collapse all files

7
src/AIModel/AIAircraft.cxx
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.
//
@ -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

3
src/AIModel/AIFlightPlan.hxx
View file

@ -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&);

157
src/AIModel/AIFlightPlanCreate.cxx
View file

@ -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");
}

318
src/AIModel/AIFlightPlanCreatePushBack.cxx Normal file
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);
}

4
src/AIModel/Makefile.am
View file

@ -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 \

21
src/Airports/dynamicloader.cxx
View file

@ -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);
}

2
src/Airports/dynamics.cxx
View file

@ -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;

1
src/Airports/dynamics.hxx
View file

@ -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

4
src/Airports/gnnode.cxx
View file

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

86
src/Airports/gnnode.hxx
View file

@ -37,23 +37,80 @@ private:
double lat;
double lon;
int index;
FGTaxiSegmentVector next; // a vector of pointers to all the segments leaving from this node
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); };
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() :
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<FGTaxiNode*> FGTaxiNodeVector;

107
src/Airports/groundnetwork.cxx
View file

@ -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;

72
src/Airports/groundnetwork.hxx
View file

@ -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; };
FGTaxiSegment *getAddress() { return this;};
int getPenalty(int nGates);
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,

32
src/Airports/parking.cxx
View file

@ -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;
}

72
src/Airports/parking.hxx
View file

@ -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; };
};