/* 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; }