Commit e0019c7a authored by michaero's avatar michaero
Browse files

Added bools to enable UWB

parent 4bedcb75
......@@ -162,11 +162,12 @@ class PPSRadioClient:
distances_pub.publish(distances)
def _xyzrpy_received_callback(self, timestamp, data, xyzrpy):
#arrXYZ = Float32MultiArray()
#arrXYZ.data = [data["xxx.x"], fdata["xxx.y"],data["xxx.z"]]
#bag.write('XYZ',arrXYZ)
#xyz_pub.publish(arrXYZ)
if onboardPosition:
#arrXYZ = Float32MultiArray()
#arrXYZ.data = [data["xxx.x"], fdata["xxx.y"],data["xxx.z"]]
#bag.write('XYZ',arrXYZ)
#xyz_pub.publish(arrXYZ)
arrRPY = Float32MultiArray()
arrRPY.data = [data["stabilizer.roll"],data["stabilizer.pitch"],data["stabilizer.yaw"]]
......@@ -225,9 +226,10 @@ class PPSRadioClient:
self.xyzrpy = LogConfig("XYZRPYLog", xyzrpy_polling_period)
#TODO: where are coordinates published?
#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("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");
......@@ -271,9 +273,10 @@ class PPSRadioClient:
self.init_log_battery()
self.init_log_xyzrpy()
#self.init_log_anchordist() #TODO: test
if enableUWB:
self.init_log_xyzrpy()
if not onboardPosition:
self.init_log_anchordist() #TODO: test
def _stop_logging(self):
......@@ -282,15 +285,17 @@ class PPSRadioClient:
self.batLog.delete()
rospy.loginfo("batLog deleted")
self.xyzrpy.stop()
rospy.loginfo("xyzrpy stopped")
self.xyzrpy.delete()
rospy.loginfo("xyzrpy deleted")
if enableUWB:
self.xyzrpy.stop()
rospy.loginfo("xyzrpy stopped")
self.xyzrpy.delete()
rospy.loginfo("xyzrpy deleted")
#self.anchorDist.stop()
#rospy.loginfo("anchorDist stopped")
#self.anchorDist.delete()
#rospy.loginfo("anchorDist deleted")
if not onboardPosition:
self.anchorDist.stop()
rospy.loginfo("anchorDist stopped")
self.anchorDist.delete()
rospy.loginfo("anchorDist deleted")
def _connected(self, link_uri):
"""
......@@ -407,20 +412,31 @@ if __name__ == '__main__':
#use this following two lines to connect without data from CentralManager
# 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 bool enableUWB = False
global bool useUWB = False
global bool onboardPosition = False
global cfbattery_pub
cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
global rpy_pub
rpy_pub = rospy.Publisher(node_name + '/RPY', Float32MultiArray, queue_size=10)
if enableUWB:
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 xyz_pub
#xyz_pub = rospy.Publisher(node_name + '/XYZ', Float32MultiArray, queue_size=10)
#TODO: To publish distances, uncomment these lines, uncomment self.init_log_anchordist() in _start_logging, uncomment stop functions in _stop_logging
#global distances_pub
#distances_pub = rospy.Publisher(node_name + '/distances', Float32MultiArray, queue_size=10)
#TODO: To publish position, fix _xyzrpy_received_callback and _init_log_xyzrpy
if onboardPosition:
global xyz_pub
xyz_pub = rospy.Publisher(node_name + '/XYZ', Float32MultiArray, queue_size=10)
#TODO: To publish distances, uncomment these lines, uncomment self.init_log_anchordist() in _start_logging, uncomment stop functions in _stop_logging
else:
global distances_pub
distances_pub = rospy.Publisher(node_name + '/distances', Float32MultiArray, queue_size=10)
global cf_client
......
......@@ -34,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");
......@@ -47,11 +51,18 @@ 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 = false;
usingUWB = false;
onboardPosition = false;
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback);
ROS_INFO_STREAM("LocalizationServer connected to Vicon");
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);
......
......@@ -38,6 +38,8 @@ ros::Publisher UWBDataPublisher;
double[3] rollPitchYaw;
bool onboardPosition;
int main(int argc, char* argv[])
{
ros::init(argc, argv, "UWBDataPublisher");
......@@ -50,14 +52,19 @@ int main(int argc, char* argv[])
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
onboardPosition = false;
UWBDataPublisher = nodeHandle.advertise<CrazyflieData>("UWBData", 1);
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
ros::Subscriber cfRPYSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/RPY",10,cfRPYCallback);
ros::Subscriber cfXYZSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/XYZ",10,cfXYZCallback);
ros::Subscriber cfDistancesSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/distances",10,cfDistancesCallback);
if (onboardPosition){
ros::Subscriber cfXYZSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/XYZ",10,cfXYZCallback);
}else{
ros::Subscriber cfDistancesSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/distances",10,cfDistancesCallback);
}
bag.open(record_file, rosbag::bagmode::Write);
......
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