Commit 8a9ac62c authored by bucyril's avatar bucyril
Browse files

reverted deletion of files

parent ca9a7fc5
......@@ -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)
......
#include "d_fall_pps/CrazyflieDB.h"
namespace d_fall_pps {
void readCrazyflieDB(CrazyflieDB& cfdb);
void writeCrazyflieDB(CrazyflieDB& cfdb);
}
\ No newline at end of file
......@@ -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);
......
#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
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment