DataListener.cpp 5.39 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
29
30
31
32
33
34
35
36
37
38
{
	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)
{
	printf("Offset now is (%.2lf, %.2lf, %.2lf)\n", x, y, z);
	offset.x = x;
	offset.y = y;
	offset.z = z;
}

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

41
	anchors_raw.push_back(anchor_raw_data(source, dest, distance));
42

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

	//Reset Timer
46
	locTimer = std::chrono::high_resolution_clock::now();
47
48
}

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

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

*/

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

106
	anchors_raw.sort(compare_anchor_data);
107
108

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

	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++)
	{
131
132
		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)
133
134
135
		                             };


136
137
138
139
140
		//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]));
141
142
143
144
145
146
	}

	//Save anchor positions to yaml file

	ros::NodeHandle nodeHandle("~");

147
148
	//std::vector<float> anchorArr;
	std::vector<double> anchorArr;
149

150
	for (unsigned int i = 0; i < anchor_position.size(); i++)
151
	{
tiagos's avatar
tiagos committed
152
		//printf("anchor%i (%f, %f, %f)\n", i, anchor_position[i].x, anchor_position[i].y, anchor_position[i].z);
153
154
155
156
157
		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);
	}


158
159
160
	finishedLoc = true;
	return true;
}
161

162
163
164
void DataListener::_resetLocalisation()
{
	serial.closeConnection();
165

166
167
	anchors_raw.clear();
	anchor_position.clear();
168

169
170
171
	offset.x = 0;
	offset.y = 0;
	offset.z = 0;
172

173
174
	finishedLoc = false;
}
175

176
177
178
179
bool DataListener::_startSerialCommunication()
{
	if (!serial.startConnection("/dev/ttyACM0")) return false;
	if (!serial.writeSerial("loc100\r")) return false;
180

181
182
183
184
	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;
185

186
187
188
189
		std::string value = serial.readSerial();
		while (value != "") {
			if (value.at(0) == '$')
				_LOC_Debugger(value);
190

191
192
193
			value = serial.readSerial();
		}
	}
194
195

	return true;
196
197
198
199
}

bool DataListener::startAnchorLocalistaion()
{
tiagos's avatar
tiagos committed
200
201
	_resetLocalisation();

202
203
204
	for (unsigned int i = 0; i < 5 && !finishedLoc; i++)
	{
		if (i != 0)
tiagos's avatar
tiagos committed
205
			ROS_WARN("[Selflocalisation] Looks like something went wrong... %u tries left\n", 5 - i);
206
207
208
209
210

		_resetLocalisation();
		if (!_startSerialCommunication())
			continue;
		if (!_calculateAnchorPosition()) {
tiagos's avatar
tiagos committed
211
			ROS_ERROR("[Selflocalisation] One or more distances are missing");
212
213
214
215
216
217
218
219
220
			continue;
		}
	}
	serial.closeConnection();

	if (finishedLoc)
		return true;

	return false;
221
}