DataListener.cpp 8.31 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
//    Copyright (C) 2018, ETH Zurich, D-ITET
//          Yvan Bosshard           byvan       @ee.ethz.ch
//          Michael Rogenmoser      michaero    @ee.ethz.ch
//          Tiago Salzmann          tiagos      @ee.ethz.ch
//
//    This file is part of D-FaLL-System.
//    
//    D-FaLL-System 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.
//    
//    D-FaLL-System 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 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:
//    Handles connection to the UWB basestation and anchor settings.
//
//    ----------------------------------------------------------------------------------


36
37
38
39
40
41
42
43
44
45
#include "DataListener.h"

#include "IndoorNewton.h"

#include <string>
#include <vector>

#include "ros/ros.h"
#include <ros/package.h>

46
bool compare_anchor_data(const anchor_raw_data& a, const anchor_raw_data& b)
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
{
	if ((a.from == b.from && a.to < b.to) || a.from < b.from)
		return true;

	return false;
}

DataListener::DataListener()
{
}

DataListener::~DataListener()
{
}

void DataListener::setOffset(double x, double y, double z)
{
64
65
66
67
68
69
	//printf("Offset now is (%.2lf, %.2lf, %.2lf)\n", x, y, z);
	offset.x += x;
	offset.y += y;
	offset.z += z;

	_writeToYAML();
70
71
72
73
74
75
}

void DataListener::_LOC_Debugger(std::string value)
{
	unsigned int source, dest, distance;
	sscanf(value.c_str(), "$%u/%u/%u", &source, &dest, &distance);
76
	distance -= 500;
77

78
	anchors_raw.push_back(anchor_raw_data(source, dest, distance));
79

tiagos's avatar
tiagos committed
80
	//printf("--- LOC: \t%u \t%u \t%u\n", source, dest, distance);
81
82

	//Reset Timer
83
	locTimer = std::chrono::high_resolution_clock::now();
84
85
}

86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
bool DataListener::_check_data_validity()
{
	if (anchors_raw.size() == 0)
		return false;

	for (unsigned int i = 0; i < 5; i++)
	{
		for (unsigned int j = i + 1; j < 6; j++)
		{
			if (_get_distance_average(i, j) == -1)
				return false;
		}
	}

	return true;
}

double DataListener::_get_distance_average(unsigned int anchor_from, unsigned int anchor_to)
104
105
106
{
	unsigned int sum = 0;
	unsigned int count = 0;
107
108
	std::list<anchor_raw_data>::iterator it;
	for (it = anchors_raw.begin(); it != anchors_raw.end(); it++)
109
110
111
112
113
114
115
116
117
118
119
120
121
122
	{
		//Not a measurement from and to
		if (it->from != anchor_from || it->to != anchor_to)
			continue;

		if (it->distance < 10000) //Simple filter -> if the distance is less than 10m = 10'000mm
		{
			sum += it->distance;
			count++;
		}
	}

	//FAILED
	if (count == 0 || sum == 0)
123
		return -1;
124
125
126
127
128
129
130
131
132
133
134
135
136
137

	//Divide by 100 to get it in dm
	return ((double)sum / (double)count) / 100.0;
}


////////////////////////////////////////////////////////////
/*

I KNOW, THIS NEWTON ALGORITHM IS MADE FOR 6 ANCHORS. NOT THE BEST SOLUTION WHEN USING ONLY 3 TO DETERMINE
THE POSITION OF THE OTHER ANCHORS, BUT HEY, IT WORKS xD

*/

