def test_current_point_is_not_reached_if_not_close_with_overstep_360(
self, zero_point):
assert not is_current_point_reached(
pan_angle=300,
tilt_angle=354,
current_target=zero_point,
next_target=PointOfMotion(pan_angle=100, tilt_angle=20, time=2))
def test_current_point_is_reached_if_close(self):
c = PointOfMotion(pan_angle=180,
pan_clockwise=True,
tilt_angle=30,
tilt_clockwise=True)
n = PointOfMotion(pan_angle=270,
pan_clockwise=True,
tilt_angle=15,
tilt_clockwise=False)
assert is_current_point_reached(pan_angle=179.96,
tilt_angle=30.04,
current_target=c,
next_target=n)
assert is_current_point_reached(pan_angle=179.96,
tilt_angle=30.04,
current_target=c,
next_target=None)
def test_current_point_is_not_reached_if_only_one_is_close(self):
c = PointOfMotion(pan_angle=180,
pan_clockwise=True,
tilt_angle=30,
tilt_clockwise=True)
n = PointOfMotion(pan_angle=270,
pan_clockwise=True,
tilt_angle=15,
tilt_clockwise=False)
# tilt angle is reached before pan angle is reached
assert not is_current_point_reached(pan_angle=179.91,
tilt_angle=30.04,
current_target=c,
next_target=n)
# pan angle is reached before tilt angle is reached
assert not is_current_point_reached(pan_angle=179.96,
tilt_angle=29.91,
current_target=c,
next_target=n)
def test_current_point_is_not_reached_if_not_close_with_counter_clockwise_overstep_360(
self, zero_point):
assert not is_current_point_reached(pan_angle=300,
tilt_angle=354,
current_target=PointOfMotion(
pan_angle=359.99,
pan_clockwise=False,
tilt_angle=359.99,
tilt_clockwise=False),
next_target=zero_point)
def test_calculate_respects_max_speed(self, calculator, max_pan_speed,
max_tilt_speed):
target_speeds = calculator.calculate(state=CameraState(speeds=Mock(),
pan_angle=0,
tilt_angle=0),
target=PointOfMotion(
pan_angle=180,
tilt_angle=90,
time=1))
assert max_pan_speed == target_speeds.pan_speed
assert max_tilt_speed == target_speeds.tilt_speed
def test_calculate(self, calculator):
target_speeds = calculator.calculate(state=CameraState(speeds=Mock(),
pan_angle=0,
tilt_angle=0),
target=PointOfMotion(
pan_angle=180,
tilt_angle=90,
time=9))
# gimbal has to pan 20°/s to get in 9s from 0° to 180°
assert 20 == target_speeds.pan_speed
# gimbal has to tilt 10°/s to get in 9s from 0° to 90°
assert 10 == target_speeds.tilt_speed
def test_calculate_overstep_360(self, calculator):
target_speeds = calculator.calculate(state=CameraState(speeds=Mock(),
pan_angle=280,
tilt_angle=320),
target=PointOfMotion(
pan_angle=10,
tilt_angle=20,
time=6))
# gimbal has to pan 15°/s counter-clockwise to get
# in 6s from 280° to 10° (90° difference)
assert 15 == target_speeds.pan_speed
# gimbal has to tilt 10°/s counter-clockwise to get
# in 6s from 320° to 20° (60° difference)
assert 10 == target_speeds.tilt_speed
def test_current_point_is_reached_if_one_is_close_after_the_other(self):
c = PointOfMotion(pan_angle=180,
pan_clockwise=True,
tilt_angle=30,
tilt_clockwise=True)
n = PointOfMotion(pan_angle=270,
pan_clockwise=True,
tilt_angle=45,
tilt_clockwise=True)
# If the next point is in the same direction as the current point,
# there is no stop at the intermediate point.
# Tilt angle has been reached before pan angle is reached,
# but tilting did not stop and tilt angle is not close anymore.
assert is_current_point_reached(pan_angle=179.96,
tilt_angle=30.14,
current_target=c,
next_target=n)
# Pan angle has been reached before tilt angle is reached,
# but panning did not stop and pan angle is not close anymore.
assert is_current_point_reached(pan_angle=180.14,
tilt_angle=29.96,
current_target=c,
next_target=n)
def zero_point(self):
return PointOfMotion(pan_angle=0, tilt_angle=0)
def test_no_stop_at_intermediate_point_if_next_point_is_in_same_direction(
self, controller, camera_speeds, gimbal, get_zero_angles,
zero_point, rotate_speed_manager, tilt_speed_manager,
target_speed_calculator, max_speeds, max_pan_speed,
max_tilt_speed):
gimbal.control = Mock()
target_speed_calculator.calculate = Mock(return_value=max_speeds)
rotate_speed_manager.acceleration_per_second = max_pan_speed / 2
tilt_speed_manager.acceleration_per_second = max_tilt_speed / 2
first_point = zero_point
second_point = PointOfMotion(pan_angle=100, tilt_angle=20, time=2)
controller.add_point(first_point)
controller.add_point(second_point)
controller.start()
gimbal.get_angles = Mock(return_value=get_angles_in_cmd(
pan_angle=300, pan_speed=0, tilt_angle=354, tilt_speed=0))
controller.update(camera_speeds)
# Tell gimbal to move with the speed of the first point to the second
# point.
gimbal.control.assert_called_once_with(
yaw_mode=ControlMode.angle,
yaw_speed=max_pan_speed / 2,
yaw_angle=second_point.pan_angle,
pitch_mode=ControlMode.angle,
pitch_speed=max_tilt_speed / 2,
pitch_angle=second_point.tilt_angle)
gimbal.control.reset_mock()
assert rotate_speed_manager.current_speed < rotate_speed_manager.target_speed
assert tilt_speed_manager.current_speed < tilt_speed_manager.target_speed
# increase speed
controller.update(camera_speeds)
# Tell gimbal to move with the speed of the first point to the second
# point.
gimbal.control.assert_called_once_with(
yaw_mode=ControlMode.angle,
yaw_speed=max_pan_speed,
yaw_angle=second_point.pan_angle,
pitch_mode=ControlMode.angle,
pitch_speed=max_tilt_speed,
pitch_angle=second_point.tilt_angle)
gimbal.control.reset_mock()
# First point is reached
gimbal.get_angles = get_zero_angles
# Continue moving to the second point, but with the speed of the second
# point, i.e. speed has to be decreased from max speed to:
# - pan 50°/s to get in 2s from 0° to 100°
# - tilt 10°/s to get in 2s from 0° to 20°
target_speed_calculator.calculate = Mock(
return_value=CameraSpeeds(pan_speed=50, tilt_speed=10))
controller.update(camera_speeds)
target_speed_calculator.calculate.assert_called_once_with(
CameraState(speeds=camera_speeds,
pan_angle=first_point.pan_angle,
tilt_angle=first_point.tilt_angle), second_point)
gimbal.control.assert_called_once_with(
yaw_mode=ControlMode.angle,
yaw_speed=50,
yaw_angle=second_point.pan_angle,
pitch_mode=ControlMode.angle,
pitch_speed=10,
pitch_angle=second_point.tilt_angle)
def test_target_speeds_are_calculated_before_current_speeds_are_updated(
self, controller, camera_speeds, gimbal, get_zero_angles,
rotate_speed_manager, tilt_speed_manager, target_speed_calculator,
max_speeds):
rotate_speed_manager.acceleration_per_second = 2
tilt_speed_manager.acceleration_per_second = 1
first_point = PointOfMotion(pan_angle=0,
pan_clockwise=False,
tilt_angle=0,
tilt_clockwise=False)
last_point = PointOfMotion(pan_angle=21, tilt_angle=6, time=3)
controller.add_point(first_point)
controller.add_point(last_point)
controller.start()
# gimbal is not at first point yet
gimbal.get_angles = Mock(return_value=get_angles_in_cmd(
pan_angle=180, pan_speed=60, tilt_angle=90, tilt_speed=12))
camera_speeds.pan_speed = 60
camera_speeds.tilt_speed = 12
camera_state = CameraState(speeds=camera_speeds,
pan_angle=180,
tilt_angle=90)
# gimbal should move with maximum speed to first point
target_speed_calculator.calculate = Mock(return_value=max_speeds)
# Assert that target speeds are set on speed manager before updating
# current speeds. Save update-method, replace it with assertion that
# calls saved update-method.
update_rotate_speed_manager = rotate_speed_manager.update
update_tilt_speed_manager = tilt_speed_manager.update
def assert_update_rotate_speed_manager():
assert rotate_speed_manager.target_speed == max_speeds.pan_speed, \
'target pan speed has to be set before updating current speed'
return update_rotate_speed_manager()
def assert_update_tilt_speed_manager():
assert tilt_speed_manager.target_speed == max_speeds.tilt_speed, \
'target tilt speed has to be set before updating current speed'
return update_tilt_speed_manager()
rotate_speed_manager.update = Mock(
side_effect=assert_update_rotate_speed_manager)
tilt_speed_manager.update = Mock(
side_effect=assert_update_tilt_speed_manager)
# TODO pass camera state to update (do not get angles in controller)
controller.update(camera_speeds)
# calculator has been used to calculate target speeds
target_speed_calculator.calculate.assert_called_once_with(
camera_state, first_point)
# and target speeds have been set on speed managers
assert rotate_speed_manager.target_speed == max_speeds.pan_speed
assert tilt_speed_manager.target_speed == max_speeds.tilt_speed
# and then current speeds have been updated
rotate_speed_manager.update.assert_called_once()
tilt_speed_manager.update.assert_called_once()
# and used to control the gimbal
gimbal.control.assert_called_once_with(
yaw_mode=ControlMode.angle,
yaw_speed=2,
yaw_angle=first_point.pan_angle,
pitch_mode=ControlMode.angle,
pitch_speed=1,
pitch_angle=first_point.tilt_angle)
def test_move_to_next_point_when_current_point_is_reached(
self, controller, camera_speeds, gimbal, get_zero_angles,
rotate_speed_manager, tilt_speed_manager, target_speed_calculator,
max_speeds):
gimbal.control = Mock()
rotate_speed_manager.acceleration_per_second = 4
rotate_speed_manager.target_speed = 60
rotate_speed_manager.current_speed = rotate_speed_manager.target_speed
tilt_speed_manager.acceleration_per_second = 1
tilt_speed_manager.target_speed = 12
tilt_speed_manager.current_speed = tilt_speed_manager.target_speed
current_point = PointOfMotion(pan_angle=0,
pan_clockwise=False,
tilt_angle=0,
tilt_clockwise=False)
target_point = PointOfMotion(pan_angle=21, tilt_angle=6, time=3)
controller.add_point(current_point)
controller.add_point(target_point)
controller.start()
# gimbal is already moving
gimbal.get_angles = Mock(return_value=get_angles_in_cmd(
pan_angle=1, pan_speed=60, tilt_angle=1, tilt_speed=12))
target_speed_calculator.calculate = Mock(return_value=max_speeds)
# and should move to first point
controller.update(camera_speeds)
gimbal.control.reset_mock()
# Gimbal still moves with target speed according to the speed manager
assert rotate_speed_manager.current_speed == 60
assert tilt_speed_manager.current_speed == 12
# Old target speeds used to move to first point
assert rotate_speed_manager.target_speed == 60
assert tilt_speed_manager.target_speed == 12
# First point is reached
gimbal.get_angles = get_zero_angles
camera_state = CameraState(speeds=camera_speeds,
pan_angle=current_point.pan_angle,
tilt_angle=current_point.tilt_angle)
# New target speeds are calculated based on next point of motion.
# Gimbal has to pan 7°/s to get in 3s from 0° to 21°.
# Gimbal has to tilt 2°/s to get in 3s from 0° to 6°.
target_speeds = CameraSpeeds(pan_speed=7, tilt_speed=2)
target_speed_calculator.calculate = Mock(return_value=target_speeds)
# TODO assert target speeds have been set before calculate is called
controller.update(camera_speeds)
assert controller.current_point() == target_point
assert gimbal.get_angles.call_count == 1
# calculator has been used to calculate target speeds
target_speed_calculator.calculate.assert_called_once_with(
camera_state, target_point)
# and target speeds have been set on speed managers
assert rotate_speed_manager.target_speed == target_speeds.pan_speed
assert tilt_speed_manager.target_speed == target_speeds.tilt_speed
# Gimbal stopped by itself, when it reached the last point.
# The speed managers don't know that. Hence, the current speed
# has to be set to zero and then updated.
assert rotate_speed_manager.current_speed == 4
assert tilt_speed_manager.current_speed == 1
gimbal.control.assert_called_once_with(
yaw_mode=ControlMode.angle,
yaw_speed=rotate_speed_manager.current_speed,
yaw_angle=target_point.pan_angle,
pitch_mode=ControlMode.angle,
pitch_speed=tilt_speed_manager.current_speed,
pitch_angle=target_point.tilt_angle)
def test_move_with_increasing_speed_to_point(self, controller,
camera_speeds, gimbal,
get_zero_angles, zero_point,
rotate_speed_manager,
tilt_speed_manager,
target_speed_calculator):
gimbal.get_angles = get_zero_angles
gimbal.control = Mock()
rotate_speed_manager.acceleration_per_second = 4
tilt_speed_manager.acceleration_per_second = 1
current_point = PointOfMotion(pan_angle=21, tilt_angle=6, time=3)
next_point = PointOfMotion(pan_angle=0,
pan_clockwise=False,
tilt_angle=0,
tilt_clockwise=False,
time=3)
controller.add_point(current_point)
controller.add_point(next_point)
controller.start()
assert gimbal.get_angles.call_count == 0
assert gimbal.control.call_count == 0
# Gimbal is not moving
assert rotate_speed_manager.current_speed == 0
assert tilt_speed_manager.current_speed == 0
# assert rotate_speed_manager.target_speed == 0
# assert tilt_speed_manager.target_speed == 0
# First point is not reached yet
gimbal.get_angles = get_zero_angles
camera_state = CameraState(speeds=camera_speeds,
pan_angle=zero_point.pan_angle,
tilt_angle=zero_point.tilt_angle)
# New target speeds are calculated based on next point of motion.
# Gimbal has to pan 7°/s to get in 3s from 21° to 0°.
# Gimbal has to tilt 2°/s to get in 3s from 6° to 0°.
target_speeds = CameraSpeeds(pan_speed=7, tilt_speed=2)
target_speed_calculator.calculate = Mock(return_value=target_speeds)
# TODO assert target speeds have been set before calculate is called
# should move with speeds of speed managers to first point
controller.update(camera_speeds)
# calculator has been used to calculate target speeds
target_speed_calculator.calculate.assert_called_once_with(
camera_state, current_point)
# and target speeds have been set on speed managers
assert rotate_speed_manager.target_speed == target_speeds.pan_speed
assert tilt_speed_manager.target_speed == target_speeds.tilt_speed
assert rotate_speed_manager.current_speed == 4
assert tilt_speed_manager.current_speed == 1
assert gimbal.get_angles.call_count == 1
gimbal.control.assert_called_once_with(
yaw_mode=ControlMode.angle,
yaw_speed=rotate_speed_manager.current_speed,
yaw_angle=current_point.pan_angle,
pitch_mode=ControlMode.angle,
pitch_speed=tilt_speed_manager.current_speed,
pitch_angle=current_point.tilt_angle)
assert not controller.is_target_speed_reached()
gimbal.get_angles = Mock(return_value=get_angles_in_cmd(
pan_angle=7, pan_speed=4, tilt_angle=2, tilt_speed=1))
gimbal.control.reset_mock()
# should increase speeds till target speeds are reached
controller.update(camera_speeds)
assert gimbal.get_angles.call_count == 1
# Should not have been called again, since target didn't change.
assert target_speed_calculator.calculate.call_count == 1
# Target speeds are reached
assert rotate_speed_manager.current_speed == 7
assert tilt_speed_manager.current_speed == 2
gimbal.control.assert_called_once_with(
yaw_mode=ControlMode.angle,
yaw_speed=rotate_speed_manager.current_speed,
yaw_angle=current_point.pan_angle,
pitch_mode=ControlMode.angle,
pitch_speed=tilt_speed_manager.current_speed,
pitch_angle=current_point.tilt_angle)
assert controller.is_target_speed_reached()
# reset mocks, since their call counts are checked later
gimbal.control.reset_mock()
rotate_speed_manager.update.reset_mock()
rotate_speed_manager.reset.reset_mock()
tilt_speed_manager.update.reset_mock()
tilt_speed_manager.reset.reset_mock()
gimbal.get_angles = Mock(return_value=get_angles_in_cmd(
pan_angle=14, pan_speed=60, tilt_angle=4, tilt_speed=12))
# target speeds are reached, but target is not reached yet
controller.update(camera_speeds)
# call to get_angles required to check if target is reached
assert gimbal.get_angles.call_count == 1
# Should not have been called again, since target didn't change.
assert target_speed_calculator.calculate.call_count == 1
# Target speed already reached => no control command needs to be sent
assert gimbal.control.call_count == 0
# but time of speed managers has to be updated
assert (rotate_speed_manager.update.call_count == 1
or rotate_speed_manager.reset.call_count == 1)
assert (tilt_speed_manager.update.call_count == 1
or tilt_speed_manager.reset.call_count == 1)
assert controller.current_point() == current_point
def non_zero_point(self):
return PointOfMotion(pan_angle=180, tilt_angle=30)