def test_that_logging_is_started_on_start(self):
# Fixture
with patch('cflib.utils.multiranger.LogConfig',
return_value=self.log_config_mock):
sut = Multiranger(self.cf_mock)
# Test
sut.start()
# Assert
self.log_config_mock.start.assert_called_once_with()
def test_that_logging_is_started_on_start(self):
# Fixture
with patch('cflib.utils.multiranger.LogConfig',
return_value=self.log_config_mock):
sut = Multiranger(self.cf_mock)
# Test
sut.start()
# Assert
self.log_config_mock.start.assert_called_once_with()
def test_that_log_configuration_is_added_on_connect(self):
# Fixture
with patch('cflib.utils.multiranger.LogConfig',
return_value=self.log_config_mock):
sut = Multiranger(self.cf_mock)
# Test
sut.start()
# Assert
self.log_mock.add_config.assert_called_once_with(
self.log_config_mock)
def test_that_log_configuration_is_added_on_connect(self):
# Fixture
with patch('cflib.utils.multiranger.LogConfig',
return_value=self.log_config_mock):
sut = Multiranger(self.cf_mock)
# Test
sut.start()
# Assert
self.log_mock.add_config.assert_called_once_with(
self.log_config_mock)
def test_that_data_callback_is_set(self):
# Fixture
with patch('cflib.utils.multiranger.LogConfig',
return_value=self.log_config_mock):
sut = Multiranger(self.cf_mock)
# Test
sut.start()
# Assert
self.log_config_mock.start.assert_called_once_with()
self.assertEqual(1, len(
self.log_config_mock.data_received_cb.callbacks))
def test_that_data_callback_is_set(self):
# Fixture
with patch('cflib.utils.multiranger.LogConfig',
return_value=self.log_config_mock):
sut = Multiranger(self.cf_mock)
# Test
sut.start()
# Assert
self.log_config_mock.start.assert_called_once_with()
self.assertEqual(1, len(
self.log_config_mock.data_received_cb.callbacks))
def push(scf):
cf=scf.cf
with MotionCommander(scf) as motion_commander:
with Multiranger(scf) as multiranger:
keep_flying = True
while keep_flying:
VELOCITY = 0.3
velocity_x = 0.0
velocity_y = 0.0
if is_close(multiranger.front):
velocity_x -= VELOCITY
if is_close(multiranger.back):
velocity_x += VELOCITY
if is_close(multiranger.left):
velocity_y -= VELOCITY
if is_close(multiranger.right):
velocity_y += VELOCITY
if is_close(multiranger.up):
keep_flying = False
motion_commander.start_linear_motion(
velocity_x, velocity_y, 0)
time.sleep(0.1)
print('Demo terminated!')
def test_that_log_configuration_is_correct(self):
# Fixture
with patch('cflib.utils.multiranger.LogConfig',
return_value=self.log_config_mock):
sut = Multiranger(self.cf_mock)
# Test
sut.start()
# Assert
self.log_config_mock.add_variable.assert_has_calls([
call(self.FRONT),
call(self.BACK),
call(self.LEFT),
call(self.RIGHT),
call(self.UP),
])
def test_that_log_configuration_is_correct(self):
# Fixture
with patch('cflib.utils.multiranger.LogConfig',
return_value=self.log_config_mock):
sut = Multiranger(self.cf_mock)
# Test
sut.start()
# Assert
self.log_config_mock.add_variable.assert_has_calls([
call(self.FRONT),
call(self.BACK),
call(self.LEFT),
call(self.RIGHT),
call(self.UP),
])
def simple_sequence():
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
with PositionHlCommander(scf) as pc:
with Multiranger(scf) as multiranger:
for i in xrange(4):
pc.forward(1.0)
pc.left(1.0)
pc.back(1.0)
pc.right(1.0)
def test_that_using_context_manager_calls_start_and_stop(self):
# Fixture
with patch('cflib.utils.multiranger.LogConfig',
return_value=self.log_config_mock):
with Multiranger(self.cf_mock):
pass
# Assert
self.log_config_mock.start.assert_called_once_with()
self.log_config_mock.delete.assert_called_once_with()
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.connection.emit("progress")
# The definition of the logconfig can be made before connecting
self.logger = LogConfig("Battery", 1000) # delay
self.logger.add_variable("pm.vbat", "float")
try:
self._cf.log.add_config(self.logger)
self.logger.data_received_cb.add_callback(
lambda e, f, g: self.batteryValue.emit(float(f['pm.vbat'])))
# self.logger.error_cb.add_callback(lambda: print('error'))
self.logger.start()
except KeyError as e:
print(e)
self.connection.emit("on")
self.motion_commander = MotionCommander(self._cf, 0.5)
self.multiranger = Multiranger(self._cf, rate_ms=50)
self.multiranger.start()
def _validate_distance_from_getter(self, expected, getter_name,
zranger=False):
# Fixture
with patch('cflib.utils.multiranger.LogConfig',
return_value=self.log_config_mock):
sut = Multiranger(self.cf_mock, zranger=zranger)
timestmp = 1234
logconf = None
self.log_config_mock.data_received_cb.call(timestmp, self.log_data,
logconf)
# Test
actual = getattr(sut, getter_name)
# Assert
self.assertEqual(expected, actual)
def talker():
pub = rospy.Publisher('chatter', String, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
cflib.crtp.init_drivers(enable_debug_driver=False)
cf = Crazyflie(rw_cache='./cache')
while not rospy.is_shutdown():
with SyncCrazyflie(URI, cf=cf) as scf:
with MotionCommander(scf) as motion_commander:
with Multiranger(scf) as multiranger:
#reset_estimator(scf)
start_position_printing(scf)
keep_flying = True
while keep_flying:
print("tadaaaaa-----", x)
VELOCITY = 0.5
velocity_x = 0.0
velocity_y = 0.0
if is_close(multiranger.front):
velocity_x -= VELOCITY
if is_close(multiranger.back):
velocity_x += VELOCITY
if is_close(multiranger.left):
velocity_y -= VELOCITY
if is_close(multiranger.right):
velocity_y += VELOCITY
if is_close(multiranger.up):
keep_flying = False
hello_str = "hello world %s" % multiranger.front
rospy.loginfo(hello_str)
#pub.publish(hello_str)
motion_commander.start_linear_motion(
velocity_x, velocity_y, 0)
#print(data['kalman.stateX'])
time.sleep(0.1)
print('Demo terminated!')
rate.sleep()
def simple_log(scf, logconf, x, y, z, cf):
keep_flying = True
#cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(URI, cf=cf) as scf:
with SyncLogger(scf, lg_stab) as logger:
with MotionCommander(scf) as motion_commander:
with Multiranger(scf) as multi_ranger:
for i in range(100):
#while keep_flying:
#if is_close(multi_ranger.up):
#keep_flying = False
cf.param.set_value('posCtlPid.zKp', '1')
#cf.param.set_value('posCtlPid.xKp', '1')
#cf.param.set_value('posCtlPid.yKp', '1')
#cf.param.set_value('postCtlPid.xKp','1')
for log_entry in logger:
#cf.param.set_value('posCtlPid.xKp', '1')
#cf.param.set_value('posCtlPid.yKp', '1')
cf.param.set_value('posCtlPid.zKp', '1')
motion_commander.start_linear_motion(0, 0, 0.05)
timestamp = log_entry[0]
data = log_entry[1]
logconf_name = log_entry[2]
count = 1
for i in data.values():
if count == 1:
x.append(i)
elif count == 2:
y.append(i)
else:
z.append(i)
count = count + 1
print('[%d][%s]: %s' %
(timestamp, logconf_name, data))
break
time.sleep(0.1)
motion_commander.stop()
return (x, y, z)
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)
# The definition of the logconfig can be made before connecting
cflib.crtp.init_drivers(enable_debug_driver=True)
cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(URI, cf=cf) as scf:
with MotionCommander(scf) as motion_commander:
with Multiranger(scf) as multi_ranger:
self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=10)
self._lg_stab.add_variable('stabilizer.roll', 'float')
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.')
# Start a timer to disconnect in 10s
t = Timer(5, self._cf.close_link)
t.start()
def observe(self, goal):
position_now = self.get_position()
track = self.goal_direction(goal, position_now)
distance = np.sqrt(
np.power((goal[0] - position_now[0]), 2) +
np.power((goal[1] - position_now[1]), 2))
distance_sensor = Multiranger(self.scf)
distance_sensor.start()
front_distance_sensor = distance_sensor.front
while front_distance_sensor is None:
time.sleep(0.01)
front_distance_sensor = distance_sensor.front
distance_sensor.stop()
print('Position now = %s ' % position_now)
# print('Track = %s ' % track)
print('Distance to target: %s' % distance)
print('Front distance: %s' % front_distance_sensor)
return position_now[0], position_now[
1], track, distance, front_distance_sensor
MIN_DISTANCE = 0.2 # m
if range is None:
return False
else:
return range < MIN_DISTANCE
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(URI, cf=cf) as scf:
with MotionCommander(scf) as motion_commander:
with Multiranger(scf) as multiranger:
keep_flying = True
while keep_flying:
VELOCITY = 0.5
velocity_x = 0.0
velocity_y = 0.0
if is_close(multiranger.front):
velocity_x -= VELOCITY
if is_close(multiranger.back):
velocity_x += VELOCITY
if is_close(multiranger.left):
velocity_y -= VELOCITY
if is_close(multiranger.right):
class CrazyDrone(Drone):
def __init__(self, link_uri):
super().__init__()
cache = "./cache"
if getattr(sys, 'frozen', False):
cache = sys._MEIPASS + os.path.sep + "cache"
self._cf = Crazyflie(rw_cache=cache)
self.motion_commander = None
self.multiranger = None
# maximum speeds
self.max_vert_speed = 1
self.max_horiz_speed = 1
self.max_rotation_speed = 90
self.logger = None
# 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
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.connection.emit("progress")
# The definition of the logconfig can be made before connecting
self.logger = LogConfig("Battery", 1000) # delay
self.logger.add_variable("pm.vbat", "float")
try:
self._cf.log.add_config(self.logger)
self.logger.data_received_cb.add_callback(
lambda e, f, g: self.batteryValue.emit(float(f['pm.vbat'])))
# self.logger.error_cb.add_callback(lambda: print('error'))
self.logger.start()
except KeyError as e:
print(e)
self.connection.emit("on")
self.motion_commander = MotionCommander(self._cf, 0.5)
self.multiranger = Multiranger(self._cf, rate_ms=50)
self.multiranger.start()
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
self.connection.emit("off")
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))
self.connection.emit("off")
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
self.connection.emit("off")
def take_off(self):
if self._cf.is_connected(
) and self.motion_commander and not self.motion_commander._is_flying:
self.motion_commander.take_off()
self.is_flying_signal.emit(True)
def land(self):
if self._cf.is_connected(
) and self.motion_commander and self.motion_commander._is_flying:
self.motion_commander.land()
self.is_flying_signal.emit(False)
def stop(self):
if not (self.logger is None):
self.logger.stop()
if self.motion_commander:
self.motion_commander.land()
if self.multiranger:
self.multiranger.stop()
self._cf.close_link()
def is_flying(self):
if self._cf.is_connected() and self.motion_commander:
return self.motion_commander._is_flying
return False
def process_motion(self, _up, _rotate, _front, _right):
if self.motion_commander:
# WARNING FOR CRAZYFLY
# positive X is forward, # positive Y is left # positive Z is up
velocity_z = _up * self.max_vert_speed
velocity_yaw = _rotate * self.max_rotation_speed
velocity_x = _front * self.max_horiz_speed
velocity_y = -_right * self.max_horiz_speed
# print("PRE", velocity_x, velocity_y, velocity_z, velocity_yaw)
# protect against collision by reducing speed depending on distance
ranger = self.multiranger
if ranger.front and ranger.front < ANTI_COLLISION_DISTANCE and velocity_x > 0:
velocity_x = velocity_x * (
ranger.front -
ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE
velocity_x = max(0, velocity_x)
if ranger.back and ranger.back < ANTI_COLLISION_DISTANCE and velocity_x < 0:
velocity_x = velocity_x * (
ranger.back -
ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE
velocity_x = min(0, velocity_x)
if ranger.left and ranger.left < ANTI_COLLISION_DISTANCE and velocity_y > 0:
velocity_y = velocity_y * (
ranger.left -
ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE
velocity_y = max(0, velocity_y)
if ranger.right and ranger.right < ANTI_COLLISION_DISTANCE and velocity_y < 0:
velocity_y = velocity_y * (
ranger.left -
ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE
velocity_y = min(0, velocity_y)
if ranger.up and ranger.up < ANTI_COLLISION_DISTANCE and velocity_z > 0:
velocity_z = velocity_z * (ranger.up - ANTI_COLLISION_MIN_DIST
) / ANTI_COLLISION_DISTANCE
velocity_z = max(0, velocity_z)
# print("POST", velocity_x, velocity_y, velocity_z, velocity_yaw)
if self.motion_commander._is_flying:
self.motion_commander._set_vel_setpoint(
velocity_x, velocity_y, velocity_z, velocity_yaw)
