UWBAnalysis.cpp 4.99 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
//    ROS node that can be used to log vicon and uwb data for analysis.
//    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/>.

#include <chrono>
#include <iostream>
#include <string>

#include "ros/ros.h"

#include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/CrazyflieContext.h"

#include "UWBAnalysis.h"

using namespace d_fall_pps;

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

ros::ServiceClient centralManager;
CrazyflieContext context;

36
37
38
39
40
CrazyflieData CFData_vicon;
bool vicon_started = false;
CrazyflieData CFData_uwb;
bool uwb_started = false;

41
42
43
44
45
46
47
48
49
50
51
52
53
54
std::string logPath = "/home/uwbclient01/Log/";
std::ofstream viconFile;
std::ofstream uwbFile;

std::chrono::high_resolution_clock::time_point startTime;

int main(int argc, char* argv[])
{
	ros::init(argc, argv, "UWBAnalysis");

	ros::NodeHandle nodeHandle("~");
	ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();

	// load context parameters
55
	if (!loadParameters(nodeHandle))
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
		return -1;

	centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
	loadCrazyflieContext();
	ros::Subscriber dbChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback);

	prepareLogs();

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

	ROS_INFO_STREAM("[UWBAnalysis] Logging started!");

72
	/*ros::Rate rate(25);	// 50 Hz
73
74
75
76
77
78
79
80
81
82
83
84
85

	while (ros::ok())
	{
		if (uwb_started && vicon_started)
		{
			std::chrono::high_resolution_clock::time_point cbTime = std::chrono::high_resolution_clock::now();
			long timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(cbTime - startTime).count();
			writeToLog(viconFile, timestamp, CFData_vicon);
			writeToLog(uwbFile, timestamp, CFData_uwb);
		}

		ros::spinOnce();
		rate.sleep();
86
87
88
	}*/

	ros::spin();
89
90
91
92
93
94
95
96

	ROS_WARN("[UWBAnalysis] Logging stoped!");

	return 0;
}

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

	return true;
}

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

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

113
	if (centralManager.call(contextCall))
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
	{
		context = contextCall.response.crazyflieContext;
		ROS_WARN("[UWBAnalysis] context updated");
	}
}

void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg)
{
	d_fall_pps::loadCrazyflieContext();
}

void d_fall_pps::prepareLogs()
{
	startTime = std::chrono::high_resolution_clock::now();

	viconFile.open(logPath + "Vicon.vlog", std::ofstream::out | std::ofstream::trunc);
130
	if (!viconFile)
131
132
133
134
135
136
	{
		ROS_ERROR("Error opening Vicon log");
		return;
	}

	uwbFile.open(logPath + "UWB.vlog", std::ofstream::out | std::ofstream::trunc);
137
	if (!uwbFile)
138
139
140
141
142
143
144
145
146
147
	{
		ROS_ERROR("Error opening UWB log");
		return;
	}

	ROS_INFO_STREAM("[UWBAnalysis] Log files prepared.");
}

void d_fall_pps::writeToLog(std::ofstream& file, long timestamp, const CrazyflieData& data)
{
148
149
	file << timestamp << ", " << data.x << ", " << data.y << ", " << data.z << ", "
		<< data.roll << ", " << data.pitch << ", " << data.yaw << "\n";
150
151
152
153
154
	file.flush();
}

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

159
		if (data.crazyflieName == context.crazyflieName)
160
		{
161
162
			CFData_vicon = data;
			vicon_started = true;
163
164
165
166
167
168
169
170
			// leave the for-loop so not every vector element has to be compared, once the matching context is found
			break;
		}
	}
}

void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
{
171
172
	CFData_uwb = data;
	uwb_started = true;
173
174
175
176
177

	std::chrono::high_resolution_clock::time_point cbTime = std::chrono::high_resolution_clock::now();
	long timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(cbTime - startTime).count();
	writeToLog(viconFile, timestamp, CFData_vicon);
	writeToLog(uwbFile, timestamp, CFData_uwb);
178
}