LocalizationServer.cpp 5.75 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
/*
 *
 *	Localization Server
 *
 *	This node is subscribed to the ViconDataPublisher as well
 *	as to the UWBDataPublisher. It is used to relay the data
 *	of the desired localization source to the PPSClient and
 *	can compare the data provided by both.
 *
 */

Yvan Bosshard's avatar
Yvan Bosshard committed
12
#include <chrono>
13
14
15
16
17
18
19
20
#include <stdlib.h>
#include <std_msgs/String.h>
#include <rosbag/bag.h>
#include <ros/package.h>

#include "LocalizationServer.h"

#include "d_fall_pps/CMQuery.h"
21
#include "d_fall_pps/Anchors.h"
22
23
24
25
26
27
28
29
30
31
32
33

#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "std_msgs/Float32.h"

using namespace d_fall_pps;

// the clientID defines namespace and identifier in the CentralManagerService
int clientID;

// Self explanatory variables
ros::ServiceClient centralManager;
34
ros::ServiceClient uwbManager;
35
36
37
38
ros::Publisher localizationPublisher;
CrazyflieContext context;
rosbag::Bag bag;

39
bool enableUWB = false;
40
bool useUWB = false;
Yvan Bosshard's avatar
Yvan Bosshard committed
41

42
// store resent localisation updates
Yvan Bosshard's avatar
Yvan Bosshard committed
43
CrazyflieData CFData_vicon;
44
CrazyflieData CFData_uwb;
michaero's avatar
michaero committed
45

46
47
int main(int argc, char* argv[])
{
48
	// Initialize the node and it's handles
49
50
	ros::init(argc, argv, "LocalizationServer");
	ros::NodeHandle nodeHandle("~");
51
	ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
52
53
54
55
56

	// load context parameters
	if(!loadParameters(nodeHandle))
		return -1;

57
	// connect to the central manager service to receive CF Context and subscribe to CF Context updates of the DB
58
59
	centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
	loadCrazyflieContext();
60
	ros::Subscriber dbChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback);
61

62
	uwbManager = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
63
64
65

	ros::Subscriber enableUWBSubscriber = namespaceNodeHandle.subscribe("student_GUI/uwbChanged", 1, uwbChangedCallback);
	//ros::Subscriber enableUWBSubscriber = nodeHandle.subscribe("/my_GUI/enableUWB", 1, uwbChangedCallback);
66
	loadUWBSettings();
67

68
	// subscribe to the global vicon publisher and local uwb publisher
69
	ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback);
70
	ROS_INFO_STREAM("[LocalizationServer] subscribed to Vicon");
71
72
	ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData", 100, UWBDataCallback);
	ROS_INFO_STREAM("[LocalizationServer] subscribed to UWB");
73

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

76
77
	// initialize the position data publisher
	localizationPublisher = nodeHandle.advertise<CrazyflieData>("LocalizationData", 1);
78
79
80
81
82
83

	std::string package_path;
	package_path = ros::package::getPath("d_fall_pps") + "/";
	std::string record_file = package_path + "log/LocalizationServerLog.bag";
	bag.open(record_file, rosbag::bagmode::Write);

84
85
	ROS_INFO_STREAM("[LocalizationServer] started!");

86
87
88
89
90
91
92
93
94
95
	ros::spin();
	bag.close();

	return 0;
}

bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
{
	if(!nodeHandle.getParam("clientID", clientID))
	{
96
		ROS_ERROR("[LocalizationServer] Failed to get clientID!");
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
		return false;
	}

	return true;
}

void d_fall_pps::loadCrazyflieContext()
{
	CMQuery contextCall;
	contextCall.request.studentID = clientID;

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

	if(centralManager.call(contextCall))
	{
112
113
		context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("[LocalizationServer] context updated");
114
115
116
	}
}

