Commit 1963c211 authored by michaero's avatar michaero
Browse files

Merge branch 'uwbmaster' of https://gitlab.ethz.ch/D-FaLL/PandS-System/D-FaLL-System into uwbmaster

parents 1ffcda7e 2b0195f2
/*
*
* UWB Data Validator
* Localization Server
*
* This node is subscribed to the ViconDataPublisher as well
* as to the UWBDataPublisher. It is used to compare the
......
// 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
<launch>
<node pkg="d_fall_pps" name="UWBAnalysis" output="screen" type="UWBAnalysis">
<param name="clientID" value="$(optenv ROS_NAMESPACE)" />
</node>
</launch>
......@@ -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[])
......
// 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);
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment