/* This is the function called by collisionAvoidance.cpp that calls
all necessary functions in order to generate the new collision avoidance
waypoint. If no collision avoidance maneuvers are necessary, the function
returns the current destination waypoint. */
sim::waypoint sim::findNewWaypoint(PlaneObject &plane1, std::map<int, PlaneObject> &planes) {
/* Find plane to avoid*/
sim::threatContainer greatestThreat = findGreatestThreat(plane1, planes);
/* Unpack plane to avoid*/
int threatID = greatestThreat.planeID;
double threatZEM = greatestThreat.ZEM;
double timeToGo = greatestThreat.timeToGo;
/* If there is no plane to avoid, then take Dubin's path to the
destination waypoint*/
if (((threatID < 0) && (threatZEM < 0)) || timeToGo > MINIMUM_TIME_TO_GO) {
return takeDubinsPath(plane1);
}
/* If there is a plane to avoid, then figure out which direction it
should turn*/
bool turnRight = shouldTurnRight(plane1, planes[threatID]);
/* Calculate turning radius to avoid collision*/
double turningRadius = calculateTurningRadius(threatZEM);
/* Given turning radius and orientation of the plane, calculate
next collision avoidance waypoint*/
return calculateWaypoint(plane1, turningRadius, turnRight);
}
/* This is the function called by collisionAvoidance.cpp that calls
all necessary functions in order to generate the new collision avoidance
waypoint. If no collision avoidance maneuvers are necessary, the function
returns the current destination waypoint. */
AU_UAV_ROS::waypointContainer AU_UAV_ROS::findNewWaypoint(PlaneObject &plane1, std::map<int, PlaneObject> &planes){
ROS_WARN("-----------------------------------------------------");
/* Find plane to avoid*/
AU_UAV_ROS::threatContainer greatestThreat = findGreatestThreat(plane1, planes);
/* Unpack plane to avoid*/
int threatID = greatestThreat.planeID;
double threatZEM = greatestThreat.ZEM;
double timeToGo = greatestThreat.timeToGo;
/*
if (threatID != -1) {
ROS_WARN("Distance between plane %d and plane %d = %f", plane1.getID(),
threatID, findDistance(plane1.getCurrentLoc().latitude,
plane1.getCurrentLoc().longitude, planes[threatID].getCurrentLoc().latitude,
planes[threatID].getCurrentLoc().longitude));
}
*/
AU_UAV_ROS::waypointContainer bothNewWaypoints;
/* If there is no plane to avoid, then take Dubin's path to the
destination waypoint*/
if (((threatID < 0) && (threatZEM < 0)) || timeToGo > MINIMUM_TIME_TO_GO) {
bothNewWaypoints.plane1WP = takeDubinsPath(plane1);
bothNewWaypoints.plane2ID = -1;
bothNewWaypoints.plane2WP = bothNewWaypoints.plane1WP;
return bothNewWaypoints;
}
/* If there is a plane to avoid, then figure out which direction it
should turn*/
bool turnRight = shouldTurnRight(plane1, planes[threatID]);
/* Calculate turning radius to avoid collision*/
double turningRadius = calculateTurningRadius(threatZEM);
/* Given turning radius and orientation of the plane, calculate
next collision avoidance waypoint*/
AU_UAV_ROS::waypoint plane1WP = calculateWaypoint(plane1, turningRadius, turnRight);
/* Cooperative planning*/
bothNewWaypoints.plane2ID = -1;
bothNewWaypoints.plane2WP = plane1WP;
if (findGreatestThreat(planes[threatID], planes).planeID == plane1.getID()) {
ROS_WARN("Planes %d and %d are each other's greatest threats", plane1.getID(), threatID);
ROS_WARN("Time to go = %f | ZEM = %f", timeToGo, threatZEM);
ROS_WARN("Plane %d bearing = %f | %d", plane1.getID(), plane1.getCurrentBearing(), turnRight);
ROS_WARN("Plane %d bearing = %f | %d", threatID, planes[threatID].getCurrentBearing(), turnRight);
AU_UAV_ROS::waypoint plane2WP = calculateWaypoint(planes[threatID], turningRadius, turnRight);
bothNewWaypoints.plane2WP = plane2WP;
bothNewWaypoints.plane2ID = threatID;
}
bothNewWaypoints.plane1WP = plane1WP;
return bothNewWaypoints;
}
