class CrazyflieBridgedController():
def __init__(self, link_uri, serialport, jr, xmode=True):
"""Initialize the headless client and libraries"""
self._cf = Crazyflie()
self._jr = jr
self._serial = serialport
self._cf.commander.set_client_xmode(xmode)
self._cf.open_link(link_uri)
self.is_connected = True
self._console = ""
# Callbacks
self._cf.connected.add_callback(
lambda msg: print("Connected to " + msg))
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.console.receivedChar.add_callback(self._console_char)
self._cf.packet_received.add_callback(self._data_received)
# Controller callback
if self._jr:
self._jr.input_updated.add_callback(
self._cf.commander.send_setpoint)
self.serialthread = threading.Thread(target=self._serial_listener)
self.serialthread.start()
def _console_char(self, pk):
self._console = self._console + str(pk)
if "\n" in self._console:
print("Console: " + str(self._console), end="")
self._console = ""
def _serial_listener(self):
while self.is_connected:
data = self._serial.read(size=30)
if (data):
pk = CRTPPacket()
pk.port = CRTP_PORT_MICROROS
pk.data = data
self._cf.send_packet(pk)
def _data_received(self, pk):
if pk.port == CRTP_PORT_MICROROS:
self._serial.write(pk.data)
self._serial.flush()
def _disconnected(self, link_uri):
print("Disconnected. Reconnecting...")
# self._cf.open_link(link_uri)
self.is_connected = False
def _connection_failed(self, link_uri, msg):
print("Conection failed. Reconnecting...")
# self._cf.open_link(link_uri)
self.is_connected = False
class RaspSerial:
POSITION_CH = 0
def __init__(self, link_uri):
self._cf = Crazyflie()
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print('Connecting to %s' % link_uri)
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
print('Connected to %s' % link_uri)
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.POSITION_CH
pk.data = struct.pack('<fff', 3.1, 3.2, 3.3)
self._cf.send_packet(pk)
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
self.is_connected = False
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
class RadioBridge:
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# UDP socket for interfacing with GCS
self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self._sock.bind(('127.0.0.1', 14551))
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print('Connecting to %s' % link_uri)
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
threading.Thread(target=self._server).start()
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print('Connected to %s' % link_uri)
self._cf.packet_received.add_callback(self._got_packet)
#def _data_updated(self, mem):
# print('Updated id={}'.format(mem.id))
# print('\tType : {}'.format(mem.type))
# print('\tSize : {}'.format(mem.size))
# print('\tValid : {}'.format(mem.valid))
# print('\tElements : ')
# for key in mem.elements:
# print('\t\t{}={}'.format(key, mem.elements[key]))
#
# self._mems_to_update -= 1
# if self._mems_to_update == 0:
# self._cf.close_link()
def _got_packet(self, pk):
#print("GOT: " + str(pk._port) + ' : ' + str(pk.data) + ' ' + str(type(pk.data)))
if pk.port == CRTP_PORT_MAVLINK:
self._sock.sendto(pk.data, ('127.0.0.1', 14550))
def _forward(self, data):
pk = CRTPPacket()
pk.port = CRTP_PORT_MAVLINK #CRTPPort.COMMANDER
pk.data = data #struct.pack('<fffH', roll, -pitch, yaw, thrust)
self._cf.send_packet(pk)
def _server(self):
while True:
print >> sys.stderr, '\nwaiting to receive message'
# Only receive what can be sent in one message
data, address = self._sock.recvfrom(256)
print >> sys.stderr, 'received %s bytes from %s' % (len(data),
address)
for i in range(0, len(data), 30):
self._forward(data[i:(i + 30)])
#print >>sys.stderr, data
#if data:
# self._forward(data)
def _stab_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
print('[%d][%s]: %s' % (timestamp, logconf.name, data))
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
self.is_connected = False
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
class MotorRampExample:
"""Example that connects to a Crazyflie and ramps the motors up/down and
the disconnects"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie(rw_cache='./cache')
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._cf.open_link(link_uri)
print('Connecting to %s' % link_uri)
self.is_connected = True
# change the parameters0,
self._param_check_list = []
self._param_groups = []
# def _connected(self, link_uri):
# """ This callback is called form the Crazyflie API when a Crazyflie
# has been connected and the TOCs have been downloaded."""
# # Start a separate thread to do the motor test.
# # Do not hijack the calling thread!
# Thread(target=self._ramp_motors).start()
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print('Connected to %s' % link_uri)
###########################################################
# LOG
# The definition of the logconfig can be made before connecting
# self._lg_stab = LogConfig(name='packet', period_in_ms=10)
# self._lg_stab.add_variable('crtp.COMMANDER_FLAG', 'uint8_t')
# self._lg_stab.add_variable('stabilizer.pitch', 'float')
# self._lg_stab.add_variable('stabilizer.yaw', 'float')
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
# try:
# self._cf.log.add_config(self._lg_stab)
# # This callback will receive the data
# self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# # This callback will be called on errors
# self._lg_stab.error_cb.add_callback(self._stab_log_error)
# # Start the logging
# self._lg_stab.start()
# except KeyError as e:
# print('Could not start log configuration,'
# '{} not found in TOC'.format(str(e)))
# except AttributeError:
# print('Could not add Stabilizer log config, bad configuration.')
###########################################################
# PARAM
# self._cf.param.add_update_callback(group='motorPowerSet', name='enable',
# cb=self._a_propTest_callback)
# self._cf.param.set_value('motorPowerSet.enable',
# '{:d}'.format(1))
# thv = 2000
# self._cf.param.set_value('motorPowerSet.m4',
# '{:d}'.format(8000))
# time.sleep(5)
# self._cf.param.set_value('motorPowerSet.m4',
# '{:d}'.format(0))
# self._cf.param.set_value('motorPowerSet.enable',
# '{:d}'.format(0))
###########################################################
# RAMP
# Thread(target=self._ramp_motors).start()
self._ramp_motors()
# Start a timer to disconnect in 15s
t = Timer(3, self._cf.close_link)
t.start()
def _a_propTest_callback(self, name, value):
"""Callback for pid_attitude.pitch_kd"""
print('Readback: {0}={1}'.format(name, value))
def _stab_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
print('[%d][%s]: %s' % (timestamp, logconf.name, data), flush=True)
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
print('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)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
def _ramp_motors(self):
thrust_mult = 1
thrust_step = 1500
thrust_max = 22000
thrust = 2000
pitch = 0
roll = 0
yawrate = 0
start_height = 0.2
defatult_height = 0.6
target_height = 0.6
vx = 0
vy = 0
# Unlock startup thrust protection
pk = CRTPPacket()
pk.set_header(0xF, 2) # unlock drone
pk.data = '0'
self._cf.send_packet(pk)
self._cf.commander.send_usb_disable()
fd = sys.stdin.fileno()
oldterm = termios.tcgetattr(fd)
newattr = termios.tcgetattr(fd)
newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO
termios.tcsetattr(fd, termios.TCSANOW, newattr)
oldflags = fcntl.fcntl(fd, fcntl.F_GETFL)
fcntl.fcntl(fd, fcntl.F_SETFL, oldflags | os.O_NONBLOCK)
def _constrain(value, vmin, vmax):
if value > vmax:
return vmax
if value < vmin:
return vmin
return round(value, 2)
# take off
while thrust <= thrust_max:
self._cf.commander.send_setpoint(0, 0, 0, thrust_max)
time.sleep(0.1)
thrust += thrust_step * thrust_mult
# goto target height
cnt = 0
while cnt < 30:
self._cf.commander.send_hover_setpoint(0, 0, 0, target_height)
cnt = cnt + 1
time.sleep(0.1)
# enable usb control
# self._cf.commander.send_usb_enable()
print("begin control")
try:
while 1:
try:
c = sys.stdin.read(1)
if c and c != '\x1b':
if c == 'w':
vx = _constrain(vx + 0.1, -0.4, 0.4)
elif c == 's':
vx = _constrain(vx - 0.1, -0.4, 0.4)
elif c == 'a':
vy = _constrain(vy + 0.1, -0.4, 0.4)
elif c == 'd':
vy = _constrain(vy - 0.1, -0.4, 0.4)
elif c == '8':
target_height = _constrain(target_height + 0.1,
0.2, 1.7)
elif c == '2':
target_height = _constrain(target_height - 0.1,
0.2, 1.7)
elif c == 'r':
vx = 0
vy = 0
elif c == 'p':
self._cf.commander.send_usb_disable()
vx = 0
vy = 0
target_height = defatult_height
elif c == 'o':
self._cf.commander.send_usb_enable()
elif c == 'h':
target_height = defatult_height
self._cf.commander.send_hover_setpoint(
vx, vy, 0, target_height)
# esc
elif c == '\x1b':
self._cf.commander.send_usb_disable()
for x in range(10):
self._cf.commander.send_hover_setpoint(
0, 0, 0, target_height -
(target_height - start_height) / 10.0 * x)
time.sleep(0.2)
break
else:
self._cf.commander.send_keep_alive()
print('vx: {}, vy: {}, height: {}'.format(
vx, vy, target_height))
except IOError:
pass
time.sleep(0.1)
finally:
termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm)
fcntl.fcntl(fd, fcntl.F_SETFL, oldflags)
while thrust > 2000:
self._cf.commander.send_setpoint(0, 0, 0, thrust)
time.sleep(0.1)
thrust -= thrust_step * thrust_mult
# cnt = 0
# while cnt < 20:
# self._cf.commander.send_hover_setpoint(0, 0, 0, target_height)
# # self._cf.commander.send_hover_setpoint(0, 0, 0, start_height + (target_height - start_height) * (cnt / 100.0))
# # self._cf.commander.send_setpoint(0, 0, 0, thrust)
# # self._cf.commander.send_setpoint(0, 0, 0, 18000)
# cnt = cnt + 1
# time.sleep(0.1)
# print("move")
# cnt = 0
# while cnt < 20:
# self._cf.commander.send_hover_setpoint(0.3, 0, 0, target_height) # (forward, left)
# cnt = cnt + 1
# time.sleep(0.1)
# cnt = 0
# while cnt < 40:
# self._cf.commander.send_hover_setpoint(0, 0, 0, target_height) # (forward, left)
# cnt = cnt + 1
# time.sleep(0.1)
# print("down")
# cnt = 0
# while cnt < 10:
# self._cf.commander.send_hover_setpoint(0, 0, 0, start_height)
# cnt += 1
# time.sleep(0.1)
# while thrust >= 1000 and thrust < 10000:
# self._cf.commander.send_setpoint(0, 0, 0, thrust)
# time.sleep(0.1)
# thrust += thrust_step * thrust_mult
# cnt = 0
# while cnt < 1:
# self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
# cnt = cnt + 1
# time.sleep(0.1)
# print("cnt")
# while thrust >= 37000:
# self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
# time.sleep(0.1)
# thrust -= thrust_dstep * thrust_mult
self._cf.commander.send_setpoint(0, 0, 0, 0)
print('end', flush=True)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
time.sleep(2)
class RadioBridge:
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# UDP socket for interfacing with GCS
self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self._sock.bind(('127.0.0.1', 14551))
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print('Connecting to %s' % link_uri)
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
threading.Thread(target=self._server).start()
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print('Connected to %s' % link_uri)
self._cf.packet_received.add_callback(self._got_packet)
def _got_packet(self, pk):
if pk.port == CRTP_PORT_MAVLINK:
self._sock.sendto(pk.data, ('127.0.0.1', 14550))
def _forward(self, data):
pk = CRTPPacket()
pk.port = CRTP_PORT_MAVLINK # CRTPPort.COMMANDER
pk.data = data # struct.pack('<fffH', roll, -pitch, yaw, thrust)
self._cf.send_packet(pk)
def _server(self):
while True:
print('\nwaiting to receive message')
# Only receive what can be sent in one message
data, address = self._sock.recvfrom(256)
print('received %s bytes from %s' % (len(data), address))
for i in range(0, len(data), 30):
self._forward(data[i:(i+30)])
def _stab_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
print('[%d][%s]: %s' % (timestamp, logconf.name, data))
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
self.is_connected = False
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
class CrazyFlie():
def __init__(self, ble=False, sim_vel=np.array([0., 0., 0.])):
print("[CrazyFlie] Attaching CrazyFlie Plug with Keyboard handle")
if ble:
link = BLEDriver()
inter = link.scan_interface()[0][0]
# inter = "e0:c3:b3:86:a6:13"
link.connect(inter)
self._cf = Crazyflie(link=link, rw_cache="./")
self.rate = 0.05
print("[CrazyFlie] Connected to Crazyflie on bluetooth {}.".format(
inter))
else:
crtp.init_drivers()
self.inter = crtp.scan_interfaces()
self._cf = Crazyflie(rw_cache="./")
self._cf.open_link(self.inter[0][0])
time.sleep(1) # wait for a while to let crazyflie fetch the TOC
self.rate = 0.01
print("[CrazyFlie] Connected to Crazyflie on radio {}.".format(
self.inter))
self.roll = 0
self.pitch = 0
self.yaw = 0
self.hover_thrust = 36330
self.tilt_angle = 10
self.x_dot = 0.0
self.y_dot = 0.0
self.z_dot = 0.0
self.yaw_rate = 0.0
self.step = 0.1
self.max_xyz_speed = 0.4
self.max_yaw_rate = 90.
self.x, self.y, self.z = 0, 0, 0.5
self.running = True
self.roll_calib_offset = 0
self.pitch_calib_offset = 0
signal.signal(signal.SIGINT, self.interrupt_handle)
# tty.setcbreak(sys.stdin)
self.keyboard_handle = Listener(on_press=self.on_press,
on_release=self.on_release)
self.keyboard_handle.start()
# unlocking thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
self.sim_vel = sim_vel
# if self.sim_vel is not None:
self.hover_thread_flag = True
self.hover_thread = threading.Thread(target=self.hover_threaded)
# self.set_param(START_PROP_TEST)
# what to do on a key press
def on_press(self, key):
try:
if key.char == 'w':
self.z_dot = self.max_xyz_speed
self.hover_thrust += 500
self.z += 0.5
elif key.char == 's':
self.z_dot = -self.max_xyz_speed
self.hover_thrust -= 500
self.z -= 0.5
if key.char == 'a':
self.yaw -= 10
self.yaw_rate = -self.max_yaw_rate
elif key.char == 'd':
self.yaw += 10
self.yaw_rate = self.max_yaw_rate
if key.char == 'i':
self.pitch = self.tilt_angle
self.x_dot = self.max_xyz_speed
self.x += 0.1
elif key.char == 'k':
self.pitch = -self.tilt_angle
self.x_dot = -self.max_xyz_speed
self.x -= 0.1
if key.char == 'j':
self.roll = -self.tilt_angle
self.y_dot = self.max_xyz_speed
self.y += 0.5
elif key.char == 'l':
self.roll = self.tilt_angle
self.y_dot = -self.max_xyz_speed
self.y -= 0.5
if key.char == 'c':
self.x_dot = self.sim_vel[0]
self.y_dot = self.sim_vel[1]
self.z_dot = -self.sim_vel[2]
if key.char == 'r':
self.running = True
except AttributeError:
if key == Key.up:
self.pitch_calib_offset = min(self.pitch_calib_offset + 1, 5)
elif key == Key.down:
self.pitch_calib_offset = max(self.pitch_calib_offset - 1, -5)
if key == Key.left:
self.roll_calib_offset = min(self.roll_calib_offset + 1, 5)
elif key == Key.right:
self.roll_calib_offset = max(self.roll_calib_offset - 1, -5)
if key == Key.shift:
self.tilt_angle = min(self.tilt_angle + 5, 45)
self.max_xyz_speed = min(self.max_xyz_speed + self.step, 1)
self.max_yaw_rate = min(self.max_yaw_rate + 5, 90)
elif key == Key.ctrl:
self.tilt_angle = max(self.tilt_angle - 5, 5)
self.max_xyz_speed = max(self.max_xyz_speed - self.step, -1)
self.max_yaw_rate = max(self.max_yaw_rate - 5, -90)
# stop the crazyflie (to be used in case if Crazyflie is getting out of control)
if key == Key.space:
self.running = False
self._cf.commander.send_stop_setpoint()
# when a key is released
def on_release(self, key):
try:
if key.char == 'w' or key.char == 's':
self.z_dot = 0.
elif key.char == 'a' or key.char == 'd':
self.yaw_rate = 0.
elif key.char == 'i' or key.char == 'k':
self.pitch = 0.
self.x_dot = 0.
elif key.char == 'j' or key.char == 'l':
self.roll = 0.
self.y_dot = 0.
if key.char == 'c':
self.x_dot = 0.
self.y_dot = 0.
self.z_dot = 0.
except AttributeError:
if key == Key.esc:
return False
def interrupt_handle(self, signal, frame):
print('\n[CrazyFlie] Stopping')
self.close()
exit(0)
def close(self):
self.running = False
self._cf.commander.send_stop_setpoint()
self._cf.close_link()
Controller().press(Key.esc)
Controller().release(Key.esc)
self.keyboard_handle.join()
self.hover_thread_flag = False
self.hover_thread.join()
# termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings)
print("[CrazyFlie] Closed...")
def print_rpyt(self):
print("roll = {:.2f}, pitch = {:.2f}, yaw = {:.2f}, thrust = {:.2f}, SA = {:.2f}\r".format(\
self.roll, self.pitch, self.yaw, self.hover_thrust, self.tilt_angle), end="")
def print_vx_vy_vz_yr(self):
print("x_dot = {:.2f}, y_dot = {:.2f}, z_dot = {:.2f}, yaw_rate = {:.2f}, Max Speed = {:.2f}\r".format(\
self.x_dot, self.y_dot, self.z_dot, self.yaw_rate, self.max_xyz_speed), end="")
def takeoff(self):
for i in range(int(1 * 20)):
# self.print_vx_vy_vz_yr()
self._cf.commander.send_velocity_world_setpoint(0, 0, 0.1, 0)
# self._cf.commander.send_setpoint(self.roll + self.roll_calib_offset, \
# self.pitch + self.pitch_calib_offset, \
# self.yaw, 10000)
time.sleep(0.01)
# self._cf.commander.send_setpoint(0, 0, 0, self.hover_thrust)
def hover(self):
self.set_param(ALT_HOLD)
while self.running:
self.print_vx_vy_vz_yr()
# self.print_rpyt()
self._cf.commander.send_velocity_world_setpoint(
self.x_dot, self.y_dot, self.z_dot, self.yaw_rate)
# self._cf.commander.send_setpoint(self.roll + self.roll_calib_offset, \
# self.pitch + self.pitch_calib_offset, \
# self.yaw, self.hover_thrust)
time.sleep(self.rate)
def hover_threaded(self):
self.set_param(ALT_HOLD)
while self.hover_thread_flag:
while self.running:
self.print_vx_vy_vz_yr()
# self.print_rpyt()
self._cf.commander.send_velocity_world_setpoint(
self.x_dot, self.y_dot, self.z_dot, self.yaw_rate)
# self._cf.commander.send_setpoint(self.roll + self.roll_calib_offset, \
# self.pitch + self.pitch_calib_offset, \
# self.yaw, self.hover_thrust)
time.sleep(self.rate)
time.sleep(self.rate)
def land(self):
while self.hover_thrust != 0:
self.hover_thrust -= 2000
if self.hover_thrust < 0:
self.hover_thrust = 0
self.print_rpyt()
self._cf.commander.send_setpoint(0, 0, 0, self.hover_thrust)
time.sleep(0.4)
def set_param(self, value):
WRITE_CHANNEL = 2
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL)
pk.data = struct.pack('<H', value)
self._cf.send_packet(pk)
class MotorRampExample:
"""Example that connects to a Crazyflie and ramps the motors up/down and
the disconnects"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie(rw_cache='./cache')
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._cf.open_link(link_uri)
print('Connecting to %s' % link_uri)
self.is_connected = True
# change the parameters0,
self._param_check_list = []
self._param_groups = []
# def _connected(self, link_uri):
# """ This callback is called form the Crazyflie API when a Crazyflie
# has been connected and the TOCs have been downloaded."""
# # Start a separate thread to do the motor test.
# # Do not hijack the calling thread!
# Thread(target=self._ramp_motors).start()
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print('Connected to %s' % link_uri)
###########################################################
# LOG
# The definition of the logconfig can be made before connecting
# self._lg_stab = LogConfig(name='packet', period_in_ms=10)
# self._lg_stab.add_variable('crtp.COMMANDER_FLAG', 'uint8_t')
# self._lg_stab.add_variable('stabilizer.pitch', 'float')
# self._lg_stab.add_variable('stabilizer.yaw', 'float')
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
# try:
# self._cf.log.add_config(self._lg_stab)
# # This callback will receive the data
# self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# # This callback will be called on errors
# self._lg_stab.error_cb.add_callback(self._stab_log_error)
# # Start the logging
# self._lg_stab.start()
# except KeyError as e:
# print('Could not start log configuration,'
# '{} not found in TOC'.format(str(e)))
# except AttributeError:
# print('Could not add Stabilizer log config, bad configuration.')
###########################################################
# PARAM
# self._cf.param.add_update_callback(group='motorPowerSet', name='enable',
# cb=self._a_propTest_callback)
# self._cf.param.set_value('motorPowerSet.enable',
# '{:d}'.format(1))
# thv = 2000
# self._cf.param.set_value('motorPowerSet.m4',
# '{:d}'.format(8000))
# time.sleep(5)
# self._cf.param.set_value('motorPowerSet.m4',
# '{:d}'.format(0))
# self._cf.param.set_value('motorPowerSet.enable',
# '{:d}'.format(0))
###########################################################
# LOG
# self._lg_stab = LogConfig(name='motor', period_in_ms=100)
# self._lg_stab.add_variable('motor.m1', 'uint16_t')
# self._lg_stab.add_variable('motor.m2', 'uint16_t')
# self._lg_stab.add_variable('motor.m3', 'uint16_t')
# self._lg_stab.add_variable('motor.m4', 'uint16_t')
# try:
# self._cf.log.add_config(self._lg_stab)
# # This callback will receive the data
# self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# # This callback will be called on errors
# self._lg_stab.error_cb.add_callback(self._stab_log_error)
# # Start the logging
# self._lg_stab.start()
# except KeyError as e:
# print('Could not start log configuration,'
# '{} not found in TOC'.format(str(e)))
# except AttributeError:
# print('Could not add Stabilizer log config, bad configuration.')
# RAMP
# Thread(target=self._ramp_motors).start()
self._ramp_motors()
# Start a timer to disconnect in 1s
t = Timer(1, self._cf.close_link)
t.start()
def _a_propTest_callback(self, name, value):
"""Callback for pid_attitude.pitch_kd"""
print('Readback: {0}={1}'.format(name, value))
def _stab_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
print('[%d][%s]: %s' % (timestamp, logconf.name, data), flush=True)
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
print('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)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
def _ramp_motors(self):
thrust_mult = 1
thrust_step = 100
thrust_dstep = 10
thrust = 3000
pitch = 0
roll = 0
yawrate = 0
start_height = 0.3
target_height = 0.4 # the distance is not accurate, 1.2 => 1.5m
# Unlock startup thrust protection
pk = CRTPPacket()
pk.set_header(0xF, 2) # unlock drone
pk.data = '0'
self._cf.send_packet(pk)
# cnt = 0
# while cnt < 20:
# self._cf.commander.send_hover_setpoint(0, 0, 0, start_height)
# cnt = cnt + 1
# time.sleep(0.1)
print("up")
cnt = 0
while cnt < 50:
self._cf.commander.send_hover_setpoint(0, 0, 0, target_height)
# print(h)
cnt = cnt + 1
time.sleep(0.1)
cnt = 0
while cnt < 10:
self._cf.commander.send_keep_alive()
# print(h)
cnt = cnt + 1
time.sleep(0.1)
# print("move")
# cnt = 0
# while cnt < 20:
# self._cf.commander.send_hover_setpoint(0, 0, 0, target_height)
# cnt = cnt + 1
# time.sleep(0.1)
# cnt = 0
# while cnt < 40:
# self._cf.commander.send_hover_setpoint(0, 0, 0, target_height) # (forward, left)
# cnt = cnt + 1
# time.sleep(0.1)
print("down")
cnt = 0
while cnt < 20:
h = 0.2
# print(h)
self._cf.commander.send_hover_setpoint(0, 0, 0, h)
cnt += 1
time.sleep(0.1)
# while thrust >= 1000 and thrust < 10000:
# self._cf.commander.send_setpoint(0, 0, 0, thrust)
# time.sleep(0.1)
# thrust += thrust_step * thrust_mult
# cnt = 0
# while cnt < 1:
# self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
# cnt = cnt + 1
# time.sleep(0.1)
# print("cnt")
# while thrust >= 37000:
# self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
# time.sleep(0.1)
# thrust -= thrust_dstep * thrust_mult
self._cf.commander.send_setpoint(0, 0, 0, 0)
print('end', flush=True)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
time.sleep(3)
class MainUI(QtWidgets.QMainWindow, main_window_class):
connectionLostSignal = pyqtSignal(str, str)
connectionInitiatedSignal = pyqtSignal(str)
batteryUpdatedSignal = pyqtSignal(int, object, object)
connectionDoneSignal = pyqtSignal(str)
connectionFailedSignal = pyqtSignal(str, str)
disconnectedSignal = pyqtSignal(str)
linkQualitySignal = pyqtSignal(int)
_input_device_error_signal = pyqtSignal(str)
_input_discovery_signal = pyqtSignal(object)
_log_error_signal = pyqtSignal(object, str)
def __init__(self, *args):
super(MainUI, self).__init__(*args)
self.setupUi(self)
# Restore window size if present in the config file
try:
size = Config().get("window_size")
self.resize(size[0], size[1])
except KeyError:
pass
# self.setWindowState(QtCore.Qt.WindowMaximized)
######################################################
# By lxrocks
# 'Skinny Progress Bar' tweak for Yosemite
# Tweak progress bar - artistic I am not - so pick your own colors !!!
# Only apply to Yosemite
######################################################
import platform
if platform.system() == 'Darwin':
(Version, junk, machine) = platform.mac_ver()
logger.info("This is a MAC - checking if we can apply Progress "
"Bar Stylesheet for Yosemite Skinny Bars ")
yosemite = (10, 10, 0)
tVersion = tuple(map(int, (Version.split("."))))
if tVersion >= yosemite:
logger.info("Found Yosemite - applying stylesheet")
tcss = """
QProgressBar {
border: 1px solid grey;
border-radius: 5px;
text-align: center;
}
QProgressBar::chunk {
background-color: """ + COLOR_BLUE + """;
}
"""
self.setStyleSheet(tcss)
else:
logger.info("Pre-Yosemite - skinny bar stylesheet not applied")
######################################################
self.cf = Crazyflie(ro_cache=None,
rw_cache=cfclient.config_path + "/cache")
cflib.crtp.init_drivers(
enable_debug_driver=Config().get("enable_debug_driver"))
zmq_params = ZMQParamAccess(self.cf)
zmq_params.start()
zmq_leds = ZMQLEDDriver(self.cf)
zmq_leds.start()
self.scanner = ScannerThread()
self.scanner.interfaceFoundSignal.connect(self.foundInterfaces)
self.scanner.start()
# Create and start the Input Reader
self._statusbar_label = QLabel("No input-device found, insert one to"
" fly.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader()
self._active_device = ""
# self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._mux_group = QActionGroup(self._menu_inputdevice)
self._mux_group.setExclusive(True)
# TODO: Need to reload configs
# ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
self.connect_input = QShortcut("Ctrl+I", self.connectButton,
self._connect)
self.cf.connection_failed.add_callback(
self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self._connection_failed)
self._input_device_error_signal.connect(
self._display_input_device_error)
self.joystickReader.device_error.add_callback(
self._input_device_error_signal.emit)
self._input_discovery_signal.connect(self.device_discovery)
self.joystickReader.device_discovery.add_callback(
self._input_discovery_signal.emit)
# Hide the 'File' menu on OS X, since its only item, 'Exit', gets
# merged into the application menu.
if sys.platform == 'darwin':
self.menuFile.menuAction().setVisible(False)
# Connect UI signals
self.logConfigAction.triggered.connect(self._show_connect_dialog)
self.interfaceCombo.currentIndexChanged['QString'].connect(
self.interfaceChanged)
self.connectButton.clicked.connect(self._connect)
self.scanButton.clicked.connect(self._scan)
self.menuItemConnect.triggered.connect(self._connect)
self.menuItemConfInputDevice.triggered.connect(
self._show_input_device_config_dialog)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self._update_battery)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(
self._open_config_folder)
self.address.setValue(0xE7E7E7E7E7)
self._auto_reconnect_enabled = Config().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect"))
self._disable_input = False
self.joystickReader.input_updated.add_callback(
lambda *args: self._disable_input or self.cf.commander.
send_setpoint(*args))
self.joystickReader.assisted_input_updated.add_callback(
lambda *args: self._disable_input or self.cf.commander.
send_velocity_world_setpoint(*args))
self.joystickReader.heighthold_input_updated.add_callback(
lambda *args: self._disable_input or self.cf.commander.
send_zdistance_setpoint(*args))
self.joystickReader.hover_input_updated.add_callback(
self.cf.commander.send_hover_setpoint)
# Connection callbacks and signal wrappers for UI protection
self.cf.connected.add_callback(self.connectionDoneSignal.emit)
self.connectionDoneSignal.connect(self._connected)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(self._disconnected)
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self._connection_lost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(self._connection_initiated)
self._log_error_signal.connect(self._logging_error)
self.batteryBar.setTextVisible(False)
self.batteryBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
self.linkQualityBar.setTextVisible(False)
self.linkQualityBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
self._selected_interface = None
self._initial_scan = True
self._scan()
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
self._current_input_config = None
self._active_config = None
self._active_config = None
self.inputConfig = None
# Add things to helper so tabs can access it
cfclient.ui.pluginhelper.cf = self.cf
cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader
cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader
cfclient.ui.pluginhelper.mainUI = self
self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper)
self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper)
self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper)
self._cf1config_dialog = Cf1ConfigDialog(cfclient.ui.pluginhelper)
self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show)
self._about_dialog = AboutDialog(cfclient.ui.pluginhelper)
self.menuItemAbout.triggered.connect(self._about_dialog.show)
self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show)
self._menu_cf1_config.triggered.connect(self._cf1config_dialog.show)
# Load and connect tabs
self.tabsMenuItem = QMenu("Tabs", self.menuView, enabled=True)
self.menuView.addMenu(self.tabsMenuItem)
# self.tabsMenuItem.setMenu(QtWidgets.QMenu())
tabItems = {}
self.loadedTabs = []
for tabClass in cfclient.ui.tabs.available:
tab = tabClass(self.tabs, cfclient.ui.pluginhelper)
item = QtWidgets.QAction(tab.getMenuName(), self, checkable=True)
item.toggled.connect(tab.toggleVisibility)
self.tabsMenuItem.addAction(item)
tabItems[tab.getTabName()] = item
self.loadedTabs.append(tab)
if not tab.enabled:
item.setEnabled(False)
# First instantiate all tabs and then open them in the correct order
try:
for tName in Config().get("open_tabs").split(","):
logger.info("opening tabs [{}]".format(tName))
t = tabItems[tName]
if (t is not None and t.isEnabled()):
# Toggle though menu so it's also marked as open there
t.toggle()
except Exception as e:
logger.warning("Exception while opening tabs [{}]".format(e))
# Loading toolboxes (A bit of magic for a lot of automatic)
self.toolboxesMenuItem = QMenu("Toolboxes",
self.menuView,
enabled=True)
self.menuView.addMenu(self.toolboxesMenuItem)
self.toolboxes = []
for t_class in cfclient.ui.toolboxes.toolboxes:
toolbox = t_class(cfclient.ui.pluginhelper)
dockToolbox = MyDockWidget(toolbox.getName())
dockToolbox.setWidget(toolbox)
self.toolboxes += [
dockToolbox,
]
# Add menu item for the toolbox
item = QtWidgets.QAction(toolbox.getName(), self)
item.setCheckable(True)
item.triggered.connect(self.toggleToolbox)
self.toolboxesMenuItem.addAction(item)
dockToolbox.closed.connect(lambda: self.toggleToolbox(False))
# Setup some introspection
item.dockToolbox = dockToolbox
item.menuItem = item
dockToolbox.dockToolbox = dockToolbox
dockToolbox.menuItem = item
# References to all the device sub-menus in the "Input device" menu
self._all_role_menus = ()
# Used to filter what new devices to add default mapping to
self._available_devices = ()
# Keep track of mux nodes so we can enable according to how many
# devices we have
self._all_mux_nodes = ()
# Check which Input muxes are available
self._mux_group = QActionGroup(self._menu_inputdevice)
self._mux_group.setExclusive(True)
for m in self.joystickReader.available_mux():
node = QAction(m.name,
self._menu_inputdevice,
checkable=True,
enabled=False)
node.toggled.connect(self._mux_selected)
self._mux_group.addAction(node)
self._menu_inputdevice.addAction(node)
self._all_mux_nodes += (node, )
mux_subnodes = ()
for name in m.supported_roles():
sub_node = QMenu(" {}".format(name),
self._menu_inputdevice,
enabled=False)
self._menu_inputdevice.addMenu(sub_node)
mux_subnodes += (sub_node, )
self._all_role_menus += ({
"muxmenu": node,
"rolemenu": sub_node
}, )
node.setData((m, mux_subnodes))
self._mapping_support = True
self.sendHexButton.clicked.connect(self.send_hex)
def send_hex(self):
import struct
from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
hex_string = self.hexEdit.text()
hex_string = hex_string.replace(":", " ")
pk = CRTPPacket()
pk.header = 0x00
pk.data = bytes.fromhex(hex_string)
self.cf.send_packet(pk)
def disable_input(self, disable):
"""
Disable the gamepad input to be able to send setpoint from a tab
"""
self._disable_input = disable
def interfaceChanged(self, interface):
if interface == INTERFACE_PROMPT_TEXT:
self._selected_interface = None
else:
self._selected_interface = interface
self._update_ui_state()
def foundInterfaces(self, interfaces):
selected_interface = self._selected_interface
self.interfaceCombo.clear()
self.interfaceCombo.addItem(INTERFACE_PROMPT_TEXT)
formatted_interfaces = []
for i in interfaces:
if len(i[1]) > 0:
interface = "%s - %s" % (i[0], i[1])
else:
interface = i[0]
formatted_interfaces.append(interface)
self.interfaceCombo.addItems(formatted_interfaces)
if self._initial_scan:
self._initial_scan = False
try:
if len(Config().get("link_uri")) > 0:
formatted_interfaces.index(Config().get("link_uri"))
selected_interface = Config().get("link_uri")
except KeyError:
# The configuration for link_uri was not found
pass
except ValueError:
# The saved URI was not found while scanning
pass
if len(interfaces) == 1 and selected_interface is None:
selected_interface = interfaces[0][0]
newIndex = 0
if selected_interface is not None:
try:
newIndex = formatted_interfaces.index(selected_interface) + 1
except ValueError:
pass
self.interfaceCombo.setCurrentIndex(newIndex)
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
def _update_ui_state(self):
if self.uiState == UIState.DISCONNECTED:
self.setWindowTitle("Not connected")
canConnect = self._selected_interface is not None
self.menuItemConnect.setText("Connect to Crazyflie")
self.menuItemConnect.setEnabled(canConnect)
self.connectButton.setText("Connect")
self.connectButton.setToolTip("Connect to the Crazyflie on"
"the selected interface (Ctrl+I)")
self.connectButton.setEnabled(canConnect)
self.scanButton.setText("Scan")
self.scanButton.setEnabled(True)
self.address.setEnabled(True)
self.batteryBar.setValue(3000)
self._menu_cf2_config.setEnabled(False)
self._menu_cf1_config.setEnabled(True)
self.linkQualityBar.setValue(0)
self.menuItemBootloader.setEnabled(True)
self.logConfigAction.setEnabled(False)
self.interfaceCombo.setEnabled(True)
elif self.uiState == UIState.CONNECTED:
s = "Connected on %s" % self._selected_interface
self.setWindowTitle(s)
self.menuItemConnect.setText("Disconnect")
self.menuItemConnect.setEnabled(True)
self.connectButton.setText("Disconnect")
self.connectButton.setToolTip("Disconnect from"
"the Crazyflie (Ctrl+I)")
self.scanButton.setEnabled(False)
self.logConfigAction.setEnabled(True)
# Find out if there's an I2C EEPROM, otherwise don't show the
# dialog.
if len(self.cf.mem.get_mems(MemoryElement.TYPE_I2C)) > 0:
self._menu_cf2_config.setEnabled(True)
self._menu_cf1_config.setEnabled(False)
elif self.uiState == UIState.CONNECTING:
s = "Connecting to {} ...".format(self._selected_interface)
self.setWindowTitle(s)
self.menuItemConnect.setText("Cancel")
self.menuItemConnect.setEnabled(True)
self.connectButton.setText("Cancel")
self.connectButton.setToolTip("Cancel connecting to the Crazyflie")
self.scanButton.setEnabled(False)
self.address.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.interfaceCombo.setEnabled(False)
elif self.uiState == UIState.SCANNING:
self.setWindowTitle("Scanning ...")
self.connectButton.setText("Connect")
self.menuItemConnect.setEnabled(False)
self.connectButton.setText("Connect")
self.connectButton.setEnabled(False)
self.scanButton.setText("Scanning...")
self.scanButton.setEnabled(False)
self.address.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.interfaceCombo.setEnabled(False)
@pyqtSlot(bool)
def toggleToolbox(self, display):
menuItem = self.sender().menuItem
dockToolbox = self.sender().dockToolbox
if display and not dockToolbox.isVisible():
dockToolbox.widget().enable()
self.addDockWidget(dockToolbox.widget().preferedDockArea(),
dockToolbox)
dockToolbox.show()
elif not display:
dockToolbox.widget().disable()
self.removeDockWidget(dockToolbox)
dockToolbox.hide()
menuItem.setChecked(False)
def _rescan_devices(self):
self._statusbar_label.setText("No inputdevice connected!")
self._menu_devices.clear()
self._active_device = ""
self.joystickReader.stop_input()
# for c in self._menu_mappings.actions():
# c.setEnabled(False)
# devs = self.joystickReader.available_devices()
# if (len(devs) > 0):
# self.device_discovery(devs)
def _show_input_device_config_dialog(self):
self.inputConfig = InputConfigDialogue(self.joystickReader)
self.inputConfig.show()
def _auto_reconnect_changed(self, checked):
self._auto_reconnect_enabled = checked
Config().set("auto_reconnect", checked)
logger.info("Auto reconnect enabled: {}".format(checked))
def _show_connect_dialog(self):
self.logConfigDialogue.show()
def _update_battery(self, timestamp, data, logconf):
self.batteryBar.setValue(int(data["pm.vbat"] * 1000))
color = COLOR_BLUE
# TODO firmware reports fully-charged state as 'Battery',
# rather than 'Charged'
if data["pm.state"] in [BatteryStates.CHARGING, BatteryStates.CHARGED]:
color = COLOR_GREEN
elif data["pm.state"] == BatteryStates.LOW_POWER:
color = COLOR_RED
self.batteryBar.setStyleSheet(progressbar_stylesheet(color))
self._aff_volts.setText(("%.3f" % data["pm.vbat"]))
def _connected(self):
self.uiState = UIState.CONNECTED
self._update_ui_state()
Config().set("link_uri", str(self._selected_interface))
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
lg.add_variable("pm.state", "int8_t")
try:
self.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
mems = self.cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED)
if len(mems) > 0:
mems[0].write_data(self._led_write_done)
def _disconnected(self):
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
def _connection_initiated(self):
self.uiState = UIState.CONNECTING
self._update_ui_state()
def _led_write_done(self, mem, addr):
logger.info("LED write done callback")
def _logging_error(self, log_conf, msg):
QMessageBox.about(
self, "Log error", "Error when starting log config"
" [{}]: {}".format(log_conf.name, msg))
def _connection_lost(self, linkURI, msg):
if not self._auto_reconnect_enabled:
if self.isActiveWindow():
warningCaption = "Communication failure"
error = "Connection lost to {}: {}".format(linkURI, msg)
QMessageBox.critical(self, warningCaption, error)
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self._connect()
def _connection_failed(self, linkURI, error):
if not self._auto_reconnect_enabled:
msg = "Failed to connect on {}: {}".format(linkURI, error)
warningCaption = "Communication failure"
QMessageBox.critical(self, warningCaption, msg)
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self._connect()
def closeEvent(self, event):
self.hide()
self.cf.close_link()
Config().save_file()
def resizeEvent(self, event):
Config().set(
"window_size",
[event.size().width(), event.size().height()])
def _connect(self):
if self.uiState == UIState.CONNECTED:
self.cf.close_link()
elif self.uiState == UIState.CONNECTING:
self.cf.close_link()
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self.cf.open_link(self._selected_interface)
def _scan(self):
self.uiState = UIState.SCANNING
self._update_ui_state()
self.scanner.scanSignal.emit(self.address.value())
def _display_input_device_error(self, error):
self.cf.close_link()
QMessageBox.critical(self, "Input device error", error)
def _mux_selected(self, checked):
"""Called when a new mux is selected. The menu item contains a
reference to the raw mux object as well as to the associated device
sub-nodes"""
if not checked:
(mux, sub_nodes) = self.sender().data()
for s in sub_nodes:
s.setEnabled(False)
else:
(mux, sub_nodes) = self.sender().data()
for s in sub_nodes:
s.setEnabled(True)
self.joystickReader.set_mux(mux=mux)
# Go though the tree and select devices/mapping that was
# selected before it was disabled.
for role_node in sub_nodes:
for dev_node in role_node.children():
if type(dev_node) is QAction and dev_node.isChecked():
dev_node.toggled.emit(True)
self._update_input_device_footer()
def _get_dev_status(self, device):
msg = "{}".format(device.name)
if device.supports_mapping:
map_name = "No input mapping"
if device.input_map:
map_name = device.input_map_name
msg += " ({})".format(map_name)
return msg
def _update_input_device_footer(self):
"""Update the footer in the bottom of the UI with status for the
input device and its mapping"""
msg = ""
if len(self.joystickReader.available_devices()) > 0:
mux = self.joystickReader._selected_mux
msg = "Using {} mux with ".format(mux.name)
for key in list(mux._devs.keys())[:-1]:
if mux._devs[key]:
msg += "{}, ".format(self._get_dev_status(mux._devs[key]))
else:
msg += "N/A, "
# Last item
key = list(mux._devs.keys())[-1]
if mux._devs[key]:
msg += "{}".format(self._get_dev_status(mux._devs[key]))
else:
msg += "N/A"
else:
msg = "No input device found"
self._statusbar_label.setText(msg)
def _inputdevice_selected(self, checked):
"""Called when a new input device has been selected from the menu. The
data in the menu object is the associated map menu (directly under the
item in the menu) and the raw device"""
(map_menu, device, mux_menu) = self.sender().data()
if not checked:
if map_menu:
map_menu.setEnabled(False)
# Do not close the device, since we don't know exactly
# how many devices the mux can have open. When selecting a
# new mux the old one will take care of this.
else:
if map_menu:
map_menu.setEnabled(True)
(mux, sub_nodes) = mux_menu.data()
for role_node in sub_nodes:
for dev_node in role_node.children():
if type(dev_node) is QAction and dev_node.isChecked():
if device.id == dev_node.data()[1].id \
and dev_node is not self.sender():
dev_node.setChecked(False)
role_in_mux = str(self.sender().parent().title()).strip()
logger.info("Role of {} is {}".format(device.name, role_in_mux))
Config().set("input_device", str(device.name))
self._mapping_support = self.joystickReader.start_input(
device.name, role_in_mux)
self._update_input_device_footer()
def _inputconfig_selected(self, checked):
"""Called when a new configuration has been selected from the menu. The
data in the menu object is a referance to the device QAction in parent
menu. This contains a referance to the raw device."""
if not checked:
return
selected_mapping = str(self.sender().text())
device = self.sender().data().data()[1]
self.joystickReader.set_input_map(device.name, selected_mapping)
self._update_input_device_footer()
def device_discovery(self, devs):
"""Called when new devices have been added"""
for menu in self._all_role_menus:
role_menu = menu["rolemenu"]
mux_menu = menu["muxmenu"]
dev_group = QActionGroup(role_menu)
dev_group.setExclusive(True)
for d in devs:
dev_node = QAction(d.name,
role_menu,
checkable=True,
enabled=True)
role_menu.addAction(dev_node)
dev_group.addAction(dev_node)
dev_node.toggled.connect(self._inputdevice_selected)
map_node = None
if d.supports_mapping:
map_node = QMenu(" Input map", role_menu, enabled=False)
map_group = QActionGroup(role_menu)
map_group.setExclusive(True)
# Connect device node to map node for easy
# enabling/disabling when selection changes and device
# to easily enable it
dev_node.setData((map_node, d))
for c in ConfigManager().get_list_of_configs():
node = QAction(c,
map_node,
checkable=True,
enabled=True)
node.toggled.connect(self._inputconfig_selected)
map_node.addAction(node)
# Connect all the map nodes back to the device
# action node where we can access the raw device
node.setData(dev_node)
map_group.addAction(node)
# If this device hasn't been found before, then
# select the default mapping for it.
if d not in self._available_devices:
last_map = Config().get("device_config_mapping")
if d.name in last_map and last_map[d.name] == c:
node.setChecked(True)
role_menu.addMenu(map_node)
dev_node.setData((map_node, d, mux_menu))
# Update the list of what devices we found
# to avoid selecting default mapping for all devices when
# a new one is inserted
self._available_devices = ()
for d in devs:
self._available_devices += (d, )
# Only enable MUX nodes if we have enough devies to cover
# the roles
for mux_node in self._all_mux_nodes:
(mux, sub_nodes) = mux_node.data()
if len(mux.supported_roles()) <= len(self._available_devices):
mux_node.setEnabled(True)
# TODO: Currently only supports selecting default mux
if self._all_mux_nodes[0].isEnabled():
self._all_mux_nodes[0].setChecked(True)
# If the previous length of the available devies was 0, then select
# the default on. If that's not available then select the first
# on in the list.
# TODO: This will only work for the "Normal" mux so this will be
# selected by default
if Config().get("input_device") in [d.name for d in devs]:
for dev_menu in self._all_role_menus[0]["rolemenu"].actions():
if dev_menu.text() == Config().get("input_device"):
dev_menu.setChecked(True)
else:
# Select the first device in the first mux (will always be "Normal"
# mux)
self._all_role_menus[0]["rolemenu"].actions()[0].setChecked(True)
logger.info("Select first device")
self._update_input_device_footer()
def _open_config_folder(self):
QDesktopServices.openUrl(
QUrl("file:///" + QDir.toNativeSeparators(cfclient.config_path)))
def closeAppRequest(self):
subprocess.Popen("./run_cfclient.sh stop", shell=True)
self.close()
class MotorRampExample:
"""Example that connects to a Crazyflie and ramps the motors up/down and
the disconnects"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie(rw_cache='./cache')
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._cf.open_link(link_uri)
print('Connecting to %s' % link_uri)
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self._ramp_motors).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
print('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)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
def _ramp_motors(self):
thrust_mult = 1
thrust_step = 100
thrust_base = 3000
thrust = thrust_base
pitch = 0
roll = 0
yawrate = 0
# Unlock startup thrust protection
pk = CRTPPacket()
pk.set_header(0xF, 2) # unlock drone
pk.data = '0'
self._cf.send_packet(pk)
while thrust >= thrust_base:
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.1)
if thrust >= thrust_base + 1000:
thrust_mult = -1
thrust += thrust_step * thrust_mult
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.2)
self._cf.close_link()
class CrazyRadioClient:
"""
CrazyRadio client that recieves the commands from the controller and
sends them in a CRTP package to the crazyflie with the specified
address.
"""
def __init__(self):
# Setpoints to be sent to the CrazyFlie
self.roll = 0.0
self.pitch = 0.0
self.yaw = 0.0
self.motor1cmd = 0.0
self.motor2cmd = 0.0
self.motor3cmd = 0.0
self.motor4cmd = 0.0
self._status = DISCONNECTED
self.link_uri = ""
# self.status_pub = rospy.Publisher(node_name + '/CrazyRadioStatus', Int32, queue_size=1)
# self.FlyingAgentClient_command_pub = rospy.Publisher('FlyingAgentClient/Command', Int32, queue_size=1)
time.sleep(1.0)
# Initialize the CrazyFlie and add callbacks
self._init_cf()
# Connect to the Crazyflie
self.connect()
def _init_cf(self):
self._cf = Crazyflie()
# Add callbacks that get executed depending on the connection _status.
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
def get_status(self):
return self._status
def update_link_uri(self):
self.link_uri = "radio://0/79/2M"
def connect(self):
# update link from ros params
self.update_link_uri()
print "Connecting to %s" % self.link_uri
self._cf.open_link(self.link_uri)
def disconnect(self):
print "Motors OFF"
self._send_to_commander(0, 0, 0, 0, 0, 0, 0, 0, CONTROLLER_MOTOR)
print "Disconnecting from %s" % self.link_uri
self._cf.close_link()
def _data_received_callback(self, timestamp, data, logconf):
#print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
batteryVolt = Float32()
stabilizerYaw = Float32()
stabilizerPitch = Float32()
stabilizerRoll = Float32()
batteryVolt.data = data["pm.vbat"]
stabilizerYaw.data = data["stabilizer.yaw"]
stabilizerPitch.data = data["stabilizer.pitch"]
def _logging_error(self, logconf, msg):
print "Error when logging %s" % logconf.name
# def _init_logging(self):
def _start_logging(self):
self.logconf = LogConfig("LoggingTest",
100) # second variable is freq in ms
self.logconf.add_variable("stabilizer.roll", "float")
self.logconf.add_variable("stabilizer.pitch", "float")
self.logconf.add_variable("stabilizer.yaw", "float")
self.logconf.add_variable("pm.vbat", "float")
self._cf.log.add_config(self.logconf)
if self.logconf.valid:
self.logconf.data_received_cb.add_callback(
self._data_received_callback)
self.logconf.error_cb.add_callback(self._logging_error)
print "logconf valid"
else:
print "logconf invalid"
self.logconf.start()
print "logconf start"
def _connected(self, link_uri):
"""
This callback is executed as soon as the connection to the
quadrotor is established.
"""
# cf_client._send_to_commander(15000, 15000, 15000, 15000);
# cf_client._send_to_commander_rate(1000, 0, 1000, 0, 1, 1, 1)
cf_client._send_to_commander_angle(1000, 0, 1000, 0, 1, 1, 1)
print "sent command to commander"
# Config for Logging
self._start_logging()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
rospy.logerr("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)"""
rospy.logerr("Connection to %s lost: %s" % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
self.logconf.stop()
rospy.loginfo("logconf stopped")
self.logconf.delete()
rospy.loginfo("logconf deleted")
def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<BHHHH', CF_COMMAND_TYPE_MOTORS, cmd1, cmd2,
cmd3, cmd4)
self._cf.send_packet(pk)
def _send_to_commander_rate(self, cmd1, cmd2, cmd3, cmd4, roll_rate,
pitch_rate, yaw_rate):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_RATE, cmd1, cmd2,
cmd3, cmd4, roll_rate, pitch_rate, yaw_rate)
self._cf.send_packet(pk)
def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch,
yaw):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_ANGLE, cmd1, cmd2,
cmd3, cmd4, roll, pitch, yaw)
self._cf.send_packet(pk)
class CrazyflieController():
def __init__(self, link_uri, radio_port, serialport, xmode=True):
"""Initialize the headless client and libraries"""
from cfclient.utils.input import JoystickReader
self._cf = Crazyflie()
self._serial = serialport
self._cf.commander.set_client_xmode(xmode)
self._cf.open_link(link_uri)
self._port = radio_port
self.is_connected = True
self.running = True
self._console = ""
# Callbacks
self._cf.connected.add_callback(
lambda msg: print("Connected to " + msg))
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.console.receivedChar.add_callback(self._console_char)
self._cf.packet_received.add_callback(self._data_received)
# Search for controllers
self._jr = JoystickReader(do_device_discovery=False)
available_devices = self._jr.available_devices()
if len(available_devices):
used_device = available_devices[0]
print("Using input device: " + str(used_device))
self._jr.device_error.add_callback(
lambda msg: print("Input device error: " + str(msg)))
self._jr.start_input(used_device.name)
self._jr.set_input_map(used_device.name, "xbox360_mode1")
# Controller callback
self._jr.input_updated.add_callback(
self._cf.commander.send_setpoint)
else:
print("No input device detected")
self.serialthread = threading.Thread(target=self._serial_listener)
self.serialthread.start()
def stop(self):
self.running = False
self.serialthread.join()
self._cf.close_link()
def _console_char(self, pk):
self._console = self._console + str(pk)
if "\n" in self._console:
print("Console: " + str(self._console), end="")
self._console = ""
def _disconnected(self, link_uri):
print("Disconnected...")
if self.running:
self.stop()
def _connection_failed(self, link_uri, msg):
print("Conection failed...")
if self.running:
self.stop()
def _serial_listener(self):
while self.running and self.is_connected:
data = self._serial.read_until(size=30)
if (data):
pk = CRTPPacket()
pk.port = self._port
pk.data = data
pk.size = len(data)
self._cf.send_packet(pk)
#print("Got data from agent: len {}, data {}".format(pk.size, pk.data))
def _data_received(self, pk):
if pk and pk.port == self._port:
self._serial.write(pk.data)
self._serial.flush()
class DroneController:
COLOR_BALL_TRACKED = (255, 0, 0)
COLOR_BALL_UNTRACKED = (0, 0, 255)
COLOR_LINE_TRACKED = (255, 0, 0)
COLOR_LINE_UNTRACKED = (0, 0, 255)
COLOR_TARGET_TRACKED = (0, 255, 0)
COLOR_TARGET_UNTRACKED = (0, 0, 255)
FIGURE_NAME = "Output"
SETTINGS_ENVIRONMENT = "Very bright bedroom"
CAMERA_SETTINGS_FILE = "config/cam_settings/Camera settings - USB 2.0 Camera - {}.txt".format(SETTINGS_ENVIRONMENT)
COLOR_THRESH_SETTINGS_FILE = "config/color_thresh/Color threshold settings - {}.txt".format(SETTINGS_ENVIRONMENT)
BLOB_DETECTOR_SETTINGS_FILE = "config/blob_detector/Blob detector settings.txt"
SETTINGS_SEPARATOR = UvcCapture.SETTINGS_SEPARATOR # We save files in a csv type of way
ASK_FOR_TARGET_YAW = False
TAKEOFF_THRUST = 44000
def __init__(self):
self.t_start = self.t_frame = self.t_last_frame = datetime.now()
self.t_events = []
self.EXPERIMENT_START_DATETIME = str(self.t_start)[:-7].replace(':', '-')
self.VIDEO_FOLDER = "img-ns/{}".format(self.EXPERIMENT_START_DATETIME)
self.experiment_log = plot_tools.ExperimentLog(self.EXPERIMENT_START_DATETIME, {"Roll": "piv", "Pitch": "piv", "Yaw": "pid", "Thrust": "piv", "Estimated_Z": "log", "Velocity_Z": "log"})
self.window_for_kb_input = None
self.video_capture = None
self.cv_HSV_thresh_min = np.array([ 0, 0, 0], dtype=np.uint8)
self.cv_HSV_thresh_max = np.array([255, 255, 255], dtype=np.uint8)
self.cv_blob_detect_params = None
self.cv_cam_frame = None
self.cv_filtered_HSV_mask = None
self.cv_frame_out = None
self.crazyflie = None
self.cf_log = None
self.cf_radio_connecting = True
self.cf_radio_connected = False
self.cf_ignore_camera = False
self.cf_pos_tracked = False
self.cf_taking_off = True
self.cf_str_status = "TAKING OFF"
self.cf_roll = self.cf_pitch = self.cf_yaw = self.cf_estimated_z = self.cf_vel_z = 0
self.cf_curr_pos = np.array([0, 0, 0])
self.cf_PID_roll = PID.PIDposAndVel(posP=0.5, velP=0.05, velI=0.01, vel_offs=0, pos_out_max=300, vel_out_max=30, vel_invert_error=True)
self.cf_PID_pitch = PID.PIDposAndVel(posP=0.7, velP=0.3, velI=0.002, vel_offs=0, pos_out_max=30, vel_out_max=30)
self.cf_PID_yaw = PID.PID(P=0.5, I=0.3, D=0, offs=0, out_max=20, invert_error=True, error_in_degrees=True)
self.cf_PID_thrust = PID.PIDposAndVel(posP=1, velP=35, velI=25, vel_offs=43000, pos_out_max=300, vel_out_max=7000, pos_invert_error=True, vel_invert_input=True)
def init_video_cam_and_cv_algorithm(self, create_video_folder=True):
"""
Initializes camera: connects to it, loads settings from config file (if available),
loads color threshold and blob detector settings (if available), creates a folder to save cv output images...
:param create_video_folder: True to create folder specified by VIDEO_FOLDER (where output frames will be saved)
"""
self.video_capture = UvcCapture.new_from_settings(self.CAMERA_SETTINGS_FILE) # Connect to device specified by settings, and load its desired param values
if self.video_capture is None: # If unable to connect to device specified by settings, open first available camera
self.video_capture = UvcCapture(0)
if self.video_capture is None: # If still unable to connect, raise an exception
raise Exception("Couldn't open camera! :(")
# If we're here, we couldn't connect to device specified by settings but were able to open 1st available cam
if not self.video_capture.load_settings(self.CAMERA_SETTINGS_FILE): # Try to load frame size & rate from settings
self.video_capture.select_best_frame_mode(60) # If loading settings failed, choose best frame size with fps >= 60
logging.info("Camera opened! :)")
# Initialize cv algorithm too: load settings for color thresholding and blob detector
self.load_color_thresh_settings()
self.load_blob_detector_settings()
# Sometimes, first couple frames take a long time to be obtained, do it before quad goes in flying mode
self.video_capture.get_frame_robust()
self.video_capture.get_frame_robust()
# Initialize PID setpoints and initial input value to the center of the frame
self.cf_PID_roll.setSetPoint(self.video_capture.frame_size[0] / 2)
self.cf_PID_thrust.setSetPoint(self.video_capture.frame_size[1] / 2)
self.cf_PID_pitch.setSetPoint(40)
self.cf_PID_roll.PIDpos.curr_input = self.cf_PID_roll.getSetPoint()
self.cf_PID_thrust.PIDpos.curr_input = self.cf_PID_thrust.getSetPoint()
self.cf_PID_pitch.PIDpos.curr_input = self.cf_PID_pitch.getSetPoint()
# Prepare the folder self.VIDEO_FOLDER so we can store each frame we processed (for debugging)
if create_video_folder:
shutil.rmtree(self.VIDEO_FOLDER, ignore_errors=True) # Delete the folder and its contents, if it exists (ignore errors if it doesn't)
os.makedirs(self.VIDEO_FOLDER) # Now create the folder, which won't throw any exceptions as we made sure it didn't already exist
def init_UI_window(self):
"""
Initializes the appropriate user interface depending on experiment's purpose (see is_input_for_drone_commands)
:param is_input_for_drone_commands: True if we have communication with the CF and need input to send it commands
False if we're just debugging the computer vision side (camera settings, color threshold, etc.) and need input
to debug pixel information
"""
# Open an SDL2 window to track keyboard keydown events and use it to send commands to the CF
sdl2.ext.init()
self.window_for_kb_input = sdl2.ext.Window("Window to receive keyboard input", size=(400, 300))
self.window_for_kb_input.show()
def save_color_thresh_settings(self, HSV_min, HSV_max, file_name=COLOR_THRESH_SETTINGS_FILE, sep=SETTINGS_SEPARATOR):
"""
Saves specified color threshold settings to a file.
:param HSV_min: np.array (1x3) containing minimum H, S and V values for the color thresholding CF detection
:param HSV_max: np.array (1x3) containing maximum H, S and V values for the color thresholding CF detection
:param file_name: Name of the file to use when saving the settings
:param sep: String that will be used to separate parameters (in a csv fashion). Eg: '\t', ',', etc.
:return: True if everything went well; False if settings couldn't be saved
"""
try:
logging.debug("Saving current color threshold settings to file '{}'".format(file_name))
with open(file_name, 'w') as f: # Open file for output
f.write("{}\n".format(sep.join(HSV_min.astype(str)))) # Store HSV_min
f.write("{}\n".format(sep.join(HSV_max.astype(str)))) # Store HSV_max
except:
logging.exception("Something went wrong while saving current color threshold settings to '{}'.".format(file_name))
return False
return True
def load_color_thresh_settings(self, file_name=COLOR_THRESH_SETTINGS_FILE, sep=SETTINGS_SEPARATOR):
"""
Loads color threshold settings from a file.
:param file_name: Path to the file to load the settings from
:param sep: String that was used to separate parameters (in a csv fashion). Eg: '\t', ',', etc.
:return: True if everything went well; False if settings couldn't be loaded
"""
try:
logging.debug("Loading color threshold settings from file '{}'".format(file_name))
with open(file_name, 'r') as f: # Open file for input
self.cv_HSV_thresh_min = np.array(f.readline().rstrip('\r\n').split(sep), dtype=np.uint8)
self.cv_HSV_thresh_max = np.array(f.readline().rstrip('\r\n').split(sep), dtype=np.uint8)
logging.debug("\tLoaded color threshold settings: HSV_min={}; HSV_max={}".format(self.cv_HSV_thresh_min, self.cv_HSV_thresh_max))
except:
logging.exception("Something went wrong while loading color threshold settings from '{}'.".format(file_name))
return False
return True
def save_blob_detector_settings(self, detector_params, file_name=BLOB_DETECTOR_SETTINGS_FILE, sep=SETTINGS_SEPARATOR):
"""
Saves specified blob detector settings to a file.
:param detector_params: cv2.SimpleBlobDetector_Params object containing the params which want to be saved
:param file_name: Name of the file to use when saving the settings
:param sep: String that will be used to separate parameters (in a csv fashion). Eg: '\t', ',', etc.
:return: True if everything went well; False if settings couldn't be saved
"""
try:
logging.debug("Saving current blob detector settings to file '{}'".format(file_name))
with open(file_name, 'w') as f: # Open file for output
for m in dir(detector_params): # Traverse all methods and properties of detector_params
if m[0] != '_': # For every property that's not "built-in" (ie: doesn't start by '_')
f.write("{}{}{}\n".format(m, sep, getattr(detector_params, m))) # Store the property name and value
except:
logging.exception("Something went wrong while saving current blob detector settings to '{}'.".format(file_name))
return False
return True
def load_blob_detector_settings(self, file_name=BLOB_DETECTOR_SETTINGS_FILE, sep=SETTINGS_SEPARATOR):
"""
Loads blob detector settings from a file. Leave file_name empty to only load default params.
cv2 will use these params to detect the drone from a binary image mask (the output of the color thresholding step).
:param file_name: Path to the file to load the settings from
:param sep: String that was used to separate parameters (in a csv fashion). Eg: '\t', ',', etc.
:return: True if everything went well; False if settings couldn't be loaded
"""
detector_params = cv2.SimpleBlobDetector_Params()
self.cv_blob_detect_params = detector_params
# Filter by color
detector_params.filterByColor = False
detector_params.blobColor = 255
# Change thresholds
detector_params.minThreshold = 254
detector_params.maxThreshold = 255
# Filter by Area.
detector_params.filterByArea = True
detector_params.minArea = 30
detector_params.maxArea = 40000
# Filter by Circularity
detector_params.filterByCircularity = False
detector_params.minCircularity = 0.7
detector_params.maxCircularity = 1.0
# Filter by Convexity
detector_params.filterByConvexity = False
detector_params.minConvexity = 0.7
detector_params.maxConvexity = 1.0
# Filter by Inertia
detector_params.filterByInertia = False
detector_params.minInertiaRatio = 0.5
detector_params.maxInertiaRatio = 1.0
detector_params.minRepeatability = 1
detector_params.minDistBetweenBlobs = 3000
try:
logging.debug("Loading blob detector settings from file '{}'".format(file_name))
with open(file_name, 'r') as f: # Open file for input
for line in f: # Every line contains one property: name + sep + value
name, value = line.split(sep)
setattr(detector_params, name, eval(value)) # Use eval to cast to right type (eg: False instead of "False")
logging.debug("\tLoaded blob detector setting: '{}' = {}".format(name, value))
except:
logging.exception("Something went wrong while loading blob detector settings from '{}'.".format(file_name))
return False
return True
def connect_to_cf(self, retry_after=10, max_timeout=20):
"""
Initializes radio drivers, looks for available CrazyRadios, and attempts to connect to the first available one.
Doesn't return anything, but raises exceptions if anything goes wrong.
:param retry_after: Time in seconds after which we should abort current connection and restart the attempt.
:param max_timeout: Time in seconds after which we should give up if we haven't been able to establish comm. yet
"""
logging.info("Initializing drivers.")
crtp.init_drivers(enable_debug_driver=False)
logging.info("Setting up the communication link. Looking for available CrazyRadios in range.")
available_links = crtp.scan_interfaces()
if len(available_links) == 0:
raise Exception("Error, no Crazyflies found. Exiting.")
else:
logging.info("CrazyFlies found:") # For debugging purposes, show info about available links
for i in available_links:
logging.info("\t" + i[0])
link_uri = available_links[0][0] # Choose first available link
logging.info("Initializing CrazyFlie (connecting to first available interface: '{}').".format(link_uri))
self.crazyflie = Crazyflie(ro_cache="cachero", rw_cache="cacherw") # Create an instance of Crazyflie
self.crazyflie.connected.add_callback(self.on_cf_radio_connected) # Set up callback functions for communication feedback
self.crazyflie.disconnected.add_callback(self.on_cf_radio_disconnected)
self.crazyflie.connection_failed.add_callback(self.on_cf_radio_conn_failed)
self.crazyflie.connection_lost.add_callback(self.on_cf_radio_conn_lost)
cnt = 0 # Initialize a time counter
while self.cf_radio_connecting and cnt < max_timeout:
if cnt % retry_after == 0:
if cnt > 0: # Only show warning after first failed attempt
logging.warning("Unable to establish communication with Crazyflie ({}) after {}s. Retrying...".format(link_uri, retry_after))
self.crazyflie.close_link() # Closing the link will call on_disconnect, which will set cf_radio_connecting to False
self.cf_radio_connecting = True # Reset cf_radio_connecting back to True
self.crazyflie.open_link(link_uri) # Connect/Reconnect to the CrazyRadio through the selected interface
time.sleep(1) # Sleep for a second (give time for callback functions to detect whether we are connected)
cnt += 1 # Increase the "waiting seconds" counter
if cnt >= max_timeout:
self.crazyflie.close_link()
raise Exception("Unable to establish communication with CrazyFlie after {}s. Given up :(".format(max_timeout))
elif not self.cf_radio_connected:
raise Exception("Something failed while attempting to connect to the CrazyFlie, exiting.")
# self.crazyflie.commander.send_setpoint(0, 0, 0, 0) # If we successfully connected to the CF, send thrust=0 (new firmware initializes thrustLock=True, only way to unlock it so it executes commands is by setting thrust=0)
def setup_cf(self):
"""
Sets up the CrazyFlie before running the experiment. This includes configuring some params to default values
and requesting the CrazyFlie to log certain variables back at constant intervals.
Doesn't return anything, but raises exceptions if anything goes wrong.
"""
try: # Send some default values for CF params
self.crazyflie.param.set_value('controller.tiltComp', '{:d}'.format(True))
self.crazyflie.param.set_value('flightmode.poshold', '{:d}'.format(False)) # Disable poshold and althold by default
self.crazyflie.param.set_value('flightmode.althold', '{:d}'.format(False))
self.crazyflie.param.set_value('flightmode.posSet', '{:d}'.format(False))
self.crazyflie.param.set_value('flightmode.yawMode', '0')
self.crazyflie.param.set_value('flightmode.timeoutStab', '{:d}'.format(1000*60*10)) # Stabilize (rpy=0) CF if doesn't receive a radio command in 10min
self.crazyflie.param.set_value('flightmode.timeoutShut', '{:d}'.format(1000*60*20)) # Shutdown CF if doesn't receive a radio command in 20min
self.crazyflie.param.set_value('posCtlPid.thrustBase', '{}'.format(self.TAKEOFF_THRUST))
self.crazyflie.param.set_value("ring.effect", "1") # Turn off LED ring
self.crazyflie.param.set_value("ring.headlightEnable", "0") # Turn off LED headlight
except Exception as e:
raise Exception("Couldn't initialize CrazyFlie params to their desired values. Details: {}".format(e.message))
# Create a log configuration and include all variables that want to be logged
self.cf_log = LogConfig(name="cf_log", period_in_ms=10)
self.cf_log.add_variable("stabilizer.roll", "float")
self.cf_log.add_variable("stabilizer.pitch", "float")
self.cf_log.add_variable("stabilizer.yaw", "float")
self.cf_log.add_variable("posEstimatorAlt.estimatedZ", "float")
self.cf_log.add_variable("posEstimatorAlt.velocityZ", "float")
try:
self.crazyflie.log.add_config(self.cf_log) # Validate the log configuration and attach it to our CF
except Exception as e:
raise Exception("Couldn't attach the log config to the CrazyFlie, bad configuration. Details: {}".format(e.message))
self.cf_log.data_received_cb.add_callback(self.on_cf_log_new_data) # Register appropriate callbacks
self.cf_log.error_cb.add_callback(self.on_cf_log_error)
self.cf_log.start() # Start logging!
# Get the CF's initial yaw (should be facing the camera) so we can have PID_yaw maintain that orientation
if self.ASK_FOR_TARGET_YAW: # Either ask the user to press Enter to indicate the CF's orientation is ready
raw_input("\nRotate the drone so it faces the camera, press Enter when you're ready...\n")
else: # Or automatically detect the first yaw log packet and set the current orientation as the desired yaw
while abs(self.cf_yaw) < 0.01: # Wait until first cf_yaw value is received (cf_yaw=0 by default)
time.sleep(0.1)
self.cf_PID_yaw.SetPoint = self.cf_yaw
print "Target yaw set at {:.2f}.".format(self.cf_yaw)
self.crazyflie.add_port_callback(CRTPPort.CONSOLE, self.print_cf_console)
self.crazyflie.commander.send_setpoint(0, 0, 0, 0) # New firmware version requires to send thrust=0 at least once to "unlock thrust"
def fly_cf(self):
"""
Provides the structure to make the drone fly (actual flight control is done in hover() though).
hover() is called until user stops the experiment, then hover is called for a few more secs to record more data
(usually includes information about the CF crashing...)
:return: Whether or not to keep (store) the logs, based on whether the CF ever actually took off and flew
"""
print "Giving you 5s to prepare for take-off..."
time.sleep(5)
# t = Timer(20, self.m_CrazyFlie.close_link) # Start a timer to automatically disconnect in 20s
# t.start()
# Prepare for take off: clear PIDs, log start time...
self.cf_str_status = "TAKING OFF"
self.t_start = datetime.now()
self.cf_PID_roll.clear()
self.cf_PID_pitch.clear()
self.cf_PID_yaw.clear()
self.cf_PID_thrust.clear()
# Alright, let's fly!
tStop = None
while tStop is None: # tStop will remain None while everything's fine
tStop = self.hover() # hover returns None while everything's fine; the time to end the experiment otherwise
# If we get here, either the user stopped the experiment or the code detected something went wrong
print "AT t={}, A KEY WAS PRESSED -> STOPPING!".format(datetime.now().strftime("%H:%M:%S.%f")[:-3])
save_logs = (self.cf_str_status != "TAKING OFF") # Only store the logs if the drone ever started flying (not just took off)
self.cf_str_status = "STOPPED" # Updated new drone status
while datetime.now() < tStop: # Keep calling hover until tStop, so data is still logged
self.hover()
return save_logs # Return whether or not to keep the logs
def cleanup_experiment(self, save_logs=True):
"""
"Cleans up" the experiment: closes any open windows, saves logs, disconnects camera and drone, etc.
"""
# If we get here, either the user ended the experiment, or an exception occurred. Same action regardless:
if self.cf_log is not None: # If we were logging data from the CF, stop it (will reconnect faster next time)
self.cf_log.stop()
self.cf_log.delete()
if self.crazyflie is not None: # If the CF was ever initialized, make sure the communication link is closed
self.crazyflie.close_link()
self.video_capture = None # Destroy the video capture object (this takes care of closing the camera etc.)
cv2.destroyAllWindows() # Close all UI windows that could be open
if self.window_for_kb_input is not None:
self.window_for_kb_input.hide()
sdl2.ext.quit()
if save_logs: # If the experiment didn't crash before starting (the CF ever took off), plot and save the logs
self.experiment_log.plot(False)
self.experiment_log.save()
else: # Otherwise just delete the folder (and its contents) where cam frames would have been/were saved
shutil.rmtree(self.VIDEO_FOLDER, ignore_errors=False)
def run_experiment(self):
"""
DroneController's main method: initializes all components (vision, communication, drone params, etc.) then
runs the experiment.
"""
logging.disable(logging.DEBUG) # Seems to work better than .basicConfig(INFO), especially if logging has already been initialized -> Only show messages of level INFO or higher
save_logs = True # Use this auxiliary variable to prevent saving logs if the drone never took off
try:
self.connect_to_cf() # Connect to the CrazyFlie
self.setup_cf() # Can't initialize LogConfig until we're connected, because it needs to check that the variables we'd like to add are in the TOC. So this function must be called after connect_to_cf()
self.init_video_cam_and_cv_algorithm() # Connect to the first available camera, load default settings, etc.
self.init_UI_window() # Open a window to receive user input to control the CF
save_logs = self.fly_cf() # And finally fly the CF
except:
logging.exception("Shutting down due to an exception =( See details below:")
# If we got here, either the user ended the experiment, or an exception occurred. Same action regardless:
self.cleanup_experiment(save_logs) # "Cleanup" the experiment: close windows, save logs, etc.
def get_cf_target_pos(self):
"""
:return: np.array containing the current (estimated) 3D position of the CrazyFlie
"""
return np.array([int(round(x)) for x in [self.cf_PID_roll.getSetPoint(), self.cf_PID_thrust.getSetPoint(), self.cf_PID_pitch.getSetPoint()]])
def get_cf_curr_pos(self):
"""
:return: np.array containing the current (estimated) 3D position of the CrazyFlie
"""
return np.array([self.cf_PID_roll.getCurrPos(), self.cf_PID_thrust.getCurrPos(), self.cf_PID_pitch.getCurrPos()])
def get_cf_curr_vel(self):
"""
:return: np.array containing the current (estimated) 3D velocity of the CrazyFlie
"""
return np.array([self.cf_PID_roll.getCurrVel(), self.cf_PID_thrust.getCurrVel(), self.cf_PID_pitch.getCurrVel()])
def on_cf_radio_connected(self, link_uri):
logging.info("Successfully connected to Crazyflie at '{}'!".format(link_uri))
self.cf_radio_connecting = False
self.cf_radio_connected = True
def on_cf_radio_conn_failed(self, link_uri, msg):
logging.error("Connection to '{}' failed: {}.".format(link_uri, msg)) # Initial connection fails (i.e no Crazyflie at the speficied address)
self.cf_radio_connecting = False
self.cf_radio_connected = False
def on_cf_radio_conn_lost(self, link_uri, msg):
logging.warning("Connection to '{}' lost: {}.".format(link_uri, msg)) # Disconnected after a connection has been made (i.e Crazyflie moves out of range)
def on_cf_radio_disconnected(self, link_uri):
logging.error("Disconnected from '{}'.".format(link_uri)) # Crazyflie is disconnected (called in all cases)
self.cf_radio_connecting = False
self.cf_radio_connected = False
def on_cf_log_error(self, logconf, msg):
logging.error("Error when logging %s: %s." % (logconf.name, msg))
def on_cf_log_new_data(self, timestamp, data, logconf):
logging.debug("[%d][%s]: %s" % (timestamp, logconf.name, data))
self.cf_roll = data['stabilizer.roll']
self.cf_pitch = data['stabilizer.pitch']
self.cf_yaw = data['stabilizer.yaw']
self.cf_estimated_z = data['posEstimatorAlt.estimatedZ']
self.cf_vel_z = data['posEstimatorAlt.velocityZ']
print "\rCurrent yaw: {:.2f}deg".format(self.cf_yaw),
def print_cf_console(self, packet):
console_text = packet.data.decode('UTF-8')
print("Console: {}".format(console_text))
def send_cf_param(self, complete_name, value):
"""
Modified version of crazyflie.param.set_value that sends the packet immediately (instead of using a Thread+Queue
"""
element = self.crazyflie.param.toc.get_element_by_complete_name(complete_name)
if not element:
raise KeyError("Couldn't set {}={}, param is not in the TOC!".format(complete_name, value))
elif element.access == ParamTocElement.RO_ACCESS:
raise AttributeError("Couldn't set {}={}, param is read-only!".format(complete_name, value))
else:
varid = element.ident
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL)
pk.data = struct.pack('<B', varid)
pk.data += struct.pack(element.pytype, eval(value))
self.crazyflie.send_packet(pk, expected_reply=(tuple(pk.data[0:2])))
def process_kb_input(self):
"""
Processes all keydown events, and takes the corresponding action depending on which key was pressed.
:return: Boolean indicating whether the experiment shall go on: True while everything's fine, False to stop it
"""
events = sdl2.ext.get_events() # Fetch any new input event
for event in events: # Iterate through all of them
if event.type == sdl2.SDL_KEYDOWN: # And only process keydown events
key_orig = event.key.keysym.sym # Fetch the key that was pressed down
try:
key = chr(key_orig) # Try to convert the key to char
except: # Usually, this exeption occurs when a key can't be converted to char (arrows, Esc, etc.)
if key_orig == sdl2.SDLK_UP:
key = "Up"
elif key_orig == sdl2.SDLK_DOWN:
key = "Down"
elif key_orig == sdl2.SDLK_ESCAPE:
key = "Esc"
else:
key = hex(key_orig)
logging.info("Key: '{}'".format(key))
self.window_for_kb_input.title = "Last key pressed: '{}' at t={}".format(key, str(datetime.now().time())[:-3])
key = key.lower() # Convert to lowercase so we don't have to worry about different cases
if key == 'a': # Move left
self.cf_PID_roll.setSetPoint(self.cf_PID_roll.getSetPoint() + 20)
elif key == 's': # Move back
self.cf_PID_pitch.setSetPoint(max(self.cf_PID_pitch.getSetPoint() - 2, 1)) # Size (ball radius) can't be negative! Make sure depth value is at least 1px
elif key == 'd': # Move right
self.cf_PID_roll.setSetPoint(self.cf_PID_roll.getSetPoint() - 20)
elif key == 'w': # Move forward
self.cf_PID_pitch.setSetPoint(self.cf_PID_pitch.getSetPoint() + 2)
elif key == 'u': # Move up
self.cf_PID_thrust.setSetPoint(self.cf_PID_thrust.getSetPoint() - 20)
elif key == 'h': # Move down
self.cf_PID_thrust.setSetPoint(self.cf_PID_thrust.getSetPoint() + 20)
elif key == 'f': # Toggle altitude hold mode
if self.cf_ignore_camera: self.cf_PID_thrust.clear() # If we were ignoring the camera for Z, thrust PID will have a wrong I component
self.cf_ignore_camera = not self.cf_ignore_camera
self.cf_str_status = "NO CAM for Z" if self.cf_ignore_camera else "FULL CAM"
# self.crazyflie.param.set_value('flightmode.althold', '{:d}'.format(self.cf_ignore_camera))
# while not self.crazyflie.param.param_updater.request_queue.empty(): # Wait for the packet to be sent
# time.sleep(0.01)
self.send_cf_param('flightmode.althold', '{:d}'.format(self.cf_ignore_camera))
elif key == 'e': # Stop taking off and start flying (hover at current position)
self.cf_taking_off = False
self.cf_str_status = "FLYING"
self.cf_PID_roll.setSetPoint(self.cf_PID_roll.getCurrPos())
self.cf_PID_pitch.setSetPoint(self.cf_PID_pitch.getCurrPos() + 2)
self.cf_PID_thrust.setSetPoint(self.cf_PID_thrust.getCurrPos())
self.cf_PID_roll.clear()
self.cf_PID_pitch.clear()
self.cf_PID_thrust.clear()
else: # Any other key ends the experiment
# self.crazyflie.param.set_value('flightmode.althold', '{:d}'.format(False)) # Make sure we're not on althold mode, so sending a thrust 0 will kill the motors and not just descend
self.send_cf_param('flightmode.althold', '{:d}'.format(False)) # Make sure we're not on althold mode, so sending a thrust 0 will kill the motors and not just descend
return False
return True
def detect_cf_in_camera(self, frame=None, find_blob=True):
"""
Runs an iteration of the computer vision algorithm that estimates the CrazyFlie's position in 3D. That is,
it captures a frame from the camera, converts it to HSV, filters by color, and detects a blob.
Saves the 3D position in cf_curr_pos, camera frame in cv_cam_frame and color filter mask in cv_filtered_HSV_mask.
"""
#########################################################
# CAPTURE FRAME #
#########################################################
self.t_events = [datetime.now()]
self.cv_cam_frame = frame # Allow the function to receive a frame if we already got it from the camera
if self.cv_cam_frame is None: # Otherwise, if frame is None (default), grab a frame from the camera
try:
uvc_frame = self.video_capture.get_frame_robust() # read() blocks execution until a new frame arrives! -> Obtain t AFTER grabbing the frame
self.t_events.append(datetime.now())
self.t_frame = self.t_events[-1]
self.cv_cam_frame = uvc_frame.bgr # .copy()
self.t_events.append(datetime.now())
except Exception as e:
raise Exception("Unexpected error accessing the camera frame :( Details: {}.".format(e.message))
#########################################################
# COLOR THRESHOLD #
#########################################################
# self.cv_cam_frame = cv2.resize(self.cv_cam_frame, None, fx=0.5, fy=0.5)
# self.t_events.append(datetime.now())
# self.cv_cam_frame = cv2.GaussianBlur(self.cv_cam_frame, (3, 3), 0) # Not needed, camera is already physically "blurring" (sharpness parameter set to 0)
# self.t_events.append(datetime.now())
frame_HSV = cv2.cvtColor(self.cv_cam_frame, cv2.COLOR_BGR2HSV)
self.t_events.append(datetime.now())
self.cv_filtered_HSV_mask = cv2.inRange(frame_HSV, self.cv_HSV_thresh_min, self.cv_HSV_thresh_max)
self.t_events.append(datetime.now())
self.cv_filtered_HSV_mask = cv2.morphologyEx(self.cv_filtered_HSV_mask, cv2.MORPH_OPEN, cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5)))
self.t_events.append(datetime.now())
#########################################################
# DRONE DETECTION #
#########################################################
if find_blob:
keypoints = cv2.SimpleBlobDetector_create(self.cv_blob_detect_params).detect(self.cv_filtered_HSV_mask)
self.t_events.append(datetime.now())
self.cf_pos_tracked = bool(keypoints) # For now we determine the CF's position is tracked if we find at least 1 blob
if keypoints: # If the cv algorithm detected at least one blob
keypoint = max(keypoints, key=attrgetter('size')) # Focus on the biggest blob
self.cf_curr_pos = np.hstack((keypoint.pt, keypoint.size/2)) # And save the position estimated by the CV algorithm
def control_cf(self):
"""
Sends messages to control the CrazyFlie (roll-pitch-yaw-thrust setpoints) using the current location and PID loops.
:param drone_curr_pos: 3D np.array containing the current (estimated) position of the drone
"""
if not self.cf_pos_tracked: # If cv algorithm wasn't able to detect the drone, linearly estimate its position based on previous position and speed
# Since PIDs haven't been updated with current values yet, don't have to multiply velocity by (curr_time-last_time) but rather by (t_frame-curr_time)
self.cf_curr_pos = \
np.array([self.cf_PID_roll.getCurrPos(), self.cf_PID_thrust.getCurrPos(), self.cf_PID_pitch.getCurrPos()]) + \
np.array([self.cf_PID_roll.getCurrVel(), -self.cf_PID_thrust.getCurrVel(), self.cf_PID_pitch.getCurrVel()]) * \
(self.t_frame - self.cf_PID_thrust.PIDvel.curr_time).total_seconds()
self.cf_curr_pos[2] = max(1, self.cf_curr_pos[2]) # Make sure 3D (size) stays positive! (Prevents errors further down the line)
# Update PID loops with new position at t=self.t_frame
self.cf_PID_roll.update(self.cf_curr_pos[0], self.t_frame)
self.cf_PID_pitch.update(self.cf_curr_pos[2], self.t_frame)
self.cf_PID_yaw.update(self.cf_yaw, self.t_frame)
self.cf_PID_thrust.update(self.cf_curr_pos[1], self.t_frame)
# Log all relevant variables after each iteration
self.experiment_log.update(roll=self.cf_PID_roll, pitch=self.cf_PID_pitch, yaw=self.cf_PID_yaw, thrust=self.cf_PID_thrust, estimated_z=self.cf_estimated_z, velocity_z=self.cf_vel_z)
# Send the appropriate roll-pitch-yaw-thrust setpoint depending on the scenario (eg: taking off, stopping, etc.)
if self.cf_radio_connected: # While the experiment is running
if self.cf_taking_off: # If taking off, send a constant RPYT setpoint that will make the CF go up "straight"
# self.crazyflie.commander.send_setpoint(self.cf_PID_roll.PIDvel.out_offs, self.cf_PID_pitch.PIDvel.out_offs, self.cf_PID_yaw.output, self.TAKEOFF_THRUST)
self.crazyflie.commander.send_setpoint(self.cf_PID_roll.PIDvel.out_offs, self.cf_PID_pitch.PIDvel.out_offs, 0, self.TAKEOFF_THRUST)
else: # This condition holds ever after take off (once the user presses the key to start "flying")
if self.cf_ignore_camera: # If user selected it, control the drone in althold mode (altitude is done on-board, rest is still controlled with the cam)
# self.crazyflie.commander.send_setpoint(self.cf_PID_roll.getOutput(), self.cf_PID_pitch.getOutput(), self.cf_PID_yaw.output, 32767)
self.crazyflie.commander.send_setpoint(self.cf_PID_roll.getOutput(), self.cf_PID_pitch.getOutput(), 0, 32767)
elif not self.cf_pos_tracked: # If we couldn't find the drone, don't send any commands (not to mislead it)
pass
else: # Otherwise, just use the camera to control all 4 independent axes
# self.crazyflie.commander.send_setpoint(self.cf_PID_roll.getOutput(), self.cf_PID_pitch.getOutput(), self.cf_PID_yaw.output, self.cf_PID_thrust.getOutput())
self.crazyflie.commander.send_setpoint(self.cf_PID_roll.getOutput(), self.cf_PID_pitch.getOutput(), 0, self.cf_PID_thrust.getOutput())
else: # If the user has decided to end the experiment, kill the motors and reset PIDs (this is not really necessary)
self.crazyflie.commander.send_setpoint(0, 0, 0, 0)
self.cf_PID_roll.clear()
self.cf_PID_pitch.clear()
self.cf_PID_yaw.clear()
self.cf_PID_thrust.clear()
self.t_events.append(datetime.now())
def get_OSD_text(self, t):
"""
Generates written debug information (OSD=on-screen display) to be displayed at the bottom of the current frame
:param t: Datetime at which camera frame was obtained (through datetime.now())
:return: String containing relevant debug information (PID values, drone estimated position, etc.)
"""
formatNum = "{:+6.2f}"
strPrint = ("ROLLp. ={:+3.0f};" + formatNum + " [" + formatNum + "," + formatNum + "," + formatNum + "]\t\t" +
"ROLLv. ={:+3.0f};" + formatNum + " [" + formatNum + "," + formatNum + "," + formatNum + "]\t\t" +
"PITCHp={:+3.0f};" + formatNum + " [" + formatNum + "," + formatNum + "," + formatNum + "]\t\t" +
"PITCHv={:+3.0f};" + formatNum + " [" + formatNum + "," + formatNum + "," + formatNum + "]\t\t" +
"YAW.. ={:+3.0f};" + formatNum + " [" + formatNum + "," + formatNum + "," + formatNum + "]\t\t" +
"THRUSp={:3.0f};{:6.0f} [{:+6.0f}, {:+6.0f}, {:+6.0f}]\t\t" +
"THRUSv={:3.0f};{:6.0f} [{:+6.0f}, {:+6.0f}, {:+6.0f}]\t\t" +
"[x:{:4.0f}, y:{:4.0f}, z:{:4.0f}], [vx:{:4.0f}, vy:{:4.0f}, vz:{:4.0f}], " +
"rpy: " + formatNum + "," + formatNum + "," + formatNum + "]\t\t" + "@{} (FPS: {:5.2f}) - " + self.cf_str_status).format(
self.cf_PID_roll.PIDpos.SetPoint, self.cf_PID_roll.PIDpos.output, self.cf_PID_roll.PIDpos.PTerm, self.cf_PID_roll.PIDpos.Ki * self.cf_PID_roll.PIDpos.ITerm, self.cf_PID_roll.PIDpos.Kd * self.cf_PID_roll.PIDpos.DTerm,
self.cf_PID_roll.PIDvel.SetPoint, self.cf_PID_roll.PIDvel.output, self.cf_PID_roll.PIDvel.PTerm, self.cf_PID_roll.PIDvel.Ki * self.cf_PID_roll.PIDvel.ITerm, self.cf_PID_roll.PIDvel.Kd * self.cf_PID_roll.PIDvel.DTerm,
self.cf_PID_pitch.PIDpos.SetPoint, self.cf_PID_pitch.PIDpos.output, self.cf_PID_pitch.PIDpos.PTerm, self.cf_PID_pitch.PIDpos.Ki * self.cf_PID_pitch.PIDpos.ITerm, self.cf_PID_pitch.PIDpos.Kd * self.cf_PID_pitch.PIDpos.DTerm,
self.cf_PID_pitch.PIDvel.SetPoint, self.cf_PID_pitch.PIDvel.output, self.cf_PID_pitch.PIDvel.PTerm, self.cf_PID_pitch.PIDvel.Ki * self.cf_PID_pitch.PIDvel.ITerm, self.cf_PID_pitch.PIDvel.Kd * self.cf_PID_pitch.PIDvel.DTerm,
self.cf_PID_yaw.SetPoint, self.cf_PID_yaw.output, self.cf_PID_yaw.PTerm, self.cf_PID_yaw.Ki * self.cf_PID_yaw.ITerm, self.cf_PID_yaw.Kd * self.cf_PID_yaw.DTerm,
self.cf_PID_thrust.PIDpos.SetPoint, self.cf_PID_thrust.PIDpos.output, self.cf_PID_thrust.PIDpos.PTerm, self.cf_PID_thrust.PIDpos.Ki * self.cf_PID_thrust.PIDpos.ITerm, self.cf_PID_thrust.PIDpos.Kd * self.cf_PID_thrust.PIDpos.DTerm,
self.cf_PID_thrust.PIDvel.SetPoint, self.cf_PID_thrust.PIDvel.output, self.cf_PID_thrust.PIDvel.PTerm, self.cf_PID_thrust.PIDvel.Ki * self.cf_PID_thrust.PIDvel.ITerm, self.cf_PID_thrust.PIDvel.Kd * self.cf_PID_thrust.PIDvel.DTerm,
self.cf_PID_roll.getCurrPos(), self.cf_PID_thrust.getCurrPos(), self.cf_PID_pitch.getCurrPos(), self.cf_PID_roll.getCurrVel(), self.cf_PID_thrust.getCurrVel(), self.cf_PID_pitch.getCurrVel(), self.cf_roll, self.cf_pitch, self.cf_yaw, str(t-self.t_start)[3:-3], 1./(t-self.t_last_frame).total_seconds())
# logging.debug(strPrint)
return " SP | SENT [ P , I , D ]\t\t" + strPrint
def save_algo_iteration(self, str_OSD="", newline_separator='\t\t', margin_x=25, margin_y=25, text_color=(200, 200, 200), font=cv2.FONT_HERSHEY_DUPLEX, font_scale=0.7, font_thickness=1, line_type=cv2.LINE_AA, mask_color=(255, 0, 255), img_resize_factor=0.5, save_cam_frame_before_resizing=True):
if str_OSD == "": # Allow for custom OSD text, but if no text specified, print the default debug info (get_OSD_text)
str_OSD = self.get_OSD_text(self.t_frame)
self.t_events.append(datetime.now())
# Resize camera frame and CrazyFlie's current&target positions according to img_resize_factor
curr_pos_resized = self.get_cf_curr_pos()*img_resize_factor
target_pos_resized = self.get_cf_target_pos()*img_resize_factor
frame_resized = cv2.resize(self.cv_cam_frame, None, fx=img_resize_factor, fy=img_resize_factor)
mask_resized = cv2.resize(self.cv_filtered_HSV_mask, None, fx=img_resize_factor, fy=img_resize_factor)
# Save the original camera frame to disk (for post-debugging if necessary)
###### cv2.imwrite(os.path.join(self.VIDEO_FOLDER, self.t_frame.strftime("frame_%H-%M-%S-%f.jpg")), self.cv_cam_frame if save_cam_frame_before_resizing else frame_resized)
self.t_events.append(datetime.now())
# Plot OSD related to CF's current and target positions (2 circles and a connecting line)
cv2.circle(frame_resized, tuple(curr_pos_resized[0:2].astype(int)), int(curr_pos_resized[2]), self.COLOR_BALL_TRACKED if self.cf_pos_tracked else self.COLOR_BALL_UNTRACKED, -1)
cv2.line(frame_resized, tuple(curr_pos_resized[0:2].astype(int)), tuple(target_pos_resized[0:2].astype(int)), self.COLOR_LINE_TRACKED if self.cf_pos_tracked else self.COLOR_LINE_UNTRACKED, int(10*img_resize_factor))
cv2.circle(frame_resized, tuple(target_pos_resized[0:2].astype(int)), int(target_pos_resized[2]), self.COLOR_TARGET_TRACKED if self.cf_pos_tracked else self.COLOR_TARGET_UNTRACKED, -1)
self.t_events.append(datetime.now())
# On top of that, overlay the HSV mask (so we can debug color filtering + blob detection steps)
np.putmask(frame_resized, cv2.cvtColor(mask_resized, cv2.COLOR_GRAY2BGR).astype(bool), list(mask_color))
self.t_events.append(datetime.now())
# Generate the output image: upper part is the cam frame downsized according to img_resize_factor; lower part, str_OSD
lines = str_OSD.split(newline_separator)
self.cv_frame_out = np.zeros(((frame_resized.shape[0] + margin_y*(len(lines)+1)), frame_resized.shape[1], frame_resized.shape[2]), dtype=frame_resized.dtype)
self.cv_frame_out[0:frame_resized.shape[0], :] = frame_resized
self.t_events.append(datetime.now())
for cnt, l in enumerate(lines): # Add every line of text in str_OSD. Note that putText asks for bottom-left corner of text and that cnt=0 for 1st line. Therefore vertical component should be frame_resized height + OSD padding/border (0.5*margin_y) + text height (1*margin_y)
cv2.putText(self.cv_frame_out, l.replace('\t', '; '), (margin_x, frame_resized.shape[0] + int(margin_y*(cnt+1.4))), font, font_scale, text_color, font_thickness, line_type)
# Save the output image to disk (for post-debugging if necessary)
cv2.imwrite(os.path.join(self.VIDEO_FOLDER, self.t_frame.strftime("out_%H-%M-%S-%f.jpg")), self.cv_frame_out)
self.t_events.append(datetime.now())
def hover(self):
try: # First, run the cv algorithm to estimate the CF's position
self.detect_cf_in_camera()
except: # Only way detect_cf_in_camera raises an Exception is if a camera frame couldn't be grabbed
logging.exception("Couldn't grab a frame from the camera. Exiting")
return datetime.now() # Need to stop now (if I wanted to stop in now+2sec and camera kept throwing exceptions, it would keep delaying the stop and never actually stop)
# Now that we know where the drone currently is, send messages to control it (roll-pitch-yaw-thrust setpoints)
self.control_cf()
# And save the intermediate and output frames/images to disk for debugging
self.save_algo_iteration()
# Last, process kb input to control the experiment
if not self.process_kb_input(): # Process kb input and take action if necessary (will return False when user wants to stop the experiment)
self.cf_radio_connected = False # Set connected to False so next calls to this function just send thrust=0 messages
return datetime.now() + timedelta(seconds=2) # For debugging purposes, it's great to have a few additional seconds of video&log after an experiment is stopped (helps see why it crashed)
self.t_events.append(datetime.now())
logging.debug("DeltaT = {:5.2f}ms -> Total: {:5.2f}ms{}".format(
(datetime.now() - self.t_frame).total_seconds()*1000, (self.t_frame - self.t_last_frame).total_seconds()*1000,
"".join(["\t{}->{}: {}ms".format(i+1, i+2, (self.t_events[i+1]-self.t_events[i]).total_seconds()*1000) for i in range(len(self.t_events) - 1)])))
self.t_last_frame = self.t_frame # Remember the time this frame was taken so we can estimate FPS in next iteration
return None # Return None to indicate that the experiment shall go on. All good.
def estimate_cf_circle_depth(self):
if False:
t1 = datetime.now()
_, contours, _ = cv2.findContours(self.cv_filtered_HSV_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
contour = contours[0]
r = cv2.boundingRect(contour)
c = cv2.minEnclosingCircle(contour)
cv2.rectangle(self.cv_frame_out, r[0:2], (r[0] + r[2], r[1] + r[3]), (0, 255, 0), 2, cv2.LINE_AA)
cv2.circle(self.cv_frame_out, tuple(int(x) for x in c[0]), int(c[1]), (0, 255, 0), 2, cv2.LINE_AA)
t2 = datetime.now()
print "\t\t{} ms;\t\t\tc:{}\t\tblob:{}".format((t2-t1).total_seconds()*1000, c[1], self.cf_curr_pos[2])
class CrazyRadioClient:
"""
CrazyRadio client that recieves the commands from the controller and
sends them in a CRTP package to the crazyflie with the specified
address.
"""
def __init__(self):
# Setpoints to be sent to the CrazyFlie
self.roll = 0.0
self.pitch = 0.0
self.yaw = 0.0
self.motor1cmd = 0.0
self.motor2cmd = 0.0
self.motor3cmd = 0.0
self.motor4cmd = 0.0
self._status = DISCONNECTED
self.link_uri = ""
self.status_pub = rospy.Publisher(node_name + '/CrazyRadioStatus',
Int32,
queue_size=1)
self.FlyingAgentClient_command_pub = rospy.Publisher(
'FlyingAgentClient/Command', IntWithHeader, queue_size=1)
time.sleep(1.0)
# Initialize the CrazyFlie and add callbacks
self._init_cf()
# Connect to the Crazyflie
self.connect()
def _init_cf(self):
self._cf = Crazyflie()
# Add callbacks that get executed depending on the connection _status.
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self.change_status_to(DISCONNECTED)
def change_status_to(self, new_status):
print("[CRAZY RADIO] 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://0/60/2M"
def connect(self):
# update link from ros params
self.update_link_uri()
print("[CRAZY RADIO] Connecting to %s" % self.link_uri)
self.change_status_to(CONNECTING)
rospy.loginfo("[CRAZY RADIO] connecting...")
self._cf.open_link(self.link_uri)
def disconnect(self):
print("[CRAZY RADIO] sending Motors OFF command before disconnecting")
self._send_to_commander_motor(0, 0, 0, 0)
# change state to motors OFF
msg = IntWithHeader()
msg.shouldCheckForAgentID = False
msg.data = CMD_CRAZYFLY_MOTORS_OFF
self.FlyingAgentClient_command_pub.publish(msg)
time.sleep(0.1)
print("[CRAZY RADIO] Disconnecting from %s" % self.link_uri)
self._cf.close_link()
self.change_status_to(DISCONNECTED)
def data_received_callback(self, timestamp, data, logconf):
#print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
batteryVolt = Float32()
stabilizerYaw = Float32()
stabilizerPitch = Float32()
stabilizerRoll = Float32()
batteryVolt.data = data["pm.vbat"]
stabilizerYaw.data = data["stabilizer.yaw"]
stabilizerPitch.data = data["stabilizer.pitch"]
#bag.write('batteryVoltage', batteryVolt)
#bag.write('stabilizerYaw', stabilizerYaw)
#bag.write('stabilizerPitch', stabilizerPitch)
#bag.write('stabilizerRoll', stabilizerRoll)
#publish battery voltage for GUI
#cfbattery_pub.publish(std_msgs.Float32(batteryVolt.data))
# print "batteryVolt: %s" % batteryVolt
cfbattery_pub.publish(batteryVolt)
def _logging_error(self, logconf, msg):
print("[CRAZY RADIO] Error when logging %s" % logconf.name)
# def _init_logging(self):
def _start_logging(self):
self.logconf = LogConfig(
"LoggingTest",
battery_polling_period) # second variable is period in ms
self.logconf.add_variable("stabilizer.roll", "float")
self.logconf.add_variable("stabilizer.pitch", "float")
self.logconf.add_variable("stabilizer.yaw", "float")
self.logconf.add_variable("pm.vbat", "float")
self._cf.log.add_config(self.logconf)
if self.logconf.valid:
self.logconf.data_received_cb.add_callback(
self._data_received_callback)
self.logconf.error_cb.add_callback(self._logging_error)
print("[CRAZY RADIO] logconf valid")
else:
print("[CRAZY RADIO] logconf invalid")
self.logconf.start()
print("[CRAZY RADIO] logconf start")
def _connected(self, link_uri):
"""
This callback is executed as soon as the connection to the
quadrotor is established.
"""
self.change_status_to(CONNECTED)
# change state to motors OFF
msg = IntWithHeader()
msg.shouldCheckForAgentID = False
msg.data = CMD_CRAZYFLY_MOTORS_OFF
cf_client.FlyingAgentClient_command_pub.publish(msg)
self._lg_stab = LogConfig(name='stateEstimate', period_in_ms=50)
self._lg_stab.add_variable('stateEstimate.x', 'float')
self._lg_stab.add_variable('stateEstimate.y', 'float')
self._lg_stab.add_variable('stateEstimate.z', 'float')
self._lg_stab.add_variable('stateEstimate.vx', 'float')
self._lg_stab.add_variable('stateEstimate.vy', 'float')
self._lg_stab.add_variable('stateEstimate.vz', 'float')
#self._lg_stab.add_variable('stateEstimate.roll', 'float')
self._lg_att = LogConfig(name='stateEstimate', period_in_ms=50)
self._lg_att.add_variable('stateEstimate.roll', 'float')
self._lg_att.add_variable('stateEstimate.pitch', 'float')
self._lg_att.add_variable('stateEstimate.yaw', 'float')
self._cf.param.set_value('kalman.initialX', '{:.2f}'.format(0.0))
self._cf.param.set_value('kalman.initialY', '{:.2f}'.format(0.0))
self._cf.param.set_value('kalman.initialZ', '{:.2f}'.format(0.0))
self._cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
self._cf.param.set_value('kalman.resetEstimation', '0')
try:
self._cf.log.add_config(self._lg_stab)
self._cf.log.add_config(self._lg_att)
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
self._lg_att.data_received_cb.add_callback(self._att_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
self._lg_att.error_cb.add_callback(self._att_log_error)
# Start the logging
self._lg_stab.start()
self._lg_att.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Stabilizer log config, bad configuration.')
rospy.loginfo("[CRAZY RADIO] Connection to %s successful: " % link_uri)
# Config for Logging
self._start_logging()
def _stab_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _att_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _att_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
global at_msg, at_data_pub
#at_msg.x = data["stateEstimate.x"]
#at_msg.y = data["stateEstimate.y"]
at_msg.roll = data["stateEstimate.roll"]
at_msg.pitch = data["stateEstimate.pitch"]
at_msg.yaw = data["stateEstimate.yaw"]
#print(data["stateEstimate.yaw"])
at_data_pub.publish(at_msg)
pass
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
#print('[%d][%s]: %s' % (timestamp, logconf.name, data))
#msg = CrazyflieData()
fs_msg.x = round(data["stateEstimate.x"], 3)
fs_msg.y = round(data["stateEstimate.y"], 3)
fs_msg.z = round(data["stateEstimate.z"], 3)
fs_msg.vx = round(data["stateEstimate.vx"], 3)
fs_msg.vy = round(data["stateEstimate.vy"], 3)
fs_msg.vz = round(data["stateEstimate.vz"], 3)
fs_msg.acquiringTime = 0.01
#fs_msg.roll = data["stateEstimate.roll"]
#print(data["stateEstimate.x"])
#msg.y = CMD_CRAZYFLY_MOTORS_OFF
fs_data_pub.publish(fs_msg)
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("[CRAZY RADIO] 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("[CRAZY RADIO] 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("[CRAZY RADIO] Disconnected from %s" % link_uri)
# change state to motors OFF
msg = IntWithHeader()
msg.shouldCheckForAgentID = False
msg.data = CMD_CRAZYFLY_MOTORS_OFF
self.FlyingAgentClient_command_pub.publish(msg)
#self.logconf.stop()
#rospy.loginfo("logconf stopped")
#self.logconf.delete()
#rospy.loginfo("logconf deleted")
def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<BHHHH', CF_COMMAND_TYPE_MOTORS, cmd1, cmd2,
cmd3, cmd4)
self._cf.send_packet(pk)
def _send_to_commander_rate(self, cmd1, cmd2, cmd3, cmd4, roll_rate,
pitch_rate, yaw_rate):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_RATE, cmd1, cmd2,
cmd3, cmd4, roll_rate * RAD_TO_DEG,
pitch_rate * RAD_TO_DEG, yaw_rate * RAD_TO_DEG)
self._cf.send_packet(pk)
def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch,
yaw):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_ANGLE, cmd1, cmd2,
cmd3, cmd4, roll * RAD_TO_DEG,
pitch * RAD_TO_DEG, yaw * RAD_TO_DEG)
self._cf.send_packet(pk)
# def _send_to_commander(self,roll, pitch, yaw, thrust, cmd1, cmd2, cmd3, cmd4, mode):
# pk = CRTPPacket()
# pk.port = CRTPPort.COMMANDER
# pk.data = struct.pack('<fffHHHHHH', roll * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG, thrust, cmd1, cmd2, cmd3, cmd4, mode)
# self._cf.send_packet(pk)
def crazyRadioCommandCallback(self, msg):
"""Callback to tell CrazyRadio to reconnect"""
# Initialise a boolean flag that the command is NOT relevant
command_is_relevant = False
# Only consider the command if it is relevant
if (command_is_relevant):
#print "[CRAZY RADIO] received command to: %s" % msg.data
if msg.data == CMD_RECONNECT:
if self.get_status() == DISCONNECTED:
print(
"[CRAZY RADIO] received command to CONNECT (current status is DISCONNECTED)"
)
self.connect()
elif self.get_status() == CONNECTING:
print(
"[CRAZY RADIO] received command to CONNECT (current status is CONNECTING)"
)
#self.status_pub.publish(CONNECTING)
elif self.get_status() == CONNECTED:
print(
"[CRAZY RADIO] received command to CONNECT (current status is CONNECTED)"
)
#self.status_pub.publish(CONNECTED)
elif msg.data == CMD_DISCONNECT:
if self.get_status() == CONNECTED:
print(
"[CRAZY RADIO] received command to DISCONNECT (current status is CONNECTED)"
)
self.disconnect()
elif self.get_status() == CONNECTING:
print(
"[CRAZY RADIO] received command to DISCONNECT (current status is CONNECTING)"
)
#self.status_pub.publish(CONNECTING)
elif self.get_status() == DISCONNECTED:
print(
"[CRAZY RADIO] received command to DISCONNECT (current status is DISCONNECTED)"
)
#self.status_pub.publish(DISCONNECTED)
def getCurrentCrazyRadioStatusServiceCallback(self, req):
"""Callback to service the request for the connection status"""
# Directly return the current status
return self._status
