Commit 16ac4f00 authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

bools are working now in Crazyradio

parent e0019c7a
......@@ -163,11 +163,19 @@ class PPSRadioClient:
def _xyzrpy_received_callback(self, timestamp, data, xyzrpy):
if onboardPosition:
#if onboardPosition:
#arrXYZ = Float32MultiArray()
#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
arrRPY = Float32MultiArray()
arrRPY.data = [data["stabilizer.roll"],data["stabilizer.pitch"],data["stabilizer.yaw"]]
......@@ -226,10 +234,14 @@ class PPSRadioClient:
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");
#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.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");
......@@ -275,8 +287,8 @@ class PPSRadioClient:
if enableUWB:
self.init_log_xyzrpy()
if not onboardPosition:
self.init_log_anchordist() #TODO: test
#if not onboardPosition:
#self.init_log_anchordist() #TODO: test
def _stop_logging(self):
......@@ -291,11 +303,11 @@ class PPSRadioClient:
self.xyzrpy.delete()
rospy.loginfo("xyzrpy deleted")
if not onboardPosition:
'''if not onboardPosition:
self.anchorDist.stop()
rospy.loginfo("anchorDist stopped")
self.anchorDist.delete()
rospy.loginfo("anchorDist deleted")
rospy.loginfo("anchorDist deleted")'''
def _connected(self, link_uri):
"""
......@@ -414,9 +426,12 @@ if __name__ == '__main__':
# 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 bool enableUWB = False
global bool useUWB = False
global bool onboardPosition = False
global enableUWB
enableUWB = True
global useUWB
useUWB = False
global onboardPosition
onboardPosition = False
global cfbattery_pub
......
......@@ -59,14 +59,15 @@ int main(int argc, char* argv[])
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::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!");
......
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