UWBDataPublisher.cpp 10.2 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
//    ROS node that publishes the localization data from sensor fusion of the crazyflie, using uwb for coordinates
//    Copyright (C) 2017  Michael Rogenmoser, Tiago Salzmann, Yvan Bosshard
//
//    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
18
#define DEG2RAD	0.01745329251

19
#include <stdlib.h>
20
#include <string.h>
21
#include <chrono>
22
23
24
25

#include <ros/package.h>
#include <rosbag/bag.h>

26
#include "d_fall_pps/CMQuery.h"
27
#include "d_fall_pps/Anchors.h"
28
29
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CrazyflieContext.h"
30

Yvan Bosshard's avatar
Yvan Bosshard committed
31
32
#include "IndoorNewton.h"

33
#include "UWBDataPublisher.h"
34

35
36
using namespace d_fall_pps;

Yvan Bosshard's avatar
Yvan Bosshard committed
37
// Kalmanfilter Variables
Yvan Bosshard's avatar
Yvan Bosshard committed
38
39
#define KALMAN_M_VARIANCE	0.01	// was 1
#define KALMAN_P_VARIANCE	0.0001
Yvan Bosshard's avatar
Yvan Bosshard committed
40
41

#define DISTANCE_ERROR_TRESHOLD	0.9
Yvan Bosshard's avatar
Yvan Bosshard committed
42

43
44
45
// the clientID defines namespace and identifier in the CentralManagerService
int clientID;

46
47
48
49
50
51
// UWB Anchor error handling
const unsigned long print_timeout = 1000;	// in ms
bool printBadReading[UWB_NUM_ANCHORS] = {false};
bool printError[UWB_NUM_ANCHORS] = {false};
std::chrono::high_resolution_clock::time_point lastPrint;

52
53
54
CrazyflieContext context;
rosbag::Bag bag;

55
ros::ServiceClient centralManager;
56
ros::ServiceClient uwbManager;
57
ros::Publisher UWBDataPublisher;
58
ros::Publisher UWBLogPublisher;
59

60
double rollPitchYaw[3];
61

62
UWBAnchorArray anchors;
63
double anchorPositions[3*UWB_NUM_ANCHORS];
Yvan Bosshard's avatar
Yvan Bosshard committed
64
double lastDistances[UWB_NUM_ANCHORS] = {0};
65

Yvan Bosshard's avatar
Yvan Bosshard committed
66
67
68
static KalmanState xState;
static KalmanState yState;
static KalmanState zState;
69

Yvan Bosshard's avatar
Yvan Bosshard committed
70
KalmanState kalmanDist[UWB_NUM_ANCHORS];
71

72
73
74
75
76
77
78
79
80
81

std::chrono::high_resolution_clock::time_point lastRPY;
unsigned long whatever = 0;
unsigned int whatever_counter = 0;

std::chrono::high_resolution_clock::time_point lastDist;
unsigned long whatever2 = 0;
unsigned int whatever_counter2 = 0;


82
83
84
85
int main(int argc, char* argv[])
{
    ros::init(argc, argv, "UWBDataPublisher");
    ros::NodeHandle nodeHandle("~");
86
    ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
87

88
89
    lastPrint = std::chrono::high_resolution_clock::now();

90
    if(!loadParameters(nodeHandle))
91
    	return -1;
92

93
    centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
94
	loadCrazyflieContext();
95
96
97
98
	ros::Subscriber dbChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback);

	uwbManager = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
	ros::Subscriber enableUWBSubscriber = nodeHandle.subscribe("/my_GUI/enableUWB", 1, uwbChangedCallback);
99
100
101

	// Subscribe to the anchor positions and all relevant Data from crazyradio
	ros::Subscriber cfRPYSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/RPY",10,cfRPYCallback);
102
	ros::Subscriber uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/AnchorDistances", 10, cfDistancesCallback);
Yvan Bosshard's avatar
Yvan Bosshard committed
103

104
	UWBDataPublisher = nodeHandle.advertise<CrazyflieData>("UWBData", 10);
105
	UWBLogPublisher = nodeHandle.advertise<std_msgs::Float32MultiArray>("Distances", 1);
106

107
108
109
    std::string package_path;
	package_path = ros::package::getPath("d_fall_pps") + "/";
	std::string record_file = package_path + "log/UWBDataPublisherLog.bag";
110
111
    bag.open(record_file, rosbag::bagmode::Write);

112
113
    initKalmanStates();

Yvan Bosshard's avatar
Yvan Bosshard committed
114
115
    loadUWBSettings();

116
    ROS_INFO_STREAM("[UWBDataPublisher] started!");
117

118
    ros::spin();
119
120
121
122
123
    bag.close();
    
    return 0;
}

124
125
126
127
128
void d_fall_pps::dbChangedCallback(const std_msgs::Int32& msg)
{
	d_fall_pps::loadCrazyflieContext();
}

129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
void d_fall_pps::uwbChangedCallback(const std_msgs::Int32& msg)
{
	d_fall_pps::loadUWBSettings();
}

