diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 3dbceb99a15b237f5eab79f282b68988f00cd4fa..0b32f4a8c4f1111776a17aac090a83a005c73273 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/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)
diff --git a/pps_ws/src/d_fall_pps/include/CrazyflieIO.h b/pps_ws/src/d_fall_pps/include/CrazyflieIO.h
new file mode 100644
index 0000000000000000000000000000000000000000..d736fe522044f766fe5f500df3a4c5a4fd34a55f
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/CrazyflieIO.h
@@ -0,0 +1,9 @@
+#include "d_fall_pps/CrazyflieDB.h"
+
+namespace d_fall_pps {
+
+void readCrazyflieDB(CrazyflieDB& cfdb);
+
+void writeCrazyflieDB(CrazyflieDB& cfdb);
+
+}
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp b/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp
index 2be7fccdf14bcdab6b4a100e7236a9c1cd00b47a..0b8babb0b426d3eacf7ea8e6ce36f3747cdeaf6d 100755
--- a/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp
+++ b/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);
diff --git a/pps_ws/src/d_fall_pps/src/CrazyflieIO.cpp b/pps_ws/src/d_fall_pps/src/CrazyflieIO.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dbf23672288a91fa948fee181313b79e591727ac
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/src/CrazyflieIO.cpp
@@ -0,0 +1,150 @@
+#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