double DubinsLength3D(double x0,double y0,double z0,double yaw0,
double x1,double y1,double z1,double yaw1,
double r)
{
std::cout<< "DubinsLength3D"<< std::endl;
std::cout<< x0<<" "<<y0<<" "<<z0<<" "<<yaw0/M_PI*180.<< std::endl;
std::cout<< x1<<" "<<y1<<" "<<z1<<" "<<yaw1/M_PI*180.<< std::endl;
std::cout<<"rho: "<< r<< std::endl;
DubinsPath path2D;
double q0[]= { x0, y0, yaw0 };
double q1[]= { x1, y1, yaw1 };
dubins_init(q0, q1, r, &path2D);
double length_2d= dubins_path_length(&path2D);
return sqrt(length_2d*length_2d+(z0-z1)*(z0-z1) );
}
// form flocks based on their trajectories and waypoints
void formFlocks(void) {
for (int i = 0; i < (int)pobjects.size(); i++){
// stop UAVs from following themselves
if (pobjects[i].getLeader() == pobjects[i].getID()){
std::cout << "UAV " << pobjects[i].getID() << " tried to follow itself. Stopping it now." << std::endl;
pobjects[i].setLeader(-1);
}
// deflock when a leader gets close to its waypoint
if (findDistance(pobjects[i].getDestination().latitude, pobjects[i].getDestination().longitude,
pobjects[i].getLatitude(), pobjects[i].getLongitude()) < 2*COLLISION_THRESHOLD) {
deflock(i);
}
else{
// detect clusters, check Dubins Paths, and form flocks
for (int j = 0; j < (int)pobjects.size(); j++){
// if we're not comparing the plane to itself
if(i != j) {
// stop UAVs from following the same plane
if (pobjects[i].getLeader() == pobjects[j].getLeader() &&pobjects[i].getLeader() != -1){
if (pobjects[pobjects[i].getLeader()].getFollower() == pobjects[i].getLeader()){
pobjects[j].setLeader(-1);
}
else{
pobjects[i].setLeader(-1);
}
}
// FLOCKING CASE 1: both planes are independent
if(pobjects[i].getLeader() == -1 && pobjects[i].getFollower() == -1 &&
pobjects[j].getLeader() == -1 && pobjects[j].getFollower() == -1) {
// check their waypoints' distances
if (findDistance(pobjects[i].getDestination().latitude, pobjects[i].getDestination().longitude,
pobjects[j].getDestination().latitude, pobjects[j].getDestination().longitude) < flockVars.clusterDistance) {
// generate Dubins Paths and divide by speed to get time required to get to correct vector position
DubinsPath* dubinsPathij = setupDubins(&pobjects[i],&pobjects[j]);
double ijDubinsTime = dubins_path_length(dubinsPathij)/MPS_SPEED;
DubinsPath* dubinsPathji = setupDubins(&pobjects[j],&pobjects[i]);
double jiDubinsTime = dubins_path_length(dubinsPathji)/MPS_SPEED;
int leadingUAV = -1;
int followingUAV= -1;
// if both Dubins Paths are short enough, decide leader based on distance to waypoint
if (ijDubinsTime < flockVars.flockTime && jiDubinsTime < flockVars.flockTime){
// find out which member is closer and make it the leader
// if i is closer than j, make i the leader
if (AU_UAV_ROS::cmpDistToDest(pobjects[i], pobjects[j])){
leadingUAV = pobjects[i].getID();
followingUAV = pobjects[j].getID();
if (leadingUAV == followingUAV) { // stop rings
break;
}
else {
pobjects[leadingUAV].setFollower(followingUAV);
pobjects[followingUAV].setLeader(leadingUAV);
std::cout << "case 1 flocking " << leadingUAV << " and " << followingUAV << std::endl;
}
}
else {
leadingUAV = pobjects[j].getID();
followingUAV = pobjects[i].getID();
if (leadingUAV == followingUAV) { // stop rings
break;
}
else {
pobjects[leadingUAV].setFollower(followingUAV);
pobjects[followingUAV].setLeader(leadingUAV);
std::cout << "case 1 flocking " << leadingUAV << " and " << followingUAV << std::endl;
}
}
}
// if only the Dubins Path of i is short enough, make j the leader
else if (ijDubinsTime < flockVars.flockTime){
leadingUAV = pobjects[j].getID();
followingUAV = pobjects[i].getID();
if (leadingUAV == followingUAV) { // stop rings
break;
}
else {
pobjects[leadingUAV].setFollower(followingUAV);
pobjects[followingUAV].setLeader(leadingUAV);
std::cout << "case 1 flocking " << leadingUAV << " and " << followingUAV << std::endl;
}
}
// if only the Dubins Path of j is short enough, make i the leader
else if (jiDubinsTime < flockVars.flockTime){
leadingUAV = pobjects[i].getID();
followingUAV = pobjects[j].getID();
if (leadingUAV == followingUAV) { // stop rings
break;
}
else {
pobjects[leadingUAV].setFollower(followingUAV);
pobjects[followingUAV].setLeader(leadingUAV);
std::cout << "case 1 flocking " << leadingUAV << " and " << followingUAV << std::endl;
}
}
}
}
// FLOCKING CASE 2: independent or tail plane leads a flock
// i is independent or a tail; j is a flock leader (doesn't have a leader, but has a follower)
else if (pobjects[i].getFollower() == -1 && pobjects[j].getLeader() == -1 && pobjects[j].getFollower() != -1){
// if the waypoints are close
if (findDistance(pobjects[i].getDestination().latitude, pobjects[i].getDestination().longitude,
pobjects[j].getDestination().latitude, pobjects[j].getDestination().longitude) < flockVars.clusterDistance) {
// only need one Dubins Path here- check from flock to independent/tail
DubinsPath* dubinsPathji = setupDubins(&pobjects[j],&pobjects[i]);
double jiDubinsTime = dubins_path_length(dubinsPathji)/MPS_SPEED;
if (jiDubinsTime < flockVars.flockTime){
if (pobjects[i].getLeader() != pobjects[j].getID()) { // stop rings
pobjects[j].setLeader(pobjects[i].getID());
pobjects[i].setFollower(pobjects[j].getID());
std::cout << "case 2 flocking " << i << " and " << j << std::endl;
}
}
}
}
// FLOCKING CASE 3: independent or lead plane follows a flock
// i is independent; j is the end of a flock (has a leader, but doesn't have a follower)
else if (pobjects[i].getLeader() == -1 && pobjects[j].getLeader() != -1 && pobjects[j].getFollower() == -1){
// if the waypoints are close
if (findDistance(pobjects[i].getDestination().latitude, pobjects[i].getDestination().longitude,
pobjects[j].getDestination().latitude, pobjects[j].getDestination().longitude) < flockVars.clusterDistance) {
// only need one Dubins Path here- check from independent/lead to flock
DubinsPath* dubinsPathij = setupDubins(&pobjects[i],&pobjects[j]);
double ijDubinsTime = dubins_path_length(dubinsPathij)/MPS_SPEED;
if (ijDubinsTime < flockVars.flockTime){
if (pobjects[j].getLeader() != pobjects[i].getID()){ // stop rings
pobjects[i].setLeader(pobjects[j].getID());
pobjects[j].setFollower(pobjects[i].getID());
std::cout << "case 3 flocking " << j << " and " << i << std::endl;
}
}
}
}
}
}
}
}
}
qreal Dubins::length() const
{
return dubins_path_length(_guts.data());
}
