Merge branch 'getDataFromCF' into 'uwbmaster'

UWB Data in ROS

See merge request D-FaLL/PandS-System/D-FaLL-System!4

.vscode/settings.json

+{
+    "files.associations": {
+        "iterator": "cpp",
+        "string": "cpp",
+        "string_view": "cpp",
+        "vector": "cpp"
+    }
+}
\ No newline at end of file

pps_wiki/installation.md

@@ -8,7 +8,7 @@ Installation with the install script is the easiest. You will need:
 - the ``d_fall_pps`` package compressed in a file called ``package.tar.gz``
 - the rule files for the USB connection to the crazyradio, called ``99-crazyflie.rules`` and ``99-crazyradio.rules``
 
-These files all need to be in the same directory. To run the installation, move to the containing directory and call it with
+These files all need to be in the same directory. To run the installation, move to the containing directory (pps\ install) and call it with
 ```
 ./pps_install.sh <student id>
 ```
@@ -27,7 +27,7 @@ Create a new catkin workspace and copy the ``d_fall_pps`` package into the ``src
 Add a new line in the ``/etc/hosts`` file that links the teacher's IP with the domain name ``teacher`` and create a file called ``/etc/StudentID`` that contains the student id. Only write digits without any other symbols or whitespace characters.
 
 - USB Crazyradio: <br />
-To set up the crazyradio USB dongle just copy the rule files ``99-crazyflie.rules`` and ``99-crazyradio.rules`` to the directory ``/etc/udev/rules.d``.
+To set up the crazyradio USB dongle just copy the rule files ``99-crazyflie.rules`` and ``99-crazyradio.rules`` from directory ``pps\ install/`` to the directory ``/etc/udev/rules.d``.
 You also have to install the library pyusb:
 
 ```
@@ -43,10 +43,12 @@ sudo pip install pyusb
 ```
 
 - Source scripts in ``.bashrc``: <br />
-You need to source the following scripts in the ``.bashrc`` file:
-  - the ROS setup script: ``/opt/ros/kinetic/setup.bash``
-  - the workspace setup script: ``<catkin workspace>/devel/setup.bash``
-  - the student setup script: ``<catkin workspace>/src/d_fall_pps/launch/Config.sh``
+Add following lines to the bottom of the file ``~/.bashrc`` (replace ``<catkin workspace>`` with correct directory)
+```
+source /opt/ros/kinetic/setup.bash
+source <catkin workspace>/devel/setup.bash
+source <catkin workspace>/src/d_fall_pps/launch/Config.sh
+```
 
 The workspace setup script will only appear after the first compilation of the catkin workspace.
 
@@ -64,6 +66,9 @@ must be replaced with
 export ROS_HOSTNAME=teacher
 ```
 
+### IP-Addresses
+Currently the teacher's IP is ``10.42.0.10`` and the student's IP are of the format ``10.42.0.xx``, where xx is an unused address.
+
 ### Installation of cfclient
 The steps to install the crazyflie client are taken from [here](https://github.com/bitcraze/crazyflie-clients-python). To install the cfclient you need to install its dependencies:
 ```

pps_wiki/setup.md

@@ -77,18 +77,23 @@ Then go to the _IPv4 Settings_ and choose **Manual** as the _Method_ and then ad
 **Don't forget to choose the Vicon network manually if you choose to enable WiFi and Ethernet simultaneously as described below!**
 
 <br>
+
 ### Vicon, teacher and students
 During installation process is the IP address of the teacher set to 10.42.0.10. (This value is written to the /etc/hosts file such that this IP address is accessible through the keyword _teacher_) <br>
 Have a look at `Config.sh` in `~/pps_ws/src/d_fall_pps/launch/`
-<br><img src="./pics/setup_pics/configsh1.png" style="width: 500px;"/> <br>
+<br><img src="./pics/setup_pics/configsh2.png" style="width: 500px;"/> <br>
 Here you see, that the ROS Master URI is set to be the teacher. This means that _roscore_ runs only on the teacher's computer. Your own IP address (_ROS IP_) is also set and taken from your Ethernet settings as defined in the section _Setting up the Vicon network_.
 <br>
