Added rosnode and launch file to log vicon and uwb data to compare

pps_ws/src/d_fall_pps/include/LocalizationServer.h

 /*
  *
- *	UWB Data Validator
+ *	Localization Server
  *
  *	This node is subscribed to the ViconDataPublisher as well
  *	as to the UWBDataPublisher. It is used to compare the

pps_ws/src/d_fall_pps/include/UWBAnalysis.h

+//    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/>.
+
+#ifndef UWBANALYSIS_H
+#define UWBANALYSIS_H
+
+#include <fstream>
+
+#include "std_msgs/Int32.h"
+
+#include "d_fall_pps/ViconData.h"
+#include "d_fall_pps/CrazyflieData.h"
+
+namespace d_fall_pps
+{
+	// Loads the parameters of the node handle
+	bool loadParameters(ros::NodeHandle &nodeHandle);
+
+	// Loads the current context of the client
+	void loadCrazyflieContext();
+
+	// Callback function to handle the change in database information
+	void dbChangedCallback(const std_msgs::Int32 &msg);
+
+	// prepares logs
+	void prepareLogs();
+
+	void writeToLog(std::ofstream& file, long timestamp, const CrazyflieData& data);
+
+	// Callback function to handle incoming Vicon data
+	void viconDataCallback(const ViconData &viconData);
+
+	// Callback function to handle incoming UWB data
+	void UWBDataCallback(const CrazyflieData &data);
+}
+
+#endif // UWBANALYSIS_H included

pps_ws/src/d_fall_pps/launch/UWBAnalysis.launch

+<launch>
+
+	<node pkg="d_fall_pps" name="UWBAnalysis" output="screen" type="UWBAnalysis">
+		<param name="clientID" value="$(optenv ROS_NAMESPACE)" />
+	</node>
+
+</launch>

pps_ws/src/d_fall_pps/src/LocalizationServer.cpp

@@ -34,7 +34,7 @@ CrazyflieContext context;
 rosbag::Bag bag;
 
 bool enableUWB = true;
-bool useUWB = true;
+bool useUWB = false;
 bool onboardPosition = false;
 
 int main(int argc, char* argv[])

pps_ws/src/d_fall_pps/src/UWBAnalysis.cpp

+//    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;
+
+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
+	if(!loadParameters(nodeHandle))
+		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!");
+
+	ros::spin();
+
+	ROS_WARN("[UWBAnalysis] Logging stoped!");
+
+	return 0;
+}
+
+bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
+{
+	if(!nodeHandle.getParam("clientID", clientID))
+	{
+		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));
+
+	if(centralManager.call(contextCall))
+	{
+		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();
+
+	//std::string viconPath(logPath);
+	//std::string uwbPath(logPath);
+	//viconPath.append("Vicon.vlog");
+	//uwbPath.append("UWB.vlog")
+
+	viconFile.open(logPath + "Vicon.vlog", std::ofstream::out | std::ofstream::trunc);
+	if(!viconFile)
+	{
+		ROS_ERROR("Error opening Vicon log");
+		return;
+	}
+
+	uwbFile.open(logPath + "UWB.vlog", std::ofstream::out | std::ofstream::trunc);
+	if(!uwbFile)
+	{
+		ROS_ERROR("Error opening UWB log");
+		return;
+	}
+
+	ROS_INFO_STREAM("[UWBAnalysis] Log files prepared.");
+
+
+
+	//auto end = std::chrono::high_resolution_clock::now();
+
+	//long int startTime = std::chrono::duration_cast<std::chrono::nanoseconds>(end - 0).count();
+}
+
+void d_fall_pps::writeToLog(std::ofstream& file, long timestamp, const CrazyflieData& data)
+{
+	file << timestamp << ", " << data.x << ", " << data.y << ", " << data.z << "\n";
+	file.flush();
+}
+
+void d_fall_pps::viconDataCallback(const ViconData &viconData)
+{
+	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();
+
+	for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
+	{
+		CrazyflieData data = *it;
+
+		if(data.crazyflieName == context.crazyflieName)
+		{
+			//ROS_ERROR("UWBAnalysis received CB @ %li", timestamp);
+			d_fall_pps::writeToLog(viconFile, timestamp, data);
+			
+			//bag.write("ViconData", ros::Time::now(), data);
+
+			// 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)
+{
+	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();
+
+	d_fall_pps::writeToLog(uwbFile, timestamp, data);
+}