reverted deletion of files

pps_ws/src/d_fall_pps/CMakeLists.txt

@@ -215,7 +215,7 @@ add_executable(ViconDataPublisher src/ViconDataPublisher.cpp)
 add_executable(PPSClient src/PPSClient.cpp)
 add_executable(SafeControllerService src/SafeControllerService.cpp)
 add_executable(CustomControllerService src/CustomControllerService.cpp)
-add_executable(CentralManagerService src/CentralManagerService.cpp)
+add_executable(CentralManagerService src/CentralManagerService.cpp src/CrazyflieIO.cpp)
 
 
 

pps_ws/src/d_fall_pps/include/CrazyflieIO.h

+#include "d_fall_pps/CrazyflieDB.h"
+
+namespace d_fall_pps {
+
+void readCrazyflieDB(CrazyflieDB& cfdb);
+
+void writeCrazyflieDB(CrazyflieDB& cfdb);
+
+}
\ No newline at end of file

pps_ws/src/d_fall_pps/src/CentralManagerService.cpp

@@ -15,13 +15,10 @@
 //    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <stdlib.h>
-#include "ros/ros.h"
-#include <ros/package.h>
-#include "rosbag/bag.h"
+#include <ros/ros.h>
 #include "d_fall_pps/CentralManager.h"
 #include "d_fall_pps/CrazyflieContext.h"
 #include "d_fall_pps/CrazyflieDB.h"
-#include "d_fall_pps/CrazyflieContext.h"
 
 #include "d_fall_pps/CMRead.h"
 #include "d_fall_pps/CMQuery.h"
@@ -29,22 +26,12 @@
 #include "d_fall_pps/CMCommand.h"
 #include "CentralManagerService.h"
 
+#include "CrazyflieIO.h"
+
 using namespace d_fall_pps;
 using namespace std;
 
 CrazyflieDB crazyflieDB;
-rosbag::Bag bag;
-
-void saveCrazyflieDB() {
-    string packagePath = ros::package::getPath("d_fall_pps") + "/";
-    string dbFile = packagePath + "crazyflie.db";
-    bag.open(dbFile, rosbag::bagmode::Write);
-    bag.write("crazyflie_db", ros::Time::now(), crazyflieDB);
-}
-
-void loadCrazyflieDB() {
-//
-}
 
 bool cmRead(CMRead::Request &request, CMRead::Response &response) {
     response.crazyflieDB = crazyflieDB;
@@ -113,12 +100,13 @@ bool cmUpdate(CMUpdate::Request &request, CMUpdate::Response &response) {
 bool cmCommand(CMCommand::Request &request, CMCommand::Response &response) {
     switch(request.command) {
         case CMD_SAVE: {
-            //writeCrazyflieDB(crazyflieDB);
+            writeCrazyflieDB(crazyflieDB);
             return true;
         }
 
         case CMD_RELOAD: {
-            //crazyflieDB = readCrazyflieDB();
+            crazyflieDB.crazyflieEntries.clear();
+            readCrazyflieDB(crazyflieDB);
             return true;
         }
 
@@ -130,6 +118,8 @@ int main(int argc, char* argv[]) {
     ros::init(argc, argv, "CentralManagerService");
 
     ros::NodeHandle nodeHandle("~");
+    
+    readCrazyflieDB(crazyflieDB);
 
     ros::ServiceServer readService = nodeHandle.advertiseService("Read", cmRead);
     ros::ServiceServer queryService = nodeHandle.advertiseService("Query", cmQuery);

pps_ws/src/d_fall_pps/src/CrazyflieIO.cpp

+#include "CrazyflieIO.h"
+#include <stdlib.h>
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <iostream>
+#include <fstream>
+#include <string>
+
+#include "d_fall_pps/CrazyflieContext.h"
+#include "d_fall_pps/CrazyflieEntry.h"
+#include "d_fall_pps/CrazyflieDB.h"
+
+using namespace std;
+
+namespace d_fall_pps {
+
+string escape(string input) {
+    string escaped;
+    for (string::const_iterator i = input.begin(), end = input.end(); i != end; ++i) {
+        unsigned char c = *i;
+        switch(c) {
+            case '\\': escaped += "\\\\"; break;
+            case '\r': escaped += "\\r"; break;
+            case '\n': escaped += "\\n"; break;
+            case '\t': escaped += "\\t"; break;
+            case ',': escaped += "\\c"; break;
+            default: escaped += c; break;
+        }
+    }
+    return escaped;
+}
+
+string unescape(string input) {
+    string unescaped;
+    bool escapeSeq = false;
+    for (string::const_iterator i = input.begin(), end = input.end(); i != end; ++i) {
+        unsigned char c = *i;
+
+        if(!escapeSeq) {
+            if(c == '\\') {
+                escapeSeq = true;
+            } else {
+                unescaped += c;
+            }
+        } else {
+            switch(c) {
+                case '\\': unescaped += "\\"; break;
+                case 'r': unescaped += "\r"; break;
+                case 'n': unescaped += "\n"; break;
+                case 't': unescaped += "\t"; break;
+                case 'c': unescaped += ","; break;
+                default: ROS_ERROR_STREAM("illegal escape sequence: \"\\" << c << "\"");
+            }
+            escapeSeq = false;
+        }
+    }
+    return unescaped;
+}
+
+vector<string> nextLine(istream& str) {
+    vector<string> result;
+    string line;
+    getline(str,line);
+
+    stringstream lineStream(line);
+    string cell;
+
+    while(getline(lineStream, cell, ',')) {
+        result.push_back(cell);
+    }
+
+    //if there is a trailing comma with no data after it
+    if (!lineStream && cell.empty()) {
+        result.push_back("");
+    }
+
+    return result;
+}
+
+string getCrazyflieDBPath() {
+    string packagePath = ros::package::getPath("d_fall_pps") + "/";
+    string dbFile = packagePath + "param/Crazyflie.db";
+    return dbFile;
+}
+
+void readCrazyflieDB(CrazyflieDB& db) {
+    ifstream dbFile;
+    dbFile.open(getCrazyflieDBPath());
+
+    while(dbFile.peek() != EOF) {
+        vector<string> dataRow = nextLine(dbFile);
+
+        if(dataRow.size() == 0) {
+        } else if(dataRow.size() != 9) {
+            ROS_ERROR_STREAM("row in csv file has not the right amount of data fields, skipped");
+        } else {
+
+            CrazyflieEntry entry;
+            entry.studentID = stoi(dataRow[0]);
+
+            CrazyflieContext context;
+            context.crazyflieName = unescape(dataRow[1]);
+            context.crazyflieAddress = unescape(dataRow[2]);
+
+            AreaBounds area;
+
+            area.xmin = stof(dataRow[3]);
+            area.xmin = stof(dataRow[4]);
+            area.zmin = stof(dataRow[5]);
+
+            area.xmax = stof(dataRow[6]);
+            area.ymax = stof(dataRow[7]);
+            area.zmax = stof(dataRow[8]);
+
+            context.localArea = area;
+            entry.crazyflieContext = context;
+            db.crazyflieEntries.push_back(entry);
+        }
+    }
+}
+
+void writeCrazyflieDB(CrazyflieDB& db) {
+    ofstream dbFile;
+    dbFile.open(getCrazyflieDBPath());
+
+    for(int i = 0; i < db.crazyflieEntries.size(); ++i) {
+    	CrazyflieEntry entry = db.crazyflieEntries[i];
+    	CrazyflieContext context = entry.crazyflieContext;
+
+    	dbFile << entry.studentID << ',';
+    	dbFile << escape(context.crazyflieName) << ',';
+    	dbFile << escape(context.crazyflieAddress) << ',';
+
+    	AreaBounds area = context.localArea;
+
+    	dbFile << area.xmin << ',';
+    	dbFile << area.ymin << ',';
+    	dbFile << area.zmin << ',';
+
+    	dbFile << area.xmax << ',';
+    	dbFile << area.ymax << ',';
+    	dbFile << area.zmax;
+
+    	dbFile << '\n';
+    }
+
+    dbFile.close();
+}
+
+}
\ No newline at end of file