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

restructured Crazyradio to be more efficient

parent 310b0577
......@@ -128,98 +128,33 @@ class PPSRadioClient:
self.change_status_to(DISCONNECTED)
def change_status_to(self, new_status):
print "status changed to: %s" % new_status
self._status = new_status
self.status_pub.publish(new_status)
def get_status(self):
return self._status
def update_link_uri(self):
self.link_uri = "radio://" + rospy.get_param("~crazyFlieAddress")
def connect(self):
# update link from ros params
self.update_link_uri()
print "Connecting to %s" % self.link_uri
self.change_status_to(CONNECTING)
rospy.loginfo("connecting...")
self._cf.open_link(self.link_uri)
def disconnect(self):
print "Motors OFF"
self._send_to_commander_motor(0, 0, 0, 0)
# change state to motors OFF
self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
time.sleep(0.1)
print "Disconnecting from %s" % self.link_uri
self._cf.close_link()
self.change_status_to(DISCONNECTED)
def _anchorTime_received_callback(self, timestamp, data, anchorTime):
ticks = UInt32MultiArray()
ticks.data = []
for i in range(UWB_NUM_ANCHORS):
ticks.data.insert(i, data["UWB_Anchor_Pos.anchor" + str(i)])
# for debugging only
#for i in range(UWB_NUM_ANCHORS):
# rospy.loginfo("Anchor " + str(i) + ": " + str(ticks.data[i]))
bag.write('anchorTime', ticks)
uwb_location_pub.publish(ticks)
def _anchorDist_received_callback(self, timestamp, data, anchorDist):
distances = Float32MultiArray()
distances.data = [data["ranging.distance1"],data["ranging.distance2"],data["ranging.distance3"],data["ranging.distance4"],data["ranging.distance5"],data["ranging.distance6"]]
bag.write('anchorDistances',distances)
uwb_location_pub.publish(distances)
def _xyzrpy_received_callback(self, timestamp, data, xyzrpy):
if onboardPosition:
arrXYZ = Float32MultiArray()
#TODO: where are coordinates published?
arrXYZ.data = [data["xxx.x"], fdata["xxx.y"],data["xxx.z"]]
bag.write('XYZ',arrXYZ)
uwb_location_pub.publish(arrXYZ)
arrRPY = Float32MultiArray()
arrRPY.data = [data["stabilizer.roll"],data["stabilizer.pitch"],data["stabilizer.yaw"]]
"""
---------------------------------------
bag.write('rollPitchYaw',arrRPY)
Logging functions
rpy_pub.publish(arrRPY)
---------------------------------------
"""
def _start_logging(self):
#print arrRPY
self.init_log_battery()
if enableUWB:
self.init_log_uwb()
#def _data_received_callback(self, timestamp, data, logconf):
def _vbat_received_callback(self, timestamp, data, batLog):
#print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
batteryVolt = Float32()
batteryVolt.data = data["pm.vbat"]
bag.write('batteryVoltage', batteryVolt)
#publish battery voltage for GUI
#cfbattery_pub.publish(std_msgs.Float32(batteryVolt.data))
#print "batteryVolt: %s" % batteryVolt
cfbattery_pub.publish(batteryVolt)
def _stop_logging(self):
self.stop_log_battery()
if enableUWB:
self.stop_log_uwb()
def _logging_error(self, logconf, msg):
print "Error when logging %s" % logconf.name
rospy.logerr("[CrazyRadio] Error when logging %s", logconf.name)
# def _init_logging(self):
### Battery log
def init_log_battery(self):
global battery_polling_period
......@@ -229,127 +164,90 @@ class PPSRadioClient:
self.batLog.add_variable("pm.vbat", "float");
self._cf.log.add_config(self.batLog)
if self.batLog.valid:
self.batLog.data_received_cb.add_callback(self._vbat_received_callback)
self.batLog.error_cb.add_callback(self._logging_error)
print "batLog valid"
rospy.loginfo("[CrazyRadio] batLog valid")
else:
print "batLog invalid"
rospy.logwarn("[CrazyRadio] batLog invalid")
self.batLog.start()
print "batLog start"
def init_log_xyzrpy(self):
rospy.loginfo("[CrazyRadio] batLog started")
global xyzrpy_polling_period
xyzrpy_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/xyzrpy_polling_period")
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")
def _vbat_received_callback(self, timestamp, data, batLog):
#print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
batteryVolt = Float32()
batteryVolt.data = data["pm.vbat"]
bag.write('batteryVoltage', batteryVolt)
#publish battery voltage for GUI
#cfbattery_pub.publish(std_msgs.Float32(batteryVolt.data))
#print "batteryVolt: %s" % batteryVolt
cfbattery_pub.publish(batteryVolt)
self.xyzrpy.add_variable("stabilizer.roll", "float");
self.xyzrpy.add_variable("stabilizer.pitch", "float");
self.xyzrpy.add_variable("stabilizer.yaw", "float");
def stop_log_battery(self):
self.batLog.stop()
rospy.loginfo("[CrazyRadio] batLog stopped")
self.batLog.delete()
rospy.loginfo("[CrazyRadio] batLog deleted")
self._cf.log.add_config(self.xyzrpy)
if self.xyzrpy.valid:
self.xyzrpy.data_received_cb.add_callback(self._xyzrpy_received_callback)
self.xyzrpy.error_cb.add_callback(self._logging_error)
print "xyzrpy valid"
else:
print "xyzrpy invalid"
self.xyzrpy.start()
print "xyzrpy start"
### UWB log
def init_log_uwb(self):
def init_log_anchordist(self):
global anchordist_polling_period
anchordist_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/anchordist_polling_period")
self.anchorDist = LogConfig("AnchorDistances", anchordist_polling_period)
self.anchorDist.add_variable("ranging.distance1","float");
self.anchorDist.add_variable("ranging.distance2","float");
self.anchorDist.add_variable("ranging.distance3","float");
self.anchorDist.add_variable("ranging.distance4","float");
self.anchorDist.add_variable("ranging.distance5","float");
self.anchorDist.add_variable("ranging.distance6","float");
self._cf.log.add_config(self.anchorDist)
if self.anchorDist.valid:
self.anchorDist.data_received_cb.add_callback(self._anchorDist_received_callback)
self.anchorDist.error_cb.add_callback(self._logging_error)
print "anchorDist valid"
else:
print "anchorDist invalid"
self.anchorDist.start()
print "anchorDist start"
def init_log_tiago(self):
global anchordist_polling_period
anchordist_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/anchordist_polling_period")
self.anchorTime = LogConfig("AnchorTime", anchordist_polling_period)
self.anchorLog = LogConfig("AnchorDistances", anchordist_polling_period)
for i in range(UWB_NUM_ANCHORS):
self.anchorTime.add_variable("UWB_Anchor_Pos.anchor" + str(i), "uint32_t");
self.anchorLog.add_variable("UWB_Anchor_Pos.anchor" + str(i), "uint32_t");
#self.anchorTime.add_variable("UWB_Anchor_Pos.anchor0", "uint32_t");
#self.anchorTime.add_variable("UWB_Anchor_Pos.anchor1", "uint32_t");
self._cf.log.add_config(self.anchorLog)
self._cf.log.add_config(self.anchorTime)
if self.anchorTime.valid:
self.anchorTime.data_received_cb.add_callback(self._anchorTime_received_callback)
self.anchorTime.error_cb.add_callback(self._logging_error)
print "anchorTime valid"
if self.anchorLog.valid:
self.anchorLog.data_received_cb.add_callback(self._anchors_received_callback)
self.anchorLog.error_cb.add_callback(self._logging_error)
rospy.loginfo("[CrazyRadio] anchorLog valid")
else:
print "anchorTime invalid"
rospy.logwarn("[CrazyRadio] anchorLog invalid")
self.anchorTime.start()
print "anchorTime start"
self.anchorLog.start();
rospy.loginfo("[CrazyRadio] anchorLog started")
def _anchors_received_callback(self, timestamp, data, anchorDist):
ticks = UInt32MultiArray()
ticks.data = []
def _start_logging(self):
for i in range(UWB_NUM_ANCHORS):
ticks.data.insert(i, data["UWB_Anchor_Pos.anchor" + str(i)])
self.init_log_battery()
uwb_location_pub.publish(ticks)
#bag.write('anchorTime', ticks) # needs a lot of time
if enableUWB:
self.init_log_xyzrpy()
if not onboardPosition:
if useTiagoData:
self.init_log_tiago()
else:
self.init_log_anchordist()
for i in range(UWB_NUM_ANCHORS):
rospy.loginfo("Anchor " + str(i) + ": " + str(ticks.data[i]))
def _stop_logging(self):
def stop_log_uwb(self):
self.batLog.stop()
rospy.loginfo("batLog stopped")
self.batLog.delete()
rospy.loginfo("batLog deleted")
self.anchorLog.stop()
rospy.loginfo("[CrazyRadio] anchorLog stopped")
self.anchorLog.delete()
rospy.loginfo("[CrazyRadio] anchorLog deleted")
if enableUWB:
self.xyzrpy.stop()
rospy.loginfo("xyzrpy stopped")
self.xyzrpy.delete()
rospy.loginfo("xyzrpy deleted")
if not onboardPosition:
if useTiagoData:
self.anchorTime.stop()
rospy.loginfo("anchorTime stopped")
self.anchorTime.delete()
rospy.loginfo("anchorTime deleted")
else:
self.anchorDist.stop()
rospy.loginfo("anchorDist stopped")
self.anchorDist.delete()
rospy.loginfo("anchorDist deleted")
"""
---------------------------------------
Callback functions
---------------------------------------
"""
def _connected(self, link_uri):
"""
This callback is executed as soon as the connection to the
......@@ -359,31 +257,70 @@ class PPSRadioClient:
# change state to motors OFF
cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
rospy.loginfo("Connection to %s successful: " % link_uri)
rospy.loginfo("[CrazyRadio] Connection to %s successful: " % link_uri)
# Config for Logging
self._start_logging()
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
self.change_status_to(DISCONNECTED)
rospy.logwarn("[CrazyRadio] Disconnected from %s" % link_uri)
# change state to motors OFF
self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
self._stop_logging()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
self.change_status_to(DISCONNECTED)
rospy.logerr("Connection to %s failed: %s" % (link_uri, msg))
rospy.logerr("[CrazyRadio] Connection to %s failed: %s" % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
self.change_status_to(DISCONNECTED)
rospy.logerr("Connection to %s lost: %s" % (link_uri, msg))
rospy.logerr("[CrazyRadio] Connection to %s lost: %s" % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
self.change_status_to(DISCONNECTED)
rospy.logwarn("Disconnected from %s" % link_uri)
"""
---------------------------------------
CrazyRadio interface functions
---------------------------------------
"""
def change_status_to(self, new_status):
print "status changed to: %s" % new_status
self._status = new_status
self.status_pub.publish(new_status)
def get_status(self):
return self._status
def update_link_uri(self):
self.link_uri = "radio://" + rospy.get_param("~crazyFlieAddress")
def connect(self):
# update link from ros params
self.update_link_uri()
print "Connecting to %s" % self.link_uri
self.change_status_to(CONNECTING)
rospy.loginfo("connecting...")
self._cf.open_link(self.link_uri)
def disconnect(self):
print "Motors OFF"
self._send_to_commander_motor(0, 0, 0, 0)
# change state to motors OFF
self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
self._stop_logging()
time.sleep(0.1)
print "Disconnecting from %s" % self.link_uri
self._cf.close_link()
self.change_status_to(DISCONNECTED)
def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4):
pk = CRTPPacket()
......@@ -430,6 +367,7 @@ class PPSRadioClient:
self.status_pub.publish(DISCONNECTED)
def controlCommandCallback(data):
"""Callback for controller actions"""
#rospy.loginfo("controller callback : %s, %s, %s", data.roll, data.pitch, data.yaw)
......@@ -447,8 +385,15 @@ def controlCommandCallback(data):
"""
---------------------------------------
main function
---------------------------------------
"""
if __name__ == '__main__':
global node_name
node_name = "CrazyRadio"
rospy.init_node(node_name, anonymous=True)
......@@ -466,33 +411,20 @@ if __name__ == '__main__':
# 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
# 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 useUWB
useUWB = False
global onboardPosition #Not yet working...
onboardPosition = False #Not yet working...
global useTiagoData
useTiagoData = True
global cfbattery_pub
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 rpy_pub
#rpy_pub = rospy.Publisher(node_name + '/RPY', Float32MultiArray, queue_size=10)
global uwb_location_pub
if onboardPosition:
uwb_location_pub = rospy.Publisher(node_name + '/XYZ', Float32MultiArray, queue_size=10) #Not yet working...
else:
if useTiagoData:
uwb_location_pub = rospy.Publisher(node_name + '/times', UInt32MultiArray, queue_size=10)
else:
uwb_location_pub = rospy.Publisher(node_name + '/distances', Float32MultiArray, queue_size=10)
uwb_location_pub = rospy.Publisher(node_name + '/times', UInt32MultiArray, queue_size=10)
global cf_client
......
......@@ -5,5 +5,5 @@ 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: 50
xyzrpy_polling_period: 1000
anchordist_polling_period: 50
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