UWBDataPublisher.cpp 6.56 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
#include <stdlib.h>
18
#include <string.h>
19
20
21
22
23
24
25
26

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




27
28
29
30
31
32
33
34
#include "DataStreamClient.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Float32MultiArray.h"
#include "d_fall_pps/CrazyflieData.h"

#include "d_fall_pps/CMQuery.h"

35
#include "UWBDataPublisher.h"
36
37
38

using namespace d_fall_pps;

Yvan Bosshard's avatar
Yvan Bosshard committed
39
40
// This is a test comment to check wtf git does

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

CrazyflieContext context;
rosbag::Bag bag;

47
ros::ServiceClient centralManager;
48
49
ros::Publisher UWBDataPublisher;

50
double rollPitchYaw[3];
51

52
53
bool onboardPosition = false;
bool useTiagoData = true;
michaero's avatar
michaero committed
54

55
56
UWBAnchorArray anchors;

57
58
59
60
int main(int argc, char* argv[])
{
    ros::init(argc, argv, "UWBDataPublisher");
    ros::NodeHandle nodeHandle("~");
61
62
    ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
    //ros::Time::init();
63
64

    if(!loadParameters(nodeHandle))
65
    	return -1;
66

67
    centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
	loadCrazyflieContext();
	ros::Subscriber dbChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback);

	// Subscribe to the anchor positions and all relevant Data from crazyradio
	//ros::Subscriber anchorPosSubscriber = namespaceNodeHandle.subscribe("/TeacherService/anchorPos",1,anchorPosCallback);
	ros::Subscriber anchorPosSubscriber = nodeHandle.subscribe("/TeacherService/anchorPos",1,anchorPosCallback);
	ros::Subscriber cfRPYSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/RPY",10,cfRPYCallback);
	ros::Subscriber uwbLocationSubscriber;

	if(onboardPosition)
		uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/XYZ",10,cfXYZCallback);
	else
	{
		if(useTiagoData)
			uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/times",10,cfTimesCallback);
		else
			uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/distances",10,cfDistancesCallback);
	}
Yvan Bosshard's avatar
Yvan Bosshard committed
86

87
	// TODO: need to add a switch or something in a gui (teacher or student) with a publisher, to be subscribed here
88

89
	UWBDataPublisher = nodeHandle.advertise<CrazyflieData>("UWBData", 1);
90

91
92
93
    std::string package_path;
	package_path = ros::package::getPath("d_fall_pps") + "/";
	std::string record_file = package_path + "log/UWBDataPublisherLog.bag";
94
95
    bag.open(record_file, rosbag::bagmode::Write);

96
    ROS_INFO_STREAM("[UWBDataPublisher] started!");
97

98
    ros::spin();
99
100
101
102
103
    bag.close();
    
    return 0;
}

104
105
106
107
108
void d_fall_pps::dbChangedCallback(const std_msgs::Int32& msg)
{
	d_fall_pps::loadCrazyflieContext();
}

109
110
111
//Callback function to handle roll pitch yaw information from CF
void d_fall_pps::cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY)
{
112
113
114
	rollPitchYaw[0] = arrRPY.data[0];
	rollPitchYaw[1] = arrRPY.data[1];
	rollPitchYaw[2] = arrRPY.data[2];
115
116
117
}

//Callback funtion to hande x y z information from CF
118
void d_fall_pps::cfXYZCallback(const std_msgs::Float32MultiArray &arrXYZ)
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
{
	CrazyflieData data;

	data.crazyflieName = context.crazyflieName;
	data.x = (double)arrXYZ.data[0];
	data.y = (double)arrXYZ.data[1];
	data.z = (double)arrXYZ.data[2];
	data.roll = rollPitchYaw[0];
	data.pitch = rollPitchYaw[1];
	data.yaw = rollPitchYaw[2];
	data.occluded = false;
	data.acquiringTime = 0; //???

	UWBDataPublisher.publish(data);
	
}

//Callback function to handle distance information from CF
137
void d_fall_pps::cfDistancesCallback(const std_msgs::Float32MultiArray &distances)
138
{
139
	CrazyflieData data;
140
    
141
    double arrXYZ[3] = {0};
142

143
	calculateXYZ(distances.data, distances.layout.dim.size(), arrXYZ);   //TODO: Implement!!!
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
	
	data.crazyflieName = context.crazyflieName;
	data.x = arrXYZ[0];
	data.y = arrXYZ[1];
	data.z = arrXYZ[2];
	data.roll = rollPitchYaw[0];
	data.pitch = rollPitchYaw[1];
	data.yaw = rollPitchYaw[2];
	data.occluded = false;
	data.acquiringTime = 0; //???
	
	UWBDataPublisher.publish(data);
	
}

159
160
161
162
163
164
165
166
// Callback functions to handle times information from CF
void d_fall_pps::cfTimesCallback(const std_msgs::UInt32MultiArray &times)
{
	CrazyflieData data;
    
    double arrXYZ[3] = {0};
    std::vector<float> distances;

167
168
    for (int i = 0; i < UWB_NUM_ANCHORS; i++)
    	distances.push_back(tiagosAwesomeFormula(times.data[i]));
169

170
	calculateXYZ(distances, arrXYZ);   //TODO: Implement!!!
171
172
173
174
175
176
177
178
179
180
181
182
183
184
	
	data.crazyflieName = context.crazyflieName;
	data.x = arrXYZ[0];
	data.y = arrXYZ[1];
	data.z = arrXYZ[2];
	data.roll = rollPitchYaw[0];
	data.pitch = rollPitchYaw[1];
	data.yaw = rollPitchYaw[2];
	data.occluded = false;
	data.acquiringTime = 0; //???
	
	UWBDataPublisher.publish(data);
}

185
//calculates position from anchor locations and distances -> forces algorithm?
186
void d_fall_pps::calculateXYZ(const std::vector<float> &distances, double(&xyz)[3])
187
188
189
{
    //add algorithm from forces - paul

190
191


192
193
194
195
196
197
198
199
200
201
202
203
204
    return;
}

// 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))
	{
205
206
		context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("[UWBDataPublisher] context updated");
207
208
209
210
211
212
213
214
215
216
217
218
219
	}
}

// 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;
220
221
222
223
224
}

// calculatse distances from passed microseconds
float d_fall_pps::tiagosAwesomeFormula(uint32_t time)
{
225
226
	return time * SPEED_OF_LIGHT / LOCO_DECK_FRQ;
	//return (time * 75.0 * (1.0 / (128.0 * 499.2)));
227
228
229
230
231
232
233
234
}

// Callback function to handle new anchor positions
void d_fall_pps::anchorPosCallback(const UWBAnchorArray &newAnchors)
{
	anchors = newAnchors;

	return;
235
}