Commit f735a432 authored by Cyrill Burgener's avatar Cyrill Burgener
Browse files

removed useless outputs, fake testing data

parent 6dc87ccb
......@@ -136,10 +136,6 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
est[4] = (request.ownCrazyflie.y - previousLocation.y) / request.ownCrazyflie.acquiringTime;
est[5] = (request.ownCrazyflie.z - previousLocation.z) / request.ownCrazyflie.acquiringTime;
ROS_INFO_STREAM("vx: " << est[3]);
ROS_INFO_STREAM("vy: " << est[4]);
ROS_INFO_STREAM("vz: " << est[5]);
est[6] = request.ownCrazyflie.roll;
est[7] = request.ownCrazyflie.pitch;
est[8] = request.ownCrazyflie.yaw;
......
......@@ -20,9 +20,6 @@
#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;
......@@ -36,87 +33,6 @@ 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;
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;
......@@ -161,8 +77,6 @@ int main(int argc, char* argv[]) {
unsigned int unlabeledMarkerCount = client.GetUnlabeledMarkerCount().MarkerCount;
UnlabeledMarker marker;
// ROS_INFO_STREAM("unlabeledMarkerCount: " << unlabeledMarkerCount);
for(int unlabeledMarkerIndex = 0; unlabeledMarkerIndex < unlabeledMarkerCount; unlabeledMarkerIndex++)
{
......@@ -230,7 +144,6 @@ int main(int argc, char* argv[]) {
if(!outputTranslation.Occluded) viconData.crazyflies.push_back(cfData);
}
viconDataPublisher.publish(viconData);
ROS_INFO_STREAM("ViconDataPublisher: " << viconData);
}
client.DisableSegmentData();
......@@ -239,6 +152,4 @@ int main(int argc, char* argv[]) {
client.DisableDeviceData();
client.Disconnect();
#endif
}
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