From ef5fafa668d1f6b6dcd749935172301158966afb Mon Sep 17 00:00:00 2001
From: roangel <roangel@student.ethz.ch>
Date: Mon, 14 Aug 2017 14:31:29 +0200
Subject: [PATCH] Discovered that we crash when we press multiple times
loadfromDB button in GUI. It also doesn't work properly
---
pps_ws/src/d_fall_pps/param/Crazyflie.db | 1 +
.../src/d_fall_pps/src/ViconDataPublisher.cpp | 86 +++++++++++++++++++
2 files changed, 87 insertions(+)
diff --git a/pps_ws/src/d_fall_pps/param/Crazyflie.db b/pps_ws/src/d_fall_pps/param/Crazyflie.db
index e69de29b..537875bc 100644
--- a/pps_ws/src/d_fall_pps/param/Crazyflie.db
+++ b/pps_ws/src/d_fall_pps/param/Crazyflie.db
@@ -0,0 +1 @@
+1,CF2,123,0,0.04,0.24,-0.2,1.14,1.81,2
diff --git a/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp b/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp
index 9058b60e..ec636e50 100755
--- a/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp
+++ b/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp
@@ -20,6 +20,8 @@
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/UnlabeledMarker.h"
+#define TESTING_FAKE_DATA
+
// notice that unit here are in milimeters
using namespace ViconDataStreamSDK::CPP;
using namespace d_fall_pps;
@@ -33,6 +35,88 @@ int main(int argc, char* argv[]) {
ros::Publisher viconDataPublisher =
nodeHandle.advertise<ViconData>("ViconData", 1);
+
+ #ifdef TESTING_FAKE_DATA
+ // Test faking data part
+ float f = 0;
+ int i = 0;
+
+ ROS_INFO("TESTING_FAKE_DATA.................................");
+ while(ros::ok())
+ {
+ if(i % 1000 == 0)
+ {
+ ROS_INFO("iteration #%d",i);
+ }
+
+ // Testing piece of code
+ ViconData viconData;
+ UnlabeledMarker marker;
+
+ marker.index = 0;
+ marker.x = f;
+ marker.y = 0;
+ marker.z = 0;
+
+ viconData.markers.push_back(marker);
+
+
+ marker.index = 1;
+ marker.x = 0;
+ marker.y = f;
+ marker.z = 0;
+
+ viconData.markers.push_back(marker);
+
+ if(i > 50 && i < 100)
+ {
+ marker.index = 2;
+ marker.x = f;
+ marker.y = f;
+ marker.z = 0;
+ viconData.markers.push_back(marker);
+ }
+
+ ros::Duration(0.1).sleep();
+ f += 10/1000.0f;
+ i++;
+ // TODO: Fake CF data
+ CrazyflieData crazyfly;
+
+ crazyfly.occluded = false;
+
+ crazyfly.crazyflieName = "CF1";
+ crazyfly.x = 0;
+ crazyfly.y = 0;
+ crazyfly.z = 0;
+ crazyfly.yaw = 3.14159 * f;
+ viconData.crazyflies.push_back(crazyfly);
+
+ crazyfly.crazyflieName = "CF2";
+ crazyfly.x = 1;
+ crazyfly.y = 1;
+ crazyfly.z = 0;
+ crazyfly.yaw = -3.14159 * f;
+ viconData.crazyflies.push_back(crazyfly);
+
+ crazyfly.crazyflieName = "CF3";
+ crazyfly.x = 1;
+ crazyfly.y = -1;
+ crazyfly.z = 0;
+ crazyfly.yaw = -3.14159 * f;
+
+
+ if(i > 50 && i < 200)
+ {
+ crazyfly.occluded = true;
+ }
+
+ viconData.crazyflies.push_back(crazyfly);
+
+ viconDataPublisher.publish(viconData); // testing data
+ }
+ #else
+
Client client;
std::string hostName;
@@ -152,4 +236,6 @@ int main(int argc, char* argv[]) {
client.DisableDeviceData();
client.Disconnect();
+ #endif
+
}
--
GitLab