Commit 838b70c0 authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

Data from both systems is simultaneously logged

parent 1bd2a187
......@@ -33,6 +33,11 @@ int clientID;
ros::ServiceClient centralManager;
CrazyflieContext context;
CrazyflieData CFData_vicon;
bool vicon_started = false;
CrazyflieData CFData_uwb;
bool uwb_started = false;
std::string logPath = "/home/uwbclient01/Log/";
std::ofstream viconFile;
std::ofstream uwbFile;
......@@ -47,7 +52,7 @@ int main(int argc, char* argv[])
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
// load context parameters
if(!loadParameters(nodeHandle))
if (!loadParameters(nodeHandle))
return -1;
centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
......@@ -64,7 +69,21 @@ int main(int argc, char* argv[])
ROS_INFO_STREAM("[UWBAnalysis] Logging started!");
ros::spin();
ros::Rate rate(25); // 50 Hz
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();
}
ROS_WARN("[UWBAnalysis] Logging stoped!");
......@@ -73,7 +92,7 @@ int main(int argc, char* argv[])
bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
{
if(!nodeHandle.getParam("clientID", clientID))
if (!nodeHandle.getParam("clientID", clientID))
{
ROS_ERROR("[UWBAnalysis] Failed to get clientID!");
return false;
......@@ -89,7 +108,7 @@ void d_fall_pps::loadCrazyflieContext()
centralManager.waitForExistence(ros::Duration(-1));
if(centralManager.call(contextCall))
if (centralManager.call(contextCall))
{
context = contextCall.response.crazyflieContext;
ROS_WARN("[UWBAnalysis] context updated");
......@@ -105,56 +124,40 @@ 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)
if (!viconFile)
{
ROS_ERROR("Error opening Vicon log");
return;
}
uwbFile.open(logPath + "UWB.vlog", std::ofstream::out | std::ofstream::trunc);
if(!uwbFile)
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 << timestamp << ", " << data.x << ", " << data.y << ", " << data.z << ", "
<< data.roll << ", " << data.pitch << ", " << data.yaw << "\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)
for (std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
{
CrazyflieData data = *it;
if(data.crazyflieName == context.crazyflieName)
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);
CFData_vicon = data;
vicon_started = true;
// leave the for-loop so not every vector element has to be compared, once the matching context is found
break;
}
......@@ -163,8 +166,6 @@ void d_fall_pps::viconDataCallback(const ViconData &viconData)
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);
CFData_uwb = data;
uwb_started = true;
}
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