117
118
119
120
121
122
void d_fall_pps::loadUWBSettings()
{
	Anchors a;

	if(uwbManager.call(a))
	{
123
		enableUWB = a.response.enableUWB;
124
125
126
	}
	else
	{
127
		enableUWB = false;
128
129
130
		ROS_ERROR("[LocalizationServer] Could not update UWB Settings!");
	}

131
132
	if(enableUWB)
		ROS_WARN("[LocalizationServer] Use of UWB activated!");
133
	else
134
		ROS_WARN("[LocalizationServer] Use of UWB deactivated!");
135
136
}

Yvan Bosshard's avatar
Yvan Bosshard committed
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
void tiagosPrint(double x, double y, double z)
{
	// Anchor2
	//double anchorPos[3] = {1.89, 0.61, 1.6};

	// Anchor5
	double anchorPos[3] = {-0.76, -1.36, 0.6};

	/*ROS_WARN("Dist to A5: %lf", sqrt(
									(x - anchorPos[0])*(x - anchorPos[0])
									+ (y - anchorPos[1])*(y - anchorPos[1])
									+ (z - anchorPos[2])*(z - anchorPos[2])
								)
	);*/
}

153
154
155
156
157
158
159
160
void d_fall_pps::viconDataCallback(const ViconData &viconData)
{
	for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
	{
		CrazyflieData data = *it;

		if(data.crazyflieName == context.crazyflieName)
		{
161
162
			if(! (enableUWB && useUWB))
				localizationPublisher.publish(data);
163

Yvan Bosshard's avatar
Yvan Bosshard committed
164
			CFData_vicon = data;
165
166
			bag.write("ViconData", ros::Time::now(), data);

Yvan Bosshard's avatar
Yvan Bosshard committed
167

168
169
170
171
			// leave the for-loop so not every vector element has to be compared, once the matching context is found
			break;
		}
	}
Yvan Bosshard's avatar
Yvan Bosshard committed
172
173

	tiagosPrint(CFData_vicon.x, CFData_vicon.y, CFData_vicon.z);
174
175
}

176
177
// Callback function to handle incoming UWB data
void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
178
{
Yvan Bosshard's avatar
Yvan Bosshard committed
179
	// override UWB Data
180
	CFData_uwb = data;
Yvan Bosshard's avatar
Yvan Bosshard committed
181

182
183
	//ROS_WARN("Roll: \tV: %f\tU: %f", CFData_vicon.roll, data.roll);
	//ROS_WARN("E(R): V-U = %f", (CFData_vicon.roll - data.roll) * 180/3.14159);
Yvan Bosshard's avatar
Yvan Bosshard committed
184

185
186
187
	// Uncomment the lines to be overriden by vicon when using UWB
	//CFData_uwb.x = CFData_vicon.x;
	//CFData_uwb.y = CFData_vicon.y;
Yvan Bosshard's avatar
Yvan Bosshard committed
188
	//CFData_uwb.z = CFData_vicon.z;
189
190
	//CFData_uwb.roll = CFData_vicon.roll;
	//CFData_uwb.pitch = CFData_vicon.pitch;
Yvan Bosshard's avatar
Yvan Bosshard committed
191
192
	//CFData_uwb.pitch *= -1;
	//CFData_uwb.yaw = CFData_vicon.yaw;
Yvan Bosshard's avatar
Yvan Bosshard committed
193

194
	if(enableUWB && useUWB)
Yvan Bosshard's avatar
Yvan Bosshard committed
195
	{
Yvan Bosshard's avatar
Yvan Bosshard committed
196
		localizationPublisher.publish(CFData_uwb);
Yvan Bosshard's avatar
Yvan Bosshard committed
197
	}
198
	
Yvan Bosshard's avatar
Yvan Bosshard committed
199
	bag.write("UWBData", ros::Time::now(), CFData_uwb);
200
201
}

202
203
204
void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg)
{
	d_fall_pps::loadCrazyflieContext();
205
206
207
208
}

void d_fall_pps::uwbChangedCallback(const std_msgs::Int32 &msg)
{
209
	useUWB = msg.data;
210
211

	if(useUWB)
212
213
214
215
		ROS_WARN("[LocalizationServer] UWB activated!");
	else
		ROS_WARN("[LocalizationServer] UWB deactivated!");
}