Commit 6561ea96 authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

replaced polling variables with just one for onboard data

parent f2616529
......@@ -142,6 +142,7 @@ class PPSRadioClient:
if enableUWB:
self.init_log_uwb()
self.init_log_rpy()
def _stop_logging(self):
......@@ -149,6 +150,7 @@ class PPSRadioClient:
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)
......@@ -199,10 +201,8 @@ class PPSRadioClient:
### UWB log
def init_log_uwb(self):
global anchordist_polling_period
anchordist_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/anchordist_polling_period")
self.anchorLog = LogConfig("AnchorDistances", anchordist_polling_period)
polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/positioning_polling_period")
self.anchorLog = LogConfig("AnchorDistances", polling_period)
for i in range(UWB_NUM_ANCHORS):
self.anchorLog.add_variable("UWB_Anchor_Pos.anchor" + str(i), "uint32_t");
......@@ -216,22 +216,23 @@ class PPSRadioClient:
else:
rospy.logwarn("[CrazyRadio] anchorLog invalid")
self.anchorLog.start();
self.anchorLog.start()
rospy.loginfo("[CrazyRadio] anchorLog started")
def _anchors_received_callback(self, timestamp, data, anchorDist):
ticks = UInt32MultiArray()
ticks.data = []
dist = UInt32MultiArray()
dist.data = []
for i in range(UWB_NUM_ANCHORS):
ticks.data.insert(i, data["UWB_Anchor_Pos.anchor" + str(i)])
dist.data.insert(i, data["UWB_Anchor_Pos.anchor" + str(i)])
uwb_location_pub.publish(ticks)
#bag.write('anchorTime', ticks) # needs a lot of time
uwb_location_pub.publish(dist)
#bag.write('anchorTime', dist) # needs a lot of time
for i in range(UWB_NUM_ANCHORS):
rospy.loginfo("Anchor " + str(i) + ": " + str(ticks.data[i]))
# for debugging
#for i in range(UWB_NUM_ANCHORS):
# rospy.loginfo("Anchor " + str(i) + ": " + str(dist.data[i]))
def stop_log_uwb(self):
......@@ -241,6 +242,47 @@ class PPSRadioClient:
rospy.loginfo("[CrazyRadio] anchorLog deleted")
### RPY log
def init_log_rpy(self):
polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/positioning_polling_period")
self.rpyLog = LogConfig("RPY", polling_period)
self.rpyLog.add_variable("stabilizer.roll", "float");
self.rpyLog.add_variable("stabilizer.pitch", "float");
self.rpyLog.add_variable("stabilizer.yaw", "float");
self._cf.log.add_config(self.rpyLog)
if self.rpyLog.valid:
self.rpyLog.data_received_cb.add_callback(self._rpy_received_callback)
self.rpyLog.error_cb.add_callback(self._logging_error)
rospy.loginfo("[CrazyRadio] rpyLog valid")
else:
rospy.logwarn("[CrazyRadio] rpyLog invalid")
self.rpyLog.start()
rospy.loginfo("[CrazyRadio] rpyLog started")
def _rpy_received_callback(self, timestamp, data, rpy):
rpy_data = Float32MultiArray()
rpy_data.data = [data["stabilizer.roll"],data["stabilizer.pitch"],data["stabilizer.yaw"]]
rpy_pub.publish(rpy_data)
#bag.write('RPY', rpy_data) # needs a lot of time
# for debugging
#rospy.loginfo("Roll: %f\nPitch: %f\nYaw: %f", rpy_data.data[0], rpy_data.data[1], rpy_data.data[2])
def stop_log_rpy(self):
self.rpyLog.stop()
rospy.loginfo("[CrazyRadio] rpyLog stopped")
self.rpyLog.delete()
rospy.loginfo("[CrazyRadio] rpyLog deleted")
"""
---------------------------------------
......@@ -420,11 +462,10 @@ if __name__ == '__main__':
cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
if enableUWB:
#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 + '/times', UInt32MultiArray, queue_size=10)
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
......
......@@ -5,5 +5,4 @@ angleMargin: 0.6
battery_threshold_while_flying: 2.9 # in V
battery_threshold_while_motors_off: 3.34 # in V
battery_polling_period: 500 # in ms
xyzrpy_polling_period: 1000
anchordist_polling_period: 50
positioning_polling_period: 10
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