Commit a87a94b8 authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

Data source published from LocalizationServer is now changeable

parent 37685fef
......@@ -53,7 +53,7 @@ namespace d_fall_pps
bool loadParameters(ros::NodeHandle &nodeHandle);
// calculates coordinates from input distances
void calculateXYZ(const std::vector<float> &distances, int distLen, double(&xyz)[3]);
void calculateXYZ(const std::vector<float> &distances, double(&xyz)[3]);
// Callback function to handle times information from CF
void cfTimesCallback(const std_msgs::UInt32MultiArray &times);
......
......@@ -34,7 +34,7 @@ CrazyflieContext context;
rosbag::Bag bag;
bool enableUWB = true;
bool usingUWB = false;
bool useUWB = false;
bool onboardPosition = false;
int main(int argc, char* argv[])
......@@ -57,8 +57,11 @@ int main(int argc, char* argv[])
// subscribe to the global vicon publisher and local uwb publisher
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback);
ROS_INFO_STREAM("[LocalizationServer] subscribed to Vicon");
ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData",100,UWBDataCallback);
ROS_INFO_STREAM("[LocalizationServer] subscribed to UWB");
if(enableUWB)
{
ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData",100,UWBDataCallback);
ROS_INFO_STREAM("[LocalizationServer] subscribed to UWB");
}
//TODO: need to add a switch or something in a gui (teacher or student) with a publisher, to be subscribed here
......@@ -111,7 +114,9 @@ void d_fall_pps::viconDataCallback(const ViconData &viconData)
if(data.crazyflieName == context.crazyflieName)
{
localizationPublisher.publish(data);
if(! (enableUWB && useUWB))
localizationPublisher.publish(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
......@@ -123,7 +128,9 @@ void d_fall_pps::viconDataCallback(const ViconData &viconData)
// Callback function to handle incoming UWB data
void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
{
localizationPublisher.publish(data);
if(enableUWB && useUWB)
localizationPublisher.publish(data);
bag.write("UWBData", ros::Time::now(), data);
}
......
......@@ -140,7 +140,7 @@ void d_fall_pps::cfDistancesCallback(const std_msgs::Float32MultiArray &distance
double arrXYZ[3] = {0};
calculateXYZ(distances.data, distances.layout.dim.size(), arrXYZ); //TODO: Implement!!!
calculateXYZ(distances.data, arrXYZ); //TODO: Implement!!!
data.crazyflieName = context.crazyflieName;
data.x = arrXYZ[0];
......
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