LocalizationServer.cpp 5.98 KB
Newer Older
flashingx's avatar
flashingx committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
//    This ROS node is subscribed to the ViconDataPublisher as well as
//	  to the UWBDataPublisher. It is used to relay the data from the
//	  desired localisation source to the controller and the GUI.
//
//    Copyright (C) 2018
//          Yvan Bosshard           byvan       @ee.ethz.ch
//          Michael Rogenmoser      michaero    @ee.ethz.ch
//          Tiago Salzmann          tiagos      @ee.ethz.ch
//
//    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/>.
22

Yvan Bosshard's avatar
Yvan Bosshard committed
23
#include <chrono>
24
25
26
27
28
29
30
#include <stdlib.h>
#include <std_msgs/String.h>
#include <ros/package.h>

#include "LocalizationServer.h"

#include "d_fall_pps/CMQuery.h"
31
#include "d_fall_pps/Anchors.h"
32
33
34
35
36
37
38
39
40
41
42
43

#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;
44
ros::ServiceClient uwbManager;
45
46
47
ros::Publisher localizationPublisher;
CrazyflieContext context;

48
bool enableUWB = false;
49
bool useUWB = false;
Yvan Bosshard's avatar
Yvan Bosshard committed
50

51
// store resent localisation updates
Yvan Bosshard's avatar
Yvan Bosshard committed
52
CrazyflieData CFData_vicon;
53
CrazyflieData CFData_uwb;
michaero's avatar
michaero committed
54

flashingx's avatar
flashingx committed
55
56
57
/************************************************************
 *	Main			  										*
 ***********************************************************/
58
59
int main(int argc, char* argv[])
{
60
	// Initialize the node and it's handles
61
62
	ros::init(argc, argv, "LocalizationServer");
	ros::NodeHandle nodeHandle("~");
63
	ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
64
65
66
67
68

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

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

74
	uwbManager = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
75
76

	ros::Subscriber enableUWBSubscriber = namespaceNodeHandle.subscribe("student_GUI/uwbChanged", 1, uwbChangedCallback);
77
	loadUWBSettings();
78

79
	// subscribe to the global vicon publisher and local uwb publisher
80
	ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback);
81
	ROS_INFO_STREAM("[LocalizationServer] subscribed to Vicon");
82
83
	ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData", 100, UWBDataCallback);
	ROS_INFO_STREAM("[LocalizationServer] subscribed to UWB");
84

85
86
	// initialize the position data publisher
	localizationPublisher = nodeHandle.advertise<CrazyflieData>("LocalizationData", 1);
87

88
89
	ROS_INFO_STREAM("[LocalizationServer] started!");

90
91
92
93
94
	ros::spin();

	return 0;
}

flashingx's avatar
flashingx committed
95
96
97
98

/************************************************************
 *	Functions			  									*
 ***********************************************************/
99
100
101
102
bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
{
	if(!nodeHandle.getParam("clientID", clientID))
	{
103
		ROS_ERROR("[LocalizationServer] Failed to get clientID!");
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
		return false;
	}

	return true;
}

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

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

	if(centralManager.call(contextCall))
	{
119
120
		context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("[LocalizationServer] context updated");
121
122
123
	}
}

124
125
126
127
128
129
void d_fall_pps::loadUWBSettings()
{
	Anchors a;

	if(uwbManager.call(a))
	{
130
		enableUWB = a.response.enableUWB;
131
132
133
	}
	else
	{
134
		enableUWB = false;
135
136
137
		ROS_ERROR("[LocalizationServer] Could not update UWB Settings!");
	}

138
139
	if(enableUWB)
		ROS_WARN("[LocalizationServer] Use of UWB activated!");
140
	else
141
		ROS_WARN("[LocalizationServer] Use of UWB deactivated!");
142
143
}

Yvan Bosshard's avatar
Yvan Bosshard committed
144

flashingx's avatar
flashingx committed
145
146
147
/************************************************************
 *	Callbacks		  										*
 ***********************************************************/
148
149
150
151
152
153
154
155
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)
		{
flashingx's avatar
flashingx committed
156
			// if desired, publish Vicon data
157
158
			if(! (enableUWB && useUWB))
				localizationPublisher.publish(data);
159

Yvan Bosshard's avatar
Yvan Bosshard committed
160
161
			CFData_vicon = data;

162
163
164
165
			// leave the for-loop so not every vector element has to be compared, once the matching context is found
			break;
		}
	}
flashingx's avatar
flashingx committed
166
}
Yvan Bosshard's avatar
Yvan Bosshard committed
167

flashingx's avatar
flashingx committed
168
169
170
void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg)
{
	d_fall_pps::loadCrazyflieContext();
171
172
}

173
void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
174
{
Yvan Bosshard's avatar
Yvan Bosshard committed
175
	// override UWB Data
176
	CFData_uwb = data;
Yvan Bosshard's avatar
Yvan Bosshard committed
177

flashingx's avatar
flashingx committed
178
179
	// For the sake of nostalgia, the following lines
	// were kept in the code
Yvan Bosshard's avatar
Yvan Bosshard committed
180

181
182
183
	// 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
184
	//CFData_uwb.z = CFData_vicon.z;
185
186
	//CFData_uwb.roll = CFData_vicon.roll;
	//CFData_uwb.pitch = CFData_vicon.pitch;
flashingx's avatar
flashingx committed
187
	//CFData_uwb.pitch *= -1; // (deprecated)
Yvan Bosshard's avatar
Yvan Bosshard committed
188
	//CFData_uwb.yaw = CFData_vicon.yaw;
Yvan Bosshard's avatar
Yvan Bosshard committed
189

flashingx's avatar
flashingx committed
190
	// if desired, publish UWB data
191
	if(enableUWB && useUWB)
Yvan Bosshard's avatar
Yvan Bosshard committed
192
	{
Yvan Bosshard's avatar
Yvan Bosshard committed
193
		localizationPublisher.publish(CFData_uwb);
Yvan Bosshard's avatar
Yvan Bosshard committed
194
	}
195
196
197
198
}

void d_fall_pps::uwbChangedCallback(const std_msgs::Int32 &msg)
{
199
	useUWB = msg.data;
200
201

	if(useUWB)
202
203
204
205
		ROS_WARN("[LocalizationServer] UWB activated!");
	else
		ROS_WARN("[LocalizationServer] UWB deactivated!");
}