def calculate(self, state: CameraState, target: PointOfMotion):
if target.time == 0:
return CameraSpeeds(pan_speed=self._max_pan_speed,
tilt_speed=self._max_tilt_speed)
return CameraSpeeds(pan_speed=min(
self._max_pan_speed,
self.get_degree_per_second(state.pan_angle, target.pan_angle,
target.pan_clockwise, target.time)),
tilt_speed=min(
self._max_tilt_speed,
self.get_degree_per_second(
state.tilt_angle, target.tilt_angle,
target.tilt_clockwise, target.time)))
def test_move_to_point_with_minimum_speed(
self, controller, camera_speeds, gimbal, get_zero_angles,
non_zero_point, rotate_speed_manager, tilt_speed_manager,
target_speed_calculator, max_speeds):
gimbal.get_angles = get_zero_angles
gimbal.control = Mock()
target_speed_calculator.calculate = Mock(return_value=max_speeds)
controller.add_point(non_zero_point)
controller.start()
assert gimbal.get_angles.call_count == 0
assert gimbal.control.call_count == 0
assert rotate_speed_manager.current_speed == 0
assert tilt_speed_manager.current_speed == 0
assert rotate_speed_manager.acceleration_per_second == 0
assert tilt_speed_manager.acceleration_per_second == 0
rotate_speed_manager.target_speed = 60
tilt_speed_manager.target_speed = 12
# should move with minimum speed to first point,
# since current speed and acceleration are zero
controller.update(camera_speeds)
assert gimbal.get_angles.call_count == 1
min_speed = 1
gimbal.control.assert_called_once_with(
yaw_mode=ControlMode.angle,
yaw_speed=min_speed,
yaw_angle=non_zero_point.pan_angle,
pitch_mode=ControlMode.angle,
pitch_speed=min_speed,
pitch_angle=non_zero_point.tilt_angle)
assert camera_speeds == CameraSpeeds()
def test_update_of_empty_path(self, controller, camera_speeds, gimbal,
zero_angles, get_zero_angles):
gimbal.get_angles = get_zero_angles
controller.start()
controller.update(camera_speeds)
assert gimbal.get_angles.call_count == 0
assert camera_speeds == CameraSpeeds()
def __init__(self, camera_controller: CameraController,
align_tracking_strategy: AlignTrackingStrategy,
tracking_strategy: TrackingStrategy,
search_target_strategy: SearchTargetStrategy) -> None:
self._camera_controller = camera_controller
self._align_tracking_strategy = align_tracking_strategy
self._tracking_strategy = tracking_strategy
self._search_target_strategy = search_target_strategy
self._camera_speeds: CameraSpeeds = CameraSpeeds()
self.mode_name = 'manual'
self.is_zoom_enabled = True
def test_do_not_move_if_point_is_already_reached(
self, controller, camera_speeds, gimbal, zero_point,
get_zero_angles, rotate_speed_manager, tilt_speed_manager,
target_speed_calculator, max_speeds):
gimbal.get_angles = get_zero_angles
gimbal.control = Mock()
target_speed_calculator.calculate = Mock(return_value=max_speeds)
controller.add_point(zero_point)
controller.start()
assert gimbal.get_angles.call_count == 0
controller.update(camera_speeds)
# point is reached => do not send any control command to gimbal
assert gimbal.control.call_count == 0
# required to check if point is reached
assert gimbal.get_angles.call_count == 1
# speed should not be increased
assert rotate_speed_manager.update.call_count == 0
assert tilt_speed_manager.update.call_count == 0
assert camera_speeds == CameraSpeeds()
def _main():
from time import sleep
logging.basicConfig(
level=logging.DEBUG,
format='%(asctime)s %(name)-50s %(levelname)-8s %(message)s')
controller = BaseCamPathOfMotionCameraController(
Gimbal(),
rotate_speed_manager=SpeedManager(60),
tilt_speed_manager=SpeedManager(12),
target_speed_calculator=PointOfMotionTargetSpeedCalculator())
controller.add_point(
PointOfMotion(pan_angle=0,
pan_clockwise=False,
tilt_angle=0,
tilt_clockwise=False))
controller.add_point(
PointOfMotion(pan_angle=180,
pan_clockwise=True,
tilt_angle=30,
tilt_clockwise=True,
time=6))
controller.add_point(
PointOfMotion(pan_angle=270,
pan_clockwise=True,
tilt_angle=15,
tilt_clockwise=False,
time=3))
controller.add_point(
PointOfMotion(pan_angle=0,
pan_clockwise=False,
tilt_angle=0,
tilt_clockwise=False,
time=3))
camera_speeds = CameraSpeeds()
controller.start()
while not controller.is_end_of_path_reached():
sleep(1 / 15)
controller.update(camera_speeds)
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_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 test_update_empty_path(self, controller, camera_speeds):
controller.start()
controller.update(camera_speeds)
assert camera_speeds == CameraSpeeds()
def camera_speeds(self):
return CameraSpeeds()
def max_speeds(self, max_pan_speed, max_tilt_speed):
return CameraSpeeds(pan_speed=max_pan_speed, tilt_speed=max_tilt_speed)
def update(self, camera_speeds: CameraSpeeds) -> None:
assert self._state is not self._State.STOPPED
if not self.has_points():
return
angles = self._gimbal.get_angles()
_log_angles(angles)
camera_speeds.pan_speed = to_degree_per_sec(angles.target_speed_3)
camera_speeds.tilt_speed = to_degree_per_sec(angles.target_speed_2)
if self._state is self._State.STARTED:
self._state = self._State.RUNNING
if self._is_current_point_reached(angles):
if self.has_next_point():
self.next_point()
else:
self._stop()
return
else:
pan_angle = to_degree(angles.target_angle_3)
tilt_angle = to_degree(angles.target_angle_2)
self._previous_point = PointOfMotion(pan_angle=pan_angle,
tilt_angle=tilt_angle)
assert self._previous_point is not None
self._update_target_speeds(camera_speeds, self._previous_point)
self._update_speed_managers()
self._move_gimbal_to_current_point()
elif self._is_current_point_reached(angles):
logger.debug('move to next point')
if self.has_next_point():
self.next_point()
current_point = self.current_point()
previous = self._previous_point
# If the gimbal switches moving direction (clockwise <-> counter
# clockwise), then it 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.
if previous.pan_clockwise != current_point.pan_clockwise:
logger.debug('reset pan speed to 0')
self._rotate_speed_manager.current_speed = 0
if previous.tilt_clockwise != current_point.tilt_clockwise:
logger.debug('reset tilt speed to 0')
self._tilt_speed_manager.current_speed = 0
# TODO consider to update target speeds based on real angles
# of gimbal, since they may be different than the target
# point, which is assumed has been reached exactly.
# However, overstepping has to be considered.
# pan_angle = to_degree(angles.target_angle_3)
# tilt_angle = to_degree(angles.target_angle_2)
# real_current_point = PointOfMotion(pan_angle=pan_angle,
# tilt_angle=tilt_angle)
# self._update_target_speeds(camera_speeds, real_current_point)
# TODO only pass angles instead of PointOfMotion
self._update_target_speeds(camera_speeds, previous)
self._update_speed_managers()
self._move_gimbal_to_current_point()
else:
self._stop()
elif not self.is_target_speed_reached():
logger.debug('increase speed')
self._update_speed_managers()
self._move_gimbal_to_current_point()
else:
logger.debug('reached target speed')
# update elapsed time of speed managers
self._update_speed_managers()
if args.liveView == 'Webcam':
live_view = WebcamLiveView()
elif args.liveView == 'Panasonic':
live_view = PanasonicLiveView(args.ip, args.port)
camera_observable = PanasonicCameraObservable(
min_focal_length=args.cameraMinFocalLength)
live_view.add_ex_header_listener(camera_observable.on_ex_header)
camera_observable.add_listener(
ObservableCameraProperty.ZOOM_RATIO,
max_speed_and_acceleration_updater.on_zoom_ratio)
else:
print(f"Unknown live view {args.liveView}")
exit(1)
manual_camera_speeds = max_speed_and_acceleration_updater.add(
CameraSpeeds(pan_speed=8, tilt_speed=4, zoom_speed=ZoomSpeed.ZOOM_IN_SLOW))
# noinspection PyUnboundLocalVariable
cameraman = Cameraman(
live_view=live_view,
annotator=ImageAnnotator(args.targetLabelId, labels, font),
detection_engine=detection_engine,
destination=destination,
mode_manager=cameraman_mode_manager,
object_tracker=ObjectTracker(max_disappeared=25),
target_label_id=args.targetLabelId,
output=create_video_writer(args.output, live_view_image_size),
user_interfaces=user_interfaces,
# TODO get max speeds from separate CLI arguments
manual_camera_speeds=manual_camera_speeds)
to_exit = threading.Event()