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.
-// FGAIAircraft - FGAIBase-derived class creates an AI airplane
+// // FGAIAircraft - FGAIBase-derived class creates an AI airplane
//
// Written by David Culp, started October 2003.
//
// 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
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&);
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.
//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()) {
} 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))
{
// 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;
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.
//taxiRoute->next(&node);
//taxiRoute->next(&node);
}
- }
+ }*/
} // taxiRoute not empty
}
else
if ((fltType == "mil-fighter") || (fltType == "mil-transport")) {
return string("mil");
}
+ return string("com");
}
--- /dev/null
+/******************************************************************************
+ * 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);
+}
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 \
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++)
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);
}
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);
}
FGParking *FGAirportDynamics::getParking(int i)
{
- if (i < (int)parkings.size())
+ if (i < (int)parkings.size() && (i >= 0))
return &(parkings[i]);
else
return 0;
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
/**************************************************************************
* FGTaxiNode
*************************************************************************/
-FGTaxiNode::FGTaxiNode()
-{
-}
+
void FGTaxiNode::sortEndSegments(bool byLength)
{
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(); };
void sortEndSegments(bool);
- // used in way finding
- double pathscore;
- FGTaxiNode* previousnode;
- FGTaxiSegment* previousseg;
-
};
typedef vector<FGTaxiNode*> FGTaxiNodeVector;
/***************************************************************************
* 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();
}
i++;
}
+ SG_LOG(SG_GENERAL, SG_ALERT, "Could not find start node " << startNode << endl);
}
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();
}
i++;
}
+ SG_LOG(SG_GENERAL, SG_ALERT, "Could not find end node " << endNode << endl);
}
delete (*node);
}
nodes.clear();
+ pushBackNodes.clear();
for (FGTaxiSegmentVectorIterator seg = segments.begin();
seg != segments.end();
seg++)
//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++;
}
}
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);
}
}
-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);
}
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)
// {
totalDistance -= distance;
return;
}*/
-
+/*
void FGGroundNetwork::printRoutingError(string mess)
{
SG_LOG(SG_GENERAL, SG_ALERT, "Error in ground network trace algorithm " << mess);
}
//exit(1);
}
-
+*/
void FGGroundNetwork::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition,
double lat, double lon, double heading,
//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;
double course;
double headingDiff;
bool isActive;
+ bool isPushBackRoute;
FGTaxiNode *start;
FGTaxiNode *end;
int index;
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; };
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; };
//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();
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,
#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;
}
SG_USING_STD(string);
SG_USING_STD(vector);
+class FGTaxiRoute;
+
+
class FGParking : public FGTaxiNode {
private:
double heading;
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; };
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; };
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; };
};
projects / flightgear.git / commitdiff