DataListener.cpp 5.57 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
#include "DataListener.h"

#include "IndoorNewton.h"

#include <string>
#include <vector>

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

11
bool compare_anchor_data(const anchor_raw_data& a, const anchor_raw_data& b)
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
{
	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)
{
29
30
31
32
33
34
	//printf("Offset now is (%.2lf, %.2lf, %.2lf)\n", x, y, z);
	offset.x += x;
	offset.y += y;
	offset.z += z;

	_writeToYAML();
35
36
37
38
39
40
}

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

43
	anchors_raw.push_back(anchor_raw_data(source, dest, distance));
44

tiagos's avatar
tiagos committed
45
	//printf("--- LOC: \t%u \t%u \t%u\n", source, dest, distance);
46
47

	//Reset Timer
48
	locTimer = std::chrono::high_resolution_clock::now();
49
50
}

51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
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)
69
70
71
{
	unsigned int sum = 0;
	unsigned int count = 0;
72
73
	std::list<anchor_raw_data>::iterator it;
	for (it = anchors_raw.begin(); it != anchors_raw.end(); it++)
74
75
76
77
78
79
80
81
82
83
84
85
86
87
	{
		//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)
88
		return -1;
89
90
91
92
93
94
95
96
97
98
99
100
101
102

	//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

*/

103
bool DataListener::_calculateAnchorPosition()
104
{
105
	if (!_check_data_validity())
106
107
		return false;

108
	anchors_raw.sort(compare_anchor_data);
109
110

	double d01, d02, d12;
111
112
113
	d01 = _get_distance_average(0, 1);
	d02 = _get_distance_average(0, 2);
	d12 = _get_distance_average(1, 2);
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132

	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, \
	                               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]));

	//For anchor n we only need d0n, d1n, d2n
	for (unsigned int n = 3; n < 6; n++)
	{
133
134
		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)
135
136
137
		                             };


138
139
140
141
142
		//Calculate position of anchor n using IndoorNewton
		double result[3];
		IndoorNewton(anchor_positions, anchor_distances, result, 200);

		anchor_position.push_back(anchor_position_data(n, result[0], result[1], result[2]));
143
144
145
	}

	//Save anchor positions to yaml file
146
	_writeToYAML();
147

148
149
150
	finishedLoc = true;
	return true;
}
151

152
153
154
void DataListener::_resetLocalisation()
{
	serial.closeConnection();
155

156
157
	anchors_raw.clear();
	anchor_position.clear();
158

159
160
161
	//offset.x = 0;
	//offset.y = 0;
	//offset.z = 0;
162

163
164
	finishedLoc = false;
}
165

166
167
168
169
bool DataListener::_startSerialCommunication()
{
	if (!serial.startConnection("/dev/ttyACM0")) return false;
	if (!serial.writeSerial("loc100\r")) return false;
170

171
172
173
174
	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;
175

176
177
178
179
		std::string value = serial.readSerial();
		while (value != "") {
			if (value.at(0) == '$')
				_LOC_Debugger(value);
180

181
182
183
			value = serial.readSerial();
		}
	}
184
185

	return true;
186
187
188
189
}

bool DataListener::startAnchorLocalistaion()
{
tiagos's avatar
tiagos committed
190
191
	_resetLocalisation();

192
193
194
	for (unsigned int i = 0; i < 5 && !finishedLoc; i++)
	{
		if (i != 0)
tiagos's avatar
tiagos committed
195
			ROS_WARN("[Selflocalisation] Looks like something went wrong... %u tries left\n", 5 - i);
196
197
198
199
200

		_resetLocalisation();
		if (!_startSerialCommunication())
			continue;
		if (!_calculateAnchorPosition()) {
tiagos's avatar
tiagos committed
201
			ROS_ERROR("[Selflocalisation] One or more distances are missing");
202
203
204
205
206
207
208
209
210
			continue;
		}
	}
	serial.closeConnection();

	if (finishedLoc)
		return true;

	return false;
211
212
}

213
214
215
216
217
218
219
void DataListener::invertAnchor(unsigned int id)
{
	anchor_position[id].z *= -1;

	_writeToYAML();
}

220
221
222
223
224
225
226
227
228
229
230
231
232
233
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);
	}
}