void parseLine(String str) {
if (str.empty()) { return; }
if ( parseSetCommand( str ) ) { return; }
std::stringstream ss;
ss.str(str);
ss >> str;
if ( (str[0] >= '0') && (str[0] <= '9') ) {
int dest = std::stoi(str);
if ( (dest < 0) || (dest >= indexes) ) { return; }
for ( int i = currentLine; i < currentLine + noteLength; ++i ) {
outputFiles[dest] << lines[i] << "\n";
}
removeCurrentNote();
return;
}
if ( str == "save" ) {
saveState();
} else if ( str == "s" ) {
if (currentLine != (int)lines.size()) {
log.push_back( String("Skipped ") + std::to_string(currentLine) );
}
int before = currentLine;
getNextNoteLine();
if ((before != currentLine) && (currentLine == (int)lines.size())) {
log.push_back("Reached End of Input");
}
return;
}
if ( str == "i" ) {
std::cout << "Insert Custom Note > ";
String line;
getline(std::cin, line);
if ( !isBlankLine(line) ) {
lines.insert( lines.begin() + currentLine, line );
}
}
if ( str == "d" ) {
log.push_back( String("Deleted ") + std::to_string(currentLine) );
removeCurrentNote();
}
if ( str == "p" ) {
log.push_back("Previous");
--currentLine;
currentLine = std::max(0, currentLine);
}
if ( str == "q" ) {
doQuit = true;
return;
}
if ( str == "\t" ) {
enterSubNote();
}
ss.str("");
ss.clear();
}
void getNextNoteLine() {
currentLine += noteLength;
while (currentLine < (int)lines.size()) {
if (isBlankLine(lines[currentLine])) {
++currentLine;
} else { break; }
}
}
bool Document::read(std::istream& in) {
if (mProcessed) return false;
token::Container *tokens=dynamic_cast<token::Container*>(mTokenContainer.get());
assert(tokens!=0);
std::string line;
TokenGroup tgt;
while (_getline(in, line)) {
if (isBlankLine(line)) {
tgt.push_back(TokenPtr(new token::BlankLine(line)));
} else {
tgt.push_back(TokenPtr(new token::RawText(line)));
}
}
tokens->appendSubtokens(tgt);
return true;
}
int main()
{
//get the filename
char filename[256];
printf("Enter the file with all the courses in it:");
scanf("%s", filename);
//open the file
FILE *fp;
fp = fopen(filename, "r");
//try to create our pipe for use later
int pfds[2];
if(pipe(pfds) == -1)
{
perror("Pipe");
exit(1);
}
//check for a good open
if(fp != NULL)
{
char buffer[256];
//while we have something in the file
while(fgets(buffer, sizeof(buffer), fp))
{
//check for useless lines
if(buffer[0] == '#' || isBlankLine(buffer))
{
//this line is a comment
continue;
}
//construct our strings to send
std::string myStr = std::string(buffer);
myStr = myStr.substr(0, myStr.size() - LENGTH_OF_EXTENSION - 1);
myStr = myStr + OUTPUT_ADDITION;
//fork our process
int pid;
pid = fork();
if(pid == 0)
{
//we're redirecting STDIN such that it comes from the pipe
//close standard in
close(STDIN_FILENO);
//duplicate our stdin as the pipe output
dup2(pfds[0], STDIN_FILENO);
//child process
system("roslaunch AU_UAV_ROS evaluation.launch");
}
else
{
//send out output over that there pipe
printf("Writing to the pipe! %s\n", buffer);
write(pfds[1], buffer, strlen(buffer));
printf("Writing to the pipe! %s\n", myStr.c_str());
write(pfds[1], myStr.c_str(), strlen(myStr.c_str()));
//parent waits some time, then kills before starting new one
sleep(SLEEP_TIME);
printf("Killing Process ID #%d\n", pid);
kill(pid, SIGTERM);
waitpid(pid, NULL, 0);
//give the SIGTERM time to work
sleep(BUFFER_TIME);
}
}
fclose(fp);
}
else
{
printf("ERROR: Bad file name\n");
}
return 0;
}
/*
loadCourseCallback(...)
This callback takes a filename and parses that file to load a course into the coordinator. A course in
this sense would be several paths for multiple UAVs. We want this so we can test a collision avoidance
course by preplanning the course.
*/
bool loadCourseCallback(AU_UAV_ROS::LoadCourse::Request &req, AU_UAV_ROS::LoadCourse::Response &res)
{
ROS_INFO("Received request: Load course from \"%s\"\n", req.filename.c_str());
//open our file
FILE *fp;
//JC - April 9th - GUI gives full path
fp = fopen(req.filename.c_str(), "r");
//fp = fopen((ros::package::getPath("AU_UAV_ROS")+"/courses/"+req.filename).c_str(), "r");
//check for a good file open
if(fp != NULL)
{
char buffer[256];
std::map<int, bool> isFirstPoint;
std::map<int, std::vector<AU_UAV_ROS::waypoint> > allPlanesWaypoints;
std::map<int, AU_UAV_ROS::PlaneObject> realPlanes;
bool isAvoidance = false;
//while we have something in the file
while(fgets(buffer, sizeof(buffer), fp))
{
//check first character
if(buffer[0] == '#' || isBlankLine(buffer))
{
//this line is a comment
continue;
}
else
{
//set some invalid defaults
int planeID = -1;
struct AU_UAV_ROS::waypoint temp;
temp.latitude = temp.longitude = temp.altitude = -1000;
//parse the string
sscanf(buffer, "%d %lf %lf %lf\n", &planeID, &temp.latitude, &temp.longitude, &temp.altitude);
ROS_INFO("Loaded %d to %lf %lf %lf", planeID, temp.latitude, temp.longitude, temp.altitude);
//check for the invalid defaults
if(planeID == -1 || temp.latitude == -1000 || temp.longitude == -1000 || temp.altitude == -1000)
{
//this means we have a bad file somehow
ROS_ERROR("Bad file parse");
res.error = "Bad file parse, some points loaded";
return false;
}
//check our map for an entry, if we dont have one then this is the first time
//that this plane ID has been referenced so it's true
if(isFirstPoint.find(planeID) == isFirstPoint.end())
{
isFirstPoint[planeID] = true;
realPlanes[planeID] = AU_UAV_ROS::PlaneObject();
// add this as waypoint to the front of queue, simulation is set up so that the
// first waypoint is the starting location, then the simulation really begins
realPlanes[planeID].setCurrentLoc(temp.latitude, temp.longitude, temp.altitude);
realPlanes[planeID].setID(planeID);
}
else {
allPlanesWaypoints[planeID].push_back(temp);
}
//call the goToPoint function for the correct plane
//planesArray[planeID].goToPoint(temp, isAvoidance, isFirstPoint[planeID]);
//only clear the queue with the first point
if(isFirstPoint[planeID]) isFirstPoint[planeID] = false;
}
}
std::map<int, std::vector<waypointPath> > *allPlanesPaths = getAvoidancePlan(&realPlanes, 0, &allPlanesWaypoints);
// set the first waypoint to the planes starting location; this is for the simulation
for (std::map<int, AU_UAV_ROS::PlaneObject>::iterator it = realPlanes.begin(); it != realPlanes.end(); it++) {
AU_UAV_ROS::waypoint temp;
temp.latitude = it->second.getCurrentLoc().latitude;
temp.longitude = it->second.getCurrentLoc().longitude;
temp.altitude = it->second.getCurrentLoc().altitude;
planesArray[it->first].goToPoint(temp, false, true);
}
for (std::map<int, std::vector<waypointPath> >::iterator it = allPlanesPaths->begin(); it != allPlanesPaths->end(); it++) {
vector<AU_UAV_ROS::waypoint> combinedWays = generateCombinedWaypoints(&(allPlanesWaypoints[it->first]), &(it->second));
ROS_INFO("Size of combined waypoints for plane %d is %d", it->first, combinedWays.size());
for (int i = 0; i < combinedWays.size(); i++) {
planesArray[it->first].goToPoint(combinedWays[i], false, false);
}
}
//end of file, return
return true;
}
else
{
ROS_ERROR("Invalid filename or location: %s", req.filename.c_str());
res.error = "Invalid filename or location";
return false;
}
}
/*
loadPathCallback(...)
This is the callback used when the user requests for a plane to start a certain path.
*/
bool loadPathCallback(AU_UAV_ROS::LoadPath::Request &req, AU_UAV_ROS::LoadPath::Response &res)
{
ROS_INFO("Received request: Load path from \"%s\" to plane #%d\n", req.filename.c_str(), req.planeID);
//check for a valid plane ID sent
if(isValidPlaneID(req.planeID))
{
//open our file
FILE *fp;
fp = fopen((ros::package::getPath("AU_UAV_ROS")+"/paths/"+req.filename).c_str(), "r");
//check for a good file open
if(fp != NULL)
{
char buffer[256];
bool isFirstLine = true;
bool isAvoidance = false;
//while we have something in the file
while(fgets(buffer, sizeof(buffer), fp))
{
//check first character for comment or if the line is blank
if(buffer[0] == '#' || isBlankLine(buffer))
{
//this line is a comment
continue;
}
else
{
//set some invalid defaults
struct AU_UAV_ROS::waypoint temp;
temp.latitude = temp.longitude = temp.altitude = -1000;
//parse the string
sscanf(buffer, "%lf %lf %lf\n", &temp.latitude, &temp.longitude, &temp.altitude);
ROS_INFO("Parsing path file: %lf %lf %lf", temp.latitude, temp.longitude, temp.altitude);
//check for the invalid defaults
if(temp.latitude == -1000 || temp.longitude == -1000 || temp.altitude == -1000)
{
//this means we have a bad file somehow
ROS_ERROR("Bad file parse");
res.error = "Bad file parse, some points loaded";
return false;
}
//call the goToPoint function for the correct plane
planesArray[req.planeID].goToPoint(temp, isAvoidance, isFirstLine);
//only clear the queue with the first point
if(isFirstLine) isFirstLine = false;
}
}
//end of file, return
return true;
}
else
{
ROS_ERROR("Invalid filename or location: %s", req.filename.c_str());
res.error = "Invalid filename or location";
return false;
}
}
else
{
ROS_ERROR("Invalid plane ID");
res.error = "Invalid plane ID";
return false;
}
}
/*
loadCourseCallback(...)
This callback takes a filename and parses that file to load a course into the coordinator. A course in
this sense would be several paths for multiple UAVs. We want this so we can test a collision avoidance
course by preplanning the course.
*/
bool loadCourseCallback(AU_UAV_ROS::LoadCourse::Request &req, AU_UAV_ROS::LoadCourse::Response &res)
{
ROS_INFO("Received request: Load course from \"%s\"\n", req.filename.c_str());
//open our file
FILE *fp;
//JC - April 9th - GUI gives full path
fp = fopen(req.filename.c_str(), "r");
//fp = fopen((ros::package::getPath("AU_UAV_ROS")+"/courses/"+req.filename).c_str(), "r");
//check for a good file open
if(fp != NULL)
{
char buffer[256];
std::map<int, bool> isFirstPoint;
bool isAvoidance = false;
//while we have something in the file
while(fgets(buffer, sizeof(buffer), fp))
{
//check first character
if(buffer[0] == '#' || isBlankLine(buffer))
{
//this line is a comment
continue;
}
else
{
//set some invalid defaults
int planeID = -1;
struct AU_UAV_ROS::waypoint temp;
temp.latitude = temp.longitude = temp.altitude = -1000;
//parse the string
sscanf(buffer, "%d %lf %lf %lf\n", &planeID, &temp.latitude, &temp.longitude, &temp.altitude);
ROS_INFO("Loaded %d to %lf %lf %lf", planeID, temp.latitude, temp.longitude, temp.altitude);
//check for the invalid defaults
if(planeID == -1 || temp.latitude == -1000 || temp.longitude == -1000 || temp.altitude == -1000)
{
//this means we have a bad file somehow
ROS_ERROR("Bad file parse");
res.error = "Bad file parse, some points loaded";
return false;
}
//check our map for an entry, if we dont have one then this is the first time
//that this plane ID has been referenced so it's true
if(isFirstPoint.find(planeID) == isFirstPoint.end())
{
isFirstPoint[planeID] = true;
}
//call the goToPoint function for the correct plane
planesArray[planeID].goToPoint(temp, isAvoidance, isFirstPoint[planeID]);
//only clear the queue with the first point
if(isFirstPoint[planeID]) isFirstPoint[planeID] = false;
}
}
//DEBUG: print out planesArray to see if waypoints were loaded to the real planes
//AU_UAV_ROS::PlaneCoordinator uav32 = planesArray.at(32);
//ROS_ERROR("The size of planesArray[32] = %d", uav32.getNormalSize());
//end of file, return
return true;
}
else
{
ROS_ERROR("Invalid filename or location: %s", req.filename.c_str());
res.error = "Invalid filename or location";
return false;
}
}
/*
createCourseUAVs(...)
takes a filename and will parse it to determine how many UAVs there are and create them as needed
*/
bool createCourseUAVs(std::string filename)
{
//open our file
FILE *fp;
fp = fopen(filename.c_str(), "r");
//check for a good file open
if(fp != NULL)
{
char buffer[256];
std::map<int, bool> isFirstPoint;
while(fgets(buffer, sizeof(buffer), fp))
{
if(buffer[0] == '#' || isBlankLine(buffer))
{
//this line is a comment
continue;
}
else
{
//set some invalid defaults
int planeID = -1;
struct AU_UAV_ROS::waypoint temp;
temp.latitude = temp.longitude = temp.altitude = -1000;
//parse the string
sscanf(buffer, "%d %lf %lf %lf\n", &planeID, &temp.latitude, &temp.longitude, &temp.altitude);
//check for the invalid defaults
if(planeID == -1 || temp.latitude == -1000 || temp.longitude == -1000 || temp.altitude == -1000)
{
//this means we have a bad file somehow
ROS_ERROR("Bad file parse");
return false;
}
//check our map for an entry, if we dont have one then this is the first time
//that this plane ID has been referenced so it's true
if(isFirstPoint.find(planeID) == isFirstPoint.end())
{
if (planeID >= 0 && planeID <= 31) {
isFirstPoint[planeID] = true;
//this is the first time we've seen this ID in the file, attempt to create it
AU_UAV_ROS::CreateSimulatedPlane srv;
srv.request.startingLatitude = temp.latitude;
srv.request.startingLongitude = temp.longitude;
srv.request.startingAltitude = temp.altitude;
srv.request.startingBearing = 0;
srv.request.requestedID = planeID;
//send the service request
printf("\nRequesting to create new simulated plane with ID #%d...\n", planeID);
if(createSimulatedPlaneClient.call(srv))
{
printf("New simulated plane with ID #%d has been created!\n", srv.response.planeID);
}
else
{
ROS_ERROR("Did not receive a response from simulator");
}
}
else if (planeID >= 32 && planeID <= 63){
isFirstPoint[planeID] = true;
//this planeID is for a real UAV
// use the requestPlaneID service to initialize the planesArray in coordinator with real UAV object
AU_UAV_ROS::RequestPlaneID srv;
srv.request.requestedID = planeID;
printf("\nRequesting to create new real plane with ID #%d...\n", planeID);
//check to make sure the client call worked (regardless of return values from service)
if(requestPlaneIDClient.call(srv))
{
if(srv.response.planeID == -1)
{
ROS_ERROR("Couldn't create plane with ID %d", planeID);
return false;
}
//plane added successfully
printf("New real plane with ID #%d has been created!\n", srv.response.planeID);
}
else
{
//if this happens, chances are the coordinator isn't running
ROS_ERROR("Did not receive an ID from coordinator");
return false;
}
}
else {
//this means we have a bad file somehow
ROS_ERROR("Bad file parse");
return false;
}
}
//only clear the queue with the first point
if(isFirstPoint[planeID]) isFirstPoint[planeID] = false;
}
}
//if we make it here everything happened according to plan
return true;
}
else
{
ROS_ERROR("Invalid filename or location: %s", filename.c_str());
return false;
}
}
int main()
{
//get the filename
char filename[256];
printf("Enter the file with all the courses in it:");
scanf("%s", filename);
//try to create our pipe for use later
int pfds[2];
if(pipe(pfds) == -1)
{
perror("Pipe");
exit(1);
}
char buffer[256];
char buffer_2[256];
char str_value[50];
FILE *results, *ripna, *fp, *scorefile;
results = fopen( "/home/eric/ros_workspace/sim/scores/Results.txt", "w");
fprintf(results, "Conflicts\tCollisions\tEfficiency\n");
/* This loops through all the values you specify for your parameter of choice. Each time through
it runs all of the courses with the given value. */
for ( double value = 0.5; value <= 2.5; value += 0.2 ) {
//open the file
fp = fopen(filename, "r");
//check for a good open
if(fp != NULL)
{
sprintf(str_value, "%f\n", value);
fprintf(results, str_value);
// Change the value of the chosen parameter here
ripna = fopen("/home/eric/ros_workspace/sim/src/ripna.cpp", "r+");
fseek (ripna, 366, SEEK_SET );
fputs (str_value, ripna);
fclose (ripna);
system("rosmake sim");
//while we have something in the file
while(fgets(buffer, sizeof(buffer), fp))
{
//check for useless lines
if(buffer[0] == '#' || isBlankLine(buffer))
{
//this line is a comment
continue;
}
//construct our strings to send
std::string myStr = std::string(buffer);
myStr = myStr.substr(0, myStr.size() - LENGTH_OF_EXTENSION - 1);
myStr = myStr + OUTPUT_ADDITION;
//fork our process
int pid;
pid = fork();
if(pid == 0)
{
//we're redirecting STDIN such that it comes from the pipe
//close standard in
close(STDIN_FILENO);
//duplicate our stdin as the pipe output
dup2(pfds[0], STDIN_FILENO);
//child process
system("roslaunch sim evaluation.launch");
}
else
{
//send out output over that there pipe
printf("Writing to the pipe! %s\n", buffer);
write(pfds[1], buffer, strlen(buffer));
printf("Writing to the pipe! %s\n", myStr.c_str());
write(pfds[1], myStr.c_str(), strlen(myStr.c_str()));
//parent waits some time, then kills before starting new one
sleep(SLEEP_TIME);
printf("Killing Process ID #%d\n", pid);
kill(pid, SIGTERM);
waitpid(pid, NULL, 0);
//give the SIGTERM time to work
sleep(BUFFER_TIME);
}
}
}
else {
printf("ERROR: Bad file name\n");
}
fclose(fp);
// Writing data to a file
char buffer_3[256];
char name[256];
int j;
for (int i = 1; i <= NUM_COURSES; i++) {
// Open ith score file
sprintf(name, "/home/eric/ros_workspace/sim/scores/final_32_500m_%d_test.score", i);
scorefile = fopen(name, "r");
while(fgets(buffer_3, sizeof(buffer_3), scorefile)) {
for (j = 0; j < sizeof(buffer_3); j++) {
if (buffer_3[j] == '\n') {
break;
}
}
char output[j-1];
for (int k = 0; k < j; k++) {
output[k] = buffer_3[k];
}
output[j] = '\0';
// Print data to a different kind of score file
fprintf(results, "%s\t", output);
}
fprintf(results, "\n");
fclose(scorefile);
}
fprintf(results, "\n\n");
}
fclose(results);
return 0;
}
/*
createCourseUAVs(...)
takes a filename and will parse it to determine how many UAVs there are and create them as needed
NOTE: this was copied from the ControlMenu and modified somewhat
*/
bool createCourseUAVs(std::string filename)
{
//open our file
FILE *fp;
fp = fopen((ros::package::getPath("AU_UAV_ROS")+"/courses/"+filename).c_str(), "r");
//check for a good file open
if(fp != NULL)
{
char buffer[256];
std::map<int, bool> isFirstPoint;
while(fgets(buffer, sizeof(buffer), fp))
{
if(buffer[0] == '#' || isBlankLine(buffer))
{
//this line is a comment
continue;
}
else
{
//set some invalid defaults
int planeID = -1;
struct AU_UAV_ROS::waypoint temp;
temp.latitude = temp.longitude = temp.altitude = -1000;
//parse the string
sscanf(buffer, "%d %lf %lf %lf\n", &planeID, &temp.latitude, &temp.longitude, &temp.altitude);
//check for the invalid defaults
if(planeID == -1 || temp.latitude == -1000 || temp.longitude == -1000 || temp.altitude == -1000)
{
//this means we have a bad file somehow
ROS_ERROR("Bad file parse");
return false;
}
//add this point to the correct queue
waypointQueues[planeID].push(temp);
//check our map for an entry, if we dont have one then this is the first time
//that this plane ID has been referenced so it's true
if(isFirstPoint.find(planeID) == isFirstPoint.end())
{
isFirstPoint[planeID] = true;
//this is the first time we've seen this ID in the file, attempt to create it
AU_UAV_ROS::CreateSimulatedPlane srv;
srv.request.startingLatitude = temp.latitude;
srv.request.startingLongitude = temp.longitude;
srv.request.startingAltitude = temp.altitude;
srv.request.startingBearing = 0;
srv.request.requestedID = planeID;
//send the service request
printf("\nRequesting to create new plane with ID #%d...\n", planeID);
if(createSimulatedPlaneClient.call(srv))
{
printf("New plane with ID #%d has been created!\n", srv.response.planeID);
}
else
{
ROS_ERROR("Did not receive a response from simulator");
}
//set our last plane ID to this one
lastPlaneID = planeID;
maxAlivePlane = planeID;
//set up some base values for a new plane
latestUpdatesMap[planeID] = AU_UAV_ROS::TelemetryUpdate();
latestUpdatesMap[planeID].currentLatitude = temp.latitude;
latestUpdatesMap[planeID].currentLongitude = temp.longitude;
latestUpdatesMap[planeID].currentAltitude = temp.altitude;
previousUpdatesMap[planeID] = AU_UAV_ROS::TelemetryUpdate();
previousUpdatesMap[planeID].currentLatitude = temp.latitude;
previousUpdatesMap[planeID].currentLongitude = temp.longitude;
previousUpdatesMap[planeID].currentAltitude = temp.altitude;
distanceTraveled[planeID] = 0;
waypointDistTraveled[planeID] = 0;
distSinceLastWP[planeID] = 0;
waypointsAchieved[planeID] = -1; //we get a "free" waypoint
minimumTravelDistance[planeID] = 0;
waypointMinTravelDist[planeID] = 0;
isDead[planeID] = false;
//subtract points to make up for the "free" waypoint at the start
score = score - 5;
waypointsTotal--;
}
//only clear the queue with the first point
if(isFirstPoint[planeID]) isFirstPoint[planeID] = false;
}
}
//if we make it here everything happened according to plan
return true;
}
else
{
ROS_ERROR("Invalid filename or location: %s", filename.c_str());
return false;
}
}
