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

clean up UWBData

parent b209ad41
......@@ -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
......
......@@ -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);
......
......@@ -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);
}
}
......
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