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

UWB Data is overriden by Vicon

parent 2b0195f2
......@@ -15,7 +15,6 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <vector>
//#include <stdint.h>
#include "ros/ros.h"
......
......@@ -9,6 +9,7 @@
*
*/
#include <chrono>
#include <stdlib.h>
#include <std_msgs/String.h>
#include <rosbag/bag.h>
......@@ -34,8 +35,9 @@ CrazyflieContext context;
rosbag::Bag bag;
bool enableUWB = true;
bool useUWB = false;
bool onboardPosition = false;
bool useUWB = true;
CrazyflieData CFData_vicon;
int main(int argc, char* argv[])
{
......@@ -118,8 +120,10 @@ void d_fall_pps::viconDataCallback(const ViconData &viconData)
if(! (enableUWB && useUWB))
localizationPublisher.publish(data);
CFData_vicon = 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;
}
......@@ -129,10 +133,20 @@ void d_fall_pps::viconDataCallback(const ViconData &viconData)
// Callback function to handle incoming UWB data
void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
{
// override UWB Data
CrazyflieData CFData_uwb = data;
CFData_uwb.x = CFData_vicon.x;
CFData_uwb.y = CFData_vicon.y;
CFData_uwb.z = CFData_vicon.z;
CFData_uwb.roll = CFData_vicon.roll;
CFData_uwb.pitch = CFData_vicon.pitch;
CFData_uwb.yaw = CFData_vicon.yaw;
if(enableUWB && useUWB)
localizationPublisher.publish(data);
localizationPublisher.publish(CFData_uwb);
bag.write("UWBData", ros::Time::now(), data);
bag.write("UWBData", ros::Time::now(), CFData_uwb);
}
void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg)
......
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