/*Calculates the distance and angle to closer passing point*/
void updatePassingPoint(void){
/*Find distances to 2 PPs*/
float fDistances[2];
fDistances[0] = findDistance(fCurrentPosX, fCurrentPosY, fPassingPoint[0][0], fPassingPoint[0][1]);
fDistances[1] = findDistance(fCurrentPosX, fCurrentPosY, fPassingPoint[1][0], fPassingPoint[1][1]);
/*Decide the closer PP*/
if(fDistances[0] <= fDistances[1]){
iPassingPointNum = 0;
fDistanceToPassingPoint = fDistances[0];
}
else{
iPassingPointNum = 1;
fDistanceToPassingPoint = fDistances[1];
}
/*Check the distance to PP to flag if PP has been reached*/
if(fDistanceToPassingPoint < 0.5){
bPassingPointReached = true;
}
/*Calculate angle*/
fPassingPointAngle = atan2((fPassingPoint[iPassingPointNum][1] - fCurrentPosY), (fPassingPoint[iPassingPointNum][0] - fCurrentPosX));
fPassingPointAngle /= M_PI;
fPassingPointAngle *= 180.00;
if(fPassingPointAngle < 0.00) fPassingPointAngle += 360.00;
if(fPassingPointAngle > 360.00) fPassingPointAngle -= 360.00;
}
void Weapon::FindRotationGun(XYZ start, XYZ target)
{
XYZ temppoint1, temppoint2, tempforward;
float distance;
temppoint1 = start;
temppoint2 = target;
distance = findDistance(temppoint1, temppoint2);
gunrotate2 = asin((temppoint1.y - temppoint2.y) / distance) * RAD2DEG;
temppoint1.y = 0;
temppoint2.y = 0;
gunrotate1 =
acos((temppoint1.z - temppoint2.z) / findDistance(temppoint1,
temppoint2)) * RAD2DEG;
if(temppoint1.x > temppoint2.x)
gunrotate1 = 360 - gunrotate1;
tempforward = target - start;
tempforward = DoRotation(tempforward, -90, 0, 0);
tempforward = DoRotation(tempforward, 0, gunrotate1 - 90, 0);
tempforward = DoRotation(tempforward, 0, 0, gunrotate2 - 90);
tempforward.y = 0;
Normalise(&tempforward);
gunrotate3 = acos(0 - tempforward.z) * RAD2DEG;
if(0 > tempforward.x)
gunrotate3 = 360 - gunrotate3;
}
/* 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;
}
int CSearch::findSwitches()
{
int nEngine;
int nQuery = 0;
int nTmp;
int nLongest = 0;
int nSwitches = 0;
do
{
for (nEngine = 0; nEngine < m_nNumEngines; nEngine++)
{
nTmp = findDistance(nEngine, nQuery);
if (nTmp > nLongest)
{
nLongest = nTmp;
}
}
if (nLongest != nQuery)
{
if (nQuery != 0)
{
nSwitches++;
}
nQuery = nLongest;
}
} while (nQuery < m_nNumQueries);
return (nSwitches < 0) ? 0 : nSwitches;
}
void markIssues(void){
std::stringstream ss;
double distance;
/* Force 10 figure decimal precision in output */
ss.setf(std::ios::fixed);
ss.precision(10);
/* check for conflicts / collisions between planes - Note: iter->first returns the plane id of the current element */
for (std::map<int, AU_UAV_ROS::PlaneInfo>::iterator iter = planes.begin(); iter != planes.end(); iter++){
for (std::map<int, AU_UAV_ROS::PlaneInfo>::iterator iter2 = planes.begin(); iter2->first < iter->first; iter2++){
/* find distance between the two UAVs */
distance = findDistance((iter->second).lastUpdate.currentLatitude, (iter->second).lastUpdate.currentLongitude,
(iter2->second).lastUpdate.currentLatitude, (iter2->second).lastUpdate.currentLongitude);
ss.str(""); /* clear string stream before input */
if (distance < COLLISION_THRESHOLD){ /* update collisionPts */
ss<<(iter->second).lastUpdate.currentLongitude<<","<<(iter->second).lastUpdate.currentLatitude<<","<<0;
collisionPts.push_back(ss.str());
}
else if (distance < CONFLICT_THRESHOLD){ /* update conflictPts */
ss<<(iter->second).lastUpdate.currentLongitude<<","<<(iter->second).lastUpdate.currentLatitude<<","<<0;
conflictPts.push_back(ss.str());
}
}
}
}
int reducePatterns(Pattern* allPatterns, Pattern* reducedPatterns, int numberOfPatterns){
int i,j;
int reducedCounter=0;
int newPattern;
Pattern temp;
for(i=0; i<numberOfPatterns; i++){
newPattern = 1;
temp = allPatterns[i];
//Check if this pattern is within 10px of another pattern in the reduced pattern list.
//If not, it is added to the reduced pattern list
for(j=0; j<reducedCounter; j++){
if(findDistance(temp.center, reducedPatterns[j].center) < 10){
newPattern=0;
}
}
if(newPattern){
reducedPatterns[reducedCounter] = temp;
reducedCounter++;
}
}
return reducedCounter;
}
void callback(int i)
{
float time;
clock_t t1, t2;
// Start timer
t1 = clock();
// Filtering, HSV to Binary Image, Erosions and Dilations
cvSmooth(frame, imageFiltree, CV_BLUR,seuilFiltre,seuilFiltre,0.0,0.0);
cvCvtColor(imageFiltree, imageHSV,CV_BGR2HSV);
cvInRangeS(imageHSV,cvScalar(hmin, smin, vmin, 0.0),cvScalar(hmax, smax, vmax, 0.0),imageBinaire);
cvErode(imageBinaire, imageErodee, NULL, nbErosions);
cvDilate(imageErodee, imageDilatee, NULL, nbDilatations);
//imageDilateeFiltree = lowPassFilter(imageDilatee); FILTER
// multiplication between the original image in RGB and HSV and the binary image
imageObjectRGB = multBinColor(imageDilatee, frame);
imageObjectHSV = multBinColor(imageDilatee, imageHSV);
// find the points and separate them (rows correspond to each point and the columns to the pixels belonging to the points)
vector<vector<CvPoint3D32f> > vecDistinctPoints = findPoint();
// find the centroid of the point and trace it
vector<CvPoint> centroid = centroiding(vecDistinctPoints);
// sort the centroids
centroid = sort(centroid);
// compute the distance with and without lens distortion
vector<double> distance = findDistance(imageObjectHSV, centroid, tanAlphaT);
// Contours
/*cvFindContours( imageDilatee, storage, &contours, sizeof(CvContour),
CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cvPoint(0,0) );*/
/*cvDrawContours( frame, contours,
CV_RGB(255,255,0), CV_RGB(0,255,0),
1, 2, 8, cvPoint(0,0));*/
cvNamedWindow(myWindow, CV_WINDOW_AUTOSIZE);
cvNamedWindow(myWindowObjectHSV, CV_WINDOW_AUTOSIZE);
cvNamedWindow(myWindowObjectRGB, CV_WINDOW_AUTOSIZE);
cvShowImage(myWindow, frame);
cvShowImage(myWindowObjectHSV, imageObjectHSV);
cvShowImage(myWindowObjectRGB, imageObjectRGB);
//cvSaveImage("NoisyGridCentroiding.png", imageObjectRGB,0);
// End timer
t2 = clock();
// Compute execution time
time = (float)(t2 - t1) / CLOCKS_PER_SEC;
cout << "execution time = " << time << " s" << endl;
}
/* Determine if a collision object is colliding with another in the airspace */
bool AU_UAV_ROS::CObject::isColliding(const AU_UAV_ROS::CObject& cobj) const {
double distance = findDistance(cobj);
/* Shorten distance by radius of each object to see if the
collision fields are intersecting each other */
distance -= this->collisionRadius;
distance -= cobj.collisionRadius;
/* If distance is now zero or less there is a collision */
return (distance <= 0);
}
QString AngleVariation::checkOnLineA(QPointF mousePoint)
{
QPointF closestPointToMin = closestPointOnLineSegment(branchLine.p1(), origPoint, mousePoint);
float distToMin = findDistance(mousePoint, closestPointToMin);
QPointF closestPointToMax = closestPointOnLineSegment(branchLine.p1(), variationPoint, mousePoint);
float distToMax = findDistance(mousePoint, closestPointToMax);
if (distToMin < 5.0f)
{
distToWidget = distToMin;
return "amin";
}
else if (distToMax < 5.0f)
{
distToWidget = distToMax;
return "amax";
}
else
return "NULL";
}
void AngleVariation::checkParentBranchIntersection(QPointF* parentBounds)
{
QLineF sideLine1 = QLineF(parentBounds[0], parentBounds[3]);
QLineF sideLine2 = QLineF(parentBounds[1], parentBounds[2]);
float tempSide1 = findDistance(branchLine.p1(), closestPointOnLine(sideLine1.p1(), sideLine1.p2(), branchLine.p1()));
float tempSide2 = findDistance(branchLine.p1(), closestPointOnLine(sideLine2.p1(), sideLine2.p2(), branchLine.p1()));
QLineF chosenLine;
if (tempSide1 < tempSide2)
chosenLine = sideLine1;
else
chosenLine = sideLine2;
QPointF point1 = calcVarExtremePoint(chosenLine, branchLine.p1(), length);
QPointF point2 = findAdjacentPoint(branchLine.p1(), point1);
QPointF control = calcVarExtremePoint(branchLine, branchLine.p1(), length);
if (findDistance(origPoint, control) >= findDistance(point1, control))
{
origPoint = point1;
variationPoint = calculateOpposingPoint(origPoint);
}
if (findDistance(origPoint, control) >= findDistance(point2, control))
{
origPoint = point2;
variationPoint = calculateOpposingPoint(origPoint);
}
}
void callback(int i)
{
cvSmooth(frame, imageFiltree, CV_BLUR,seuilFiltre,seuilFiltre,0.0,0.0);
cvCvtColor(imageFiltree, imageHSV,CV_BGR2HSV);
cvInRangeS(imageHSV,cvScalar(hmin, smin, vmin, 0.0),cvScalar(hmax, smax, vmax, 0.0),imageBinaire);
cvErode(imageBinaire, imageErodee, NULL, nbErosions);
cvDilate(imageErodee, imageDilatee, NULL, nbDilatations);
//imageDilateeFiltree = lowPassFilter(imageDilatee); FILTRE
imageObjectRGB = multBinColor(imageDilatee, frame);
imageObjectHSV = multBinColor(imageDilatee, imageHSV);
// find the centroid of the object and trace it
int centroid[2] = {0,0};
centroiding(imageObjectHSV,centroid,2);
cvLine(imageObjectHSV, cvPoint(centroid[0]-2,centroid[1]), cvPoint(centroid[0]+2,centroid[1]), CvScalar(255,255,255,0));
cvLine(imageObjectHSV, cvPoint(centroid[0],centroid[1]-2), cvPoint(centroid[0],centroid[1]+2), CvScalar(255,255,255,0));
cvLine(imageObjectRGB, cvPoint(centroid[0]-2,centroid[1]), cvPoint(centroid[0]+2,centroid[1]), CvScalar(255,255,255,0));
cvLine(imageObjectRGB, cvPoint(centroid[0],centroid[1]-2), cvPoint(centroid[0],centroid[1]+2), CvScalar(255,255,255,0));
double distance = findDistance(imageObjectHSV, centroid);
//double distance2 = findDistance2(imageObjectHSV, centroid);
// Contours
cvFindContours( imageDilatee, storage, &contours, sizeof(CvContour),
CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cvPoint(0,0) );
//imageFinale = multiplier(frame/*imageHSV*/, imageDilatee);
//cvDrawContours( imageFinale, contours,
cvDrawContours( frame, contours,
CV_RGB(255,255,0), CV_RGB(0,255,0),
1, 2, 8, cvPoint(0,0));
//cvCvtColor(imageFinale, imageFinale,CV_HSV2BGR);
cvNamedWindow(myWindow, CV_WINDOW_AUTOSIZE);
cvNamedWindow(myWindowObjectHSV, CV_WINDOW_AUTOSIZE);
cvNamedWindow(myWindowObjectRGB, CV_WINDOW_AUTOSIZE);
/*cvResizeWindow(myWindowObjectHSV, 500, 400);
cvResizeWindow(myWindowObjectRGB, 500, 400);
cvMoveWindow(myWindowObjectHSV, 0, 0);
cvMoveWindow(myWindowObjectRGB, 515, 0);*/
//cvShowImage(myWindow, imageFinale);
cvShowImage(myWindow, frame);
cvShowImage(myWindowObjectHSV, imageObjectHSV);
cvShowImage(myWindowObjectRGB, imageObjectRGB);
}
void CommandLineParser::run(int argc, char *argv[])
{
Command::commands typeCmd; // type of current command
int argcTemp = argc; // number of current command paramentres + 1(command name)
char** argvTemp = argv; //current command name and its paramentres
std::unique_ptr<Command> cmd; // current command
//if no arguments output <help>
if(argc == 1)
typeCmd = Command::commands::Help;
//go through all arguments step-by-step
int index = 1;
while(index < argc){
//get current command
typeCmd = Command::stringToCommand(argv[index]);
//set pointer to the name of current command
argvTemp = argv + index;
//count distance bitween current and next commands
argcTemp = findDistance(argc-index,argvTemp);
//skip parametres of current command
index += argcTemp;
if(typeCmd == Command::commands::Wrong || typeCmd == Command::commands::Help)
break;
else{
cmd = CommandFactory::getCommand(typeCmd, argcTemp, argvTemp, &user);
if(!cmd->run()){
typeCmd = Command::commands::Wrong;
break;
}
}
}
if(typeCmd == Command::commands::Help){
std::unique_ptr<Command> help = CommandFactory::getCommand(typeCmd, argcTemp, argvTemp, nullptr);
help->run();
}
else if (typeCmd == Command::commands::Wrong)
std::cout<< "Command " << *argvTemp <<
" is not supported (or was used in a wrong way), use </?> to see set of allowed commands" <<
std::endl;
}
void crossOverThree()
{
//Go through all the parents
for(int y = 0; y < pathToGen - 1; y++)
{
//Set the field to all full
setChosenToFalse();
//The first tile is always the starting tile
tmpChild = insertNum(tmpChild, 0, 25);
//Go through the length of the path
for(int x = 1; x < pathDepth; x++)
{
tmpChild = insertNum(tmpChild, x, findUnusedRandomNumber(y, x);
}
//Find the distance of the new path
int tmpDist = 0;
for(int x = 0; x < pathDepth-1; x++)
tmpDist += findDistance(extractNum(tmpChild, x), extractNum(tmpChild, x+1));
//Go through the current paths and see if this one is better
for(int x = 0; x < pathToGen; x++)
{
//Did we find a match?
if(parents[x].fitness > tmpDist)
{
//If this is the best match, update the best map
if(x==0)
{
bestChosen = chosen;
}
//Replace the current data with the new path data
parents[x].fitness = tmpDist;
parents[x].path = tmpChild;
}
break;
}
}
}
/* Returns true if the original plane (plane1) should turn right to avoid plane2,
false if otherwise. Takes original plane and its greatest threat as parameters. */
bool sim::shouldTurnRight(PlaneObject &plane1, PlaneObject &plane2) {
/* For checking whether the plane should turn right or left */
double theta, theta_dot, R;
double cartBearing1 = toCartesian(plane1.getCurrentBearing());
double cartBearing2 = toCartesian(plane2.getCurrentBearing());
double V = MPS_SPEED;
/* Calculate theta, theta1, and theta2. Theta is the cartesian angle
from 0 degrees (due East) to plane2 (using plane1 as the origin). This
may be referred to as the LOS angle. */
theta = findAngle(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
R = findDistance(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
theta_dot = (V*sin((cartBearing2 - theta)*PI/180) - V*sin((cartBearing1 - theta)*PI/180)) / R;
return (theta_dot >= 0);
}
static void computeCurve( coOrd pos0, coOrd pos1, double radius, coOrd * center, double * a0, double * a1 )
/* translate between curves described by 2 end-points and a radius to
a curve described by a center, radius and angles.
*/
{
double d, a, aa, aaa, s;
d = findDistance( pos0, pos1 )/2.0;
a = findAngle( pos0, pos1 );
s = fabs(d/radius);
if (s > 1.0)
s = 1.0;
aa = R2D(asin( s ));
if (radius > 0) {
aaa = a + (90.0 - aa);
*a0 = normalizeAngle( aaa + 180.0 );
translate( center, pos0, aaa, radius );
} else {
aaa = a - (90.0 - aa);
*a0 = normalizeAngle( aaa + 180.0 - aa *2.0 );
translate( center, pos0, aaa, -radius );
}
*a1 = aa*2.0;
}
void Path::findPath(int x, int y)
{
Destination *dest = Path::findNeighbors(x, y);
Destination * current = &Destination(x, y, 0, 0);
if (dest == NULL) {
for (int i = 0; i < signed(destinations->size()); i++)
destinations->at(i)->Print();
std::cout << "Could not find location" << std::endl;
return;
}
Direction direction;
int distance = 0;
for (int i = 0; i < 120; i++)
{
direction = findDirection(current->x, current->y, dest->x, dest->y);
distance = findDistance(current->x, current->y, dest->x, dest->y);
//const char *arr[] = {"Left", "Right", "Up", "Down"};
//std::cout << "Going " << arr[direction] << "distance " << distance << std::endl;
roadMap.push_back(new Destination(dest->x, dest->y, direction, distance * GRANULARITY));
current = dest;
int random = rand() % current->neighbors.size();
dest = current->neighbors.at(random);
}
}
double AU_UAV_ROS::CObject::findDistance(const CObject& cobj) const {
return findDistance(cobj.latitude, cobj.longitude);
}
// 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;
}
}
}
}
}
}
}
}
}
PetscErrorCode random_network(PetscInt nvertex,PetscInt *pnbranch,Node **pnode,Branch **pbranch,PetscInt **pedgelist,PetscInt seed)
{
PetscErrorCode ierr;
PetscInt i, j, nedges = 0;
PetscInt *edgelist;
PetscInt nbat, ncurr, fr, to;
PetscReal *x, *y, value, xmax = 10.0; /* generate points in square */
PetscReal maxdist = 0.0, dist, alpha, beta, prob;
PetscRandom rnd;
Branch *branch;
Node *node;
Edge *head = NULL, *nnew= NULL, *aux= NULL;
PetscFunctionBeginUser;
ierr = PetscRandomCreate(PETSC_COMM_SELF,&rnd);CHKERRQ(ierr);
ierr = PetscRandomSetFromOptions(rnd);CHKERRQ(ierr);
ierr = PetscRandomSetSeed(rnd, seed);CHKERRQ(ierr);
ierr = PetscRandomSeed(rnd);CHKERRQ(ierr);
/* These parameters might be modified for experimentation */
nbat = (PetscInt)(0.1*nvertex);
ncurr = (PetscInt)(0.1*nvertex);
alpha = 0.6;
beta = 0.2;
ierr = PetscMalloc2(nvertex,&x,nvertex,&y);CHKERRQ(ierr);
ierr = PetscRandomSetInterval(rnd,0.0,xmax);CHKERRQ(ierr);
for (i=0; i<nvertex; i++) {
ierr = PetscRandomGetValueReal(rnd,&x[i]);CHKERRQ(ierr);
ierr = PetscRandomGetValueReal(rnd,&y[i]);CHKERRQ(ierr);
}
/* find maximum distance */
for (i=0; i<nvertex; i++) {
for (j=0; j<nvertex; j++) {
dist = findDistance(x[i],x[j],y[i],y[j]);
if (dist >= maxdist) maxdist = dist;
}
}
ierr = PetscRandomSetInterval(rnd,0.0,1.0);CHKERRQ(ierr);
for (i=0; i<nvertex; i++) {
for (j=0; j<nvertex; j++) {
if (j != i) {
dist = findDistance(x[i],x[j],y[i],y[j]);
prob = beta*PetscExpScalar(-dist/(maxdist*alpha));
ierr = PetscRandomGetValueReal(rnd,&value);CHKERRQ(ierr);
if (value <= prob) {
ierr = PetscMalloc1(1,&nnew);CHKERRQ(ierr);
if (head == NULL) {
head = nnew;
head->next = NULL;
head->n = nedges;
head->i = i;
head->j = j;
} else {
aux = head;
head = nnew;
head->n = nedges;
head->next = aux;
head->i = i;
head->j = j;
}
nedges += 1;
}
}
}
}
ierr = PetscMalloc1(2*nedges,&edgelist);CHKERRQ(ierr);
for (aux = head; aux; aux = aux->next) {
edgelist[(aux->n)*2] = aux->i;
edgelist[(aux->n)*2 + 1] = aux->j;
}
aux = head;
while (aux != NULL) {
nnew = aux;
aux = aux->next;
ierr = PetscFree(nnew);CHKERRQ(ierr);
}
ierr = PetscCalloc2(nvertex,&node,nedges,&branch);CHKERRQ(ierr);
for (i = 0; i < nvertex; i++) {
node[i].id = i;
node[i].inj = 0;
node[i].gr = PETSC_FALSE;
}
for (i = 0; i < nedges; i++) {
branch[i].id = i;
branch[i].r = 1.0;
branch[i].bat = 0;
}
/* Chose random node as ground voltage */
ierr = PetscRandomSetInterval(rnd,0.0,nvertex);CHKERRQ(ierr);
ierr = PetscRandomGetValueReal(rnd,&value);CHKERRQ(ierr);
node[(int)value].gr = PETSC_TRUE;
/* Create random current and battery injectionsa */
for (i=0; i<ncurr; i++) {
ierr = PetscRandomSetInterval(rnd,0.0,nvertex);CHKERRQ(ierr);
ierr = PetscRandomGetValueReal(rnd,&value);CHKERRQ(ierr);
fr = edgelist[(int)value*2];
to = edgelist[(int)value*2 + 1];
node[fr].inj += 1.0;
node[to].inj -= 1.0;
}
for (i=0; i<nbat; i++) {
ierr = PetscRandomSetInterval(rnd,0.0,nedges);CHKERRQ(ierr);
ierr = PetscRandomGetValueReal(rnd,&value);CHKERRQ(ierr);
branch[(int)value].bat += 1.0;
}
ierr = PetscFree2(x,y);CHKERRQ(ierr);
ierr = PetscRandomDestroy(&rnd);CHKERRQ(ierr);
/* assign pointers */
*pnbranch = nedges;
*pedgelist = edgelist;
*pbranch = branch;
*pnode = node;
PetscFunctionReturn(ierr);
}
int localeDataCompareRegions(
const char *left_region, const char *right_region,
const char *requested_language, const char *requested_script,
const char *requested_region) {
if (left_region[0] == right_region[0] && left_region[1] == right_region[1]) {
return 0;
}
uint32_t left = packLocale(requested_language, left_region);
uint32_t right = packLocale(requested_language, right_region);
const uint32_t request = packLocale(requested_language, requested_region);
// If one and only one of the two locales is a special Spanish locale, we
// replace it with es-419. We don't do the replacement if the other locale
// is already es-419, or both locales are special Spanish locales (when
// es-US is being compared to es-MX).
const bool leftIsSpecialSpanish = isSpecialSpanish(left);
const bool rightIsSpecialSpanish = isSpecialSpanish(right);
if (leftIsSpecialSpanish && !rightIsSpecialSpanish && right != LATIN_AMERICAN_SPANISH) {
left = LATIN_AMERICAN_SPANISH;
} else if (rightIsSpecialSpanish && !leftIsSpecialSpanish &&
left != LATIN_AMERICAN_SPANISH) {
right = LATIN_AMERICAN_SPANISH;
}
uint32_t request_ancestors[MAX_PARENT_DEPTH + 1];
ssize_t left_right_index;
// Find the parents of the request, but stop as soon as we saw left or right
const std::array<uint32_t, 2> left_and_right = {{left, right}};
const size_t ancestor_count = findAncestors(
request_ancestors, &left_right_index,
request, requested_script,
left_and_right.data(), left_and_right.size());
if (left_right_index == 0) { // We saw left earlier
return 1;
}
if (left_right_index == 1) { // We saw right earlier
return -1;
}
// If we are here, neither left nor right are an ancestor of the
// request. This means that all the ancestors have been computed and
// the last ancestor is just the language by itself. We will use the
// distance in the parent tree for determining the better match.
const size_t left_distance = findDistance(
left, requested_script, request_ancestors, ancestor_count);
const size_t right_distance = findDistance(
right, requested_script, request_ancestors, ancestor_count);
if (left_distance != right_distance) {
return (int) right_distance - (int) left_distance; // smaller distance is better
}
// If we are here, left and right are equidistant from the request. We will
// try and see if any of them is a representative locale.
const bool left_is_representative = isRepresentative(left, requested_script);
const bool right_is_representative = isRepresentative(right, requested_script);
if (left_is_representative != right_is_representative) {
return (int) left_is_representative - (int) right_is_representative;
}
// We have no way of figuring out which locale is a better match. For
// the sake of stability, we consider the locale with the lower region
// code (in dictionary order) better, with two-letter codes before
// three-digit codes (since two-letter codes are more specific).
return (int64_t) right - (int64_t) left;
}
void telemetryCallback(const AU_UAV_ROS::TelemetryUpdate::ConstPtr &msg)
{
AU_UAV_ROS::GoToWaypoint goToWaypointSrv;
AU_UAV_ROS::RequestWaypointInfo requestWaypointInfoSrv;
int planeID = msg->planeID;
/* request this plane's current normal destination */
requestWaypointInfoSrv.request.planeID = planeID;
requestWaypointInfoSrv.request.isAvoidanceWaypoint = false;
requestWaypointInfoSrv.request.positionInQueue = 0;
if (!requestWaypointInfoClient.call(requestWaypointInfoSrv)){
ROS_ERROR("Did not receive a response from the coordinator");
return;
}
destLatArray[planeID] = requestWaypointInfoSrv.response.latitude;
destLonArray[planeID] = requestWaypointInfoSrv.response.longitude;
// check for the total number of planes present to use the correct parameters
if (planeID+1 > maxPlanesFound) maxPlanesFound++;
// check the positions of all the waypoints and planes to see the field size for the correct parameters
// it IS possible for a larger scenario to seem like a smaller scenario by chance, but that should be okay
if (planeID == maxPlanesFound-1)
{
fieldSize = 500;
for (unsigned int i = 0; i < maxPlanesFound; i += 1)
{
// check waypoints
if (destLatArray[i] < (SOUTH_MOST_LATITUDE_500M - LATITUDE_EPSILON) ||
destLonArray[i] > (EAST_MOST_LONGITUDE_500M + LONGITUDE_EPSILON))
{
fieldSize = 1000;
break;
}
}
// use the plane and waypoint information to find the correct parameters (update once per cycle)
if (fieldSize == 500){
if (maxPlanesFound <= 4){
setupVariables(param_4plane_500m);
}
else if (maxPlanesFound <= 8){
setupVariables(param_8plane_500m);
}
else if (maxPlanesFound <= 16){
setupVariables(param_16plane_500m);
}
else if (maxPlanesFound <= 32){
setupVariables(param_32plane_500m);
}
else
{
std::cout << "plane number error: more than 32 UAVs found- using 32 plane case" << std::endl;
setupVariables(param_32plane_500m);
}
}
else if (fieldSize == 1000){
if (maxPlanesFound <= 4){
setupVariables(param_4plane_1000m);
}
else if (maxPlanesFound <= 8){
setupVariables(param_8plane_1000m);
}
else if (maxPlanesFound <= 16){
setupVariables(param_16plane_1000m);
}
else if (maxPlanesFound <= 32){
setupVariables(param_32plane_1000m);
}
else
{
setupVariables(param_32plane_1000m);
std::cout << "plane number error: more than 32 UAVs found- using 32 plane case" << std::endl;
}
}
else std::cout << "field size error: invalid field size determined (this should never happen)" << std::endl;
std::cout << "Using parameters for a " << fieldSize << "m field and " << maxPlanesFound << " UAVs" << std::endl;
}
/*
Remove any planes that have been involved in a collision.
Note: This function is made for use with the evaluator node, and may not work optimally in the field.
To check for collisions, it compares the current time with the last update time of each of the UAVs.
If the values differ by more than three seconds, it is assumed the plane has been deleted. However,
packet losses / network latency may render issues in the real world.
*/
checkCollisions();
/*
if (plane has reached current destination waypoint)
move on to next normal destination waypoint in queue
*/
if (findDistance(msg->currentLatitude, msg->currentLongitude,
requestWaypointInfoSrv.response.latitude,
requestWaypointInfoSrv.response.longitude) < COLLISION_THRESHOLD){
/* request next normal destination */
requestWaypointInfoSrv.request.positionInQueue = 1;
if (!requestWaypointInfoClient.call(requestWaypointInfoSrv)){
ROS_ERROR("Did not recieve a response from the coordinator");
return;
}
}
/*
Pseudocode for the following code lines.
if (this plane is not in the map of planes and the telemetry update indicates that the plane is or has been moving towards a destination)
if (the plane had been flying but we previously detected a collision and removed it)
return - the plane is dead; the simulator is lagging behind
else
the plane has registered a new TelemetryUpdate
else
return - the plane has just been initialized but it is not moving torwards a waypoint as of now
*/
if (pobjects.find(planeID) == pobjects.end() && msg->currentWaypointIndex != -1){
if (msg->currentWaypointIndex > 0){
/* This plane is dead; it had previously been moving but was in a collision.
The simulator is lagging behind and still reporting a telemetry update */
return;
}
else{
/*
a new plane has registered a TelemetryUpdate where the destination is not (0, 0)
create new PlaneObject, collision radius is set to the distance traveled in one second
*/
AU_UAV_ROS::PlaneObject newPlane(MPS_SPEED, *msg); /* */
pobjects[planeID] = newPlane; /* put the new plane into the map */
/* update the destination of the PlaneObject with the value found with the requestWaypointInfoSrv call */
AU_UAV_ROS::waypoint newDest;
newDest.latitude = requestWaypointInfoSrv.response.latitude;
newDest.longitude = requestWaypointInfoSrv.response.longitude;
newDest.altitude = requestWaypointInfoSrv.response.altitude;
pobjects[planeID].setDestination(newDest);
}
}
else if (pobjects.find(planeID) == pobjects.end()) /* new plane without waypoint set */
return;
/*
Note: The requestWaypointInfo service returns a waypoint of -1000, -1000 when the UAV it cannot retrieve a destination from
queue.
Pseudocode:
if (the plane has no destination){
- for simulations, silence any force from this UAV so it does not affect flight paths by giving it an impossible location
- update with the new time of latest update to avoid a false detection of a collision
}
else{
update the plane with the new telemetry information
if (the plane's destination has changed)
update the map entry of the plane with this information
}
*/
if (requestWaypointInfoSrv.response.latitude == -1000){ /* plane has no waypoints to go to */
/*
useful for simulations, remove in real flights;
set location of finished planes to -1000, -1000 so no repulsive force is felt from this plane
*/
pobjects[planeID].setLatitude(-1000);
pobjects[planeID].setLongitude(-1000);
pobjects[planeID].update(); /* update the time of last update for this plane to acknowledge it is still in the air */
return;
}
else{
pobjects[planeID].update(*msg); /* update plane with new position */
/* if (destination has changed)
update pobjects[planeID] with new destination */
if (((pobjects[planeID].getDestination().latitude - requestWaypointInfoSrv.response.latitude) > EPSILON)
|| ((pobjects[planeID].getDestination().longitude - requestWaypointInfoSrv.response.longitude) > EPSILON)
|| ((pobjects[planeID].getDestination().latitude - requestWaypointInfoSrv.response.latitude) < EPSILON)
|| ((pobjects[planeID].getDestination().longitude - requestWaypointInfoSrv.response.longitude) < EPSILON)){
AU_UAV_ROS::waypoint newDest;
newDest.latitude = requestWaypointInfoSrv.response.latitude;
newDest.longitude = requestWaypointInfoSrv.response.longitude;
newDest.altitude = requestWaypointInfoSrv.response.altitude;
pobjects[planeID].setDestination(newDest);
}
}
//form flocks
formFlocks();
//organize flock information
checkFlocks();
// give the UAVs their flock numbers
for (unsigned int i = 0; i < flocks.size(); i += 1)
{
for (unsigned int j = 0; j < flocks[i].size(); j += 1)
{
flocks[i][j]->setFlock(i);
}
}
/* Find the force acting on this UAV. The plane is attracted to its waypoint or leader, and repelled from other UAVs */
AU_UAV_ROS::mathVector force = calculateForces(pobjects[planeID], pobjects, flocks, forceVars);
/*
Create a goToWaypoint service to send to the coordinator based on the force vector just calculated. The destination will be one
second away from the current position.
*/
goToWaypointSrv = updatePath(msg, force);
goToWaypointSrv.request.isAvoidanceManeuver = true;
goToWaypointSrv.request.isNewQueue = true;
if (goToWaypointClient.call(goToWaypointSrv)){
count++;
//ROS_INFO("Received response from service request %d", (count-1));
}
else{
ROS_ERROR("Did not receive response");
}
}
/* Function that returns the ID of the most dangerous neighboring plane and its ZEM */
AU_UAV_ROS::threatContainer AU_UAV_ROS::findGreatestThreat(PlaneObject &plane1, std::map<int, PlaneObject> &planes){
/* Set reference for origin (Northwest corner of the course)*/
AU_UAV_ROS::coordinate origin;
origin.latitude = 32.606573;
origin.longitude = -85.490356;
origin.altitude = 400;
/* Set preliminary plane to avoid as non-existent and most dangerous
ZEM as negative*/
int planeToAvoid = -1;
double mostDangerousZEM = -1;
/* Set the preliminary time-to-go to infinity*/
double minimumTimeToGo = std::numeric_limits<double>::infinity();
/* Declare second plane and ID variable */
PlaneObject plane2;
int ID;
/* Make a position vector representation of the current plane*/
double magnitude2, direction2;
double magnitude = findDistance(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
double direction = findAngle(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
AU_UAV_ROS::mathVector p1(magnitude,direction);
/* Make a heading vector representation of the current plane*/
AU_UAV_ROS::mathVector d1(1.0,toCartesian(plane1.getCurrentBearing()));
/* Declare variables needed for this loop*/
AU_UAV_ROS::mathVector pDiff;
AU_UAV_ROS::mathVector dDiff;
double timeToGo, zeroEffortMiss, distanceBetween, timeToDest;
std::map<int,AU_UAV_ROS::PlaneObject>::iterator it;
for ( it=planes.begin() ; it!= planes.end(); it++ ){
/* Unpacking plane to check*/
ID = (*it).first;
plane2 = (*it).second;
/* If it's not in the Check Zone, check the other plane*/
distanceBetween = plane1.findDistance(plane2);
if (distanceBetween > CHECK_ZONE || plane1.getID() == ID) continue;
else if (distanceBetween < MPS_SPEED) {
planeToAvoid = ID;
mostDangerousZEM = 0;
minimumTimeToGo = 0.1;
break;
}
/* Making a position vector representation of plane2*/
magnitude2 = findDistance(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
direction2 = findAngle(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
AU_UAV_ROS::mathVector p2(magnitude2,direction2);
/* Make a heading vector representation of the current plane*/
AU_UAV_ROS::mathVector d2(1.0,toCartesian(plane2.getCurrentBearing()));
/* Compute Time To Go*/
pDiff = p1-p2;
dDiff = d1-d2;
timeToGo = -1*pDiff.dotProduct(dDiff)/(MPS_SPEED*dDiff.dotProduct(dDiff));
/* Compute Zero Effort Miss*/
zeroEffortMiss = sqrt(pDiff.dotProduct(pDiff) +
2*(MPS_SPEED*timeToGo)*pDiff.dotProduct(dDiff) +
pow(MPS_SPEED*timeToGo,2)*dDiff.dotProduct(dDiff));
/* If the Zero Effort Miss is less than the minimum required
separation, and the time to go is the least so far, then avoid this plane*/
if(zeroEffortMiss <= DANGER_ZEM && timeToGo < minimumTimeToGo && timeToGo > 0){
// If the plane is behind you, don't avoid it
if ( fabs(plane2.findAngle(plane1)*180/PI - toCartesian(plane1.getCurrentBearing())) > 35.0) {
timeToDest = plane1.findDistance(plane1.getDestination().latitude,
plane1.getDestination().longitude) / MPS_SPEED;
/* If you're close to your destination and the other plane isn't
much of a threat, then don't avoid it */
if ( timeToDest < 5.0 && zeroEffortMiss > 3.0*MPS_SPEED ) continue;
planeToAvoid = ID;
mostDangerousZEM = zeroEffortMiss;
minimumTimeToGo = timeToGo;
}
}
}
AU_UAV_ROS::threatContainer greatestThreat;
greatestThreat.planeID = planeToAvoid;
greatestThreat.ZEM = mostDangerousZEM;
greatestThreat.timeToGo = minimumTimeToGo;
return greatestThreat;
}
/* Function that returns the ID of the most dangerous neighboring plane and its ZEM. */
sim::threatContainer sim::findGreatestThreat(PlaneObject &plane1, std::map<int, PlaneObject> &planes) {
/* Set reference for origin (Northwest corner of the course)*/
sim::coordinate origin;
origin.latitude = 32.606573;
origin.longitude = -85.490356;
origin.altitude = 400;
/* Set preliminary plane to avoid as non-existent and most dangerous ZEM as negative */
int planeToAvoid = -1;
int iPlaneToAvoid = -1;
double mostDangerousZEM = -1.0;
double iMostDangerousZEM = -1.0;
/* Set the preliminary time-to-go high */
double minimumTimeToGo = MINIMUM_TIME_TO_GO;
double iMinimumTimeToGo = 3.5;
/* Declare second plane and ID variable */
PlaneObject plane2;
int ID;
/* Make a position vector representation of the current plane */
double magnitude2, direction2;
double magnitude = findDistance(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
double direction = findAngle(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
sim::mathVector p1(magnitude,direction);
/* Make a heading vector representation of the current plane */
sim::mathVector d1(1.0,toCartesian(plane1.getCurrentBearing()));
/* Declare variables needed for this loop */
sim::mathVector pDiff, dDiff;
double timeToGo, zeroEffortMiss, distanceBetween, timeToDest, bearingDiff;
std::map<int,sim::PlaneObject>::iterator it;
for (it=planes.begin() ; it!= planes.end(); it++) {
/* Unpacking plane to check */
ID = (*it).first;
plane2 = (*it).second;
/* If it's not in the Check Zone, check the other plane */
distanceBetween = plane1.findDistance(plane2);
if (distanceBetween > CHECK_ZONE || plane1.getID() == ID) continue;
/* Making a position vector representation of plane2 */
magnitude2 = findDistance(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
direction2 = findAngle(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
sim::mathVector p2(magnitude2,direction2);
/* Make a heading vector representation of the other plane */
sim::mathVector d2(1.0,toCartesian(plane2.getCurrentBearing()));
/* Compute time-to-go */
pDiff = p1-p2;
dDiff = d1-d2;
timeToGo = -1.0*pDiff.dotProduct(dDiff)/(MPS_SPEED*dDiff.dotProduct(dDiff));
/* Compute Zero Effort Miss */
zeroEffortMiss = sqrt(fabs(pDiff.dotProduct(pDiff) +
2.0*(MPS_SPEED*timeToGo)*pDiff.dotProduct(dDiff) +
pow(MPS_SPEED*timeToGo,2.0)*dDiff.dotProduct(dDiff)));
if( zeroEffortMiss > DANGER_ZEM || (timeToGo > minimumTimeToGo && timeToGo > iMinimumTimeToGo) || timeToGo < 0 ) continue;
timeToDest = plane1.findDistance(plane1.getDestination().latitude,
plane1.getDestination().longitude) / MPS_SPEED;
/* If you're close to your destination and the other plane isn't
much of a threat, then don't avoid it */
if ( timeToDest < 5.0 && zeroEffortMiss > 3.0*MPS_SPEED ) continue;
/* If you're likely to zigzag, don't avoid the other plane */
bearingDiff = fabs(plane1.getCurrentBearing() - planes[ID].getCurrentBearing());
if ( plane1.findDistance(planes[ID]) > 3.5*MPS_SPEED && bearingDiff < CHATTERING_ANGLE) continue;
/* Second Threshold, to prevent planes from flying into others when trying to avoid less imminent collisions */
if ( zeroEffortMiss <= SECOND_THRESHOLD && timeToGo <= iMinimumTimeToGo ) {
iPlaneToAvoid = ID;
iMostDangerousZEM = zeroEffortMiss;
iMinimumTimeToGo = timeToGo;
continue;
}
planeToAvoid = ID;
mostDangerousZEM = zeroEffortMiss;
minimumTimeToGo = timeToGo;
}
sim::threatContainer greatestThreat;
if (iPlaneToAvoid > -1) {
greatestThreat.planeID = iPlaneToAvoid;
greatestThreat.ZEM = iMostDangerousZEM;
greatestThreat.timeToGo = iMinimumTimeToGo;
}
else {
greatestThreat.planeID = planeToAvoid;
greatestThreat.ZEM = mostDangerousZEM;
greatestThreat.timeToGo = minimumTimeToGo;
}
return greatestThreat;
}
int main (int argc, char** argv)
{
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RGBcloud (new pcl::PointCloud<pcl::PointXYZRGB>);
reader.read (argv[1], *RGBcloud);
std::cout << "RGBPointCloud before filtering has: " << RGBcloud->points.size () << " data points." << std::endl; //*
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudF (new pcl::PointCloud<pcl::PointXYZRGB>);
reader.read(argv[1], *cloudF);
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
clouds.reserve(10);
labels.reserve(10);
//this is only for the old data sets
//for (size_t i=0; i < RGBcloud->points.size();++i)
//{
// if (!((RGBcloud->points[i].x > -.35 && RGBcloud->points[i].x < .35) && (RGBcloud->points[i].y > -.35 && RGBcloud->points[i].y < .35)))
// {
// RGBcloud->points[i].x = 0;
// RGBcloud->points[i].y = 0;
// RGBcloud->points[i].z = 0;
// }
//}
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZRGB> vg;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
vg.setInputCloud (RGBcloud);
vg.setLeafSize (0.001f, 0.001f, 0.001f);
vg.filter (*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl;
//pcl::visualization::CloudViewer viewer("Cloud Viewer");
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.0085);//lesser the values, more points seep into the table
// Segment the largest planar component from the remaining cloud until 30% of the points remain
int i=0, nr_points = (int) cloud_filtered->points.size();
while (cloud_filtered->points.size () > 0.30* nr_points)
{
seg.setInputCloud(cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
exit(-1);
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Write the planar inliers to disk
extract.filter (*cloud_plane); //*
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
//viewer.showCloud(cloud_plane, "cloud_name");
//std::cin.get();
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_filtered); //*
std::cerr <<" The Coefficients are: " << coefficients->values[0]<< " "<< coefficients->values[1]<< " "<< coefficients->values[2]<< " " << coefficients->values[3]<< " "<< std::endl;
}
// color segmentation
//
// define this vector so we can operate over all colored point clouds in a loop
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> coloredClouds(5);
// instantiate the colored clouds
for (int i = 0; i < 5; i++)
coloredClouds[i] = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
// alias the colored clouds with names
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Rcloud = coloredClouds[0];
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Gcloud = coloredClouds[1];
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Bcloud = coloredClouds[2];
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Ycloud = coloredClouds[3];
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Ocloud = coloredClouds[4];
// color segment the cloud using min/max ranges
//colorSegment(cloud_filtered, Rcloud, Gcloud, Bcloud, Ycloud, Ocloud);
// color segment the cloud using a normal distrbuted color model
normalSegment(cloud_filtered, Rcloud, Gcloud, Bcloud, Ycloud, Ocloud);
// temp: display the colored clouds one after another
pcl::visualization::CloudViewer viewer("Cloud Viewer");
for (int i = 0; i < coloredClouds.size(); i++)
{
cout << "Showing cloud #" << i << endl;
viewer.showCloud(coloredClouds[i]);
cin.get();
}
// empty out the lists before re-filling them with colorful points
clouds.clear();
labels.clear();
// now find clusters inside each of the colored point clouds
int num_clusters = 0;
for (int i = 0; i < coloredClouds.size(); i++)
{
if (coloredClouds[i]->size() == 0)
continue;
cout << "Clustering colored cloud #" << i << " which has " << coloredClouds[i]->size() << " points" << endl;
num_clusters += clusterExtraction(coloredClouds[i], &clouds, &labels);
}
// display what we've found
std::cerr<<"Waiting 3 "<<std::endl;
viewer.runOnVisualizationThreadOnce(label);
char c_name[] = "Cloud No: ";
char cloud_name [20];
std::cerr << "Total Clusters: " << num_clusters << std::endl;
for (size_t k = 0; k < num_clusters; k++)
{
std::sprintf(cloud_name,"%s%d",c_name,k);
viewer.showCloud(clouds[k], cloud_name);
}
std::cin.get();
int xx, yy;
while(1)
{
cout << "Enter the label of the first cloud: ";
cin >> xx;
cout << "Enter the label of the second cloud: ";
cin >> yy;
cout << "Finding distance...";
cout << findDistance(clouds[xx], clouds[yy]) << endl;
viewer.runOnVisualizationThreadOnce(drawArrow);
cout << "Enter the number of the cloud you want to see alone: ";
cin >> xx;
}
while (!viewer.wasStopped ());
return 0;
}
void verticesGraph::planYourPath(std::string starting,std::string destination,int distance)
{
int totalMiles = 0;
if(findVertex(starting)==NULL||findVertex(destination)==NULL)
{
std::cout<<"At least one vertices doesn't exist"<<std::endl;
return;
}
vertex * sta=findVertex(starting);
vertex * des=findVertex(destination);
if(sta->district!=des->district)
{
std::cout<<"No path between two vertices"<<std::endl;
return;
}
if(vertices[0].district==-1)
{
std::cout<<"Please assign vertices' districts first"<<std::endl;
return;
}
if(distance<=0)
{
std::cout<<"distance must be greater than zero"<<std::endl;
return;
}
int m=vertices.size();
int previous[m];
for(int n=0;n<m;n++)
{
previous[n]=-1;
vertices[n].ID=n;
}
sta->visited=true;
vertex * v;
std::queue<vertex *> qv;
qv.push(sta);
while(!qv.empty())
{
v=qv.front();
qv.pop();
for(int m=0;m<v->adj.size();m++)
{
if(v->adj[m].v->visited==false)
{
if(v->adj[m].weight<=distance)
{
previous[v->adj[m].v->ID]=v->ID;
v->adj[m].v->visited=true;
qv.push(v->adj[m].v);
}
}
}
}
std::vector<vertex *> path;
int pos=des->ID;
path.push_back(des);
while(previous[pos]!=-1)
{
pos=previous[pos];
path.push_back(findVertex(vertices[pos].name));
}
if(path[0]!=des||path[path.size()-1]!=sta)
{
std::cout<<"No path can be found"<<std::endl;
return;
}
std::cout <<std::endl;
std::cout<<"Trip: ";
for(int m=path.size()-1;m>-1;m--)
{
if(m!=0)
{
std::cout<<path[m]->name<<" >>> "<<findDistance(path[m]->name,path[m-1]->name)<<" >>> ";
milesTraveled = milesTraveled + findDistance(path[m]->name,path[m-1]->name);
totalMiles = totalMiles + milesTraveled;
if (milesTraveled >= 500){
service *stop = findService(path[m],"Gas Station");
std::cout << "Stop for Gas at: "<<stop->name<<", Cost: $"<< stop->cost<<" per Gallon"<<" >>> ";
service *food = findService(path[m],"Food");
std::cout << "Stop for Food at: "<<food->name<<", Cost: $"<< food->cost<<" for full meal."<<" >>> ";
if (milesTraveled >= 1500){
service *hotel = findService(path[m],"Hotel");
std::cout << "Stay the Night at: "<<hotel->name<<", Cost: $"<<hotel->cost<<" per night."<<" >>> ";
}else if (totalMiles >= 1500){
service *hotel = findService(path[m],"Hotel");
std::cout << "Stay the Night at: "<<hotel->name<<", Cost: $"<<hotel->cost<<" per night."<<" >>> ";
totalMiles = 0;
}
milesTraveled = 0;
}
}
else
{
std::cout<<path[m]->name;
}
}
std::cout << std::endl;
for(int m=0;m<vertices.size();m++)
{
vertices[m].visited=false;
}
}
/* This function is run every time new telemetry information from any plane is recieved. With the new telemetry update,
information about the plane is updated, including bearing, speed, current location, etc. Additionally, we check to see
if the UAV has reached its current destination, and, if so, update the destination of the UAV. After updating, the
calculateForces function is called to find a the new force acting on the UAV; from this new force, a next waypoint is
generated and forwarded to the coordinator. */
void telemetryCallback(const AU_UAV_ROS::TelemetryUpdate::ConstPtr &msg)
{
//double time = ros::Time::now().toSec();
//GETS RID OF COLLLISON AVOIDANCE
/*if(true){
return;
}*/
//ROS_ERROR("Planes: %d",planes.size());
//ROS_ERROR("COL AVOID TELEMETRY CALLBACK");
int planeID = msg->planeID;
/* Instantiate services for use later, and get planeID*/
AU_UAV_ROS::GoToWaypoint goToWaypointSrv;
AU_UAV_ROS::GoToWaypoint goToWaypointSrv2;
AU_UAV_ROS::RequestWaypointInfo requestWaypointInfoSrv;
AU_UAV_ROS::coordinate z = planes[0].getCurrentLoc();
d = findDistance(msg->currentLatitude, msg->currentLongitude,
z.latitude,
z.longitude);
/* Request this plane's current normal destination */
requestWaypointInfoSrv.request.planeID = planeID;
requestWaypointInfoSrv.request.isAvoidanceWaypoint = false;
requestWaypointInfoSrv.request.positionInQueue = 0;
if (!requestWaypointInfoClient.call(requestWaypointInfoSrv)){
ROS_ERROR("Did not receive a response from the coordinator");
return;
}
/*if(planeID==-6){
ROS_ERROR("COL: MSG LAT: %f MSG LONG: %f WP LAT: %f WP Long %f",
msg->currentLatitude, msg->currentLongitude,
requestWaypointInfoSrv.response.latitude,
requestWaypointInfoSrv.response.longitude);
}*/
/* If the plane has reached its current destination, move onto the
next destination waypoint. This does not set the destination
of the plane object in the map "planes." */
if (findDistance(msg->currentLatitude, msg->currentLongitude,
requestWaypointInfoSrv.response.latitude,
requestWaypointInfoSrv.response.longitude) < WAYPOINT_THRESHOLD){//COLLISION_THRESHOLD){
/* request next normal destination */
requestWaypointInfoSrv.request.positionInQueue = 1;
/*if(planeID==0){
ROS_ERROR("0 POPPED A WP");
}*/
if (!requestWaypointInfoClient.call(requestWaypointInfoSrv)){
ROS_ERROR("Did not recieve a response from the coordinator");
return;
}
}
/* If the plane is not in our map of planes and has destination
waypoints, then add it as a new plane to our map of planes.
Else if the plane is not in our map of planes and does not
have waypoints, return and do nothing more. */
//added this to add robustness for real UAVs (because real UAVs don't return currentWaypointIndex of -1)
if (planes.find(planeID) == planes.end() && msg->currentWaypointIndex != -1 || (planeID >= 32 && planeID <= 63 && planes.find(planeID) == planes.end())){
/* This is a new plane, so create a new planeObject and
give it the appropriate information */
AU_UAV_ROS::PlaneObject newPlane(MPS_SPEED, *msg);
planes[planeID] = newPlane; /* put the new plane into the map */
/* Update the destination of the PlaneObject with the value
found with the requestWaypointInfoSrv call */
AU_UAV_ROS::waypoint newDest;
newDest.latitude = requestWaypointInfoSrv.response.latitude;
newDest.longitude = requestWaypointInfoSrv.response.longitude;
newDest.altitude = requestWaypointInfoSrv.response.altitude;
planes[planeID].setDestination(newDest);
}
else if (planes.find(planeID) == planes.end())
/* New plane without waypoint set */
return;
/* Note: The requestWaypointInfo service returns a waypoint of
-1000, -1000 when the UAV cannot retrieve a destination from queue.
If the plane has no waypoint to go to, put it far from all others.
Or, if the plane does have a waypoint to go to, update the plane
with new position and destination received from requestWaypointInfoSrv
response*/
if (requestWaypointInfoSrv.response.latitude == -1000){ /* plane has no waypoints to go to */
/* Remove in real flights*/
planes[planeID].setCurrentLoc(-1000,-1000,400);
/* update the time of last update for this plane to acknowledge
it is still in the air */
planes[planeID].updateTime();
return;
}
else{
planes[planeID].update(*msg); /* update plane with new position */
AU_UAV_ROS::waypoint newDest;
newDest.latitude = requestWaypointInfoSrv.response.latitude;
newDest.longitude = requestWaypointInfoSrv.response.longitude;
newDest.altitude = requestWaypointInfoSrv.response.altitude;
//ROS_ERROR("DESTINATION SET");
planes[planeID].setDestination(newDest); /* update plane destination */
}
/* This line of code calls the collision avoidance algorithm
and determines if there should be collision avoidance
maneuvers taken. Returns a waypointContainer which contains new waypoints for
the current plane and the threatening plane. If there is no threatening plane,
*/
//AU_UAV_ROS::waypointContainer bothNewWaypoints = findNewWaypoint(planes[planeID], planes);
AU_UAV_ROS::waypoint curDest;
curDest.latitude = msg->destLatitude;
curDest.longitude = msg->destLongitude;
curDest.altitude - msg->destAltitude;
AU_UAV_ROS::waypointContainer bothNewWaypoints = findNewWaypoint(planes[planeID], planes);//, curDest);
/* If plane2 has a new waypoint to go to, then send it there!*/
AU_UAV_ROS::waypoint newWaypoint = bothNewWaypoints.plane1WP;
if (bothNewWaypoints.plane2ID >= 0) {
AU_UAV_ROS::waypoint newWaypoint2 = bothNewWaypoints.plane2WP;
goToWaypointSrv.request.planeID = bothNewWaypoints.plane2ID;
goToWaypointSrv.request.latitude = newWaypoint2.latitude;
goToWaypointSrv.request.longitude = newWaypoint2.longitude;
goToWaypointSrv.request.altitude = newWaypoint2.altitude;
goToWaypointSrv.request.isAvoidanceManeuver = true;
goToWaypointSrv.request.isNewQueue = true;
if (goToWaypointClient.call(goToWaypointSrv)){
count++;
ROS_INFO("Received response from service request %d", (count-1));
}
else{
ROS_ERROR("Did not receive response");
}
}
if ((requestWaypointInfoSrv.response.longitude == newWaypoint.longitude)
&& (requestWaypointInfoSrv.response.latitude == newWaypoint.latitude)) {
//ROS_ERROR("NO CALL TO GO TO WAYPOINT");
return;
}
/* Fill in goToWaypointSrv request with new waypoint information*/
goToWaypointSrv.request.planeID = planeID;
goToWaypointSrv.request.latitude = newWaypoint.latitude;
goToWaypointSrv.request.longitude = newWaypoint.longitude;
goToWaypointSrv.request.altitude = newWaypoint.altitude;
goToWaypointSrv.request.isAvoidanceManeuver = true;
goToWaypointSrv.request.isNewQueue = true;
//ROS_ERROR("ABOUT TO CALL GO TO WAYPOINT");
if (goToWaypointClient.call(goToWaypointSrv)){
count++;
ROS_INFO("Received response from service request %d", (count-1));
}
else{
ROS_ERROR("Did not receive response");
}
/*
if(!timeTested){
double tDiff = (ros::Time::now().toSec() - time);
if(tDiff >0.0){
if(telTimes.size() < 30){
telTimes.push_front(tDiff);
telTimes.sort();
}
else{
if(tDiff > telTimes.front()){
telTimes.pop_front();
telTimes.push_front(tDiff);
telTimes.sort();
}
else if(tDiff > telTimes.back()){
telTimes.pop_back();
telTimes.push_front(tDiff);
telTimes.sort();
}
}
}//tDiff>0
//ROS_ERROR("TIME: %f",tDiff);
//ROS_ERROR("DIFF: %f %f",startTime, ros::Time::now().toSec() - startTime );
if((ros::Time::now().toSec()-startTime > 500) && (ros::Time::now().toSec()-startTime < 700)){
ROS_ERROR("HERE");
timeTested = true;
FILE* ffpp = fopen("/home/phil/telTimesIPN.txt","w");
if(ffpp ==NULL){ROS_ERROR("NO TEL TIMES");}
int i = telTimes.size();
while(i-->0){
fprintf(ffpp,"%f\n",telTimes.front());
telTimes.pop_front();
}
fclose(ffpp);
}
}//timeTested
*/
}//telcallback
/*
Comparison function for two PlaneObject* parameteters. Returns a true if the first
object's distance to the destination is less than the second's, false otherwise.
*/
bool AU_UAV_ROS::cmpDistToDest(const PlaneObject &pobj1, const PlaneObject &pobj2){
return (findDistance(pobj1.getLatitude(), pobj1.getLongitude(), pobj1.getDestination().latitude, pobj1.getDestination().longitude) <
findDistance(pobj2.getLatitude(), pobj2.getLongitude(), pobj2.getDestination().latitude, pobj2.getDestination().longitude));
}
void verticesGraph::limitedDistancePath(std::string starting,std::string destination,int distance)
{
if(findVertex(starting)==NULL||findVertex(destination)==NULL)
{
std::cout<<"At least one vertices doesn't exist"<<std::endl;
return;
}
vertex * sta=findVertex(starting);
vertex * des=findVertex(destination);
if(sta->district!=des->district)
{
std::cout<<"No path between two vertices"<<std::endl;
return;
}
if(vertices[0].district==-1)
{
std::cout<<"Please assign vertices' districts first"<<std::endl;
return;
}
if(distance<=0)
{
std::cout<<"distance must be greater than zero"<<std::endl;
return;
}
int m=vertices.size();
int previous[m];
for(int n=0;n<m;n++)
{
previous[n]=-1;
vertices[n].ID=n;
}
sta->visited=true;
vertex * v;
std::queue<vertex *> qv;
qv.push(sta);
while(!qv.empty())
{
v=qv.front();
qv.pop();
for(int m=0;m<v->adj.size();m++)
{
if(v->adj[m].v->visited==false)
{
if(v->adj[m].weight<=distance)
{
previous[v->adj[m].v->ID]=v->ID;
v->adj[m].v->visited=true;
qv.push(v->adj[m].v);
}
}
}
}
std::vector<vertex *> path;
int pos=des->ID;
path.push_back(des);
while(previous[pos]!=-1)
{
pos=previous[pos];
path.push_back(findVertex(vertices[pos].name));
}
if(path[0]!=des||path[path.size()-1]!=sta)
{
std::cout<<"No path can be found"<<std::endl;
return;
}
std::cout<<"One path: ";
for(int m=path.size()-1;m>-1;m--)
{
if(m!=0)
{
std::cout<<path[m]->name<<" >>> "<<findDistance(path[m]->name,path[m-1]->name)<<" >>> ";
}
else
{
std::cout<<path[m]->name;
}
}
std::cout << std::endl;
for(int m=0;m<vertices.size();m++)
{
vertices[m].visited=false;
}
}
