clean up UWBData

pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py

@@ -159,33 +159,28 @@ class PPSRadioClient:
         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)
-        times_pub.publish(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)
-        distances_pub.publish(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)
-            xyz_pub.publish(arrXYZ)
-        #print "anchor0: %s" % data["UWB_Anchor_Pos.anchor0"]
-        #print "anchor1: %s" % data["UWB_Anchor_Pos.anchor1"]
-
-        #dist0 = data["UWB_Anchor_Pos.anchor0"] * 75.0 * (1.0 / (128.0 * 499.2))
-        #print "dist0: %s" % dist0
-        #dist1 = data["UWB_Anchor_Pos.anchor1"] * 75.0 * (1.0 / (128.0 * 499.2))
-        #print "dist1: %s" % dist1
+            uwb_location_pub.publish(arrXYZ)
 
 
         arrRPY = Float32MultiArray()
@@ -246,13 +241,10 @@ class PPSRadioClient:
         self.xyzrpy = LogConfig("XYZRPYLog", xyzrpy_polling_period)
         #TODO: where are coordinates published?
         if onboardPosition:
-            self.xyzrpy.add_variable("UWB_Anchor_Pos.anchor0", "uint32_t");
-            self.xyzrpy.add_variable("UWB_Anchor_Pos.anchor1", "uint32_t");
+            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("UWB_Anchor_Pos.anchor0", "uint32_t");
-        #self.xyzrpy.add_variable("UWB_Anchor_Pos.anchor1", "uint32_t");
-
         self.xyzrpy.add_variable("stabilizer.roll", "float");
         self.xyzrpy.add_variable("stabilizer.pitch", "float");
         self.xyzrpy.add_variable("stabilizer.yaw", "float");
@@ -321,7 +313,7 @@ class PPSRadioClient:
                 if useTiagoData:
                     self.init_log_tiago()
                 else:
-                    self.init_log_anchordist()             #TODO: test
+                    self.init_log_anchordist()
 
     def _stop_logging(self):
 
@@ -469,8 +461,8 @@ if __name__ == '__main__':
     enableUWB = True
     global useUWB 
     useUWB = False
-    global onboardPosition
-    onboardPosition = False
+    global onboardPosition #Not yet working...
+    onboardPosition = False #Not yet working...
     global useTiagoData
     useTiagoData = True
 
@@ -482,20 +474,15 @@ if __name__ == '__main__':
         global rpy_pub
         rpy_pub = rospy.Publisher(node_name + '/RPY', Float32MultiArray, queue_size=10)
 
-
-        #TODO: To publish position, fix _xyzrpy_received_callback and _init_log_xyzrpy
+        global uwb_location_pub
         if onboardPosition:
-            global xyz_pub
-            xyz_pub = rospy.Publisher(node_name + '/XYZ', Float32MultiArray, queue_size=10)
+            uwb_location_pub = rospy.Publisher(node_name + '/XYZ', Float32MultiArray, queue_size=10) #Not yet working...
 
-        #TODO: To publish distances, uncomment these lines, uncomment self.init_log_anchordist() in _start_logging, uncomment stop functions in _stop_logging
         else:
             if useTiagoData:
-                global times_pub
-                times_pub = rospy.Publisher(node_name + '/times', UInt32MultiArray, queue_size=10)
+                uwb_location_pub = rospy.Publisher(node_name + '/times', UInt32MultiArray, queue_size=10)
             else:
-                global distances_pub
-                distances_pub = rospy.Publisher(node_name + '/distances', Float32MultiArray, queue_size=10)
+                uwb_location_pub = rospy.Publisher(node_name + '/distances', Float32MultiArray, queue_size=10)
     
 
     global cf_client

pps_ws/src/d_fall_pps/src/LocalizationServer.cpp

@@ -52,7 +52,7 @@ int main(int argc, char* argv[])
 	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 = false;
+	enableUWB = true;
 	usingUWB = false;
 	onboardPosition = false;
 
@@ -61,10 +61,10 @@ int main(int argc, char* argv[])
 
 	ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
 
-	if (enableUWB){
-		ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData",100,UWBDataCallback);
-		ROS_INFO_STREAM("LocalizationServer connected to UWB");
-	}
+	//if (enableUWB){
+	ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData",100,UWBDataCallback);
+	ROS_INFO_STREAM("LocalizationServer connected to UWB");
+	//}
 
 	localizationPublisher = nodeHandle.advertise<CrazyflieData>("LocalizationData", 1);
 

pps_ws/src/d_fall_pps/src/UWBDataPublisher.cpp

@@ -58,7 +58,7 @@ int main(int argc, char* argv[])
     centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
 	loadCrazyflieContext();    
 
-    // Bool if using UWB onboard position, TODO: need to add a switch or something in a gui (teacher or student) with a publisher, to be subscribed here
+    // Bool if using UWB onboard position, tiagoData, TODO: need to add a switch or something in a gui (teacher or student) with a publisher, to be subscribed here
     onboardPosition = false;
     useTiagoData = true;
 
@@ -67,13 +67,15 @@ int main(int argc, char* argv[])
     ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
     ros::Subscriber cfRPYSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/RPY",10,cfRPYCallback);
 
+    ros::Subscriber uwbLocationSubscriber;
+
     if (onboardPosition){
-        ros::Subscriber cfXYZSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/XYZ",10,cfXYZCallback);
+        uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/XYZ",10,cfXYZCallback);
     }else{
     	if (useTiagoData){
-    		ros::Subscriber cfTimesSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/times",10,cfTimesCallback);
+    		uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/times",10,cfTimesCallback);
     	}else{
-        	ros::Subscriber cfDistancesSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/distances",10,cfDistancesCallback);
+        	uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/distances",10,cfDistancesCallback);
         }
     }