To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

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
from std_msgs.msg import String
from std_msgs.msg import Float32MultiArray
from std_msgs.msg import UInt32MultiArray
from d_fall_pps.srv import Anchors
# Add library
#sys.path.append("lib")
......@@ -140,17 +141,28 @@ class PPSRadioClient:
self.init_log_battery()
if enableUWB:
self.init_log_uwb()
self.init_log_rpy()
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):
self.stop_log_battery()
if enableUWB:
self.stop_log_uwb()
self.stop_log_rpy()
self.delete_log_uwb()
slef.delete_log_rpy()
# if enableUWB:
# self.stop_log_uwb()
# self.stop_log_rpy()
def _logging_error(self, logconf, msg):
rospy.logerr("[CrazyRadio] Error when logging %s", logconf.name)
......@@ -216,7 +228,11 @@ class PPSRadioClient:
else:
rospy.logwarn("[CrazyRadio] anchorLog invalid")
def start_log_uwb(self):
self.anchorLog.start()
self.anchorLog._set_started(True)
rospy.loginfo("[CrazyRadio] anchorLog started")
def _anchors_received_callback(self, timestamp, data, anchorDist):
......@@ -236,11 +252,16 @@ class PPSRadioClient:
def stop_log_uwb(self):
self.anchorLog._set_started(False)
self.anchorLog.stop()
rospy.loginfo("[CrazyRadio] anchorLog stopped")
def delete_log_uwb(self):
if (self.anchorLog._get_started()):
self.stop_log_uwb()
self.anchorLog.delete()
rospy.loginfo("[CrazyRadio] anchorLog deleted")
### RPY log
def init_log_rpy(self):
......@@ -260,8 +281,12 @@ class PPSRadioClient:
rospy.loginfo("[CrazyRadio] rpyLog valid")
else:
rospy.logwarn("[CrazyRadio] rpyLog invalid")
def start_log_rpy(self):
self.rpyLog.start()
self.rpyLog._set_started(True)
rospy.loginfo("[CrazyRadio] rpyLog started")
def _rpy_received_callback(self, timestamp, data, rpy):
......@@ -277,12 +302,17 @@ class PPSRadioClient:
def stop_log_rpy(self):
self.rpyLog._set_started(False)
self.rpyLog.stop()
rospy.loginfo("[CrazyRadio] rpyLog stopped")
def delete_log_rpy(self):
if (self.rpyLog._get_started()):
self.stop_log_rpy()
self.rpyLog.delete()
rospy.loginfo("[CrazyRadio] rpyLog deleted")
"""
---------------------------------------
......@@ -325,6 +355,28 @@ class PPSRadioClient:
self.change_status_to(DISCONNECTED)
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__':
# radio_address = "radio://0/72/2M"
# 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
cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
if enableUWB:
global uwb_location_pub
uwb_location_pub = rospy.Publisher(node_name + '/AnchorDistances', UInt32MultiArray, queue_size=10)
global rpy_pub
rpy_pub = rospy.Publisher(node_name + '/RPY', Float32MultiArray, queue_size=10)
global uwb_location_pub
uwb_location_pub = rospy.Publisher(node_name + '/AnchorDistances', UInt32MultiArray, queue_size=10)
global rpy_pub
rpy_pub = rospy.Publisher(node_name + '/RPY', Float32MultiArray, queue_size=10)
global cf_client
cf_client = PPSRadioClient()
#cf_client.UWBUpdateCallback(1)
rospy.Subscriber("PPSClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts
time.sleep(1.0)
rospy.Subscriber("PPSClient/ControlCommand", ControlCommand, controlCommandCallback)
rospy.Subscriber("/my_GUI/UWBUpdate", Int32, cf_client.UWBUpdateCallback)
rospy.spin()
rospy.loginfo("Turning off crazyflie")
......
Markdown is supported
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