entity_t * newRing(char * enttype, int code) {
entity_t * ent;
ring_t * ring;
ent = newPlane(enttype, code);
sobj_t * sobj = ent->entDerived;
plane_t * plane = sobj->sobjDerived;
ring = malloc(sizeof(ring_t));
plane->planeDerived = ring;
ring->magic = RING_T;
ring->radius1 = 0;
ring->width = 0;
ring->radius2 = 0;
ent->load = loadRing;
ent->complete = completeRing;
ent->dump = dumpRing;
sobj->hit = hitRing;
return ent;
}
AS3_Val initializePrimitives( void * self, AS3_Val args )
{
Mesh * base;
Material * material;
Texture * texture;
double width, height;
DWORD segments_width, segments_height;
DWORD render_mode;
AS3_ArrayValue( args, "PtrType, PtrType, PtrType, DoubleType, DoubleType, IntType, IntType, IntType", & base, & material, & texture, & width, & height, & segments_width, & segments_height, & render_mode );
newPlane( base, material, texture, (float)width, (float)height, segments_width, segments_height, render_mode );
return 0;
}
/* 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
void buildScene(void)
{
// Sets up all objects in the scene. This involves creating each object,
// defining the transformations needed to shape and position it as
// desired, specifying the reflectance properties (albedos and colours)
// and setting up textures where needed.
// Light sources must be defined, positioned, and their colour defined.
// All objects must be inserted in the object_list. All light sources
// must be inserted in the light_list.
//
// To create hierarchical objects:
// You must keep track of transformations carried out by parent objects
// as you move through the hierarchy. Declare and manipulate your own
// transformation matrices (use the provided functions in utils.c to
// compound transformations on these matrices). When declaring a new
// object within the hierarchy
// - Initialize the object
// - Apply any object-level transforms to shape/rotate/resize/move
// the object using regular object transformation functions
// - Apply the transformations passed on from the parent object
// by pre-multiplying the matrix containing the parent's transforms
// with the object's own transformation matrix.
// - Compute and store the object's inverse transform as usual.
//
// NOTE: After setting up the transformations for each object, don't
// forget to set up the inverse transform matrix!
// struct object3D *o;
// Simple scene for Assignment 3:
// Insert a couple of objects. A plane and two spheres
// with some transformations.
// Note the parameters: ra, rd, rs, rg, R, G, B, alpha, r_index, and shinyness)
// // struct object3D *newSphere(double ra, double rd, double rs, double rg,
// double r, double g, double b,
// double alpha, double r_index, double shiny)
// Intialize a new sphere with the specified parameters:
// ra, rd, rs, rg - Albedos for the components of the Phong model
// r, g, b, - Colour for this plane
// alpha - Transparency, must be set to 1 unless you are doing refraction
// r_index - Refraction index if you are doing refraction.
// shiny -Exponent for the specular component of the Phong model
//
// This is assumed to represent a unit sphere centered at the origin.
//
struct object3D *o;
struct pointLS *l;
struct point3D p;
o=newSphere(0.15,.05,.25,0.25,0.75,0.75,0.75,0.25,1.52,6); // Initialize a sphere
Scale(o,2,2,2); // Apply a few transforms (Translate * Rotate * Scale)
RotateZ(o,PI/4);
Translate(o,0,-3,6);
// Scale(o,2,2,2);
invert(&o->T[0][0],&o->Tinv[0][0]);
// loadTexture(o, "./textures/grass.ppm", 1, &texture_list);
loadTexture(o, "./data/textures/ppm/window_n.ppm", 2, &texture_list);
insertObject(o,&object_list); // <-- If you don't insert the object into the object list,
// nothing happens! your object won't be rendered.
// That's it for defining a single sphere... let's add a couple more objects
o=newSphere(.05,.25,.55,0.55,.95,.55,0.35,0.1,1.77,6);
Scale(o,.95,1.65,.65);
RotateZ(o,-PI/1.5);
Translate(o,0,0,6);
// Scale(o,2,2,2);
invert(&o->T[0][0],&o->Tinv[0][0]);
// loadTexture(o, "./textures/grass.ppm", 1, &texture_list);
// loadTexture(o, "./textures/grass_n.ppm", 2, &texture_list);
insertObject(o,&object_list);
o=newPlane(.05,.75,0,0.25,.55,.8,.75,1,1,2);
Scale(o,11,11,11);
// RotateZ(o,PI/4);
RotateX(o,PI/4);
Translate(o,0,-4,5);
invert(&o->T[0][0],&o->Tinv[0][0]);
loadTexture(o, "./data/textures/ppm/sintfloor01.ppm", 1, &texture_list);
loadTexture(o, "./data/textures/ppm/sintfloor01_n.ppm", 2, &texture_list);
insertObject(o,&object_list);
// o=newPlane(.65,.75,.00,.00,.55,.8,1,1,1,5);
// Scale(o,500,500,11);
// RotateZ(o, PI/4);
// RotateX(o,PI);
// Translate(o,0,-10,8);
// invert(&o->T[0][0],&o->Tinv[0][0]);
// insertObject(o,&object_list);
// Insert a single point light source. We set up its position as a point structure, and specify its
// colour in terms of RGB (in [0,1]).
NUM_LIGHTS = 250;
addAreaLight(1.5,1.5,0,-1,0,
0,25.5,-3.5,NUM_LIGHTS,
0.71372549,0.796078431,0.815686275,
&object_list, &light_list);
// addAreaLight(25.0, 25.0, 0, -1, -1, 0.0, 600, 135, NUM_LIGHTS, 0.71372549,0.796078431,0.815686275, &object_list, &light_list);
// struct pointLS *l1;
// struct point3D p1;
// p1.px=0;
// p1.py=500;
// p1.pz=140;
// p1.pw=1;
// l1=newPLS(&p1,0.71372549,0.796078431,0.815686275);
// insertPLS(l1,&light_list);
// struct pointLS *l2;
// struct point3D p2;
// p2.px=0;
// p2.py=490;
// p2.pz=170;
// p2.pw=1;
// l2=newPLS(&p2,0.71372549/1.25,0.796078431/1.25,0.815686275/1.25);
// insertPLS(l2,&light_list);
// End of simple scene for Assignment 3
// Keep in mind that you can define new types of objects such as cylinders and parametric surfaces,
// or, you can create code to handle arbitrary triangles and then define objects as surface meshes.
//
// Remember: A lot of the quality of your scene will depend on how much care you have put into defining
// the relflectance properties of your objects, and the number and type of light sources
// in the scene.
///////////////////////////////////////////////////////////////////////////////////////////////////////////
// TO DO: For Assignment 4 you *MUST* define your own cool scene.
// We will be looking for the quality of your scene setup, the use of hierarchical or composite
// objects that are more interesting than the simple primitives from A3, the use of textures
// and other maps, illumination and illumination effects such as soft shadows, reflections and
// transparency, and the overall visual quality of your result. Put some work into thinking
// about these elements when designing your scene.
///////////////////////////////////////////////////////////////////////////////////////////////////////////
}
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");
}
}