+
+### IP-Addresses
+Currently the teacher's IP is ``10.42.0.10`` and the student's IP are of the format ``10.42.0.xx``, where xx is an unused address.
+
 ### Using WLAN and Vicon simultaneously
 Without some adjustments it is not possible to use an Ethernet and a wireless network at the same time. Vicon is connected via cable and therefore wouldn't allow a connection to the Internet. Because it's tedious to always unplug the cable just to be able to look something up on Google, we provide an explanation on how to enable simultaneous usage of WLAN and Vicon. <br>
 **But careful: If you enable this setting you have to choose the Vicon network manually. This is a common mistake! You will get an error if the cable is inserted but not manually chosen as shown in the picture below.**
 <br><img src="./pics/setup_pics/chooseVicon.jpg" style="width: 500px;"/> <br>
 #### Step by Step:
-Ubuntu allows multiple connections by default, but sometimes, we need to specify which one to use. In your comments, you have mentioned that you use LAN for the Intranet and WiFi for the Internet.
+Ubuntu allows multiple connections by default, but sometimes, we need to specify which one to use. Here we use LAN for the Intranet and WiFi for the Internet.
 
 So, firstly search for Network Connections in the unity dash. Then, under the Ethernet section, click 'Add' button.
 <br><img src="./pics/setup_pics/internetethernet1.png" style="width: 500px;"/> <br>

pps_ws/src/d_fall_pps/CMakeLists.txt

@@ -133,6 +133,8 @@ add_message_files(
   Setpoint.msg
   CrazyflieEntry.msg
   CrazyflieDB.msg
+  UWBAnchor.msg
+  UWBAnchorArray.msg
   #----------------------------------------------------------------------
   DebugMsg.msg
 )
