CentralManagerService.cpp 4.38 KB
Newer Older
bucyril's avatar
bucyril committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
//    The service that manages the context of the student groups.
//    Copyright (C) 2017  Cyrill Burgener, Marco Mueller, Philipp Friedli
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see <http://www.gnu.org/licenses/>.

17
#include <stdlib.h>
bucyril's avatar
bucyril committed
18
#include "ros/ros.h"
muelmarc's avatar
merging    
muelmarc committed
19
20
#include <ros/package.h>
#include "rosbag/bag.h"
bucyril's avatar
bucyril committed
21
#include "d_fall_pps/CentralManager.h"
22
23
24
#include "d_fall_pps/CrazyflieContext.h"
#include "d_fall_pps/CrazyflieDB.h"
#include "d_fall_pps/CrazyflieContext.h"
bucyril's avatar
bucyril committed
25

26
27
28
29
#include "d_fall_pps/CMRead.h"
#include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/CMUpdate.h"
#include "d_fall_pps/CMCommand.h"
30
#include "CentralManagerService.h"
bucyril's avatar
bucyril committed
31
32

using namespace d_fall_pps;
33
using namespace std;
bucyril's avatar
bucyril committed
34

35
CrazyflieDB crazyflieDB;
muelmarc's avatar
merging    
muelmarc committed
36
37
38
39
40
41
42
43
44
45
46
47
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() {

}
48
49

bool cmRead(CMRead::Request &request, CMRead::Response &response) {
50
    response.crazyflieDB = crazyflieDB;
phfriedl's avatar
phfriedl committed
51
52
	return true;
}
bucyril's avatar
bucyril committed
53

54
55
56
57
58
59
60
61
62
63
int findEntryByStudID(unsigned int studID) {
    for(int i = 0; i < crazyflieDB.crazyflieEntries.size(); i++) {
        CrazyflieEntry entry = crazyflieDB.crazyflieEntries[i];
        if(entry.studentID == studID) {
            return i;
        }
    }
    return -1;
}

64
bool cmQuery(CMQuery::Request &request, CMQuery::Response &response) {
65
66
67
68
69
70
71
72
    int cfIndex = findEntryByStudID(request.studentID);
    if(cfIndex != -1) {
        response.crazyflieContext = crazyflieDB.crazyflieEntries[cfIndex].crazyflieContext;
        return true;
    } else {
        return false;
    }
}
bucyril's avatar
bucyril committed
73

74
75
76
77
78
79
80
81
82
int findEntryByCF(string name) {
    for(int i = 0; i < crazyflieDB.crazyflieEntries.size(); i++) {
        CrazyflieEntry entry = crazyflieDB.crazyflieEntries[i];
        string cfName = entry.crazyflieContext.crazyflieName;
        if(cfName == name) {
            return i;
        }
    }
    return -1;
83
}
bucyril's avatar
bucyril committed
84

85
bool cmUpdate(CMUpdate::Request &request, CMUpdate::Response &response) {
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
    switch(request.mode) {
        case ENTRY_INSERT_OR_UPDATE: {
            string cfName = request.crazyflieEntry.crazyflieContext.crazyflieName;
            int cfIndex = findEntryByCF(cfName);
            if(cfIndex == -1) {
                crazyflieDB.crazyflieEntries.push_back(request.crazyflieEntry);
            } else {
                crazyflieDB.crazyflieEntries[cfIndex] = request.crazyflieEntry;
            }
            return true;
        }

        case ENTRY_REMOVE: {
            string cfName = request.crazyflieEntry.crazyflieContext.crazyflieName;
            int cfIndex = findEntryByCF(cfName);
            if(cfIndex == -1) {
                return false;
            } else {
                crazyflieDB.crazyflieEntries.erase(crazyflieDB.crazyflieEntries.begin() +cfIndex);
                return true;
            }
        }

        default: return false;
    }
111
112
113
}

bool cmCommand(CMCommand::Request &request, CMCommand::Response &response) {
114
115
    switch(request.command) {
        case CMD_SAVE: {
muelmarc's avatar
merging    
muelmarc committed
116
            saveCrazyflieDB();
117
118
119
120
            return true;
        }

        case CMD_RELOAD: {
muelmarc's avatar
merging    
muelmarc committed
121
            loadCrazyflieDB();
122
123
124
125
126
            return true;
        }

        default: return false;
    }
127
}
bucyril's avatar
bucyril committed
128
129
130
131
132
133

int main(int argc, char* argv[]) {
    ros::init(argc, argv, "CentralManagerService");

    ros::NodeHandle nodeHandle("~");

134
135
136
137
138
    ros::ServiceServer readService = nodeHandle.advertiseService("Read", cmRead);
    ros::ServiceServer queryService = nodeHandle.advertiseService("Query", cmQuery);
    ros::ServiceServer updateService = nodeHandle.advertiseService("Update", cmUpdate);
    ros::ServiceServer commandService = nodeHandle.advertiseService("Command", cmCommand);

bucyril's avatar
bucyril committed
139
140
141
142
143
    ROS_INFO("CentralManagerService ready");
    ros::spin();

    return 0;
}