138
bool DataListener::_calculateAnchorPosition()
139
{
140
	if (!_check_data_validity())
141
142
		return false;

143
	anchors_raw.sort(compare_anchor_data);
144
145

	double d01, d02, d12;
146
147
148
	d01 = _get_distance_average(0, 1);
	d02 = _get_distance_average(0, 2);
	d12 = _get_distance_average(1, 2);
149
150
151
152

	double x = (d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02);
	double y = d01 * sin(acos((d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02 * d01)));

tiagos's avatar
tiagos committed
153
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
183
184
185
186
187
188
189
190
191
192
193
194
195
	/*double d03, d23;
	d03 = _get_distance_average(0,3);
	d23 = _get_distance_average(2,3);*/


	/*double a01, a21;
	a01 = asin(-12.8/d01);
	a21 = asin(-12.8/d12);

	d01 *= cos(a01);
	d12 *= cos(a21);*/
	/*

	d01 *= cos(a01);
	d12 *= cos(a21);*/

	/*double a03, a23;
	a03 = asin(-6/d03);
	a23 = asin(-6/d23);

	d03 *= cos(a03);
	d23 *= cos(a23);

	double x3 = (d03 * d03 + d02 * d02 - d23 * d23) / (2 * d02);
	double y3 = d03 * sin(acos((d03 * d03 + d02 * d02 - d23 * d23) / (2 * d02 * d03)));


	

	double x = (d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02);
	double y = d01 * sin(acos((d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02 * d01)));

	double anchor_positions[18] = {0, 0, 0, \
	                               x, -y, 0, \
	                               d02, 0, 0, \
	                               0, 0, 0, \
	                               x3, -y3, -6, \
	                               d02, 0, 0
	                              };

	ROS_WARN("x: %f, y: %f", x, -y);*/


196
197
198
199
200
201
202
203
204
205
206
	double anchor_positions[18] = {0, 0, 0, \
	                               x, -y, 0, \
	                               d02, 0, 0, \
	                               0, 0, 0, \
	                               x, -y, 0, \
	                               d02, 0, 0
	                              };

	anchor_position.push_back(anchor_position_data(0, anchor_positions[0], anchor_positions[1], anchor_positions[2]));
	anchor_position.push_back(anchor_position_data(1, anchor_positions[3], anchor_positions[4], anchor_positions[5]));
	anchor_position.push_back(anchor_position_data(2, anchor_positions[6], anchor_positions[7], anchor_positions[8]));
tiagos's avatar
tiagos committed
207
	//anchor_position.push_back(anchor_position_data(3, anchor_positions[12], anchor_positions[13], anchor_positions[14]));
208
209
210
211

	//For anchor n we only need d0n, d1n, d2n
	for (unsigned int n = 3; n < 6; n++)
	{
212
213
		double anchor_distances[6] = { _get_distance_average(0, n), _get_distance_average(1, n), _get_distance_average(2, n), \
		                               _get_distance_average(0, n), _get_distance_average(1, n), _get_distance_average(2, n)
214
215
216
		                             };


217
218
		//Calculate position of anchor n using IndoorNewton
		double result[3];
tiagos's avatar
tiagos committed
219
		IndoorNewton(anchor_positions, anchor_distances, result, 100);
220

tiagos's avatar
tiagos committed
221
		anchor_position.push_back(anchor_position_data(n, result[0], result[1], 0.0));//result[2])); //result[2]
222
223
224
	}

	//Save anchor positions to yaml file
225
	_writeToYAML();
226

227
228
229
	finishedLoc = true;
	return true;
}
230

231
232
233
void DataListener::_resetLocalisation()
{
	serial.closeConnection();
234

235
236
	anchors_raw.clear();
	anchor_position.clear();
237

tiagos's avatar
tiagos committed
238
239
240
	offset.x = 0;
	offset.y = 0;
	offset.z = 0;
241

242
243
	finishedLoc = false;
}
244

245
246
247
248
bool DataListener::_startSerialCommunication()
{
	if (!serial.startConnection("/dev/ttyACM0")) return false;
	if (!serial.writeSerial("loc100\r")) return false;
249

250
251
252
253
	locTimer = std::chrono::high_resolution_clock::now();
	while (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - locTimer).count() < 2000) {
		if (!serial.manageSerial())
			return false;
254

255
256
257
258
		std::string value = serial.readSerial();
		while (value != "") {
			if (value.at(0) == '$')
				_LOC_Debugger(value);
259

260
261
262
			value = serial.readSerial();
		}
	}
263
264

	return true;
265
266
267
268
}

bool DataListener::startAnchorLocalistaion()
{
tiagos's avatar
tiagos committed
269
270
	_resetLocalisation();

271
272
273
	for (unsigned int i = 0; i < 5 && !finishedLoc; i++)
	{
		if (i != 0)
tiagos's avatar
tiagos committed
274
			ROS_WARN("[Selflocalisation] Looks like something went wrong... %u tries left\n", 5 - i);
275
276
277
278
279

		_resetLocalisation();
		if (!_startSerialCommunication())
			continue;
		if (!_calculateAnchorPosition()) {
tiagos's avatar
tiagos committed
280
			ROS_ERROR("[Selflocalisation] One or more distances are missing");
281
282
283
284
285
286
287
288
289
			continue;
		}
	}
	serial.closeConnection();

	if (finishedLoc)
		return true;

	return false;
290
291
}

292
293
294
295
296
297
298
void DataListener::invertAnchor(unsigned int id)
{
	anchor_position[id].z *= -1;

	_writeToYAML();
}

299
300
301
302
303
304
305
306
307
308
309
310
311
312
void DataListener::_writeToYAML()
{
	ros::NodeHandle nodeHandle("~");

	//std::vector<float> anchorArr;
	std::vector<double> anchorArr;

	for (unsigned int i = 0; i < anchor_position.size(); i++)
	{
		//printf("anchor%i (%f, %f, %f)\n", i, anchor_position[i].x, anchor_position[i].y, anchor_position[i].z);
		anchorArr = {anchor_position[i].x - offset.x, anchor_position[i].y - offset.y, anchor_position[i].z - offset.z};
		nodeHandle.setParam("anchor" + std::to_string(i), anchorArr);
	}
}