@@ -235,6 +237,7 @@ include_directories(
 ## The recommended prefix ensures that target names across packages don't collide
 # add_executable(${PROJECT_NAME}_node src/d_fall_pps_node.cpp)
 add_executable(ViconDataPublisher src/ViconDataPublisher.cpp)
+add_executable(UWBDataPublisher src/UWBDataPublisher.cpp)
 add_executable(TeacherService src/TeacherService.cpp)
 add_executable(LocalizationServer src/LocalizationServer.cpp)
 add_executable(PPSClient src/PPSClient.cpp)
@@ -287,6 +290,7 @@ qt5_use_modules(student_GUI Widgets)
 
 add_dependencies(ViconDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(LocalizationServer d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(UWBDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(TeacherService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(Client d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -326,6 +330,8 @@ target_link_libraries(ViconDataPublisher ${VICON_LIBRARY})
 target_link_libraries(LocalizationServer ${catkin_LIBRARIES})
 target_link_libraries(TeacherService ${catkin_LIBRARIES})
 
+target_link_libraries(UWBDataPublisher ${catkin_LIBRARIES})
+
 target_link_libraries(PPSClient ${catkin_LIBRARIES})
 
 target_link_libraries(Client ${catkin_LIBRARIES})

pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py

@@ -32,6 +32,8 @@ import rosbag
 from rospkg import RosPack
 from std_msgs.msg import Float32
 from std_msgs.msg import String
+from std_msgs.msg import Float32MultiArray
+from std_msgs.msg import UInt32MultiArray
 
 # Add library
 #sys.path.append("lib")
@@ -153,23 +155,56 @@ class PPSRadioClient:
         self._cf.close_link()
         self.change_status_to(DISCONNECTED)
 
-    def _data_received_callback(self, timestamp, data, logconf):
+    def _anchorTime_received_callback(self, timestamp, data, anchorTime):
+        times = UInt32MultiArray()
+        times.data = [data["UWB_Anchor_Pos.anchor0"], data["UWB_Anchor_Pos.anchor1"]]
+
+        #For debug, to confirm times arrive...
+        print "dist0: %s" % (times.data[0] )#* 75.0 * (1.0 / (128.0 * 499.2)))
+        print "dist1: %s" % (times.data[1] )#* 75.0 * (1.0 / (128.0 * 499.2)))
+
+        bag.write('anchorTime', times)
+        uwb_location_pub.publish(times)
+
+    def _anchorDist_received_callback(self, timestamp, data, anchorDist):
+        distances = Float32MultiArray()
+        distances.data = [data["ranging.distance1"],data["ranging.distance2"],data["ranging.distance3"],data["ranging.distance4"],data["ranging.distance5"],data["ranging.distance6"]]
+
+        bag.write('anchorDistances',distances)
+        uwb_location_pub.publish(distances)
+
+    def _xyzrpy_received_callback(self, timestamp, data, xyzrpy):
+
+        if onboardPosition:
+            arrXYZ = Float32MultiArray()
+            #TODO: where are coordinates published?
+            arrXYZ.data = [data["xxx.x"], fdata["xxx.y"],data["xxx.z"]]
+            bag.write('XYZ',arrXYZ)
+            uwb_location_pub.publish(arrXYZ)
+
+
+        arrRPY = Float32MultiArray()
+        arrRPY.data = [data["stabilizer.roll"],data["stabilizer.pitch"],data["stabilizer.yaw"]]
+
+        bag.write('rollPitchYaw',arrRPY)
+
+        rpy_pub.publish(arrRPY)
+
+        #print arrRPY
+
+
+    #def _data_received_callback(self, timestamp, data, logconf):
+    def _vbat_received_callback(self, timestamp, data, batLog):
         #print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
         batteryVolt = Float32()
-        stabilizerYaw = Float32()
-        stabilizerPitch = Float32()
-        stabilizerRoll = Float32()
+        
         batteryVolt.data = data["pm.vbat"]
-        stabilizerYaw.data = data["stabilizer.yaw"]
-        stabilizerPitch.data = data["stabilizer.pitch"]
+       
         bag.write('batteryVoltage', batteryVolt)
-        bag.write('stabilizerYaw', stabilizerYaw)
-        bag.write('stabilizerPitch', stabilizerPitch)
-        bag.write('stabilizerRoll', stabilizerRoll)
-
+        
         #publish battery voltage for GUI
         #cfbattery_pub.publish(std_msgs.Float32(batteryVolt.data))
-        # print "batteryVolt: %s" % batteryVolt
+        #print "batteryVolt: %s" % batteryVolt
         cfbattery_pub.publish(batteryVolt)
 
 
@@ -179,23 +214,131 @@ class PPSRadioClient:
 
     # def _init_logging(self):
 
-    def _start_logging(self):
-        self.logconf = LogConfig("LoggingTest", 100) # second variable is freq in ms
-        self.logconf.add_variable("stabilizer.roll", "float");
-        self.logconf.add_variable("stabilizer.pitch", "float");
-        self.logconf.add_variable("stabilizer.yaw", "float");
-        self.logconf.add_variable("pm.vbat", "float");
-
-        self._cf.log.add_config(self.logconf)
-        if self.logconf.valid:
-            self.logconf.data_received_cb.add_callback(self._data_received_callback)
-            self.logconf.error_cb.add_callback(self._logging_error)
-            print "logconf valid"
+    def init_log_battery(self):
+
+        global battery_polling_period
+        battery_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/battery_polling_period")
+
+        self.batLog = LogConfig("BatteryLog", battery_polling_period) # second variable is period in ms
+        self.batLog.add_variable("pm.vbat", "float");
+        
+        self._cf.log.add_config(self.batLog)
+        if self.batLog.valid:
+            self.batLog.data_received_cb.add_callback(self._vbat_received_callback)
+            self.batLog.error_cb.add_callback(self._logging_error)
+            print "batLog valid"
+        else:
+            print "batLog invalid"
+
+        self.batLog.start()
+        print "batLog start"
+
+    def init_log_xyzrpy(self):
+
+        global xyzrpy_polling_period
+        xyzrpy_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/xyzrpy_polling_period")
+
+        self.xyzrpy = LogConfig("XYZRPYLog", xyzrpy_polling_period)
+        #TODO: where are coordinates published?
+        if onboardPosition:
+            self.xyzrpy.add_variable("xxx.x", "float");
+            self.xyzrpy.add_variable("xxx.y", "float");
+            self.xyzrpy.add_variable("xxx.z", "float")
+
+        self.xyzrpy.add_variable("stabilizer.roll", "float");
+        self.xyzrpy.add_variable("stabilizer.pitch", "float");
+        self.xyzrpy.add_variable("stabilizer.yaw", "float");
+
+        self._cf.log.add_config(self.xyzrpy)
+        if self.xyzrpy.valid:
+            self.xyzrpy.data_received_cb.add_callback(self._xyzrpy_received_callback)
+            self.xyzrpy.error_cb.add_callback(self._logging_error)
+            print "xyzrpy valid"
+        else:
+            print "xyzrpy invalid"
+
+        self.xyzrpy.start()
+        print "xyzrpy start"
+
+    def init_log_anchordist(self):
+        global anchordist_polling_period
+        anchordist_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/anchordist_polling_period")
+
+        self.anchorDist = LogConfig("AnchorDistances", anchordist_polling_period)
+        self.anchorDist.add_variable("ranging.distance1","float");
+        self.anchorDist.add_variable("ranging.distance2","float");
+        self.anchorDist.add_variable("ranging.distance3","float");
+        self.anchorDist.add_variable("ranging.distance4","float");
+        self.anchorDist.add_variable("ranging.distance5","float");
+        self.anchorDist.add_variable("ranging.distance6","float");
+
+        self._cf.log.add_config(self.anchorDist)
+        if self.anchorDist.valid:
+            self.anchorDist.data_received_cb.add_callback(self._anchorDist_received_callback)
+            self.anchorDist.error_cb.add_callback(self._logging_error)
+            print "anchorDist valid"
         else:
-            print "logconf invalid"
+            print "anchorDist invalid"
+
+        self.anchorDist.start()
+        print "anchorDist start"
+
+    def init_log_tiago(self):
+        global anchordist_polling_period
+        anchordist_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/anchordist_polling_period")
+        self.anchorTime = LogConfig("AnchorTime", anchordist_polling_period)
+        self.anchorTime.add_variable("UWB_Anchor_Pos.anchor0", "uint32_t");
+        self.anchorTime.add_variable("UWB_Anchor_Pos.anchor1", "uint32_t");
+
+        self._cf.log.add_config(self.anchorTime)
+        if self.anchorTime.valid:
+            self.anchorTime.data_received_cb.add_callback(self._anchorTime_received_callback)
+            self.anchorTime.error_cb.add_callback(self._logging_error)
+            print "anchorTime valid"
+        else:
+            print "anchorTime invalid"
+
+        self.anchorTime.start()
+        print "anchorTime start"
+
+
+
+    def _start_logging(self):
 
-        self.logconf.start()
-        print "logconf start"
+        self.init_log_battery()
+
+        if enableUWB:
+            self.init_log_xyzrpy()
+            if not onboardPosition:
+                if useTiagoData:
+                    self.init_log_tiago()
+                else:
+                    self.init_log_anchordist()
+
+    def _stop_logging(self):
+
+        self.batLog.stop()
+        rospy.loginfo("batLog stopped")
+        self.batLog.delete()
+        rospy.loginfo("batLog deleted")
+
+        if enableUWB:
+            self.xyzrpy.stop()
+            rospy.loginfo("xyzrpy stopped")
+            self.xyzrpy.delete()
+            rospy.loginfo("xyzrpy deleted")
+
+            if not onboardPosition:
+                if useTiagoData:
+                    self.anchorTime.stop()
+                    rospy.loginfo("anchorTime stopped")
+                    self.anchorTime.delete()
+                    rospy.loginfo("anchorTime deleted")
+                else:
+                    self.anchorDist.stop()
+                    rospy.loginfo("anchorDist stopped")
+                    self.anchorDist.delete()
+                    rospy.loginfo("anchorDist deleted")
 
     def _connected(self, link_uri):
         """
@@ -210,9 +353,6 @@ class PPSRadioClient:
         # Config for Logging
         self._start_logging()
 
-
-
-
     def _connection_failed(self, link_uri, msg):
         """Callback when connection initial connection fails (i.e no Crazyflie
         at the specified address)"""
@@ -233,10 +373,7 @@ class PPSRadioClient:
         # change state to motors OFF
         self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
 
-        self.logconf.stop()
-        rospy.loginfo("logconf stopped")
-        self.logconf.delete()
-        rospy.loginfo("logconf deleted")
+        self._stop_logging()
 
     def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4):
         pk = CRTPPacket()
@@ -305,6 +442,9 @@ if __name__ == '__main__':
     global node_name
     node_name = "CrazyRadio"
     rospy.init_node(node_name, anonymous=True)
+
+    global ros_namespace
+    ros_namespace = rospy.get_namespace()
     # Initialize the low-level drivers (don't list the debug drivers)
     cflib.crtp.init_drivers(enable_debug_driver=False)
 
@@ -316,9 +456,35 @@ if __name__ == '__main__':
     # radio_address = "radio://0/72/2M"
     # rospy.loginfo("manual address loaded")
 
+    # Bool if using UWB, TODO: need to add a switch or something in a gui (teacher or student) with a publisher, to be subscribed here
+    global enableUWB
+    enableUWB = True
+    global useUWB 
+    useUWB = False
+    global onboardPosition #Not yet working...
+    onboardPosition = False #Not yet working...
+    global useTiagoData
+    useTiagoData = True
+
+    
     global cfbattery_pub
     cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
 
+    if enableUWB:
+        global rpy_pub
+        rpy_pub = rospy.Publisher(node_name + '/RPY', Float32MultiArray, queue_size=10)
+
+        global uwb_location_pub
+        if onboardPosition:
+            uwb_location_pub = rospy.Publisher(node_name + '/XYZ', Float32MultiArray, queue_size=10) #Not yet working...
+
+        else:
+            if useTiagoData:
+                uwb_location_pub = rospy.Publisher(node_name + '/times', UInt32MultiArray, queue_size=10)
+            else:
+                uwb_location_pub = rospy.Publisher(node_name + '/distances', Float32MultiArray, queue_size=10)
+    
+
     global cf_client
 
     cf_client = PPSRadioClient()

pps_ws/src/d_fall_pps/include/LocalizationServer.h

@@ -17,6 +17,7 @@
 #include "ros/ros.h"
 
 #include "d_fall_pps/ViconData.h"
+#include "d_fall_pps/CrazyflieData.h"
 
 namespace d_fall_pps
 {
@@ -31,6 +32,9 @@ namespace d_fall_pps
 
 	// Callback function to handle the change in database information
 	void dbChangedCallback(const std_msgs::Int32 &msg);
+
+	// Callback function to handle incoming UWB data
+	void UWBDataCallback(const CrazyflieData &data);
 }
 
 #endif // LOCALIZATIONSERVER_H included
\ No newline at end of file

pps_ws/src/d_fall_pps/include/UWBDataPublisher.h

+//    ROS node that publishes the localization data from sensor fusion of the crazyflie, using uwb for coordinates
+//    Copyright (C) 2017  Michael Rogenmoser, Tiago Salzmann, Yvan Bosshard
+//
+//    This program is free software: you can redistribute it and/or modify
+//    it under the terms of the GNU General Public License as published by
+//    the Free Software Foundation, either version 3 of the License, or
+//    (at your option) any later version.
+//
+//    This program is distributed in the hope that it will be useful,
+//    but WITHOUT ANY WARRANTY; without even the implied warranty of
+//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//    GNU General Public License for more details.
+//
+//    You should have received a copy of the GNU General Public License
+//    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32MultiArray.h"
+#include "std_msgs/UInt32MultiArray.h"
+#include <vector>
+#include <stdint.h>
+#include "d_fall_pps/UWBAnchor.h"
+#include "d_fall_pps/UWBAnchorArray.h"
+
+#include "ros/ros.h"
+
+namespace d_fall_pps
+{
+    //Callback function to handle roll pitch yaw information from CF
+	void cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY);
+    
+    //Callback funtion to hande x y z information from CF
+    void cfXYZCallback(const std_msgs::Float32MultiArray &arrXYZ);
+
+    //Callback function to handle distance information from CF
+    void cfDistancesCallback(const std_msgs::Float32MultiArray &distances);
+
+    // Loads the current context of the client
+    void loadCrazyflieContext();
+    
+    // Loads the parameters of the node handle
+	bool loadParameters(ros::NodeHandle &nodeHandle);
+
+    // calculates coordinates from input distances
+    void calculateXYZ(const std::vector<float> &distances, int distLen, double(&xyz)[3]);
+
+    // Callback function to handle times information from CF
+    void cfTimesCallback(const std_msgs::UInt32MultiArray &times);
+
+    // calculatse distances from passed microseconds
+    float tiagosAwesomeFormula(uint32_t time);
+
+    // Callback function to handle new anchor positions
+    void anchorPosCallback(const UWBAnchorArray &newAnchors);
+}
\ No newline at end of file

pps_ws/src/d_fall_pps/launch/UWBClient.launch

@@ -13,6 +13,10 @@
 		<param name="clientID" value="$(optenv ROS_NAMESPACE)" />
 	</node>
 
+        <node pkg="d_fall_pps" name="UWBDataPublisher" output="screen" type="UWBDataPublisher">
+                <param name="clientID" value="$(optenv ROS_NAMESPACE)" />
+        </node>
+
         <node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
                 <rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
         </node>

pps_ws/src/d_fall_pps/msg/UWBAnchor.msg

+uint32 id
+float32 x
+float32 y
+float32 z
\ No newline at end of file

pps_ws/src/d_fall_pps/msg/UWBAnchorArray.msg

+uint32 length
+UWBAnchor[] data
\ No newline at end of file

pps_ws/src/d_fall_pps/param/ClientConfig.yaml

@@ -2,6 +2,8 @@ safeController: "SafeControllerService/RateController"
 customController: "CustomControllerService/CustomController"
 strictSafety: true
 angleMargin: 0.6
-battery_threshold_while_flying: 2.9
-battery_threshold_while_motors_off: 3.3
-
+battery_threshold_while_flying: 2.9      # in V
+battery_threshold_while_motors_off: 3.34 # in V
+battery_polling_period: 500         # in ms
+xyzrpy_polling_period: 50
+anchordist_polling_period: 50

pps_ws/src/d_fall_pps/src/LocalizationServer.cpp

@@ -27,9 +27,6 @@ using namespace d_fall_pps;
 // the clientID defines namespace and identifier in the CentralManagerService
 int clientID;
 
-// values for safetyCheck
-bool strictSafety;
-
 // Self explanatory variables
 ros::ServiceClient centralManager;
 ros::Publisher localizationPublisher;
@@ -37,6 +34,10 @@ ros::Publisher localizationPublisher;
 CrazyflieContext context;
 rosbag::Bag bag;
 
+bool enableUWB;
+bool usingUWB;
+bool onboardPosition;
+
 int main(int argc, char* argv[])
 {
 	ros::init(argc, argv, "LocalizationServer");
@@ -50,12 +51,23 @@ int main(int argc, char* argv[])
 	centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
 	loadCrazyflieContext();
 
+    // Bool if using UWB, TODO: need to add a switch or something in a gui (teacher or student) with a publisher, to be subscribed here
+	enableUWB = true;
+	usingUWB = false;
+	onboardPosition = false;
+
 	ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback);
 	ROS_INFO_STREAM("LocalizationServer connected to Vicon");
 
+	ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
+
+	//if (enableUWB){
+	ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData",100,UWBDataCallback);
+	ROS_INFO_STREAM("LocalizationServer connected to UWB");
+	//}
+
 	localizationPublisher = nodeHandle.advertise<CrazyflieData>("LocalizationData", 1);
 
-	ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
 	ros::Subscriber dbChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback);
 
 	ROS_INFO_STREAM("LocalizationServer started!");
@@ -114,6 +126,13 @@ 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);
+	bag.write("UWBData", ros::Time::now(), data);
+}
+
 void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg)
 {
 	d_fall_pps::loadCrazyflieContext();

pps_ws/src/d_fall_pps/src/UWBDataPublisher.cpp

+//    ROS node that publishes the localization data from sensor fusion of the crazyflie, using uwb for coordinates
+//    Copyright (C) 2017  Michael Rogenmoser, Tiago Salzmann, Yvan Bosshard
+//
+//    This program is free software: you can redistribute it and/or modify
+//    it under the terms of the GNU General Public License as published by
+//    the Free Software Foundation, either version 3 of the License, or
+//    (at your option) any later version.
+//
+//    This program is distributed in the hope that it will be useful,
+//    but WITHOUT ANY WARRANTY; without even the implied warranty of
+//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//    GNU General Public License for more details.
+//
+//    You should have received a copy of the GNU General Public License
+//    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <stdlib.h>
+#include <string.h>
+#include "DataStreamClient.h"
+#include "ros/ros.h"
+#include "d_fall_pps/CrazyflieContext.h"
+#include "std_msgs/Float32.h"
+#include "std_msgs/Float32MultiArray.h"
+#include "d_fall_pps/CrazyflieData.h"
+#include <rosbag/bag.h>
+#include <ros/package.h>
+
+#include "d_fall_pps/CMQuery.h"
+