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

added start/stop of logging rpy and anchor distances with enableUWB

parent fde16c01
...@@ -34,6 +34,7 @@ from std_msgs.msg import Float32 ...@@ -34,6 +34,7 @@ from std_msgs.msg import Float32
from std_msgs.msg import String from std_msgs.msg import String
from std_msgs.msg import Float32MultiArray from std_msgs.msg import Float32MultiArray
from std_msgs.msg import UInt32MultiArray from std_msgs.msg import UInt32MultiArray
from d_fall_pps.srv import Anchors
# Add library # Add library
#sys.path.append("lib") #sys.path.append("lib")
...@@ -140,17 +141,28 @@ class PPSRadioClient: ...@@ -140,17 +141,28 @@ class PPSRadioClient:
self.init_log_battery() self.init_log_battery()
if enableUWB: self.init_log_rpy()
self.init_log_uwb()
self.init_log_rpy() self.init_log_uwb()
self.UWBUpdateCallback(1)
# if enableUWB:
# self.init_log_uwb()
# self.init_log_rpy()
def _stop_logging(self): def _stop_logging(self):
self.stop_log_battery() self.stop_log_battery()
if enableUWB: self.delete_log_uwb()
self.stop_log_uwb()
self.stop_log_rpy() slef.delete_log_rpy()
# if enableUWB:
# self.stop_log_uwb()
# self.stop_log_rpy()
def _logging_error(self, logconf, msg): def _logging_error(self, logconf, msg):
rospy.logerr("[CrazyRadio] Error when logging %s", logconf.name) rospy.logerr("[CrazyRadio] Error when logging %s", logconf.name)
...@@ -216,7 +228,11 @@ class PPSRadioClient: ...@@ -216,7 +228,11 @@ class PPSRadioClient:
else: else:
rospy.logwarn("[CrazyRadio] anchorLog invalid") rospy.logwarn("[CrazyRadio] anchorLog invalid")
def start_log_uwb(self):
self.anchorLog.start() self.anchorLog.start()
self.anchorLog._set_started(True)
rospy.loginfo("[CrazyRadio] anchorLog started") rospy.loginfo("[CrazyRadio] anchorLog started")
def _anchors_received_callback(self, timestamp, data, anchorDist): def _anchors_received_callback(self, timestamp, data, anchorDist):
...@@ -236,11 +252,16 @@ class PPSRadioClient: ...@@ -236,11 +252,16 @@ class PPSRadioClient:
def stop_log_uwb(self): def stop_log_uwb(self):
self.anchorLog._set_started(False)
self.anchorLog.stop() self.anchorLog.stop()
rospy.loginfo("[CrazyRadio] anchorLog stopped") rospy.loginfo("[CrazyRadio] anchorLog stopped")
def delete_log_uwb(self):
if (self.anchorLog._get_started()):
self.stop_log_uwb()
self.anchorLog.delete() self.anchorLog.delete()
rospy.loginfo("[CrazyRadio] anchorLog deleted") rospy.loginfo("[CrazyRadio] anchorLog deleted")
### RPY log ### RPY log
def init_log_rpy(self): def init_log_rpy(self):
...@@ -260,8 +281,12 @@ class PPSRadioClient: ...@@ -260,8 +281,12 @@ class PPSRadioClient:
rospy.loginfo("[CrazyRadio] rpyLog valid") rospy.loginfo("[CrazyRadio] rpyLog valid")
else: else:
rospy.logwarn("[CrazyRadio] rpyLog invalid") rospy.logwarn("[CrazyRadio] rpyLog invalid")
def start_log_rpy(self):
self.rpyLog.start() self.rpyLog.start()
self.rpyLog._set_started(True)
rospy.loginfo("[CrazyRadio] rpyLog started") rospy.loginfo("[CrazyRadio] rpyLog started")
def _rpy_received_callback(self, timestamp, data, rpy): def _rpy_received_callback(self, timestamp, data, rpy):
...@@ -277,12 +302,17 @@ class PPSRadioClient: ...@@ -277,12 +302,17 @@ class PPSRadioClient:
def stop_log_rpy(self): def stop_log_rpy(self):
self.rpyLog._set_started(False)
self.rpyLog.stop() self.rpyLog.stop()
rospy.loginfo("[CrazyRadio] rpyLog stopped") rospy.loginfo("[CrazyRadio] rpyLog stopped")
def delete_log_rpy(self):
if (self.rpyLog._get_started()):
self.stop_log_rpy()
self.rpyLog.delete() self.rpyLog.delete()
rospy.loginfo("[CrazyRadio] rpyLog deleted") rospy.loginfo("[CrazyRadio] rpyLog deleted")
""" """
--------------------------------------- ---------------------------------------
...@@ -325,6 +355,28 @@ class PPSRadioClient: ...@@ -325,6 +355,28 @@ class PPSRadioClient:
self.change_status_to(DISCONNECTED) self.change_status_to(DISCONNECTED)
rospy.logerr("[CrazyRadio] Connection to %s lost: %s" % (link_uri, msg)) rospy.logerr("[CrazyRadio] Connection to %s lost: %s" % (link_uri, msg))
def UWBUpdateCallback(self, data):
"""Callback for change in UWB Settings"""
# TODO: Tiago? Add command to CF to enable/disable ranging
rospy.wait_for_service('/UWBManagerService/UWBData')
try:
uwbdataproxy = rospy.ServiceProxy('/UWBManagerService/UWBData',Anchors)
resp = uwbdataproxy(0,0,0)
if (resp.enableUWB == False):
if (self.anchorLog._get_started()):
self.stop_log_uwb()
if (self.rpyLog._get_started):
self.stop_log_rpy()
else:
if not (self.anchorLog._get_started()):
self.start_log_uwb()
if not (self.rpyLog._get_started()):
self.start_log_rpy()
except rospy.ServiceException, e:
rospy.logerr("[CrazyRadio] Unable to activate Loggings from Service");
""" """
...@@ -453,30 +505,31 @@ if __name__ == '__main__': ...@@ -453,30 +505,31 @@ if __name__ == '__main__':
# radio_address = "radio://0/72/2M" # radio_address = "radio://0/72/2M"
# rospy.loginfo("manual address loaded") # rospy.loginfo("manual address loaded")
# 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 cfbattery_pub global cfbattery_pub
cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10) cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
if enableUWB: global uwb_location_pub
global uwb_location_pub uwb_location_pub = rospy.Publisher(node_name + '/AnchorDistances', UInt32MultiArray, queue_size=10)
uwb_location_pub = rospy.Publisher(node_name + '/AnchorDistances', UInt32MultiArray, queue_size=10) global rpy_pub
global rpy_pub rpy_pub = rospy.Publisher(node_name + '/RPY', Float32MultiArray, queue_size=10)
rpy_pub = rospy.Publisher(node_name + '/RPY', Float32MultiArray, queue_size=10)
global cf_client global cf_client
cf_client = PPSRadioClient() cf_client = PPSRadioClient()
#cf_client.UWBUpdateCallback(1)
rospy.Subscriber("PPSClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts rospy.Subscriber("PPSClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts
time.sleep(1.0) time.sleep(1.0)
rospy.Subscriber("PPSClient/ControlCommand", ControlCommand, controlCommandCallback) rospy.Subscriber("PPSClient/ControlCommand", ControlCommand, controlCommandCallback)
rospy.Subscriber("/my_GUI/UWBUpdate", Int32, cf_client.UWBUpdateCallback)
rospy.spin() rospy.spin()
rospy.loginfo("Turning off crazyflie") rospy.loginfo("Turning off crazyflie")
......
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