class TestMotionCommander(unittest.TestCase):
def setUp(self):
self.commander_mock = MagicMock(spec=Commander)
self.param_mock = MagicMock(spec=Param)
self.cf_mock = MagicMock(spec=Crazyflie)
self.cf_mock.commander = self.commander_mock
self.cf_mock.param = self.param_mock
self.cf_mock.is_connected.return_value = True
self.sut = MotionCommander(self.cf_mock)
def test_that_the_estimator_is_reset_on_take_off(
self, _SetPointThread_mock, sleep_mock):
# Fixture
# Test
self.sut.take_off()
# Assert
self.param_mock.set_value.assert_has_calls([
call('kalman.resetEstimation', '1'),
call('kalman.resetEstimation', '0')
])
def test_that_take_off_raises_exception_if_not_connected(
self, _SetPointThread_mock, sleep_mock):
# Fixture
self.cf_mock.is_connected.return_value = False
# Test
# Assert
with self.assertRaises(Exception):
self.sut.take_off()
def test_that_take_off_raises_exception_when_already_flying(
self, _SetPointThread_mock, sleep_mock):
# Fixture
self.sut.take_off()
# Test
# Assert
with self.assertRaises(Exception):
self.sut.take_off()
def test_that_it_goes_up_on_take_off(
self, _SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
# Test
self.sut.take_off(height=0.4, velocity=0.5)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.5, 0.0),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(0.4 / 0.5)
def test_that_it_goes_up_to_default_height(
self, _SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
sut = MotionCommander(self.cf_mock, default_height=0.4)
# Test
sut.take_off(velocity=0.6)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.6, 0.0),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(0.4 / 0.6)
def test_that_the_thread_is_started_on_takeoff(
self, _SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
# Test
self.sut.take_off()
# Assert
thread_mock.start.assert_called_with()
def test_that_it_goes_down_on_landing(
self, _SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
thread_mock.get_height.return_value = 0.4
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.land(velocity=0.5)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, -0.5, 0.0),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(0.4 / 0.5)
def test_that_it_takes_off_and_lands_as_context_manager(
self, _SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
thread_mock.reset_mock()
thread_mock.get_height.return_value = 0.3
# Test
with self.sut:
pass
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.2, 0.0),
call(0.0, 0.0, 0.0, 0.0),
call(0.0, 0.0, -0.2, 0.0),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(0.3 / 0.2)
sleep_mock.assert_called_with(0.3 / 0.2)
def test_that_it_starts_moving_multi_dimensional(
self, _SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.start_linear_motion(0.1, 0.2, 0.3)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.1, 0.2, 0.3, 0.0),
])
def test_that_it_starts_moving(
self, _SetPointThread_mock, sleep_mock):
# Fixture
self.sut.take_off()
vel = 0.3
data = [
[self.sut.start_forward, (vel, 0.0, 0.0, 0.0)],
[self.sut.start_back, (-vel, 0.0, 0.0, 0.0)],
[self.sut.start_left, (0.0, vel, 0.0, 0.0)],
[self.sut.start_right, (0.0, -vel, 0.0, 0.0)],
[self.sut.start_up, (0.0, 0.0, vel, 0.0)],
[self.sut.start_down, (0.0, 0.0, -vel, 0.0)],
]
# Test
# Assert
for line in data:
self._verify_start_motion(line[0], vel, line[1],
_SetPointThread_mock)
def test_that_it_moves_multi_dimensional_distance(
self, _SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
x = 0.1
y = 0.2
z = 0.3
vel = 0.2
distance = math.sqrt(x * x + y * y + z * z)
expected_time = distance / vel
expected_vel_x = vel * x / distance
expected_vel_y = vel * y / distance
expected_vel_z = vel * z / distance
self.sut.take_off()
thread_mock.reset_mock()
sleep_mock.reset_mock()
# Test
self.sut.move_distance(x, y, z)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(expected_vel_x, expected_vel_y, expected_vel_z, 0.0),
])
sleep_mock.assert_called_with(expected_time)
def test_that_it_moves(self, _SetPointThread_mock, sleep_mock):
# Fixture
vel = 0.3
self.sut.take_off()
data = [
[self.sut.forward, (vel, 0.0, 0.0, 0.0)],
[self.sut.back, (-vel, 0.0, 0.0, 0.0)],
[self.sut.left, (0.0, vel, 0.0, 0.0)],
[self.sut.right, (0.0, -vel, 0.0, 0.0)],
[self.sut.up, (0.0, 0.0, vel, 0.0)],
[self.sut.down, (0.0, 0.0, -vel, 0.0)],
]
# Test
# Assert
for test in data:
self._verify_move(test[0], vel, test[1],
_SetPointThread_mock, sleep_mock)
def test_that_it_starts_turn_right(self, _SetPointThread_mock, sleep_mock):
# Fixture
rate = 20
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.start_turn_right(rate)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.0, rate),
])
def test_that_it_starts_turn_left(self, _SetPointThread_mock, sleep_mock):
# Fixture
rate = 20
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.start_turn_left(rate)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.0, -rate),
])
def test_that_it_starts_circle_right(
self, _SetPointThread_mock, sleep_mock):
# Fixture
velocity = 0.5
radius = 0.9
expected_rate = 360 * velocity / (2 * radius * math.pi)
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.start_circle_right(radius, velocity=velocity)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(velocity, 0.0, 0.0, expected_rate),
])
def test_that_it_starts_circle_left(
self, _SetPointThread_mock, sleep_mock):
# Fixture
velocity = 0.5
radius = 0.9
expected_rate = 360 * velocity / (2 * radius * math.pi)
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.start_circle_left(radius, velocity=velocity)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(velocity, 0.0, 0.0, -expected_rate),
])
def test_that_it_turns_right(self, _SetPointThread_mock, sleep_mock):
# Fixture
rate = 20
angle = 45
turn_time = angle / rate
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.turn_right(angle, rate=rate)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.0, rate),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(turn_time)
def test_that_it_turns_left(self, _SetPointThread_mock, sleep_mock):
# Fixture
rate = 20
angle = 45
turn_time = angle / rate
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.turn_left(angle, rate=rate)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.0, -rate),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(turn_time)
def test_that_it_circles_right(self, _SetPointThread_mock, sleep_mock):
# Fixture
radius = 0.7
velocity = 0.3
angle = 45
distance = 2 * radius * math.pi * angle / 360.0
turn_time = distance / velocity
rate = angle / turn_time
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.circle_right(radius, velocity, angle)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(velocity, 0.0, 0.0, rate),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(turn_time)
def test_that_it_circles_left(self, _SetPointThread_mock, sleep_mock):
# Fixture
radius = 0.7
velocity = 0.3
angle = 45
distance = 2 * radius * math.pi * angle / 360.0
turn_time = distance / velocity
rate = angle / turn_time
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.circle_left(radius, velocity, angle)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(velocity, 0.0, 0.0, -rate),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(turn_time)
######################################################################
def _verify_start_motion(self, function_under_test, velocity, expected,
_SetPointThread_mock):
# Fixture
thread_mock = _SetPointThread_mock()
thread_mock.reset_mock()
# Test
function_under_test(velocity=velocity)
# Assert
try:
thread_mock.set_vel_setpoint.assert_has_calls([
call(*expected),
])
except AssertionError as e:
self._eprint('Failed when testing function ' +
function_under_test.__name__)
raise e
def _verify_move(self, function_under_test, velocity, expected,
_SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
distance = 1.2
expected_time = distance / velocity
thread_mock.reset_mock()
sleep_mock.reset_mock()
# Test
function_under_test(distance, velocity=velocity)
# Assert
try:
thread_mock.set_vel_setpoint.assert_has_calls([
call(*expected),
call(0.0, 0.0, 0.0, 0.0),
])
sleep_mock.assert_called_with(expected_time)
except AssertionError as e:
print('Failed when testing function ' +
function_under_test.__name__)
raise e
class CrazyflieControl:
def __init__(self, name, crazyflie):
# Instantiate motion commander
self._cf = crazyflie
self._name = name
self._mc = MotionCommander(self._cf)
# Topic Publishers
self._velocity_setpoint_pub = rospy.Publisher(self._name +
'/velocity_setpoint',
Vector3,
queue_size=10)
"""
Services hosted for this crazyflie controller
"""
self._reset_position_estimator_srv = rospy.Service(
self._name + '/reset_position_estimator', ResetPositionEstimator,
self._reset_position_estimator_cb)
self._send_hover_setpoint_srv = rospy.Service(
self._name + '/send_hover_setpoint', SendHoverSetpoint,
self._send_hover_setpoint_cb)
self._set_param_srv = rospy.Service(self._name + '/set_param',
SetParam, self._set_param_cb)
self._velocity_control_srv = rospy.Service(
self._name + '/velocity_control', VelocityControl,
self._velocity_control_cb)
"""
Action servers for this crazyflie controller
"""
self._position_control_as = actionlib.SimpleActionServer(
self._name + '/position_control', PositionControlAction,
self._position_control_cb, False)
self._position_control_as.start()
"""
Service Callbacks
"""
def _reset_position_estimator_cb(self, req):
pass
def _send_hover_setpoint_cb(self, req):
vx = req.vx
vy = req.vy
z = req.z
yaw_rate = req.yaw_rate
self._cf.commander.send_hover_setpoint(vx, vy, yaw_rate, z)
return []
def _set_param_cb(self, req):
self._cf.param.set_value(req.param, req.value)
print("set %s to %s" % (req.param, req.value))
return SetParamResponse()
def _velocity_control_cb(self, req):
try:
obj = pickle.loads(req.pickle)
print(self._mc)
if isinstance(obj, SetVelSetpoint):
self._mc._set_vel_setpoint(obj.vx, obj.vy, obj.vz,
obj.rate_yaw)
elif isinstance(obj, StartBack):
self._mc.start_back(velocity=obj.velocity)
elif isinstance(obj, StartCircleLeft):
self._mc.start_circle_left(obj.radius_m, velocity=obj.velocity)
elif isinstance(obj, StartCircleRight):
self._mc.start_turn_right(obj.radius_m, velocity=obj.velocity)
elif isinstance(obj, StartDown):
self._mc.start_down(velocity=obj.velocity)
elif isinstance(obj, StartForward):
self._mc.start_forward(velocity=obj.velocity)
elif isinstance(obj, StartLeft):
self._mc.start_left(velocity=obj.velocity)
elif isinstance(obj, StartLinearMotion):
self._mc.start_linear_motion(obj.vx, obj.vy, obj.vz)
elif isinstance(obj, StartRight):
self._mc.start_right(velocity=obj.velocity)
elif isinstance(obj, StartTurnLeft):
self._mc.start_turn_left(rate=obj.rate)
elif isinstance(obj, StartTurnRight):
self._mc.start_turn_right(rate=obj.rate)
elif isinstance(obj, StartUp):
self._mc.start_up(velocity=obj.velocity)
elif isinstance(obj, Stop):
self._mc.stop()
else:
return 'Object is not a valid velocity command'
except Exception as e:
print(str(e))
raise e
return 'ok'
"""
Action Implementations
"""
def _position_control_cb(self, goal):
try:
obj = pickle.loads(goal.pickle)
if isinstance(obj, Back):
self._mc.back(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, CircleLeft):
self._mc.circle_left(obj.radius_m,
velocity=obj.velocity,
angle_degrees=obj.angle_degrees)
elif isinstance(obj, CircleRight):
self._mc.circle_right(obj.radius_m,
velocity=obj.velocity,
angle_degrees=obj.angle_degrees)
elif isinstance(obj, Down):
self._mc.down(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, Forward):
self._mc.forward(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, Land):
self._mc.land(velocity=obj.velocity)
elif isinstance(obj, Left):
self._mc.left(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, MoveDistance):
self._mc.move_distance(obj.x,
obj.y,
obj.z,
velocity=obj.velocity)
elif isinstance(obj, Right):
self._mc.right(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, TakeOff):
self._mc.take_off(height=obj.height, velocity=obj.velocity)
elif isinstance(obj, TurnLeft):
self._mc.turn_left(obj.angle_degrees, rate=obj.rate)
elif isinstance(obj, TurnRight):
self._mc.turn_right(obj.angle_degrees, rate=obj.rate)
elif isinstance(obj, Up):
self._mc.up(obj.distance_m, velocity=obj.velocity)
except Exception as e:
print('Exception in action server %s' % self._name +
'/position_control')
print(str(e))
print('Action aborted')
self._position_control_as.set_aborted()
return
self._position_control_as.set_succeeded()
def _takeoff(self, goal):
try:
self._mc.take_off(height=goal.height)
time.sleep(5)
except BaseException as e:
self._takeoff_as.set_aborted()
print(e)
return
self._takeoff_as.set_succeeded(TakeOffResult(True))
def _land(self, goal):
try:
self._mc.land(velocity=goal.velocity)
except BaseException as e:
self._land_as.set_aborted()
print(e)
return
self._land_as.set_succeeded(LandResult(True))
def _move_distance(self, goal):
try:
x = goal.x
y = goal.y
z = goal.z
velocity = goal.velocity
dist = np.sqrt(x**2 + y**2 + z**2)
vx = x / dist * velocity
vy = y / dist * velocity
vz = z / dist * velocity
# self._velocity_setpoint_pub.publish(Vector3(vx, vy, vz))
self._mc.move_distance(x, y, z, velocity=velocity)
# self._velocity_setpoint_pub.publish(Vector3(vx, vy, vz))
except BaseException as e:
self._move_distance_as.set_aborted()
print(e)
return
self._move_distance_as.set_succeeded()
class TestMotionCommander(unittest.TestCase):
def setUp(self):
self.commander_mock = MagicMock(spec=Commander)
self.param_mock = MagicMock(spec=Param)
self.cf_mock = MagicMock(spec=Crazyflie)
self.cf_mock.commander = self.commander_mock
self.cf_mock.param = self.param_mock
self.cf_mock.is_connected.return_value = True
self.sut = MotionCommander(self.cf_mock)
def test_that_the_estimator_is_reset_on_take_off(self,
_SetPointThread_mock,
sleep_mock):
# Fixture
# Test
self.sut.take_off()
# Assert
self.param_mock.set_value.assert_has_calls([
call('kalman.resetEstimation', '1'),
call('kalman.resetEstimation', '0')
])
def test_that_take_off_raises_exception_if_not_connected(
self, _SetPointThread_mock, sleep_mock):
# Fixture
self.cf_mock.is_connected.return_value = False
# Test
# Assert
with self.assertRaises(Exception):
self.sut.take_off()
def test_that_take_off_raises_exception_when_already_flying(
self, _SetPointThread_mock, sleep_mock):
# Fixture
self.sut.take_off()
# Test
# Assert
with self.assertRaises(Exception):
self.sut.take_off()
def test_that_it_goes_up_on_take_off(self, _SetPointThread_mock,
sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
# Test
self.sut.take_off(height=0.4, velocity=0.5)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls(
[call(0.0, 0.0, 0.5, 0.0),
call(0.0, 0.0, 0.0, 0.0)])
sleep_mock.assert_called_with(0.4 / 0.5)
def test_that_the_thread_is_started_on_takeoff(self, _SetPointThread_mock,
sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
# Test
self.sut.take_off()
# Assert
thread_mock.start.assert_called_with()
def test_that_it_goes_down_on_landing(self, _SetPointThread_mock,
sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
thread_mock.get_height.return_value = 0.4
self.sut.take_off()
thread_mock = _SetPointThread_mock()
thread_mock.reset_mock()
# Test
self.sut.land(velocity=0.5)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls(
[call(0.0, 0.0, -0.5, 0.0),
call(0.0, 0.0, 0.0, 0.0)])
sleep_mock.assert_called_with(0.4 / 0.5)
def test_that_it_starts_moving_multi_dimensional(self,
_SetPointThread_mock,
sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.start_linear_motion(0.1, 0.2, 0.3)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.1, 0.2, 0.3, 0.0),
])
def test_that_it_starts_moving(self, _SetPointThread_mock, sleep_mock):
# Fixture
self.sut.take_off()
vel = 0.3
data = [
[self.sut.start_forward, (vel, 0.0, 0.0, 0.0)],
[self.sut.start_back, (-vel, 0.0, 0.0, 0.0)],
[self.sut.start_left, (0.0, vel, 0.0, 0.0)],
[self.sut.start_right, (0.0, -vel, 0.0, 0.0)],
[self.sut.start_up, (0.0, 0.0, vel, 0.0)],
[self.sut.start_down, (0.0, 0.0, -vel, 0.0)],
]
# Test
# Assert
for line in data:
self._verify_start_motion(line[0], vel, line[1],
_SetPointThread_mock)
def test_that_it_moves_multi_dimensional_distance(self,
_SetPointThread_mock,
sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
x = 0.1
y = 0.2
z = 0.3
vel = 0.2
distance = math.sqrt(x * x + y * y + z * z)
expected_time = distance / vel
expected_vel_x = vel * x / distance
expected_vel_y = vel * y / distance
expected_vel_z = vel * z / distance
self.sut.take_off()
thread_mock.reset_mock()
sleep_mock.reset_mock()
# Test
self.sut.move_distance(x, y, z)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(expected_vel_x, expected_vel_y, expected_vel_z, 0.0),
])
sleep_mock.assert_called_with(expected_time)
def test_that_it_moves(self, _SetPointThread_mock, sleep_mock):
# Fixture
vel = 0.3
self.sut.take_off()
data = [
[self.sut.forward, (vel, 0.0, 0.0, 0.0)],
[self.sut.back, (-vel, 0.0, 0.0, 0.0)],
[self.sut.left, (0.0, vel, 0.0, 0.0)],
[self.sut.right, (0.0, -vel, 0.0, 0.0)],
[self.sut.up, (0.0, 0.0, vel, 0.0)],
[self.sut.down, (0.0, 0.0, -vel, 0.0)],
]
# Test
# Assert
for test in data:
self._verify_move(test[0], vel, test[1], _SetPointThread_mock,
sleep_mock)
def test_that_it_starts_turn_right(self, _SetPointThread_mock, sleep_mock):
# Fixture
rate = 20
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.start_turn_right(rate)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.0, rate),
])
def test_that_it_starts_turn_left(self, _SetPointThread_mock, sleep_mock):
# Fixture
rate = 20
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.start_turn_left(rate)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.0, -rate),
])
def test_that_it_starts_circle_right(self, _SetPointThread_mock,
sleep_mock):
# Fixture
velocity = 0.5
radius = 0.9
expected_rate = 360 * velocity / (2 * radius * math.pi)
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.start_circle_right(radius, velocity=velocity)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(velocity, 0.0, 0.0, expected_rate),
])
def test_that_it_starts_circle_left(self, _SetPointThread_mock,
sleep_mock):
# Fixture
velocity = 0.5
radius = 0.9
expected_rate = 360 * velocity / (2 * radius * math.pi)
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.start_circle_left(radius, velocity=velocity)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(velocity, 0.0, 0.0, -expected_rate),
])
def test_that_it_turns_right(self, _SetPointThread_mock, sleep_mock):
# Fixture
rate = 20
angle = 45
turn_time = angle / rate
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.turn_right(angle, rate=rate)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls(
[call(0.0, 0.0, 0.0, rate),
call(0.0, 0.0, 0.0, 0.0)])
sleep_mock.assert_called_with(turn_time)
def test_that_it_turns_left(self, _SetPointThread_mock, sleep_mock):
# Fixture
rate = 20
angle = 45
turn_time = angle / rate
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.turn_left(angle, rate=rate)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls(
[call(0.0, 0.0, 0.0, -rate),
call(0.0, 0.0, 0.0, 0.0)])
sleep_mock.assert_called_with(turn_time)
def test_that_it_circles_right(self, _SetPointThread_mock, sleep_mock):
# Fixture
radius = 0.7
velocity = 0.3
angle = 45
distance = 2 * radius * math.pi * angle / 360.0
turn_time = distance / velocity
rate = angle / turn_time
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.circle_right(radius, velocity, angle)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls(
[call(velocity, 0.0, 0.0, rate),
call(0.0, 0.0, 0.0, 0.0)])
sleep_mock.assert_called_with(turn_time)
def test_that_it_circles_left(self, _SetPointThread_mock, sleep_mock):
# Fixture
radius = 0.7
velocity = 0.3
angle = 45
distance = 2 * radius * math.pi * angle / 360.0
turn_time = distance / velocity
rate = angle / turn_time
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.circle_left(radius, velocity, angle)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls(
[call(velocity, 0.0, 0.0, -rate),
call(0.0, 0.0, 0.0, 0.0)])
sleep_mock.assert_called_with(turn_time)
######################################################################
def _verify_start_motion(self, function_under_test, velocity, expected,
_SetPointThread_mock):
# Fixture
thread_mock = _SetPointThread_mock()
thread_mock.reset_mock()
# Test
function_under_test(velocity=velocity)
# Assert
try:
thread_mock.set_vel_setpoint.assert_has_calls([
call(*expected),
])
except AssertionError as e:
self._eprint('Failed when testing function ' +
function_under_test.__name__)
raise e
def _verify_move(self, function_under_test, velocity, expected,
_SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
distance = 1.2
expected_time = distance / velocity
thread_mock.reset_mock()
sleep_mock.reset_mock()
# Test
function_under_test(distance, velocity=velocity)
# Assert
try:
thread_mock.set_vel_setpoint.assert_has_calls([
call(*expected),
call(0.0, 0.0, 0.0, 0.0),
])
sleep_mock.assert_called_with(expected_time)
except AssertionError as e:
print('Failed when testing function ' +
function_under_test.__name__)
raise e
import logging
import time
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.positioning.motion_commander import MotionCommander
URI = 'radio://0/60/2M/E7E7E7E7E7'
# 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)
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
# We take off when the commander is create
mc = MotionCommander(scf)
mc.take_off(0.5, 0.3)
time.sleep(1)
mc.circle_right(1, velocity=0.6, angle_degrees=180)
mc.up(0.5)
mc.circle_right(1, velocity=0.6, angle_degrees=180)
mc.land(0.2)
import logging
import time
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.positioning.motion_commander import MotionCommander
URI = 'radio://0/0/2M'
# 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)
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
# We take off when the commander is create
mc = MotionCommander(scf)
time.sleep(1)
mc.take_off(1.2, 0.3)
time.sleep(1.5)
mc.circle_right(0.5, velocity=0.3, angle_degrees=360)
time.sleep(1.5)
mc.land(0.2)
class Mission:
def __init__(self, URI):
# Tool to process the data from drone's sensors
self.processing = Processing(URI)
cflib.crtp.init_drivers(enable_debug_driver=False)
self.cf = Crazyflie(ro_cache=None, rw_cache='./cache')
# Connect callbacks from the Crazyflie API
self.cf.connected.add_callback(self.connected)
self.cf.disconnected.add_callback(self.disconnected)
# Connect to the Crazyflie
self.cf.open_link(URI)
time.sleep(3)
self.pose_home = self.position
self.pose_home[2] = 0.1
self.velocity = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0}
self.goal = np.array([0.5, 0.0, 0.3])
self.mc = MotionCommander(self.cf)
time.sleep(3)
self.mc.take_off(0.15, 0.2)
time.sleep(1)
self.mc.circle_right(0.8, velocity=0.5, angle_degrees=360)
time.sleep(2)
self.mc.up(0.1)
time.sleep(1)
self.mc.circle_right(0.8, velocity=0.5, angle_degrees=360)
time.sleep(2)
self.mc.up(0.1)
time.sleep(1)
self.mc.circle_right(0.8, velocity=0.5, angle_degrees=360)
time.sleep(2)
self.mc.land(0.2)
''' Mission to a goal and return to home position '''
# self.mc.take_off(0.5, 0.2)
# time.sleep(1)
# rate = rospy.Rate(10)
# while not rospy.is_shutdown():
# self.sendVelocityCommand()
# rate.sleep()
def coverage_mission(self, height=0.2, length=1.0, width=0.3, numiters=2):
self.mc.take_off(height, 0.2)
time.sleep(1)
for _ in range(numiters):
self.mc.forward(length)
time.sleep(0.1)
self.mc.turn_right(90)
time.sleep(0.1)
self.mc.forward(width)
time.sleep(0.1)
self.mc.turn_right(90)
time.sleep(0.1)
self.mc.forward(length)
time.sleep(0.1)
self.mc.turn_left(90)
time.sleep(0.1)
self.mc.forward(width)
time.sleep(0.1)
self.mc.turn_left(90)
time.sleep(0.1)
self.mc.land(0.2)
time.sleep(1)
def square_mission(self, numiters=2):
'''
Square trajectory to collect data with multiranger.
'''
self.mc.take_off(0.15, 0.2)
time.sleep(1)
self.mc.turn_left(45)
time.sleep(0.1)
# sequence of repeated squares
for _ in range(numiters):
# sqaure
for _ in range(4):
self.mc.forward(1.4)
time.sleep(2)
self.mc.turn_right(90)
time.sleep(1)
self.mc.up(0.1)
time.sleep(1)
for _ in range(numiters):
# sqaure
for _ in range(4):
self.mc.forward(1.4)
time.sleep(2)
self.mc.turn_right(90)
time.sleep(1)
self.mc.land(0.2)
time.sleep(1)
def sendVelocityCommand(self):
direction = normalize(self.goal - self.position)
v_x = SPEED_FACTOR * direction[0]
v_y = SPEED_FACTOR * direction[1]
v_z = SPEED_FACTOR * direction[2]
# Local movement correction from obstacles
dV = 0.1 # [m/s]
if is_close(self.measurement['left']):
# print('Obstacle on the LEFT')
v_y -= dV
if is_close(self.measurement['right']):
# print('Obstacle on the RIGHT')
v_y += dV
self.velocity['x'] = v_x
self.velocity['y'] = v_y
self.velocity['z'] = v_z
# print('Sending velocity:', self.velocity)
goal_dist = norm(self.goal - self.position)
# print('Distance to goal %.2f [m]:' %goal_dist)
if goal_dist < GOAL_TOLERANCE: # goal is reached
# print('Goal is reached. Going home...')
self.goal = self.pose_home
self.cf.commander.send_velocity_world_setpoint(self.velocity['x'],
self.velocity['y'],
self.velocity['z'],
self.velocity['yaw'])
def disconnected(self, URI):
print('Disconnected')
def connected(self, URI):
print('We are now connected to {}'.format(URI))
# The definition of the logconfig can be made before connecting
lpos = LogConfig(name='Position', period_in_ms=100)
lpos.add_variable('lighthouse.x')
lpos.add_variable('lighthouse.y')
lpos.add_variable('lighthouse.z')
lpos.add_variable('stabilizer.roll')
lpos.add_variable('stabilizer.pitch')
lpos.add_variable('stabilizer.yaw')
try:
self.cf.log.add_config(lpos)
lpos.data_received_cb.add_callback(self.pos_data)
lpos.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Position log config, bad configuration.')
lmeas = LogConfig(name='Meas', period_in_ms=100)
lmeas.add_variable('range.front')
lmeas.add_variable('range.back')
lmeas.add_variable('range.up')
lmeas.add_variable('range.left')
lmeas.add_variable('range.right')
lmeas.add_variable('range.zrange')
lmeas.add_variable('stabilizer.roll')
lmeas.add_variable('stabilizer.pitch')
lmeas.add_variable('stabilizer.yaw')
try:
self.cf.log.add_config(lmeas)
lmeas.data_received_cb.add_callback(self.meas_data)
lmeas.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Measurement log config, bad configuration.')
lbat = LogConfig(
name='Battery',
period_in_ms=500) # read battery status with 2 Hz rate
lbat.add_variable('pm.vbat', 'float')
try:
self.cf.log.add_config(lbat)
lbat.data_received_cb.add_callback(self.battery_data)
lbat.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Measurement log config, bad configuration.')
def pos_data(self, timestamp, data, logconf):
# Transformation according to https://wiki.bitcraze.io/doc:lighthouse:setup
position = [
-data['lighthouse.z'], -data['lighthouse.x'], data['lighthouse.y']
# data['stateEstimate.x'],
# data['stateEstimate.y'],
# data['stateEstimate.z']
]
orientation = [
data['stabilizer.roll'], data['stabilizer.pitch'],
data['stabilizer.yaw']
]
self.position = np.array(position)
self.orientation = np.array(orientation)
self.processing.set_position(position, orientation)
def meas_data(self, timestamp, data, logconf):
measurement = {
'roll': data['stabilizer.roll'],
'pitch': data['stabilizer.pitch'],
'yaw': data['stabilizer.yaw'],
'front': data['range.front'],
'back': data['range.back'],
'up': data['range.up'],
'down': data['range.zrange'],
'left': data['range.left'],
'right': data['range.right']
}
self.measurement = measurement
self.processing.set_measurement(measurement)
def battery_data(self, timestamp, data, logconf):
self.V_bat = data['pm.vbat']
# print('Battery status: %.2f [V]' %self.V_bat)
if self.V_bat <= V_BATTERY_TO_GO_HOME:
self.battery_state = 'needs_charging'
# print('Battery is not charged: %.2f' %self.V_bat)
self.goal = self.pose_home
elif self.V_bat >= V_BATTERY_CHARGED:
self.battery_state = 'fully_charged'
