C++ PlaneObject::getCurrentBearing方法代码示例

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

AU_UAV_ROS::coordinate AU_UAV_ROS::calculateLoopingCircleCenter(PlaneObject &plane, double turnRadius, bool turnRight) {
	AU_UAV_ROS::coordinate circleCenter;
	circleCenter.altitude = plane.getCurrentLoc().altitude;
	double angle;
	if (turnRight) {
		angle = (toCartesian(plane.getCurrentBearing()) - 90 - 22.5) * PI/180.0; 
	}
	else {
		angle = (toCartesian(plane.getCurrentBearing()) + 90 + 22.5) * PI/180.0;
	}
	double xdiff = turnRadius*cos(angle);
	double ydiff = turnRadius*sin(angle);
	circleCenter.longitude = plane.getCurrentLoc().longitude + xdiff/DELTA_LON_TO_METERS;
	circleCenter.latitude = plane.getCurrentLoc().latitude + ydiff/DELTA_LAT_TO_METERS; 

	return circleCenter;
}

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

/* Find the new collision avoidance waypoint for the plane to go to */
sim::waypoint sim::calculateWaypoint(PlaneObject &plane1, double turningRadius, bool turnRight){

	sim::waypoint wp;	
	double V = MPS_SPEED;
	double delta_T = TIME_STEP;	
	double cartBearing = toCartesian(plane1.getCurrentBearing())* PI/180;
	double delta_psi = V / turningRadius * delta_T;
	if (turnRight) delta_psi *= -1.0;
	double psi = (cartBearing + delta_psi);
	wp.longitude = plane1.getCurrentLoc().longitude + V*cos(psi)/DELTA_LON_TO_METERS;
	wp.latitude = plane1.getCurrentLoc().latitude + V*sin(psi)/DELTA_LAT_TO_METERS;
	wp.altitude = plane1.getCurrentLoc().altitude;
    
	return wp;
}

/* Find the new collision avoidance waypoint for the plane to go to */
AU_UAV_ROS::waypoint AU_UAV_ROS::calculateWaypoint(PlaneObject &plane1, double turningRadius, bool turnRight){

	AU_UAV_ROS::waypoint wp;	
	double V = MPS_SPEED;
	double delta_T = TIME_STEP;	
	double cartBearing = toCartesian(plane1.getCurrentBearing())* PI/180;
	double delta_psi = V / turningRadius * delta_T;
	if (turnRight) delta_psi *= -1.0;
	ROS_WARN("Delta psi: %f", delta_psi);
	double psi = (cartBearing + delta_psi);
	wp.longitude = plane1.getCurrentLoc().longitude + V*cos(psi)/DELTA_LON_TO_METERS;
	wp.latitude = plane1.getCurrentLoc().latitude + V*sin(psi)/DELTA_LAT_TO_METERS;
	wp.altitude = plane1.getCurrentLoc().altitude;
	ROS_WARN("Plane%d new cbearing: %f", plane1.getID(), toCardinal( findAngle(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude, wp.latitude, wp.longitude) ) ); 
	//ROS_WARN("Plane%d calc lat: %f lon: %f w/ act lat: %f lon: %f", plane1.getID(), wp.latitude, wp.longitude, plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
	
	return wp;
}

/* Find the new collision avoidance waypoint for the plane to go to */
au_uav_ros::waypoint au_uav_ros::calculateWaypoint(PlaneObject &plane1, double turningRadius, bool turnRight){

	au_uav_ros::waypoint wp;	
	double V = MPS_SPEED * MPS_WAYPOINT_MULTIPLIER;
	double delta_T = TIME_STEP;	
	double cartBearing = plane1.getCurrentBearing()* PI/180;
	double delta_psi = V / turningRadius * delta_T;
	if (turnRight) delta_psi *= -1.0;
	ROS_WARN("Delta psi: %f", delta_psi);
	double psi = (cartBearing + delta_psi);
	V = V * MPS_WAYPOINT_MULTIPLIER;
	wp.longitude = plane1.getCurrentLoc().longitude + V*cos(psi)/DELTA_LON_TO_METERS;
	wp.latitude = plane1.getCurrentLoc().latitude + V*sin(psi)/DELTA_LAT_TO_METERS;
	ROS_INFO("long+%f, lat+%f, distanceBetween UAV and AvoidWP%f", V*cos(psi)/DELTA_LON_TO_METERS, V*sin(psi)/DELTA_LON_TO_METERS,
		distanceBetween(plane1.getCurrentLoc(), wp));
	wp.altitude = plane1.getCurrentLoc().altitude;
	ROS_WARN("Plane%d new cbearing: %f", plane1.getID(), toCardinal( findAngle(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude, wp.latitude, wp.longitude) ) ); 
	//ROS_WARN("Plane%d calc lat: %f lon: %f w/ act lat: %f lon: %f", plane1.getID(), wp.latitude, wp.longitude, plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
	
	return wp;
}

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

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

/* 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;		
//.........这里部分代码省略.........