/* 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 function calculates any maneuvers that are necessary for the
current plane to avoid looping. Returns a waypoint based on calculations.
If no maneuvers are necessary, then the function returns the current
destination. */
sim::waypoint sim::takeDubinsPath(PlaneObject &plane1) {
/* Initialize variables */
sim::coordinate circleCenter;
sim::waypoint wp = plane1.getDestination();
double minTurningRadius = 0.75*MINIMUM_TURNING_RADIUS;
bool destOnRight;
/* Calculate cartesian angle from plane to waypoint */
double wpBearing = findAngle(plane1.getCurrentLoc().latitude,
plane1.getCurrentLoc().longitude, wp.latitude, wp.longitude);
/* Calculate cartesian current bearing of plane (currentBearing is stored as Cardinal) */
double currentBearingCardinal = plane1.getCurrentBearing();
double currentBearingCartesian = toCartesian(currentBearingCardinal);
if (fabs(currentBearingCardinal) < 90.0)
/* Figure out which side of the plane the waypoint is on */
destOnRight = ((wpBearing < currentBearingCartesian) &&
(wpBearing > manipulateAngle(currentBearingCartesian - 180.0)));
else
destOnRight = !((wpBearing > currentBearingCartesian) &&
(wpBearing < manipulateAngle(currentBearingCartesian - 180.0)));
/* Calculate the center of the circle of minimum turning radius on the side that the waypoint is on */
circleCenter = calculateLoopingCircleCenter(plane1, minTurningRadius, destOnRight);
/* If destination is inside circle, must fly opposite direction before we can reach destination */
if (findDistance(circleCenter.latitude, circleCenter.longitude, wp.latitude, wp.longitude) <
minTurningRadius)
return calculateWaypoint(plane1, minTurningRadius, !destOnRight);
else
return wp;
}
/* 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;
}