void d_fall_pps::loadUWBSettings()
{
	Anchors a;

	if(uwbManager.call(a))
	{
		anchors = a.response.anchorArray;
	}
	else
	{
		ROS_ERROR("[UWBDataPublisher] Could not update UWB Settings!");
	}

	for(int i = 0; i < UWB_NUM_ANCHORS; ++i)
	{
		anchorPositions[3*i] = anchors.data[i].x;
		anchorPositions[3*i + 1] = anchors.data[i].y;
		anchorPositions[3*i + 2] = anchors.data[i].z;
	}
}

155
156
157
//Callback function to handle roll pitch yaw information from CF
void d_fall_pps::cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY)
{
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
	// Just to check how often these get published
	/*std::chrono::high_resolution_clock::time_point thisRPY = std::chrono::high_resolution_clock::now();
	whatever += std::chrono::duration_cast<std::chrono::milliseconds>(thisRPY - lastRPY).count();
	++whatever_counter;
	lastRPY = thisRPY;

	if(whatever_counter > 100)
	{
		double frq = 1000.0 * whatever_counter / whatever;

		ROS_WARN("RPY at %lf Hz", frq);

		whatever_counter = 0;
		whatever = 0;
	}*/

174
175
176
	rollPitchYaw[0] = arrRPY.data[0] * DEG2RAD;
	rollPitchYaw[1] = arrRPY.data[1] * DEG2RAD;
	rollPitchYaw[2] = arrRPY.data[2] * DEG2RAD;
177
178
}

179
// Callback functions to handle times information from CF
180
void d_fall_pps::cfDistancesCallback(const std_msgs::UInt32MultiArray &distances)
181
{
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
	// Just to check how often these get published
	/*std::chrono::high_resolution_clock::time_point thisDist = std::chrono::high_resolution_clock::now();
	whatever2 += std::chrono::duration_cast<std::chrono::milliseconds>(thisDist - lastDist).count();
	++whatever_counter2;
	lastDist = thisDist;

	if(whatever_counter2 > 100)
	{
		double frq = 1000.0 * whatever_counter2 / whatever2;

		ROS_WARN("Dist at %lf Hz", frq);

		whatever_counter2 = 0;
		whatever2 = 0;
	}*/

198
199
200
	CrazyflieData data;
    
    double arrXYZ[3] = {0};
201
    calculateXYZ(distances, arrXYZ);
202
203
204
205
206
207
	
	data.crazyflieName = context.crazyflieName;
	data.x = arrXYZ[0];
	data.y = arrXYZ[1];
	data.z = arrXYZ[2];
	data.roll = rollPitchYaw[0];
Yvan Bosshard's avatar
Yvan Bosshard committed
208
	data.pitch = -1*rollPitchYaw[1];
209
210
211
212
213
214
215
	data.yaw = rollPitchYaw[2];
	data.occluded = false;
	data.acquiringTime = 0; //???
	
	UWBDataPublisher.publish(data);
}

Yvan Bosshard's avatar
Yvan Bosshard committed
216
217


