def test_different_underlying_ed_instances(self):
# Fixture
# Test
sed1 = SyncEspdrone('uri 1')
sed2 = SyncEspdrone('uri 2')
# Assert
actual1 = sed1.ed
actual2 = sed2.ed
self.assertNotEqual(actual1, actual2)
def setUp(self):
self.uri = 'radio://0/60/2M'
self.ed_mock = MagicMock(spec=Espdrone)
self.ed_mock.connected = Caller()
self.ed_mock.connection_failed = Caller()
self.ed_mock.disconnected = Caller()
self.ed_mock.open_link = AsyncCallbackCaller(cb=self.ed_mock.connected,
args=[self.uri]).trigger
self.sut = SyncEspdrone(self.uri, self.ed_mock)
def slightly_more_complex_usage():
with SyncEspdrone(uri, ed=Espdrone(rw_cache='./cache')) as sed:
with PositionHlCommander(
sed,
x=0.0,
y=0.0,
z=0.0,
default_velocity=0.3,
default_height=0.5,
controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc:
# Go to a coordinate
pc.go_to(1.0, 1.0, 1.0)
# Move relative to the current position
pc.right(1.0)
# Go to a coordinate and use default height
pc.go_to(0.0, 0.0)
# Go slowly to a coordinate
pc.go_to(1.0, 1.0, velocity=0.2)
# Set new default velocity and height
pc.set_default_velocity(0.3)
pc.set_default_height(1.0)
pc.go_to(0.0, 0.0)
def simple_sequence():
with SyncEspdrone(uri, ed=Espdrone(rw_cache='./cache')) as sed:
with PositionHlCommander(sed) as pc:
pc.forward(1.0)
pc.left(1.0)
pc.back(1.0)
pc.go_to(0.0, 0.0, 1.0)
def test_that_the_same_ed_object_can_be_connected_multiple_times(self):
# Fixture
self.test_rig_support.restart_devices(self.test_rig_support.all_uris)
ed = Espdrone(rw_cache='./cache')
# Test
for uri in self.test_rig_support.all_uris:
with SyncEspdrone(uri, ed=ed):
pass
def test_memory_mapping_with_one_ed(self):
# Fixture
uri = self.test_rig_support.all_uris[0]
self.test_rig_support.restart_devices([uri])
ed = Espdrone(rw_cache='./cache')
# Test and Assert
with SyncEspdrone(uri, ed=ed) as sed:
self.assert_memory_mapping(sed)
def test_that_requested_logging_is_received_properly_from_one_ed(self):
# Fixture
uri = self.test_rig_support.all_uris[0]
self.test_rig_support.restart_devices([uri])
ed = Espdrone(rw_cache='./cache')
# Test and Assert
with SyncEspdrone(uri, ed=ed) as sed:
self.assert_add_logging_and_get_non_zero_value(sed)
def test_open_close_with_context_mangement(self):
# Fixture
# Test
with SyncEspdrone(self.uri, self.ed_mock) as sut:
self.assertTrue(sut.is_link_open())
# Assert
self.ed_mock.close_link.assert_called_once_with()
self._assertAllCallbacksAreRemoved()
def test_memory_mapping_with_reuse_of_ed_object(self):
# Fixture
uri = self.test_rig_support.all_uris[0]
self.test_rig_support.restart_devices([uri])
ed = Espdrone(rw_cache='./cache')
# Test and Assert
for connections in range(10):
with SyncEspdrone(uri, ed=ed) as sed:
for mem_ops in range(5):
self.assert_memory_mapping(sed)
def __init__(self, uri):
self.got_data = False
with SyncEspdrone(uri, ed=Espdrone(rw_cache='./cache')) as sed:
mems = sed.ed.mem.get_mems(MemoryElement.TYPE_LH)
count = len(mems)
if count != 1:
raise Exception('Unexpected nr of memories found:', count)
print('Rquesting data')
mems[0].update(self._data_updated)
while not self.got_data:
time.sleep(1)
def __init__(self, uri, bs1, bs2):
self.data_written = False
with SyncEspdrone(uri, ed=Espdrone(rw_cache='./cache')) as sed:
mems = sed.ed.mem.get_mems(MemoryElement.TYPE_LH)
count = len(mems)
if count != 1:
raise Exception('Unexpected nr of memories found:', count)
mems[0].geometry_data = [bs1, bs2]
print('Writing data')
mems[0].write_data(self._data_written)
while not self.data_written:
time.sleep(1)
class SyncEspdroneTest(unittest.TestCase):
def setUp(self):
self.uri = 'radio://0/60/2M'
self.ed_mock = MagicMock(spec=Espdrone)
self.ed_mock.connected = Caller()
self.ed_mock.connection_failed = Caller()
self.ed_mock.disconnected = Caller()
self.ed_mock.open_link = AsyncCallbackCaller(cb=self.ed_mock.connected,
args=[self.uri]).trigger
self.sut = SyncEspdrone(self.uri, self.ed_mock)
def test_different_underlying_ed_instances(self):
# Fixture
# Test
sed1 = SyncEspdrone('uri 1')
sed2 = SyncEspdrone('uri 2')
# Assert
actual1 = sed1.ed
actual2 = sed2.ed
self.assertNotEqual(actual1, actual2)
def test_open_link(self):
# Fixture
# Test
self.sut.open_link()
# Assert
self.assertTrue(self.sut.is_link_open())
def test_failed_open_link_raises_exception(self):
# Fixture
expected = 'Some error message'
self.ed_mock.open_link = AsyncCallbackCaller(
cb=self.ed_mock.connection_failed, args=[self.uri,
expected]).trigger
# Test
try:
self.sut.open_link()
except Exception as e:
actual = e.args[0]
else:
self.fail('Expect exception')
# Assert
self.assertEqual(expected, actual)
self._assertAllCallbacksAreRemoved()
def test_open_link_of_already_open_link_raises_exception(self):
# Fixture
self.sut.open_link()
# Test
# Assert
with self.assertRaises(Exception):
self.sut.open_link()
def test_close_link(self):
# Fixture
self.sut.open_link()
# Test
self.sut.close_link()
# Assert
self.ed_mock.close_link.assert_called_once_with()
self.assertFalse(self.sut.is_link_open())
self._assertAllCallbacksAreRemoved()
def test_close_link_that_is_not_open(self):
# Fixture
# Test
self.sut.close_link()
# Assert
self.assertFalse(self.sut.is_link_open())
def test_closed_if_connection_is_lost(self):
# Fixture
self.sut.open_link()
# Test
AsyncCallbackCaller(cb=self.ed_mock.disconnected,
args=[self.uri]).call_and_wait()
# Assert
self.assertFalse(self.sut.is_link_open())
self._assertAllCallbacksAreRemoved()
def test_open_close_with_context_mangement(self):
# Fixture
# Test
with SyncEspdrone(self.uri, self.ed_mock) as sut:
self.assertTrue(sut.is_link_open())
# Assert
self.ed_mock.close_link.assert_called_once_with()
self._assertAllCallbacksAreRemoved()
def _assertAllCallbacksAreRemoved(self):
self.assertEqual(0, len(self.ed_mock.connected.callbacks))
self.assertEqual(0, len(self.ed_mock.connection_failed.callbacks))
self.assertEqual(0, len(self.ed_mock.disconnected.callbacks))
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
edlib.crtp.init_drivers(enable_debug_driver=False)
# Scan for Espdrones and use the first one found
print('Scanning interfaces for Espdrones...')
available = edlib.crtp.scan_interfaces()
print('Espdrones found:')
for i in available:
print(i[0])
if len(available) == 0:
print('No Espdrones found, cannot run example')
else:
with SyncEspdrone(available[0][0],
ed=Espdrone(rw_cache='./cache')) as sed:
# We take off when the commander is created
with MotionCommander(sed) as mc:
time.sleep(1)
# There is a set of functions that move a specific distance
# We can move in all directions
mc.forward(0.8)
mc.back(0.8)
time.sleep(1)
mc.up(0.5)
mc.down(0.5)
time.sleep(1)
# We can also set the velocity
def construct(self, uri):
ed = Espdrone(ro_cache=self.ro_cache, rw_cache=self.rw_cache)
return SyncEspdrone(uri, ed=ed)
def construct(self, uri):
return SyncEspdrone(uri)
grab_controller_start))
ed0.commander.send_position_setpoint(setpoints[0][0],
setpoints[0][1],
setpoints[0][2],
0)
ed1.commander.send_position_setpoint(setpoints[1][0],
setpoints[1][1],
setpoints[1][2],
0)
time.sleep(0.02)
ed0.commander.send_setpoint(0, 0, 0, 0)
ed1.commander.send_setpoint(0, 0, 0, 0)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
time.sleep(0.1)
if __name__ == '__main__':
edlib.crtp.init_drivers(enable_debug_driver=False)
with SyncEspdrone(uri0, ed=Espdrone(rw_cache='./cache')) as sed0:
reset_estimator(sed0)
with SyncEspdrone(uri1, ed=Espdrone(rw_cache='./cache')) as sed1:
reset_estimator(sed1)
run_sequence(sed0, sed1)
openvr.shutdown()
print('Scanning interfaces for Espdrones...')
available = edlib.crtp.scan_interfaces()
print('Espdrones found:')
for i in available:
print(i[0])
if len(available) == 0:
print('No Espdrones found, cannot run example')
else:
lg_stab = LogConfig(name='StateEstimate', period_in_ms=10)
lg_stab.add_variable('stateEstimate.x', 'float')
lg_stab.add_variable('stateEstimate.y', 'float')
lg_stab.add_variable('stateEstimate.z', 'float')
ed = Espdrone(rw_cache='./cache')
with SyncEspdrone(available[0][0], ed=ed) as sed:
with SyncLogger(sed, lg_stab) as logger:
endTime = time.time() + 10
update_time = current_time = time.time()
for log_entry in logger:
timestamp = log_entry[0]
data = log_entry[1]
logconf_name = log_entry[2]
if time.time() - update_time > 2:
print("Updating state estimate")
sed.ed.extpos.send_extpose(10, 2, 4, 0, 0, 0, 1)
update_time = time.time()
if time.time() - current_time > 1:
def is_close(range):
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)
edlib.crtp.init_drivers(enable_debug_driver=False)
ed = Espdrone(rw_cache='./cache')
with SyncEspdrone(URI, ed=ed) as sed:
with MotionCommander(sed) as motion_commander:
with Multiranger(sed) 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):
if args.uri:
uri = args.uri
else:
available = edlib.crtp.scan_interfaces()
print('Espdrones found:')
if available:
print(available[0])
uri = available[0][0]
else:
quit()
lg_stab = LogConfig(name='Stabilizer', period_in_ms=10)
lg_stab.add_variable('stabilizer.roll', 'float')
lg_stab.add_variable('stabilizer.pitch', 'float')
lg_stab.add_variable('stabilizer.yaw', 'float')
ed = Espdrone(rw_cache='./cache')
with SyncEspdrone(uri, ed=ed) as sed:
with SyncLogger(sed, lg_stab) as logger:
endTime = time.time() + 10
for log_entry in logger:
timestamp = log_entry[0]
data = log_entry[1]
logconf_name = log_entry[2]
print('[%d][%s]: %s' % (timestamp, logconf_name, data))
if time.time() > endTime:
break
print('Grab ended')
grabbed = trigger
if trigger:
curr = [-1 * pose[2][3], -1 * pose[0][3], pose[1][3]]
setpoint = vector_add(
grab_setpoint_start,
vector_substract(curr, grab_controller_start))
ed.commander.send_position_setpoint(setpoint[0], setpoint[1],
setpoint[2], 0)
time.sleep(0.02)
ed.commander.send_setpoint
(0, 0, 0, 0)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
time.sleep(0.1)
if __name__ == '__main__':
edlib.crtp.init_drivers(enable_debug_driver=False)
with SyncEspdrone(uri, ed=Espdrone(rw_cache='./cache')) as sed:
reset_estimator(sed)
# start_position_printing(sed)
run_sequence(sed)
openvr.shutdown()
import logging
import time
import edlib.crtp
from edlib.espdrone.syncEspdrone import SyncEspdrone
URI = 'radio://0/80/250K'
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
edlib.crtp.init_drivers(enable_debug_driver=False)
with SyncEspdrone(URI) as sed:
ed = sed.ed
# Set solid color effect
ed.param.set_value('ring.effect', '7')
# Set the RGB values
ed.param.set_value('ring.solidRed', '100')
ed.param.set_value('ring.solidGreen', '0')
ed.param.set_value('ring.solidBlue', '0')
time.sleep(2)
# Set black color effect
ed.param.set_value('ring.effect', '0')
time.sleep(1)
# Set fade to color effect