TeacherService.cpp 7.24 KB
Newer Older
1
//    Copyright (C) 2018, ETH Zurich, D-ITET
flashingx's avatar
flashingx committed
2
3
4
5
//          Yvan Bosshard           byvan       @ee.ethz.ch
//          Michael Rogenmoser      michaero    @ee.ethz.ch
//          Tiago Salzmann          tiagos      @ee.ethz.ch
//
6
7
8
//    This file is part of D-FaLL-System.
//    
//    D-FaLL-System is free software: you can redistribute it and/or modify
flashingx's avatar
flashingx committed
9
10
11
//    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.
12
13
//    
//    D-FaLL-System is distributed in the hope that it will be useful,
flashingx's avatar
flashingx committed
14
15
16
//    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.
17
//    
flashingx's avatar
flashingx committed
18
//    You should have received a copy of the GNU General Public License
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
//    
//
//    ----------------------------------------------------------------------------------
//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
//
//
//    DESCRIPTION:
//    ROS node that handles subscription to and position data from
//	  students regardless of position data source used.
//    (Also features implementation of StudentSubscriber class)
//
//    ----------------------------------------------------------------------------------

flashingx's avatar
flashingx committed
37

38
39
40
#include <regex>

#include "ros/ros.h"
flashingx's avatar
flashingx committed
41

42
43
44
45
46
47
48
#include "std_msgs/Int32MultiArray.h"
#include "d_fall_pps/CrazyfliePositionData.h"

#include "TeacherService.h"

using namespace d_fall_pps;

flashingx's avatar
flashingx committed
49
50
51
52
53
54
55
ros::Publisher studentIDs_publisher;
ros::Publisher CFData_publisher;


/************************************************************
 *	Main			  										*
 ***********************************************************/
56
57
58
59
60
61
int main(int argc, char* argv[])
{
	ros::init(argc, argv, "TeacherService");
	ros::NodeHandle nodeHandle("~");

	ros::NodeHandle namespaceNodehandle = ros::NodeHandle();
62
	ros::Subscriber refreshStudents_subscriber = namespaceNodehandle.subscribe("my_GUI/refreshStudents", 1, refresh_callback);
63
64
65
66

	studentIDs_publisher = nodeHandle.advertise<std_msgs::Int32MultiArray>("studentIDs", 1);
	CFData_publisher = nodeHandle.advertise<CrazyfliePositionData>("positionData", 1);

67
68
69
70
71
72
73
74
75
	ros::ServiceServer CFHangar = nodeHandle.advertiseService("Hangar", serveCFList);
	
	if(!loadCFsFromHangar(nodeHandle))
		return -1;


	/*for(int i = 0; i < 1000; ++i)
		initialiseCFIndex();*/

76
77
78
79
    ros::spin();
    return 0;
}

80
81
82
83
84
85
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
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
bool d_fall_pps::loadCFsFromHangar(ros::NodeHandle& nh)
{
	channel_LUT.clear();
	XmlRpc::XmlRpcValue cf_list;
	bool success = nh.getParam("Hangar", cf_list);

	if(!success || cf_list.size() == 0)
	{
		ROS_ERROR("[TeacherService] No CrazyFlies found in hangar!");
		return false;
	}

	for(unsigned int i = 0; i < cf_list.size(); ++i)
	{
		std::string address = cf_list[i][1];

		if(address == "")
		{
			ROS_ERROR("[TeacherService] Bad Hangar entry with CF %i", i);
			channel_LUT.clear();
			return false;
		}

		channel_LUT.insert(std::pair<std::string, std::string>(cf_list[i][0], cf_list[i][1]));
	}


	std::stringstream strm;
	strm << "\n[TeacherService] " << channel_LUT.size() << " CrazyFlies in hangar:\n";

	for(auto it = channel_LUT.begin(); it != channel_LUT.end(); ++it)
		strm << "\t" << it->first.c_str() << "\n";
		
	ROS_INFO_STREAM(strm.str());

	return true;
}

/*void d_fall_pps::initialiseCFIndex()
{*/
	/*ROS_ERROR("init index");

	CrazyflieData data;
	data.crazyflieName = "HELLO";
	data.x = 0;
	data.y = 0;
	data.z = 0;
	data.roll = 0;
	data.pitch = 0;
	data.yaw = 0;
	data.acquiringTime = 0;
	data.occluded = false;*/

	//CFData_publisher.publish(data);

	/*for(auto it = channel_LUT.begin(); it != channel_LUT.end(); ++it)
	{

	}*/
//}

void d_fall_pps::refresh_callback(const std_msgs::Int32 &msg)
142
143
144
145
146
147
148
149
150
151
152
{
	// Clear existing buffer
	studentIDs.clear();

	// Poll for all connected nodes
	ros::V_string v_str;
    ros::master::getNodes(v_str);

    // iterate through all nodes to find PPSClient's
    for(int i = 0; i < v_str.size(); i++)
    {
153
        std::string s = v_str[i];     
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
        std::smatch m;
        std::regex e ("\\/(\\d)\\/PPSClient");

        if(std::regex_search(s, m, e))
        {
            std::string found_string = m[1].str();
            int studentID = std::stoi(found_string);  // one because we are interested ONLY in the first match

            studentIDs.push_back(studentID);
        }
    }

    std::sort(studentIDs.begin(), studentIDs.end());

    // Prepare the data:
    std_msgs::Int32MultiArray publish_msg;

    for(std::vector<int>::const_iterator it = studentIDs.begin(); it != studentIDs.end(); ++it)
    	publish_msg.data.push_back(*it);

    // Publish the data for the GUI
    studentIDs_publisher.publish(publish_msg);

    // Update subscriptions to the clients
    subscribeLocalization();

    ROS_INFO("[TeacherService] updated Student IDs");
}

183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
bool d_fall_pps::serveCFList(CFIndex::Request& request, CFIndex::Response& response)
{
	if(request.cfName != "")
	{
		auto it = channel_LUT.find(request.cfName);

		if(it == channel_LUT.end())
			return false;

		response.length = 1;
		response.data.push_back(it->second);

		return true;
	}
	else
	{
		response.length = channel_LUT.size();

		for(auto it = channel_LUT.begin(); it != channel_LUT.end(); ++it)
			response.data.push_back(it->first);

		return true;
	}
}

208
209
210
211
212
213
214
215
216
217
218
void d_fall_pps::subscribeLocalization()
{
	ros::NodeHandle nodeHandle("~");

	for(std::vector<StudentSubscriber>::iterator it = studentSubscribers.begin(); it != studentSubscribers.end(); ++it)
		(*it).unsubscribe();

	studentSubscribers.clear();

	for(std::vector<int>::const_iterator it = studentIDs.begin(); it != studentIDs.end(); ++it)
	{
219
		ROS_WARN("Adding Student %i.", (*it));
220
221
222
223
224
225
226
227

		std::string address = "/";
		address.append(std::to_string(*it));
		address.append("/LocalizationServer/LocalizationData");

		StudentSubscriber sub = StudentSubscriber(*it, address);
		studentSubscribers.push_back(sub);
	}
tiagos's avatar
Ja ja    
tiagos committed
228
229
230
231
	// for debugging
	StudentSubscriber sub = StudentSubscriber(8, "/8/UWBDataPublisher/UWBData");
	studentSubscribers.push_back(sub);

232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
	for(std::vector<StudentSubscriber>::iterator it = studentSubscribers.begin(); it != studentSubscribers.end(); ++it)
		(*it).subscribe(nodeHandle);
}

d_fall_pps::StudentSubscriber::StudentSubscriber(int id, std::string address)
{
	this->_studentID = id;
	this->_address = address;
}

void d_fall_pps::StudentSubscriber::subscribe(ros::NodeHandle& handle)
{
	this->_subscriber = handle.subscribe(this->_address, 1, &StudentSubscriber::callback, this);
}

void d_fall_pps::StudentSubscriber::unsubscribe()
{
	this->_subscriber.shutdown();
}

void d_fall_pps::StudentSubscriber::callback(const CrazyflieData& data)
{
	CrazyfliePositionData pos;

	pos.id = this->_studentID;
	pos.x = data.x;
	pos.y = data.y;
	pos.z = data.z;
	pos.roll = data.roll;
	pos.pitch = data.pitch;
	pos.yaw = data.yaw;
	pos.acquiringTime = data.acquiringTime;

	CFData_publisher.publish(pos);
}