218
//calculates position from anchor locations and distances -> forces algorithm?
219
void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, double(&xyz)[3])
220
{
Yvan Bosshard's avatar
Yvan Bosshard committed
221
222
	double dist[UWB_NUM_ANCHORS];
	double result[3];
223
224
225
	
	std::chrono::high_resolution_clock::time_point thisPrint = std::chrono::high_resolution_clock::now();
	unsigned long printdiff = std::chrono::duration_cast<std::chrono::milliseconds>(thisPrint - lastPrint).count();
Yvan Bosshard's avatar
Yvan Bosshard committed
226
227
228

	for(unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
	{
Yvan Bosshard's avatar
Yvan Bosshard committed
229
		if(abs(lastDistances[i] - distances.data[i] / 100.0) > DISTANCE_ERROR_TRESHOLD)
230
			printBadReading[i] = true;
Yvan Bosshard's avatar
Yvan Bosshard committed
231

Yvan Bosshard's avatar
Yvan Bosshard committed
232
		dist[i] = distances.data[i] / 100.0; //in dm
Yvan Bosshard's avatar
Yvan Bosshard committed
233
234
235
		lastDistances[i] = dist[i];
		

Yvan Bosshard's avatar
Yvan Bosshard committed
236
		if(distances.data[i] == 0)
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
			printError[i] = true;
	}

	if(printdiff > print_timeout)
	{
		for(unsigned int i = 0; i < UWB_NUM_ANCHORS; ++i)
		{
			if(printError[i])
				ROS_ERROR("Error with Anchor %i", i+1);

			printError[i] = false;

			if(printBadReading[i])
				ROS_WARN("Bad reading from Anchor %i", i+1);

			printBadReading[i] = false;
		}

		lastPrint = thisPrint;
Yvan Bosshard's avatar
Yvan Bosshard committed
256
257
	}

258
259
	//filterDistances(dist);

Yvan Bosshard's avatar
Yvan Bosshard committed
260
261
262
263
264
	/*for(int i = 0; i < UWB_NUM_ANCHORS; ++i)
	{
		ROS_WARN("Anchor %i: (%f, %f, %f)", i, anchorPositions[3*i], anchorPositions[3*i+1], anchorPositions[3*i+2]);
	}*/

265
266
	IndoorNewton(anchorPositions, dist, result, 50);

Yvan Bosshard's avatar
Yvan Bosshard committed
267
268
	//ROS_INFO_STREAM(result[0]);

269
270
271
	updateKalmanState(xState, result[0] / 10.0);
	updateKalmanState(yState, result[1] / 10.0);
	updateKalmanState(zState, result[2] / 10.0);
Yvan Bosshard's avatar
Yvan Bosshard committed
272

273
274
	
	// Use this for Kalmanfiltered position
Yvan Bosshard's avatar
Yvan Bosshard committed
275
276
277
278
279
	xyz[0] = xState.x;
	xyz[1] = yState.x;
	xyz[2] = zState.x;

	//ROS_WARN("Kalman: (%f, %f, %f)", xState.x, yState.x, zState.x);
280
281

	// Use this for unfiltered position
Yvan Bosshard's avatar
Yvan Bosshard committed
282
283
284
	// xyz[0] = result[0] / 10.0;
	// xyz[1] = result[1] / 10.0;
	//xyz[2] = result[2] / 10.0;
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311

	publishLog(distances, dist);
}

void d_fall_pps::publishLog(const std_msgs::UInt32MultiArray& distances, const double* dist)
{
	std_msgs::Float32MultiArray msg;
	msg.data = {};

	for(int i = 0; i < UWB_NUM_ANCHORS; ++i)
	{
		msg.data.push_back(distances.data[i] / 100.0);
	}

	for(int i = 0; i < UWB_NUM_ANCHORS; ++i)
	{
		msg.data.push_back(dist[i]);
	}

/*	for(int i = 0; i < 2 * UWB_NUM_ANCHORS; ++i)
		ROS_WARN("%i, %f", i, msg.data[i]);*/

	UWBLogPublisher.publish(msg);
}

void d_fall_pps::filterDistances(double* distances)
{
Yvan Bosshard's avatar
Yvan Bosshard committed
312
	for (unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
313
	{
Yvan Bosshard's avatar
Yvan Bosshard committed
314
315
316
		updateKalmanState(kalmanDist[i], distances[i]);
		// if(distances[i] != 0)
		// 	updateKalmanState(kalmanDist[i], distances[i]);
317
318
		
		distances[i] = kalmanDist[i].x;
Yvan Bosshard's avatar
Yvan Bosshard committed
319
	}
320
}
321

322
323
324
325
326
void d_fall_pps::initKalmanStates()
{
	// Init x state
	// TODO Update
	xState.x = 0;
Yvan Bosshard's avatar
Yvan Bosshard committed
327
328
	xState.q = KALMAN_P_VARIANCE;
	xState.r = KALMAN_M_VARIANCE;
329
330
331
332
333
	xState.p = 0;

	// Init y state
	// TODO Update
	yState.x = 0;
Yvan Bosshard's avatar
Yvan Bosshard committed
334
335
	yState.q = KALMAN_P_VARIANCE;
	yState.r = KALMAN_M_VARIANCE;
336
337
338
339
340
	yState.p = 0;

	// Init z state
	// TODO Update
	zState.x = 0;
Yvan Bosshard's avatar
Yvan Bosshard committed
341
342
	zState.q = 5e-6; //KALMAN_P_VARIANCE;
	zState.r = 5e-4; //KALMAN_M_VARIANCE;
343
	zState.p = 0;
344

Yvan Bosshard's avatar
Yvan Bosshard committed
345
	for(unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
346
347
	{
		kalmanDist[i].x = 0;
Yvan Bosshard's avatar
Yvan Bosshard committed
348
349
		kalmanDist[i].q = 8;
		kalmanDist[i].r = 500;
350
		kalmanDist[i].p = 0;
Yvan Bosshard's avatar
Yvan Bosshard committed
351
	}
352
}
353

354
355
356
357
358
359
void d_fall_pps::updateKalmanState(KalmanState& state, double measurment)
{
	state.p += state.q;
	state.k = state.p / (state.p + state.r);
	state.x += state.k * (measurment - state.x);
	state.p *= (1 - state.k);
360
361
362
363
364
365
366
367
368
369
370
371
}

// Loads the current context of the client
void d_fall_pps::loadCrazyflieContext()
{
	CMQuery contextCall;
	contextCall.request.studentID = clientID;

	centralManager.waitForExistence(ros::Duration(-1));

	if(centralManager.call(contextCall))
	{
372
373
		context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("[UWBDataPublisher] context updated");
374
375
376
377
378
379
380
381
382
383
384
385
386
	}
}

// Loads the parameters of the node handle
bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
{
	if(!nodeHandle.getParam("clientID", clientID))
	{
		ROS_ERROR("Failed to get clientID!");
		return false;
	}

	return true;
387
388
389
}

// calculatse distances from passed microseconds
390
float d_fall_pps::tiagosAwesomeFormula(uint32_t ticks)
391
{
392
	return ticks * SPEED_OF_LIGHT / LOCO_DECK_FRQ;
393
	//return (time * 75.0 * (1.0 / (128.0 * 499.2)));
394
}