diff --git a/.devcontainer/.clang-tidy b/.devcontainer/.clang-tidy
index 0aa217a..492f6ce 100644
--- a/.devcontainer/.clang-tidy
+++ b/.devcontainer/.clang-tidy
@@ -9,44 +9,19 @@ Checks: >
modernize-*,
performance-*,
readability-*,
+ -cppcoreguidelines-avoid-magic-numbers,
+ -modernize-use-trailing-return-type,
-readability-avoid-const-params-in-decls,
-readability-function-cognitive-complexity,
+ -readability-identifier-naming,
+ -readability-magic-numbers,
misc-unused-using-decls,
# Аналізувати лише код домашніх робіт і типові source/include директорії,
# а не системні headers або сторонні бібліотеки.
HeaderFilterRegex: '(^|/)(homework_[0-9]+|include|src)/'
-# Помилки analyzer/core-guidelines/modernize мають ламати перевірку.
-# Readability і performance лишаються попередженнями, щоб не блокувати
-# навчальні ітерації надто рано.
+# Зауваження static analyzer мають ламати перевірку: це найчастіше реальні баги.
+# Решта checks лишаються warning-ами, щоб clang-tidy був навчальним
+# інструментом, а не стіною стилістичних помилок для ДЗ.
WarningsAsErrors: clang-analyzer-*,
- cppcoreguidelines-*,
- modernize-*,
-
-# Naming rules формують один стиль для прикладів курсу.
-CheckOptions:
- - key: readability-identifier-naming.FunctionCase
- value: lower_case
- - key: readability-identifier-naming.VariableCase
- value: lower_case
- - key: readability-identifier-naming.ConstantCase
- value: CamelCase
- - key: readability-identifier-naming.ConstantPrefix
- value: k
- - key: readability-identifier-naming.LocalVariableCase
- value: lower_case
- - key: readability-identifier-naming.ParameterCase
- value: lower_case
- - key: readability-identifier-naming.MemberCase
- value: lower_case
- - key: readability-identifier-naming.MemberSuffix
- value: _
- - key: readability-identifier-naming.PrivateMemberCase
- value: lower_case
- - key: readability-identifier-naming.PrivateMemberSuffix
- value: _
- - key: readability-identifier-naming.ClassCase
- value: CamelCase
- - key: readability-identifier-naming.StructCase
- value: CamelCase
diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile
index 16a35bd..238e382 100644
--- a/.devcontainer/Dockerfile
+++ b/.devcontainer/Dockerfile
@@ -1,33 +1,15 @@
# syntax=docker/dockerfile:1
-# Ubuntu 24.04 LTS (noble), закріплено по SHA256 digest.
-#
-# Теги на Docker Hub змінюються: :noble перевипускається кілька разів
-# на рік з оновленнями безпеки. Той самий Dockerfile з тим самим тегом
-# завтра може витягнути інший образ. Пін по digest робить так, щоб
-# через будь-який час збірка давала ту саму базу.
-#
-# Оновити digest (коли планово бампаємо базу):
-# docker pull ubuntu:noble
-# docker inspect --format='{{index .RepoDigests 0}}' ubuntu:noble
-FROM ubuntu:noble@sha256:c4a8d5503dfb2a3eb8ab5f807da5bc69a85730fb49b5cfca2330194ebcc41c7b
+# ROS 2 Jazzy desktop як базовий devcontainer-образ.
+# Це лишає Ubuntu 24.04 під капотом, але одразу дає /opt/ros/jazzy,
+# rviz2, rqt-інструменти і системні залежності ROS 2.
+FROM osrf/ros:jazzy-desktop
# Неінтерактивний frontend, щоб apt не зависнув чекаючи відповіді на
# питання (часовий пояс, клавіатура, kernel upgrade prompt). У Dockerfile
# інтерактивного stdin немає - без цього build може зависнути назавжди.
ARG DEBIAN_FRONTEND="noninteractive"
-
-# Розділені deb822-джерела: amd64 з archive.ubuntu.com, arm64 з
-# ports.ubuntu.com. Обидва файли обов'язкові в build context, інакше
-# COPY провалить збірку. Без arch-обмежень apt тягнув би arm64 з
-# archive.ubuntu.com (там його немає -> 404).
-COPY ubuntu-amd64.sources /etc/apt/sources.list.d/ubuntu.sources
-COPY ubuntu-arm64.sources /etc/apt/sources.list.d/ubuntu-arm64.sources
-
-RUN dpkg --add-architecture arm64 && \
- apt-get update && \
- apt-get install --no-install-recommends -y libgpiod-dev:arm64
# Пакети зі списку apt-packages.in (по одному на рядок, коментарі через #).
#
# Прапори й чистка:
@@ -53,16 +35,27 @@ COPY .clang-format /.clang-format
COPY .clang-tidy /.clang-tidy
COPY .clangd /.clangd
COPY .cmake-format.json /.cmake-format.json
+COPY cyclonedds.xml /etc/cyclonedds.xml
# Змінні середовища:
# - TZ: UTC як нейтральний дефолт. Логи завжди в UTC, без суперечок з
# літнім часом різних регіонів.
# - LC_ALL / LANG / LANGUAGE: en_US.UTF-8 щоб stdout/stderr коректно
# виводили UTF-8 текст (кирилиця, emoji, JSON з non-ASCII ключами).
+# - RMW_IMPLEMENTATION: CycloneDDS як default middleware для ROS 2 demo.
+# Це робить discovery стабільнішим у host-network devcontainer.
+# - CYCLONEDDS_URI: явний DDS config для локального devcontainer graph.
+# Config лишає autodetect interface і multicast loopback для host network.
ENV TZ=Etc/UTC
ENV LC_ALL=en_US.UTF-8
ENV LANG=en_US.UTF-8
ENV LANGUAGE=en_US:en
+ENV ROS_DISTRO=jazzy
+ENV ROS_VERSION=2
+ENV ROS_PYTHON_VERSION=3
+ENV ROS_DOMAIN_ID=0
+ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+ENV CYCLONEDDS_URI=file:///etc/cyclonedds.xml
# sudo без пароля для dev-контейнера: зручно для apt install на ходу,
# debug tools, etc. У прод-образах ніколи - там контейнер має жорстко
@@ -103,7 +96,7 @@ RUN if id ubuntu >/dev/null 2>&1; then \
userdel -r ubuntu && groupdel ubuntu 2>/dev/null || true; \
fi && \
useradd -s /bin/bash -m ${CONTAINER_USER} && \
- echo "\nexport PROMPT_COMMAND=\"history -a; history -n\n\"" >> /home/${CONTAINER_USER}/.bashrc
+ printf '\nsource /opt/ros/jazzy/setup.bash\nexport PROMPT_COMMAND="history -a; history -n"\n' >> /home/${CONTAINER_USER}/.bashrc
# Всі подальші команди у контейнері (включно з процесами VS Code remote
# extension) виконуються від CONTAINER_USER, не від root.
diff --git a/.devcontainer/apt-packages.in b/.devcontainer/apt-packages.in
index 650c1e6..1dd665b 100644
--- a/.devcontainer/apt-packages.in
+++ b/.devcontainer/apt-packages.in
@@ -1,4 +1,4 @@
-# Базові пакети для стабільної роботи Ubuntu image.
+# Базові пакети для стабільної роботи devcontainer image.
ca-certificates
curl
locales
@@ -33,10 +33,11 @@ clang-format
cmake-format
ninja-build
+# ROS 2 інструменти для демо блоку 5: colcon workspace, ament_auto і graph GUI.
+python3-colcon-common-extensions
+ros-jazzy-ament-cmake-auto
+ros-jazzy-rqt-graph
-# work with rpi pins
-libgpiod-dev
-socat
-
-pkg-config
-file
\ No newline at end of file
+# CycloneDDS використовується як default RMW для стабільнішого discovery
+# у devcontainer.
+ros-jazzy-rmw-cyclonedds-cpp
diff --git a/.devcontainer/cyclonedds.xml b/.devcontainer/cyclonedds.xml
new file mode 100644
index 0000000..22fed7e
--- /dev/null
+++ b/.devcontainer/cyclonedds.xml
@@ -0,0 +1,30 @@
+
+
+
+
+
+ true
+ true
+
+
+
+
+
+
+
+ auto
+ 120
+
+
+
+
+
+
+
+
diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json
index 6e46918..7c36af8 100644
--- a/.devcontainer/devcontainer.json
+++ b/.devcontainer/devcontainer.json
@@ -35,6 +35,14 @@
".devcontainer/scripts/initialize"
],
+ // ROS 2 CLI daemon синхронізується з поточним CycloneDDS profile:
+ // прибрати завислий daemon, стартувати daemon заново і перевірити,
+ // що CLI graph-команди відповідають.
+ "postStartCommand": [
+ "bash",
+ ".devcontainer/scripts/post-start"
+ ],
+
// host network спрощує SSH/debug сценарії для занять з target-пристроями.
// SYS_PTRACE і seccomp=unconfined потрібні для стабільної роботи GDB
// всередині контейнера, особливо коли debug-сесія attach-иться до процесу.
@@ -77,7 +85,6 @@
"C_Cpp.intelliSenseEngine": "disabled",
"[cpp]": {
"editor.defaultFormatter": "llvm-vs-code-extensions.vscode-clangd",
- "editor.formatOnSave": true,
"editor.quickSuggestions": {
"other": "on",
"comments": "off",
diff --git a/.devcontainer/scripts/post-start b/.devcontainer/scripts/post-start
new file mode 100644
index 0000000..a0a7eea
--- /dev/null
+++ b/.devcontainer/scripts/post-start
@@ -0,0 +1,37 @@
+#!/usr/bin/env bash
+set -eo pipefail
+
+# VS Code викликає цей script після старту container-а. ROS 2 CLI daemon
+# має відповідати поточному DDS profile. Якщо лишився завислий daemon, звичайні
+# `ros2 node list` / `ros2 topic list` можуть зависати на XML-RPC connect.
+source /opt/ros/jazzy/setup.bash
+
+if [[ "${RMW_IMPLEMENTATION:-}" != "rmw_cyclonedds_cpp" ]]; then
+ echo "RMW_IMPLEMENTATION=${RMW_IMPLEMENTATION:-unset}, expected rmw_cyclonedds_cpp" >&2
+ exit 1
+fi
+
+if [[ "${CYCLONEDDS_URI:-}" != "file:///etc/cyclonedds.xml" ]]; then
+ echo "CYCLONEDDS_URI=${CYCLONEDDS_URI:-unset}, expected file:///etc/cyclonedds.xml" >&2
+ exit 1
+fi
+
+if command -v timeout >/dev/null 2>&1; then
+ timeout 3s ros2 daemon stop >/dev/null 2>&1 || true
+else
+ ros2 daemon stop >/dev/null 2>&1 || true
+fi
+
+if command -v pgrep >/dev/null 2>&1; then
+ while IFS= read -r ros_daemon_pid; do
+ kill "${ros_daemon_pid}" >/dev/null 2>&1 || true
+ done < <(pgrep -f 'ros2cli.daemon.daemonize|--name ros2-daemon|_ros2_daemon' || true)
+fi
+
+if command -v timeout >/dev/null 2>&1; then
+ timeout 5s ros2 daemon start >/dev/null
+ timeout 5s ros2 daemon status >/dev/null
+else
+ ros2 daemon start >/dev/null
+ ros2 daemon status >/dev/null
+fi
diff --git a/.devcontainer/ubuntu-amd64.sources b/.devcontainer/ubuntu-amd64.sources
deleted file mode 100644
index 69c7771..0000000
--- a/.devcontainer/ubuntu-amd64.sources
+++ /dev/null
@@ -1,15 +0,0 @@
-# Стандартні Ubuntu-репозиторії для нативних amd64 пакетів devcontainer.
-Types: deb
-URIs: http://archive.ubuntu.com/ubuntu
-Suites: noble noble-updates noble-backports
-Components: main universe restricted multiverse
-Architectures: amd64
-Signed-By: /usr/share/keyrings/ubuntu-archive-keyring.gpg
-
-# Security updates винесено окремо, бо вони публікуються на іншому mirror.
-Types: deb
-URIs: http://security.ubuntu.com/ubuntu
-Suites: noble-security
-Components: main universe restricted multiverse
-Architectures: amd64
-Signed-By: /usr/share/keyrings/ubuntu-archive-keyring.gpg
diff --git a/.devcontainer/ubuntu-arm64.sources b/.devcontainer/ubuntu-arm64.sources
deleted file mode 100644
index d6edd5e..0000000
--- a/.devcontainer/ubuntu-arm64.sources
+++ /dev/null
@@ -1,7 +0,0 @@
-# ARM64 пакети Ubuntu публікуються через ports.ubuntu.com.
-Types: deb
-URIs: http://ports.ubuntu.com/ubuntu-ports
-Suites: noble noble-updates noble-backports noble-security
-Components: main universe restricted multiverse
-Architectures: arm64
-Signed-By: /usr/share/keyrings/ubuntu-archive-keyring.gpg
diff --git a/.vscode/settings.json b/.vscode/settings.json
index d801ba8..2733f66 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -3,5 +3,8 @@
"editor.defaultFormatter": "llvm-vs-code-extensions.vscode-clangd",
"editor.formatOnSave": true,
"editor.formatOnPaste": false
- }
+ },
+ "clangd.arguments": [
+ "--compile-commands-dir=${workspaceFolder}/homework_13/robot_ws/build/antidrone_turret"
+ ]
}
\ No newline at end of file
diff --git a/homework_13/README.md b/homework_13/README.md
new file mode 100644
index 0000000..4dcaa5a
--- /dev/null
+++ b/homework_13/README.md
@@ -0,0 +1,718 @@
+# ДЗ 13: ROS 2 система antidrone-turret
+
+## Мета
+
+Розробити маленьку ROS 2 систему на C++20 для спрощеного сценарію протидії
+малому FPV-дрону. Одна нода читає файл з треком цілі та публікує багато
+повідомлень: координати в кадрі, дистанцію і оцінку надійності розпізнавання
+цілі. Контролер турелі має навести гімбал і серво, а коли в потоці настає
+потрібний момент - надіслати актуатору команду пострілу через сервіс
+`/actuator/trigger`.
+
+У цьому ДЗ вже надано стартовий пакет `antidrone_turret`:
+
+- власні `.msg` і `.srv` типи;
+- `target_track_publisher_node`;
+- `actuator_node`;
+- YAML-файл;
+- launch-файл, який запускає готові ноди;
+- тест для наданої стартової логіки.
+
+Готовий `actuator_node` не керує реальним обладнанням, але поводиться як
+невеликий драйвер: приймає команду пострілу через сервіс, рахує кількість
+прийнятих команд і публікує стан `READY` або `RELOADING`.
+
+Трек продовжує публікуватися під час перезаряджання. Студентський алгоритм
+має бачити `RELOADING` і не запитувати постріл повторно, доки актуатор не
+повернеться у `READY`.
+
+Потрібно самостійно дописати контролер, ноди драйверів наведення, окрему
+C++ логіку, тести, CMake targets і записи у launch-файл.
+
+---
+
+## Система, яку потрібно отримати
+
+У component diagram у першому рядку прямокутника вказана коротка псевдо-назва
+інстансу, у другому - реальна назва ROS 2 ноди.
+
+```plantuml
+@startuml
+skinparam monochrome true
+skinparam shadowing false
+
+rectangle "tgt\nROS 2 нода\ntarget_track_publisher_node" as Target
+rectangle "ctrl\nROS 2 нода\nturret_controller_node" as Controller
+rectangle "gmb\nROS 2 нода\ngimbal_driver_node" as Gimbal
+rectangle "yaw\nROS 2 нода\nyaw_servo_driver_node" as Servo
+rectangle "act\nROS 2 нода\nactuator_node" as Actuator
+rectangle "CLI / перевірка\nros2 topic echo" as Cli
+
+Target --> Controller : топік /perception/target\nTarget
+Actuator --> Controller : топік /actuator/status\nActuatorStatus
+
+Controller --> Gimbal : топік /gimbal/cmd\nGimbalCommand
+Controller --> Servo : топік /servo/cmd\nServoCommand
+Controller --> Actuator : сервіс /actuator/trigger\nTriggerActuator
+Controller --> Cli : публікує топік /turret/status\nTurretStatus
+
+@enduml
+```
+
+Послідовність:
+
+У sequence diagram зліва від двокрапки вказана коротка псевдо-назва
+інстансу, справа - назва ROS 2 ноди.
+
+```plantuml
+@startuml
+skinparam monochrome true
+skinparam shadowing false
+
+participant "tgt: target_track_publisher_node" as Target
+participant "act: actuator_node" as Actuator
+participant "ctrl: turret_controller_node" as Controller
+participant "gmb: gimbal_driver_node" as Gimbal
+participant "yaw: yaw_servo_driver_node" as Servo
+
+Actuator -> Controller : /actuator/status\nstate=READY
+Target -> Controller : /perception/target\nconfidence=0.90 distance_m=25
+Controller -> Gimbal : /gimbal/cmd\nUP
+Controller -> Servo : /servo/cmd\nRIGHT
+Controller -> Actuator : /actuator/trigger\nconfidence=0.90 distance_m=25
+Actuator --> Controller : accepted=true trigger_count=1
+Actuator -> Controller : /actuator/status\nstate=RELOADING
+Actuator -> Controller : /actuator/status\nstate=READY
+
+@enduml
+```
+
+---
+
+## Ролі гімбала, серво і актуатора
+
+У цьому ДЗ гімбал і yaw-серво - це дві частини наведення турелі, а актуатор -
+окремий вузол для пострілу:
+
+- `gimbal_driver_node` відповідає за вертикальне наведення. У цьому спрощенні
+ гімбал - це вузол, який піднімає, опускає або втримує лінію наведення. Нода
+ отримує `GimbalCommand`: `UP`, `DOWN`, `CENTER`;
+- `yaw_servo_driver_node` відповідає за горизонтальний поворот турелі.
+ Yaw-серво - це сервопривід повороту навколо вертикальної осі. Нода отримує
+ `ServoCommand`: `LEFT`, `RIGHT`, `CENTER`;
+- `actuator_node` не займається наведенням. Він відповідає тільки за команду
+ пострілу через сервіс `/actuator/trigger` і за стан `READY` або
+ `RELOADING`.
+
+Актуатор потрібно сприймати як корисне навантаження, закріплене на тій самій
+лінії наведення. Це може бути умовний сіткостріл або інший виконавчий модуль:
+куди гімбал і yaw-серво навели центр турелі, туди "дивиться" актуатор.
+
+Тобто контролер спочатку вирішує, куди навести турель, а команду пострілу
+надсилає окремо і тільки тоді, коли ціль достатньо близько, розпізнавання
+достатньо надійне, а актуатор має стан `READY`. У цьому ДЗ не потрібно
+моделювати фізику пострілу або реальне обладнання.
+
+---
+
+## Готові файли
+
+```text
+homework_13/
+ robot_ws/
+ src/
+ antidrone_turret/
+ package.xml
+ CMakeLists.txt
+ msg/
+ Target.msg
+ ActuatorStatus.msg
+ srv/
+ TriggerActuator.srv
+ include/
+ antidrone_turret/
+ target_sequence.hpp
+ target_track_loader.hpp
+ target_track_simulation.hpp
+ actuator_model.hpp
+ src/
+ target_track_publisher_node.cpp
+ actuator_node.cpp
+ test/
+ provided_logic_test.cpp
+ tracks/
+ approach_trigger.csv
+ far_flyby_no_trigger.csv
+ low_confidence_no_trigger.csv
+ reload_pressure.csv
+ config/
+ sequence.yaml
+ launch/
+ system.launch.py
+```
+
+Готовий `system.launch.py` запускає тільки:
+
+- `target_track_publisher_node`;
+- `actuator_node`.
+
+Готовий тест `test/provided_logic_test.cpp` перевіряє тільки надану стартову
+логіку:
+
+- читання CSV-треку;
+- валідність усіх наданих CSV-треків;
+- послідовність повідомлень цілі;
+- допоміжну логіку публікації треку;
+- перехід актуатора `READY` -> `RELOADING` -> `READY`;
+- відхилення повторної команди пострілу під час `RELOADING`.
+
+---
+
+## Що потрібно дописати
+
+Потрібно розширити наданий пакет `antidrone_turret`, а не створювати новий
+пакет. Конкретні назви `.cpp` і `.hpp` файлів можна обрати самостійно.
+
+`turret_controller_node`, `gimbal_driver_node` і `yaw_servo_driver_node` у
+стартовому коді не надаються. Їх потрібно реалізувати самостійно. Готові ноди
+`target_track_publisher_node` і `actuator_node` тільки дають вхідний потік
+цілей, сервіс пострілу і статус актуатора.
+
+Обов'язкові частини рішення:
+
+- реалізувати ноду `turret_controller_node`;
+- реалізувати ноду `gimbal_driver_node`;
+- реалізувати ноду `yaw_servo_driver_node`;
+- `msg/GimbalCommand.msg`;
+- `msg/ServoCommand.msg`;
+- `msg/TurretStatus.msg`;
+- окрема C++ логіка контролера турелі без `rclcpp`;
+- мінімальні тести для власної логіки контролера: хоча б один тест на кожну
+ частину рішення;
+- CMake targets для нових виконуваних файлів і тестів;
+- записи для нових нод у `launch/system.launch.py`;
+- YAML-параметри для `turret_controller_node`.
+
+У `CMakeLists.txt` потрібно користуватися `ament_auto`, як у стартовому пакеті
+і прикладах з `repository/demos/lesson_5_4`.
+
+---
+
+## Надані типи
+
+```text
+# msg/Target.msg
+bool visible
+float32 x
+float32 y
+float32 distance_m
+float32 confidence
+```
+
+Поле `confidence` означає оцінку надійності розпізнавання цілі від `0.0` до
+`1.0`. Чим більше значення, тим надійніше perception-модуль вважає, що в кадрі
+саме FPV-ціль, а не фон, шум або інший об'єкт.
+
+```text
+# msg/ActuatorStatus.msg
+uint8 READY=0
+uint8 RELOADING=1
+
+uint8 state
+uint32 trigger_count
+```
+
+```text
+# srv/TriggerActuator.srv
+float32 confidence
+float32 distance_m
+---
+bool accepted
+uint32 trigger_count
+```
+
+## Типи, які потрібно додати
+
+Ці `.msg` не надаються у стартовому пакеті. Їх потрібно створити самостійно,
+додати у `rosidl_generate_interfaces(...)` і підключити до targets, які їх
+використовують.
+
+```text
+# msg/GimbalCommand.msg
+int8 DOWN=-1
+int8 CENTER=0
+int8 UP=1
+
+int8 direction
+float32 target_y
+float32 error_y
+```
+
+```text
+# msg/ServoCommand.msg
+int8 LEFT=-1
+int8 CENTER=0
+int8 RIGHT=1
+
+int8 direction
+float32 target_x
+float32 error_x
+```
+
+```text
+# msg/TurretStatus.msg
+uint8 TARGET_NONE=0
+uint8 TARGET_LOW_CONFIDENCE=1
+uint8 TARGET_LOCKED=2
+
+uint8 ACTION_IDLE=0
+uint8 ACTION_TRACK=1
+
+uint8 TRIGGER_SKIP=0
+uint8 TRIGGER_REQUESTED=1
+uint8 TRIGGER_RELOADING=2
+
+uint8 target_state
+uint8 action
+uint8 trigger_state
+float32 confidence
+float32 distance_m
+```
+
+### Контракт команд наведення
+
+`Target.x` і `Target.y` - це координати центру цілі в кадрі. У цьому ДЗ
+використовується спрощена модель кадру:
+
+```text
+ширина кадру: 640
+висота кадру: 480
+центр кадру: x=320, y=240
+
+x росте праворуч
+y росте вниз
+```
+
+`turret_controller_node` має перетворити координати цілі на дві окремі
+команди наведення:
+
+- `/servo/cmd` керує горизонтальним поворотом турелі;
+- `/gimbal/cmd` керує вертикальним наведенням.
+
+Поля `ServoCommand`:
+
+- `target_x` - скопійоване значення `Target.x`;
+- `error_x = Target.x - 320.0`;
+- `direction=RIGHT`, якщо `error_x > 0`;
+- `direction=LEFT`, якщо `error_x < 0`;
+- `direction=CENTER`, якщо `error_x == 0`.
+
+Поля `GimbalCommand`:
+
+- `target_y` - скопійоване значення `Target.y`;
+- `error_y = 240.0 - Target.y`;
+- `direction=UP`, якщо `error_y > 0`;
+- `direction=DOWN`, якщо `error_y < 0`;
+- `direction=CENTER`, якщо `error_y == 0`.
+
+Чому `error_y` рахується як `240.0 - Target.y`: у координатах кадру `y`
+росте вниз, тому ціль вище центру має менше `y`, але для команди гімбала це
+означає рух `UP`.
+
+`gimbal_driver_node` і `yaw_servo_driver_node` у цьому ДЗ не керують реальним
+залізом. Вони мають підписатися на свої топіки, отримати типізовану команду і
+залогоувати, що команда дійшла. Наприклад:
+
+```text
+gimbal_driver_node отримав: direction=UP target_y=180 error_y=60
+yaw_servo_driver_node отримав: direction=RIGHT target_x=420 error_x=100
+```
+
+Якщо ціль не видима або розпізнавання недостатньо надійне, нові команди
+`/gimbal/cmd` і `/servo/cmd` публікувати не потрібно. У такому випадку
+`TurretStatus.action` має бути `ACTION_IDLE`.
+
+Runtime-контракти:
+
+```text
+/perception/target antidrone_turret/msg/Target
+/turret/status antidrone_turret/msg/TurretStatus
+/gimbal/cmd antidrone_turret/msg/GimbalCommand
+/servo/cmd antidrone_turret/msg/ServoCommand
+/actuator/status antidrone_turret/msg/ActuatorStatus
+/actuator/trigger antidrone_turret/srv/TriggerActuator
+```
+
+---
+
+## Готові ноди
+
+`target_track_publisher_node` читає CSV-трек і публікує кожен рядок як
+`Target.msg`. Якщо launch запускається без `track:=...`, використовується
+`track:=all`: усі готові треки йдуть послідовно від `approach_trigger.csv`
+до `reload_pressure.csv`. Між епізодами є рядок `visible=false`.
+
+За замовчуванням послідовність повторюється циклом, тобто повідомлення
+продовжують приходити навіть після команди пострілу або під час `RELOADING`.
+
+Формат CSV:
+
+```text
+visible,x,y,distance_m,confidence
+true,320,240,70,0.70
+true,340,230,55,0.82
+```
+
+Готові треки:
+
+- `approach_trigger.csv` - FPV підлітає до турелі; має з'явитися момент для
+ команди пострілу через `/actuator/trigger`;
+- `far_flyby_no_trigger.csv` - FPV розпізнається з достатньою оцінкою
+ надійності, але не заходить у робочу дистанцію;
+- `low_confidence_no_trigger.csv` - FPV близько, але оцінка надійності
+ розпізнавання завжди нижче `0.80`, тобто нижче 80%;
+- `reload_pressure.csv` - два близькі епізоди підряд: перший може витратити
+ постріл, а наступний FPV доходить до захищеної зони, поки актуатор ще у
+ `RELOADING`. У файлі це видно по другому близькому епізоду, де дистанція
+ доходить до `distance_m=7`.
+
+Параметри `target_track_publisher_node`:
+
+- `publish_hz`;
+- `repeat_sequence`;
+- `track_file` - передається launch-файлом, напряму в YAML міняти не потрібно.
+
+Launch-аргумент `track` обирає набір або окремий файл:
+
+```bash
+ros2 launch antidrone_turret system.launch.py
+ros2 launch antidrone_turret system.launch.py track:=approach_trigger.csv
+ros2 launch antidrone_turret system.launch.py track:=far_flyby_no_trigger.csv
+ros2 launch antidrone_turret system.launch.py track:=low_confidence_no_trigger.csv
+ros2 launch antidrone_turret system.launch.py track:=reload_pressure.csv
+```
+
+Перший запуск без `track:=...` еквівалентний `track:=all`.
+
+### Можливі сценарії роботи турелі
+
+Трек потрібно сприймати як невеликий епізод: один FPV з'являється у полі
+зору, рухається від кадру до кадру, може наближатися до турелі або пролітати
+повз. Рядок з `visible=false` означає паузу між епізодами або втрату цілі.
+
+У ДЗ можливі такі сценарії:
+
+- дальній проліт: ціль видима і має достатню оцінку надійності
+ розпізнавання, тому турель має наводити гімбал і серво, але сервіс
+ пострілу `/actuator/trigger` не викликається, бо
+ `distance_m > max_distance_m`;
+- ненадійне розпізнавання: ціль близько, але оцінка надійності розпізнавання
+ нижче `confidence_threshold`. За замовчуванням це `0.80`, тобто нижче 80%.
+ У цьому випадку контролер має перейти в `TARGET_LOW_CONFIDENCE`, не рухати
+ наведення і не запитувати постріл;
+- вдалий момент для пострілу: ціль видима, оцінка надійності розпізнавання
+ достатня, `distance_m <= max_distance_m`, актуатор у стані `READY`;
+ контролер має навести гімбал і серво, опублікувати статус і один раз
+ викликати сервіс пострілу;
+- перезаряджання: після прийнятої команди пострілу актуатор публікує
+ `RELOADING`;
+ поки цей стан активний, близька ціль може продовжувати приходити у
+ `/perception/target`, але повторну команду пострілу викликати не потрібно;
+- наступний епізод: після вдалого пострілу або після завершення поточного
+ епізоду трек може перейти до наступної цілі, тому контролер має працювати
+ як потокова система і не покладатися на одноразовий запуск;
+- запізнілий запит пострілу: якщо близька ціль приходить під час
+ `RELOADING`, її потрібно позначити як `TRIGGER_RELOADING`; це демонструє,
+ що правильна поведінка системи залежить не тільки від дистанції і оцінки
+ надійності розпізнавання, а й від останнього стану актуатора;
+- тиск на перезаряджання: в `reload_pressure.csv` перший близький FPV може
+ отримати постріл, але наступний FPV підлітає, поки актуатор ще не готовий.
+ Контролер не має робити повторний запит пострілу під час `RELOADING`.
+
+`actuator_node`:
+
+- є навчальною реалізацією драйвера актуатора без логіки, прив'язаної до
+ конкретного обладнання;
+- на старті публікує `/actuator/status` зі `state=READY`;
+- має сервіс `/actuator/trigger`;
+- після прийнятої команди пострілу повертає відповідь сервісу;
+- після прийнятої команди пострілу публікує `/actuator/status` зі
+ `state=RELOADING`;
+- після `reload_ms` публікує `/actuator/status` зі `state=READY`.
+
+Параметри `actuator_node`:
+
+- `reload_ms`;
+- `status_publish_hz`.
+
+Готові `target_track_publisher_node` і `actuator_node` показують розділення:
+ROS 2 нода відповідає за параметри, топіки, сервіс і таймери.
+`TargetTrackSimulation` і `ActuatorModel` не залежать від `rclcpp` і тримають
+звичайну C++ логіку.
+
+Для цієї наданої логіки вже є `test/provided_logic_test.cpp`. Його не потрібно
+видаляти або переписувати. Нові тести мають додаватися поруч і перевіряти
+саме логіку контролера турелі.
+
+---
+
+## Логіка, яку потрібно реалізувати
+
+Логіка ухвалення рішення не має бути пов'язана з ROS 2. У ній не має бути
+`rclcpp`, publisher, subscriber, клієнт сервісу або `Node`. ROS-нода має
+тільки читати повідомлення, викликати чисту C++ логіку і перекладати результат
+у ROS 2 повідомлення або запит сервісу.
+
+У чистій C++ логіці потрібно виділити щонайменше такі частини:
+
+- оцінка цілі: `visible`, `confidence_threshold` -> `TARGET_NONE`,
+ `TARGET_LOW_CONFIDENCE` або `TARGET_LOCKED`;
+- команда yaw-серво: `Target.x` -> `ServoCommand.direction`, `target_x`,
+ `error_x`;
+- команда гімбала: `Target.y` -> `GimbalCommand.direction`, `target_y`,
+ `error_y`;
+- рішення щодо пострілу: `distance_m`, `max_distance_m`,
+ останній `ActuatorStatus.state` -> `TRIGGER_SKIP`, `TRIGGER_REQUESTED` або
+ `TRIGGER_RELOADING`;
+- складання `TurretStatus` для перевірки через `/turret/status`.
+
+Приклад такого розділення є у
+`repository/demos/lesson_5_4/06_logic_split`. Всередині package
+`logic_split_demo` чистий C++ код лежить у
+`include/logic_split_demo/target_decision.hpp`, тести для нього лежать у
+`test/target_decision_test.cpp`, а ROS 2 wrapper-нода лежить окремо в
+`src/decision_node.cpp`. Чистий C++ код не включає `rclcpp`.
+
+Параметри `turret_controller_node`:
+
+- `confidence_threshold=0.80`;
+- `max_distance_m=30.0`.
+
+`confidence_threshold=0.80` означає, що значення `confidence` від `0.80` і
+вище вважається достатньо надійним розпізнаванням. Значення нижче `0.80`
+потрібно трактувати як `TARGET_LOW_CONFIDENCE`.
+
+`TurretStatus` потрібен не для дублювання логів, а для перевірки рішення
+контролера через `ros2 topic echo /turret/status`. Його публікує
+`turret_controller_node` після обробки кожного нового повідомлення
+`/perception/target`.
+
+Поля статусу:
+
+- `target_state` описує якість цілі:
+ - `TARGET_NONE` - ціль не видима;
+ - `TARGET_LOW_CONFIDENCE` - ціль є, але `confidence < confidence_threshold`.
+ За default-порогом `0.80` це менше 80%;
+ - `TARGET_LOCKED` - ціль видима і розпізнавання достатньо надійне.
+- `action` описує, чи треба рухати наведення:
+ - `ACTION_IDLE` - не публікувати нові команди наведення, бо ціль не придатна;
+ - `ACTION_TRACK` - публікувати `GimbalCommand` і `ServoCommand`.
+- `trigger_state` описує рішення щодо команди пострілу через
+ `/actuator/trigger`:
+ - `TRIGGER_SKIP` - не викликати сервіс пострілу;
+ - `TRIGGER_REQUESTED` - викликати сервіс пострілу, бо ціль близько і
+ актуатор готовий;
+ - `TRIGGER_RELOADING` - ціль близько, але актуатор ще перезаряджається.
+
+Правила для кожного нового повідомлення `/perception/target`:
+
+- `visible=false` -> `TARGET_NONE`, `ACTION_IDLE`, `TRIGGER_SKIP`;
+- оцінка надійності розпізнавання нижче порога
+ (`confidence < confidence_threshold`) -> `TARGET_LOW_CONFIDENCE`,
+ `ACTION_IDLE`, `TRIGGER_SKIP`;
+- коректна ціль -> опублікувати `GimbalCommand`, `ServoCommand`, `TurretStatus`;
+- `distance_m <= max_distance_m` і актуатор `READY` -> викликати
+ `/actuator/trigger`, встановити `TRIGGER_REQUESTED`;
+- `distance_m <= max_distance_m` і актуатор `RELOADING` -> сервіс пострілу
+ не викликається, встановити `TRIGGER_RELOADING`;
+- `distance_m > max_distance_m` -> `TRIGGER_SKIP`.
+
+Правила вибору напрямку:
+
+- `x > 320` -> `ServoCommand.direction=RIGHT`;
+- `x < 320` -> `ServoCommand.direction=LEFT`;
+- `x == 320` -> `ServoCommand.direction=CENTER`;
+- `y < 240` -> `GimbalCommand.direction=UP`;
+- `y > 240` -> `GimbalCommand.direction=DOWN`;
+- `y == 240` -> `GimbalCommand.direction=CENTER`.
+
+Очікувані рішення для готових треків:
+
+```text
+approach_trigger.csv:
+ коли distance_m <= 30 і confidence >= 0.80:
+ розпізнавання достатньо надійне
+ READY -> TRIGGER_REQUESTED
+
+far_flyby_no_trigger.csv:
+ distance_m завжди > 30:
+ TRIGGER_SKIP
+
+low_confidence_no_trigger.csv:
+ confidence завжди < 0.80, тобто нижче 80%:
+ розпізнавання недостатньо надійне
+ ACTION_IDLE, TRIGGER_SKIP
+
+reload_pressure.csv:
+ перший близький кадр із надійним розпізнаванням:
+ READY -> TRIGGER_REQUESTED
+ наступний близький FPV приходить, поки актуатор у RELOADING:
+ RELOADING -> TRIGGER_RELOADING
+ якщо FPV доходить до захищеної зони до READY:
+ повторний постріл заборонений, епізод вважається пропущеним
+```
+
+---
+
+## Що потрібно перевірити
+
+### 1. Збірка і тести
+
+```bash
+source /opt/ros/jazzy/setup.bash
+cd homework_13/robot_ws
+colcon build --symlink-install --packages-select antidrone_turret
+source install/setup.bash
+colcon test --packages-select antidrone_turret
+colcon test-result --verbose
+```
+
+У стартовому пакеті вже є тести для наданої логіки. Для власної логіки
+контролера потрібно додати мінімальні тести: хоча б один тест на кожну
+частину рішення.
+
+```text
+оцінка цілі:
+ confidence нижче порога -> ACTION_IDLE, TRIGGER_SKIP
+
+команда yaw-серво:
+ x > 320 -> ServoCommand RIGHT, error_x > 0
+
+команда гімбала:
+ y < 240 -> GimbalCommand UP, error_y > 0
+
+рішення щодо пострілу:
+ близька ціль + актуатор READY -> TRIGGER_REQUESTED
+ близька ціль + актуатор RELOADING -> TRIGGER_RELOADING
+
+статус контролера:
+ далека коректна ціль -> TARGET_LOCKED, ACTION_TRACK, TRIGGER_SKIP
+```
+
+### 2. Перевірка під час роботи
+
+```bash
+ros2 launch antidrone_turret system.launch.py
+ros2 topic echo /perception/target --once
+ros2 topic echo /turret/status --once
+ros2 topic echo /gimbal/cmd --once
+ros2 topic echo /servo/cmd --once
+ros2 topic echo /actuator/status --once
+```
+
+Запуск без `track:=...` проганяє всі готові треки послідовно. Окремо можна
+запустити кожен трек:
+
+```bash
+ros2 launch antidrone_turret system.launch.py track:=approach_trigger.csv
+ros2 launch antidrone_turret system.launch.py track:=far_flyby_no_trigger.csv
+ros2 launch antidrone_turret system.launch.py track:=low_confidence_no_trigger.csv
+ros2 launch antidrone_turret system.launch.py track:=reload_pressure.csv
+```
+
+Якщо потрібно більше діагностичних повідомлень, можна змінити рівень логів
+під час запуску:
+
+```bash
+ros2 launch antidrone_turret system.launch.py track:=reload_pressure.csv log_level:=debug
+```
+
+За замовчуванням використовується `log_level:=info`. Для діагностики корисні
+`debug`, `info`, `warn`, `error`. У нормальній перевірці ДЗ не потрібно
+покладатися на debug-логи: очікувану поведінку мають доводити топіки, сервіс,
+граф ROS 2 і модульні тести.
+
+Для `reload_pressure.csv` рівень `debug` може показати діагностичне
+повідомлення симулятора про те, що FPV дійшов до захищеної зони, поки
+актуатор був у `RELOADING`. Основний критерій для студентського рішення все
+одно лишається через топіки і сервіс: під час `RELOADING` повторний сервіс
+пострілу не викликається.
+
+Очікування:
+
+- `/perception/target` має тип `antidrone_turret/msg/Target`;
+- `/turret/status` має тип `antidrone_turret/msg/TurretStatus`;
+- `/gimbal/cmd` має тип `antidrone_turret/msg/GimbalCommand`;
+- `/servo/cmd` має тип `antidrone_turret/msg/ServoCommand`;
+- `/actuator/status` має тип `antidrone_turret/msg/ActuatorStatus`.
+
+### 3. Сервіс
+
+```bash
+ros2 service call /actuator/trigger antidrone_turret/srv/TriggerActuator "{confidence: 0.9, distance_m: 25.0}"
+ros2 topic echo /actuator/status --once
+```
+
+Очікування:
+
+```text
+accepted: true
+trigger_count: 1
+```
+
+Після прийнятої команди пострілу `/actuator/status` має перейти у
+`RELOADING`, а після `reload_ms` повернутись у `READY`.
+
+### 4. Graph
+
+```bash
+ros2 node list
+ros2 topic list
+ros2 service list
+ros2 param list
+```
+
+Очікувані ноди:
+
+```text
+/target_track_publisher_node
+/actuator_node
+/turret_controller_node
+/gimbal_driver_node
+/yaw_servo_driver_node
+```
+
+Очікувані топіки:
+
+```text
+/perception/target
+/turret/status
+/gimbal/cmd
+/servo/cmd
+/actuator/status
+```
+
+Очікуваний сервіс:
+
+```text
+/actuator/trigger
+```
+
+---
+
+## Формат здачі
+
+Здається посилання на власний GitHub-репозиторій або PR.
+
+У README рішення мають бути:
+
+- короткий опис реалізованої системи;
+- PlantUML або текстова схема нод, топіків і сервісу;
+- команди build/test;
+- команда запуску;
+- фактичний результат `ros2 topic echo /turret/status --once`;
+- фактичний результат `ros2 topic echo /gimbal/cmd --once`;
+- фактичний результат `ros2 topic echo /servo/cmd --once`;
+- фактичний результат `ros2 topic echo /actuator/status --once`;
+- висновок, чи команда пострілу через актуатор викликається тільки при
+ `READY`.
diff --git a/homework_13/robot_ws/.gitignore b/homework_13/robot_ws/.gitignore
new file mode 100644
index 0000000..db0c2bd
--- /dev/null
+++ b/homework_13/robot_ws/.gitignore
@@ -0,0 +1,4 @@
+# ROS 2 colcon workspace artifacts — відтворюються з colcon build
+build/
+install/
+log/
\ No newline at end of file
diff --git a/homework_13/robot_ws/src/antidrone_turret/CMakeLists.txt b/homework_13/robot_ws/src/antidrone_turret/CMakeLists.txt
new file mode 100644
index 0000000..44edac9
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/CMakeLists.txt
@@ -0,0 +1,69 @@
+cmake_minimum_required(VERSION 3.20)
+project(antidrone_turret)
+
+# C++20 - базовий стандарт курсу.
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+# ament_auto бере залежності з package.xml і зменшує шаблонний CMake-код.
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+# Власні msg/srv генерують C++ типи для типізованих топіків і сервісу.
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "msg/Target.msg"
+ "msg/ActuatorStatus.msg"
+ "srv/TriggerActuator.srv"
+ "msg/GimbalCommand.msg"
+ "msg/ServoCommand.msg"
+ "msg/TurretStatus.msg"
+)
+
+ament_auto_add_executable(target_track_publisher_node src/target_track_publisher_node.cpp)
+ament_auto_add_executable(actuator_node src/actuator_node.cpp)
+ament_auto_add_executable(turret_controller_node src/turret_controller_node.cpp)
+ament_auto_add_executable(yaw_servo_driver_node src/yaw_servo_driver_node.cpp)
+ament_auto_add_executable(gimbal_driver_node src/gimbal_driver_node.cpp)
+
+target_include_directories(target_track_publisher_node PRIVATE include)
+target_include_directories(actuator_node PRIVATE include)
+target_include_directories(turret_controller_node PRIVATE include)
+
+rosidl_get_typesupport_target(cpp_typesupport_target
+ ${PROJECT_NAME}
+ "rosidl_typesupport_cpp"
+)
+
+target_link_libraries(target_track_publisher_node "${cpp_typesupport_target}")
+target_link_libraries(actuator_node "${cpp_typesupport_target}")
+target_link_libraries(turret_controller_node "${cpp_typesupport_target}")
+target_link_libraries(yaw_servo_driver_node "${cpp_typesupport_target}")
+target_link_libraries(gimbal_driver_node "${cpp_typesupport_target}")
+
+if(BUILD_TESTING)
+ find_package(ament_cmake_gtest REQUIRED)
+
+ ament_add_gtest(provided_logic_test
+ test/provided_logic_test.cpp
+ )
+ target_include_directories(provided_logic_test PRIVATE include)
+ target_compile_definitions(provided_logic_test
+ PRIVATE ANTIDRONE_TURRET_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}"
+ )
+ ament_add_gtest(turret_controller_test
+ test/turret_controller_test.cpp
+ )
+ target_include_directories(turret_controller_test PRIVATE include)
+endif()
+
+ament_export_dependencies(rosidl_default_runtime)
+
+# launch/ і config/ встановлюються у share, щоб ros2 launch знаходив файли.
+ament_auto_package(USE_SCOPED_HEADER_INSTALL_DIR INSTALL_TO_SHARE
+ launch
+ config
+ tracks
+)
diff --git a/homework_13/robot_ws/src/antidrone_turret/config/sequence.yaml b/homework_13/robot_ws/src/antidrone_turret/config/sequence.yaml
new file mode 100644
index 0000000..57a6bf2
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/config/sequence.yaml
@@ -0,0 +1,20 @@
+# target_track_publisher_node читає CSV-трек з launch-аргумента track.
+# Тут налаштовується частота публікації і повторення треку.
+target_track_publisher_node:
+ ros__parameters:
+ publish_hz: 5.0
+ repeat_sequence: true
+
+# Actuator є навчальною driver-нодою. reload_ms визначає, коли після trigger
+# стан повертається з RELOADING у READY.
+actuator_node:
+ ros__parameters:
+ reload_ms: 1500
+ status_publish_hz: 2.0
+
+# Ці параметри почнуть використовуватись після додавання
+# turret_controller_node студентським рішенням.
+turret_controller_node:
+ ros__parameters:
+ confidence_threshold: 0.8
+ max_distance_m: 30.0
diff --git a/homework_13/robot_ws/src/antidrone_turret/include/antidrone_turret/actuator_model.hpp b/homework_13/robot_ws/src/antidrone_turret/include/antidrone_turret/actuator_model.hpp
new file mode 100644
index 0000000..5c86373
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/include/antidrone_turret/actuator_model.hpp
@@ -0,0 +1,43 @@
+#pragma once
+
+#include
+
+namespace antidrone_turret {
+
+enum class ActuatorState : std::uint8_t {
+ kReady,
+ kReloading,
+};
+
+struct TriggerResult {
+ bool accepted{false};
+ std::uint32_t trigger_count{0};
+};
+
+class ActuatorModel {
+public:
+ [[nodiscard]] ActuatorState state() const { return ready_ ? ActuatorState::kReady : ActuatorState::kReloading; }
+
+ [[nodiscard]] std::uint32_t trigger_count() const { return trigger_count_; }
+
+ [[nodiscard]] bool ready() const { return ready_; }
+
+ TriggerResult trigger()
+ {
+ if (!ready_) {
+ return TriggerResult{false, trigger_count_};
+ }
+
+ ++trigger_count_;
+ ready_ = false;
+ return TriggerResult{true, trigger_count_};
+ }
+
+ void mark_ready() { ready_ = true; }
+
+private:
+ bool ready_{true};
+ std::uint32_t trigger_count_{0};
+};
+
+} // namespace antidrone_turret
diff --git a/homework_13/robot_ws/src/antidrone_turret/include/antidrone_turret/target_sequence.hpp b/homework_13/robot_ws/src/antidrone_turret/include/antidrone_turret/target_sequence.hpp
new file mode 100644
index 0000000..cf93749
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/include/antidrone_turret/target_sequence.hpp
@@ -0,0 +1,80 @@
+#pragma once
+
+#include
+#include
+#include
+
+namespace antidrone_turret {
+
+struct TargetSample {
+ bool visible{false};
+ float x{0.0F};
+ float y{0.0F};
+ float distance_m{0.0F};
+ float confidence{0.0F};
+};
+
+class TargetSequence {
+public:
+ TargetSequence() = default;
+
+ explicit TargetSequence(std::vector samples)
+ : samples_(std::move(samples))
+ {
+ }
+
+ [[nodiscard]] bool empty() const { return samples_.empty(); }
+
+ [[nodiscard]] std::size_t size() const { return samples_.size(); }
+
+ [[nodiscard]] bool finished(bool repeat) const { return !repeat && next_index_ >= samples_.size(); }
+
+ [[nodiscard]] const TargetSample& current() const { return samples_.at(next_index_ % samples_.size()); }
+
+ void advance() { ++next_index_; }
+
+ void skip_to_next_track()
+ {
+ if (samples_.empty()) {
+ return;
+ }
+
+ auto candidate_index = next_index_;
+ const auto stop_index = next_index_ + samples_.size();
+ bool passed_gap = false;
+
+ while (candidate_index < stop_index) {
+ const auto& sample = samples_.at(candidate_index % samples_.size());
+
+ if (!sample.visible) {
+ passed_gap = true;
+ }
+ else if (passed_gap) {
+ next_index_ = candidate_index;
+ return;
+ }
+
+ ++candidate_index;
+ }
+
+ next_index_ = stop_index;
+ }
+
+private:
+ std::vector samples_;
+ std::size_t next_index_{0};
+};
+
+inline std::vector default_target_samples()
+{
+ return {
+ TargetSample{true, 320.0F, 240.0F, 70.0F, 0.70F},
+ TargetSample{true, 340.0F, 230.0F, 55.0F, 0.82F},
+ TargetSample{true, 365.0F, 215.0F, 42.0F, 0.87F},
+ TargetSample{true, 390.0F, 200.0F, 31.0F, 0.91F},
+ TargetSample{true, 420.0F, 180.0F, 25.0F, 0.93F},
+ TargetSample{true, 445.0F, 170.0F, 19.0F, 0.95F},
+ };
+}
+
+} // namespace antidrone_turret
diff --git a/homework_13/robot_ws/src/antidrone_turret/include/antidrone_turret/target_track_loader.hpp b/homework_13/robot_ws/src/antidrone_turret/include/antidrone_turret/target_track_loader.hpp
new file mode 100644
index 0000000..8b9db15
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/include/antidrone_turret/target_track_loader.hpp
@@ -0,0 +1,166 @@
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "antidrone_turret/target_sequence.hpp"
+
+namespace antidrone_turret {
+
+struct TargetTrackLoadResult {
+ std::vector samples;
+ std::string error;
+};
+
+inline std::string trim_copy(std::string value)
+{
+ const auto first = std::find_if_not(value.begin(), value.end(), [](const unsigned char ch) { return std::isspace(ch) != 0; });
+ const auto last = std::find_if_not(value.rbegin(), value.rend(), [](const unsigned char ch) { return std::isspace(ch) != 0; }).base();
+
+ if (first >= last) {
+ return {};
+ }
+
+ return {first, last};
+}
+
+inline std::string lowercase_copy(std::string value)
+{
+ std::transform(value.begin(), value.end(), value.begin(), [](const unsigned char ch) { return static_cast(std::tolower(ch)); });
+ return value;
+}
+
+inline bool parse_bool_token(const std::string& token, bool& value)
+{
+ const auto normalized = lowercase_copy(trim_copy(token));
+ if (normalized == "true" || normalized == "1") {
+ value = true;
+ return true;
+ }
+ if (normalized == "false" || normalized == "0") {
+ value = false;
+ return true;
+ }
+ return false;
+}
+
+inline bool parse_float_token(const std::string& token, float& value)
+{
+ const auto trimmed = trim_copy(token);
+ try {
+ std::size_t parsed_chars = 0;
+ value = std::stof(trimmed, &parsed_chars);
+ return parsed_chars == trimmed.size();
+ }
+ catch (...) {
+ return false;
+ }
+}
+
+inline std::vector split_csv_row(const std::string& line)
+{
+ std::vector columns;
+ std::stringstream stream{line};
+ std::string column;
+
+ while (std::getline(stream, column, ',')) {
+ columns.push_back(trim_copy(column));
+ }
+
+ return columns;
+}
+
+inline TargetTrackLoadResult load_target_track_csv(const std::filesystem::path& path)
+{
+ std::ifstream input{path};
+ if (!input.is_open()) {
+ return {{}, "cannot open target track file: " + path.string()};
+ }
+
+ TargetTrackLoadResult result;
+ std::string line;
+ std::size_t line_number = 0;
+
+ while (std::getline(input, line)) {
+ ++line_number;
+ const auto trimmed = trim_copy(line);
+
+ if (trimmed.empty() || trimmed.front() == '#') {
+ continue;
+ }
+
+ if (lowercase_copy(trimmed).find("visible") != std::string::npos) {
+ continue;
+ }
+
+ const auto columns = split_csv_row(trimmed);
+ if (columns.size() != 5U) {
+ result.error = "invalid column count at line " + std::to_string(line_number);
+ return result;
+ }
+
+ TargetSample sample;
+ if (!parse_bool_token(columns[0], sample.visible) || !parse_float_token(columns[1], sample.x) ||
+ !parse_float_token(columns[2], sample.y) || !parse_float_token(columns[3], sample.distance_m) ||
+ !parse_float_token(columns[4], sample.confidence)) {
+ result.error = "invalid value at line " + std::to_string(line_number);
+ return result;
+ }
+
+ result.samples.push_back(sample);
+ }
+
+ if (result.samples.empty()) {
+ result.error = "target track file has no samples: " + path.string();
+ }
+
+ return result;
+}
+
+inline const std::vector& default_target_track_files()
+{
+ static const auto files = std::vector{
+ "approach_trigger.csv",
+ "far_flyby_no_trigger.csv",
+ "low_confidence_no_trigger.csv",
+ "reload_pressure.csv",
+ };
+ return files;
+}
+
+inline void append_track_gap(std::vector& samples)
+{
+ if (!samples.empty() && samples.back().visible) {
+ samples.push_back(TargetSample{false, 0.0F, 0.0F, 0.0F, 0.0F});
+ }
+}
+
+inline TargetTrackLoadResult load_target_track_csv_files(const std::filesystem::path& directory, const std::vector& file_names)
+{
+ TargetTrackLoadResult combined;
+
+ for (const auto& file_name : file_names) {
+ auto result = load_target_track_csv(directory / file_name);
+ if (!result.error.empty()) {
+ combined.samples.clear();
+ combined.error = file_name + ": " + result.error;
+ return combined;
+ }
+
+ combined.samples.insert(combined.samples.end(), result.samples.begin(), result.samples.end());
+ append_track_gap(combined.samples);
+ }
+
+ if (combined.samples.empty()) {
+ combined.error = "no target tracks loaded from: " + directory.string();
+ }
+
+ return combined;
+}
+
+} // namespace antidrone_turret
diff --git a/homework_13/robot_ws/src/antidrone_turret/include/antidrone_turret/target_track_simulation.hpp b/homework_13/robot_ws/src/antidrone_turret/include/antidrone_turret/target_track_simulation.hpp
new file mode 100644
index 0000000..6457ac6
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/include/antidrone_turret/target_track_simulation.hpp
@@ -0,0 +1,95 @@
+#pragma once
+
+#include
+#include
+#include
+#include
+
+#include "antidrone_turret/target_sequence.hpp"
+
+namespace antidrone_turret {
+
+struct TargetTrackSimulationConfig {
+ bool repeat_sequence{true};
+ float hit_confidence{0.80F};
+ float hit_distance_m{30.0F};
+ float impact_distance_m{8.0F};
+};
+
+struct PublishedTarget {
+ TargetSample sample;
+ bool reached_protected_zone_while_reloading{false};
+};
+
+class TargetTrackSimulation {
+public:
+ TargetTrackSimulation() = default;
+
+ TargetTrackSimulation(TargetSequence sequence, TargetTrackSimulationConfig config)
+ : sequence_(std::move(sequence))
+ , config_(config)
+ {
+ }
+
+ [[nodiscard]] bool empty() const { return sequence_.empty(); }
+
+ [[nodiscard]] std::size_t size() const { return sequence_.size(); }
+
+ std::optional next_target()
+ {
+ if (sequence_.empty() || sequence_.finished(config_.repeat_sequence)) {
+ return std::nullopt;
+ }
+
+ const auto sample = sequence_.current();
+ has_published_sample_ = true;
+
+ if (!sample.visible) {
+ hit_window_open_ = false;
+ }
+ else if (sample.confidence >= config_.hit_confidence && sample.distance_m <= config_.hit_distance_m) {
+ hit_window_open_ = true;
+ }
+
+ sequence_.advance();
+
+ const auto reached_protected_zone = sample.visible && sample.distance_m <= config_.impact_distance_m && !actuator_ready_;
+
+ if (reached_protected_zone) {
+ sequence_.skip_to_next_track();
+ has_published_sample_ = false;
+ hit_window_open_ = false;
+ }
+
+ return PublishedTarget{sample, reached_protected_zone};
+ }
+
+ [[nodiscard]] bool apply_actuator_status(const bool actuator_ready, const std::uint32_t trigger_count)
+ {
+ actuator_ready_ = actuator_ready;
+
+ if (trigger_count <= last_trigger_count_) {
+ return false;
+ }
+
+ last_trigger_count_ = trigger_count;
+ if (!has_published_sample_ || !hit_window_open_) {
+ return false;
+ }
+
+ sequence_.skip_to_next_track();
+ has_published_sample_ = false;
+ hit_window_open_ = false;
+ return true;
+ }
+
+private:
+ TargetSequence sequence_;
+ TargetTrackSimulationConfig config_;
+ bool actuator_ready_{true};
+ bool has_published_sample_{false};
+ bool hit_window_open_{false};
+ std::uint32_t last_trigger_count_{0};
+};
+
+} // namespace antidrone_turret
diff --git a/homework_13/robot_ws/src/antidrone_turret/include/antidrone_turret/turret_controller.hpp b/homework_13/robot_ws/src/antidrone_turret/include/antidrone_turret/turret_controller.hpp
new file mode 100644
index 0000000..3a7ae30
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/include/antidrone_turret/turret_controller.hpp
@@ -0,0 +1,113 @@
+#pragma once
+
+namespace turret_controller {
+
+const float frame_center_y = 240.0f;
+const float frame_center_x = 320.0f;
+
+enum class ActuatorStatuses { READY, RELOADING };
+
+enum class TargetStates { TARGET_NONE, TARGET_LOW_CONFIDENCE, TARGET_LOCKED };
+
+enum class ServoDirections { RIGHT, LEFT, CENTER };
+
+enum class GimbalDirections { UP, DOWN, CENTER };
+
+enum class TriggerStates { TRIGGER_SKIP, TRIGGER_REQUESTED, TRIGGER_RELOADING };
+
+enum class ActionStates { ACTION_IDLE, ACTION_TRACK };
+
+struct TurretStatus {
+ TargetStates target_state;
+ ActionStates action;
+ TriggerStates trigger_state;
+ float confidence;
+ float distance_m;
+};
+
+struct ServoCommand {
+ float target_x;
+ float error_x;
+ ServoDirections direction;
+};
+
+struct GimbalCommand {
+ float target_y;
+ float error_y;
+ GimbalDirections direction;
+};
+
+inline TargetStates evaluate_target(bool visible, float confidence, float confidence_threshold)
+{
+ if (!visible) {
+ return TargetStates::TARGET_NONE;
+ }
+ if (confidence < confidence_threshold) {
+ return TargetStates::TARGET_LOW_CONFIDENCE;
+ }
+
+ return TargetStates::TARGET_LOCKED;
+}
+
+inline ServoCommand make_servo_command(float target_x)
+{
+ ServoDirections direction = ServoDirections::CENTER;
+
+ if (target_x > frame_center_x) {
+ direction = ServoDirections::RIGHT;
+ }
+ else if (target_x < frame_center_x) {
+ direction = ServoDirections::LEFT;
+ }
+
+ return {.target_x = target_x, .error_x = target_x - frame_center_x, .direction = direction};
+}
+
+inline GimbalCommand make_gimbal_command(float target_y)
+{
+ GimbalDirections direction = GimbalDirections::CENTER;
+
+ if (target_y > frame_center_y) {
+ direction = GimbalDirections::DOWN;
+ }
+ else if (target_y < frame_center_y) {
+ direction = GimbalDirections::UP;
+ }
+
+ return {.target_y = target_y, .error_y = frame_center_y - target_y, .direction = direction};
+}
+
+inline TriggerStates make_trigger_decision(float distance_m, float max_distance_m, ActuatorStatuses actuatorStatus)
+{
+ if (distance_m > max_distance_m) {
+ return TriggerStates::TRIGGER_SKIP;
+ }
+
+ if (actuatorStatus == ActuatorStatuses::RELOADING) {
+ return TriggerStates::TRIGGER_RELOADING;
+ }
+
+ return TriggerStates::TRIGGER_REQUESTED;
+}
+
+inline TurretStatus build_turret_status(
+ bool visible, float confidence, float distance_m, float confidence_threshold, float max_distance_m, ActuatorStatuses actuator_status)
+{
+ TurretStatus status;
+ status.confidence = confidence;
+ status.distance_m = distance_m;
+ status.target_state = evaluate_target(visible, confidence, confidence_threshold);
+
+ if (status.target_state == TargetStates::TARGET_LOCKED) {
+ status.action = ActionStates::ACTION_TRACK;
+ status.trigger_state = make_trigger_decision(distance_m, max_distance_m, actuator_status);
+
+ return status;
+ }
+
+ status.action = ActionStates::ACTION_IDLE;
+ status.trigger_state = TriggerStates::TRIGGER_SKIP;
+
+ return status;
+}
+} // namespace turret_controller
\ No newline at end of file
diff --git a/homework_13/robot_ws/src/antidrone_turret/launch/system.launch.py b/homework_13/robot_ws/src/antidrone_turret/launch/system.launch.py
new file mode 100644
index 0000000..0c28570
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/launch/system.launch.py
@@ -0,0 +1,56 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+ profile = LaunchConfiguration("profile")
+ track = LaunchConfiguration("track")
+ log_level = LaunchConfiguration("log_level")
+
+ config = PathJoinSubstitution([
+ FindPackageShare("antidrone_turret"),
+ "config",
+ profile,
+ ])
+ track_file = PathJoinSubstitution([
+ FindPackageShare("antidrone_turret"),
+ "tracks",
+ track,
+ ])
+
+ return LaunchDescription([
+ DeclareLaunchArgument("profile", default_value="sequence.yaml"),
+ DeclareLaunchArgument("track", default_value="all"),
+ DeclareLaunchArgument("log_level", default_value="info"),
+ Node(
+ package="antidrone_turret",
+ executable="target_track_publisher_node",
+ parameters=[config, {"track_file": track_file}],
+ arguments=["--ros-args", "--log-level", log_level],
+ ),
+ Node(
+ package="antidrone_turret",
+ executable="actuator_node",
+ parameters=[config],
+ arguments=["--ros-args", "--log-level", log_level],
+ ),
+ Node(
+ package="antidrone_turret",
+ executable="turret_controller_node",
+ parameters=[config],
+ arguments=["--ros-args", "--log-level", log_level],
+ ),
+ Node(
+ package="antidrone_turret",
+ executable="gimbal_driver_node",
+ arguments=["--ros-args", "--log-level", log_level],
+ ),
+ Node(
+ package="antidrone_turret",
+ executable="yaw_servo_driver_node",
+ arguments=["--ros-args", "--log-level", log_level],
+ ),
+ ])
diff --git a/homework_13/robot_ws/src/antidrone_turret/msg/ActuatorStatus.msg b/homework_13/robot_ws/src/antidrone_turret/msg/ActuatorStatus.msg
new file mode 100644
index 0000000..55775e8
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/msg/ActuatorStatus.msg
@@ -0,0 +1,5 @@
+uint8 READY=0
+uint8 RELOADING=1
+
+uint8 state
+uint32 trigger_count
diff --git a/homework_13/robot_ws/src/antidrone_turret/msg/GimbalCommand.msg b/homework_13/robot_ws/src/antidrone_turret/msg/GimbalCommand.msg
new file mode 100644
index 0000000..0a952ac
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/msg/GimbalCommand.msg
@@ -0,0 +1,6 @@
+int8 DOWN=-1
+int8 CENTER=0
+int8 UP=1
+int8 direction
+float32 target_y
+float32 error_y
diff --git a/homework_13/robot_ws/src/antidrone_turret/msg/ServoCommand.msg b/homework_13/robot_ws/src/antidrone_turret/msg/ServoCommand.msg
new file mode 100644
index 0000000..5628c6f
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/msg/ServoCommand.msg
@@ -0,0 +1,6 @@
+int8 LEFT=-1
+int8 CENTER=0
+int8 RIGHT=1
+int8 direction
+float32 target_x
+float32 error_x
diff --git a/homework_13/robot_ws/src/antidrone_turret/msg/Target.msg b/homework_13/robot_ws/src/antidrone_turret/msg/Target.msg
new file mode 100644
index 0000000..121efda
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/msg/Target.msg
@@ -0,0 +1,5 @@
+bool visible
+float32 x
+float32 y
+float32 distance_m
+float32 confidence
diff --git a/homework_13/robot_ws/src/antidrone_turret/msg/TurretStatus.msg b/homework_13/robot_ws/src/antidrone_turret/msg/TurretStatus.msg
new file mode 100644
index 0000000..2ef8e11
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/msg/TurretStatus.msg
@@ -0,0 +1,13 @@
+uint8 TARGET_NONE=0
+uint8 TARGET_LOW_CONFIDENCE=1
+uint8 TARGET_LOCKED=2
+uint8 ACTION_IDLE=0
+uint8 ACTION_TRACK=1
+uint8 TRIGGER_SKIP=0
+uint8 TRIGGER_REQUESTED=1
+uint8 TRIGGER_RELOADING=2
+uint8 target_state
+uint8 action
+uint8 trigger_state
+float32 confidence
+float32 distance_m
\ No newline at end of file
diff --git a/homework_13/robot_ws/src/antidrone_turret/package.xml b/homework_13/robot_ws/src/antidrone_turret/package.xml
new file mode 100644
index 0000000..5c1c445
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/package.xml
@@ -0,0 +1,26 @@
+
+
+ antidrone_turret
+ 0.1.0
+ Starter ROS 2 package for Homework 13 antidrone turret system.
+
+ robot_dreams
+ Apache-2.0
+
+ ament_cmake_auto
+
+ rclcpp
+
+ rosidl_default_generators
+ rosidl_default_runtime
+ launch
+ launch_ros
+
+ ament_cmake_gtest
+
+ rosidl_interface_packages
+
+
+ ament_cmake
+
+
diff --git a/homework_13/robot_ws/src/antidrone_turret/src/actuator_node.cpp b/homework_13/robot_ws/src/antidrone_turret/src/actuator_node.cpp
new file mode 100644
index 0000000..fdcf99a
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/src/actuator_node.cpp
@@ -0,0 +1,110 @@
+#include
+#include
+#include
+#include
+
+#include
+
+#include "antidrone_turret/actuator_model.hpp"
+#include "antidrone_turret/msg/actuator_status.hpp"
+#include "antidrone_turret/srv/trigger_actuator.hpp"
+
+namespace {
+
+constexpr auto kActuatorStatusTopic = "/actuator/status";
+constexpr auto kTriggerService = "/actuator/trigger";
+
+std::uint8_t to_message_state(const antidrone_turret::ActuatorState state)
+{
+ using ActuatorStatus = antidrone_turret::msg::ActuatorStatus;
+ return state == antidrone_turret::ActuatorState::kReady ? ActuatorStatus::READY : ActuatorStatus::RELOADING;
+}
+
+} // namespace
+
+class ActuatorNode final : public rclcpp::Node {
+public:
+ using ActuatorStatus = antidrone_turret::msg::ActuatorStatus;
+ using TriggerActuator = antidrone_turret::srv::TriggerActuator;
+
+ ActuatorNode()
+ : Node("actuator_node")
+ {
+ reload_ms_ = declare_parameter("reload_ms", 1000);
+ const auto status_publish_hz = declare_parameter("status_publish_hz", 2.0);
+
+ status_publisher_ = create_publisher(kActuatorStatusTopic, 10);
+ trigger_service_ =
+ create_service(kTriggerService,
+ [this](const std::shared_ptr request,
+ std::shared_ptr response) { on_trigger(request, response); });
+
+ const auto safe_hz = std::max(0.1, status_publish_hz);
+ const auto period = std::chrono::duration_cast(std::chrono::duration(1.0 / safe_hz));
+ status_timer_ = create_wall_timer(period, [this]() { publish_status(); });
+
+ publish_status();
+
+ RCLCPP_INFO(get_logger(), "serving %s and publishing %s", kTriggerService, kActuatorStatusTopic);
+ }
+
+private:
+ void on_trigger(const std::shared_ptr& request, const std::shared_ptr& response)
+ {
+ const auto result = actuator_.trigger();
+ response->accepted = result.accepted;
+ response->trigger_count = result.trigger_count;
+
+ if (!result.accepted) {
+ RCLCPP_WARN(get_logger(), "trigger rejected while actuator is reloading");
+ return;
+ }
+
+ RCLCPP_INFO(get_logger(),
+ "trigger accepted confidence=%.2f distance_m=%.1f trigger_count=%u",
+ request->confidence,
+ request->distance_m,
+ result.trigger_count);
+
+ publish_status();
+ schedule_reload();
+ }
+
+ void schedule_reload()
+ {
+ if (reload_timer_) {
+ reload_timer_->cancel();
+ }
+
+ const auto delay = std::chrono::milliseconds{std::max(0, reload_ms_)};
+ reload_timer_ = create_wall_timer(delay, [this]() {
+ actuator_.mark_ready();
+ publish_status();
+ RCLCPP_INFO(get_logger(), "actuator ready after reload");
+ reload_timer_->cancel();
+ });
+ }
+
+ void publish_status()
+ {
+ auto status = ActuatorStatus{};
+ status.state = to_message_state(actuator_.state());
+ status.trigger_count = actuator_.trigger_count();
+ status_publisher_->publish(status);
+ }
+
+ int reload_ms_{1000};
+ antidrone_turret::ActuatorModel actuator_;
+ rclcpp::Publisher::SharedPtr status_publisher_;
+ rclcpp::Service::SharedPtr trigger_service_;
+ rclcpp::TimerBase::SharedPtr status_timer_;
+ rclcpp::TimerBase::SharedPtr reload_timer_;
+};
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
diff --git a/homework_13/robot_ws/src/antidrone_turret/src/gimbal_driver_node.cpp b/homework_13/robot_ws/src/antidrone_turret/src/gimbal_driver_node.cpp
new file mode 100644
index 0000000..b78c681
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/src/gimbal_driver_node.cpp
@@ -0,0 +1,61 @@
+#include
+#include
+
+#include
+#include "antidrone_turret/msg/gimbal_command.hpp"
+
+using GimbalCommand = antidrone_turret::msg::GimbalCommand;
+
+namespace {
+
+constexpr auto kGimbalTopic = "/gimbal/cmd";
+
+const char* direction_to_string(std::int8_t direction)
+{
+ switch (direction) {
+ case GimbalCommand::UP: {
+ return "UP";
+ }
+ case GimbalCommand::DOWN: {
+ return "DOWN";
+ }
+ case GimbalCommand::CENTER: {
+ return "CENTER";
+ }
+ }
+
+ return "UNKNOWN";
+}
+} // namespace
+
+class GimbalDriverNode final : public rclcpp::Node {
+public:
+ GimbalDriverNode()
+ : Node("gimbal_driver_node")
+ {
+ subscription_ = create_subscription(kGimbalTopic, 10, [this](const GimbalCommand& msg) { on_command(msg); });
+
+ RCLCPP_INFO(get_logger(), "gimbal_driver_node listening on %s", kGimbalTopic);
+ }
+
+private:
+ void on_command(const GimbalCommand& msg)
+ {
+ RCLCPP_INFO(get_logger(),
+ "gimbal_driver_node got: direction=%s target_y=%.1f error_y=%.1f",
+ direction_to_string(msg.direction),
+ msg.target_y,
+ msg.error_y);
+ }
+
+ rclcpp::Subscription::SharedPtr subscription_;
+};
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+
+ return 0;
+}
\ No newline at end of file
diff --git a/homework_13/robot_ws/src/antidrone_turret/src/target_track_publisher_node.cpp b/homework_13/robot_ws/src/antidrone_turret/src/target_track_publisher_node.cpp
new file mode 100644
index 0000000..61519de
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/src/target_track_publisher_node.cpp
@@ -0,0 +1,131 @@
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#include "antidrone_turret/target_sequence.hpp"
+#include "antidrone_turret/target_track_loader.hpp"
+#include "antidrone_turret/target_track_simulation.hpp"
+#include "antidrone_turret/msg/actuator_status.hpp"
+#include "antidrone_turret/msg/target.hpp"
+
+namespace {
+
+constexpr auto kTargetTopic = "/perception/target";
+constexpr auto kActuatorStatusTopic = "/actuator/status";
+constexpr auto kAllTracksSelection = "all";
+
+antidrone_turret::msg::Target to_message(const antidrone_turret::TargetSample& sample)
+{
+ auto message = antidrone_turret::msg::Target{};
+ message.visible = sample.visible;
+ message.x = sample.x;
+ message.y = sample.y;
+ message.distance_m = sample.distance_m;
+ message.confidence = sample.confidence;
+ return message;
+}
+
+std::vector load_samples_from_parameter(const std::string& track_file, const rclcpp::Logger& logger)
+{
+ if (track_file.empty()) {
+ RCLCPP_WARN(logger, "track_file is empty, using built-in fallback target track");
+ return antidrone_turret::default_target_samples();
+ }
+
+ const auto track_path = std::filesystem::path{track_file};
+ auto load_result =
+ track_path.filename().string() == kAllTracksSelection
+ ? antidrone_turret::load_target_track_csv_files(track_path.parent_path(), antidrone_turret::default_target_track_files())
+ : antidrone_turret::load_target_track_csv(track_path);
+
+ if (!load_result.error.empty()) {
+ throw std::runtime_error{load_result.error};
+ }
+
+ RCLCPP_INFO(logger, "loaded %zu target samples from %s", load_result.samples.size(), track_file.c_str());
+ return load_result.samples;
+}
+
+} // namespace
+
+class TargetTrackPublisherNode final : public rclcpp::Node {
+public:
+ TargetTrackPublisherNode()
+ : Node("target_track_publisher_node")
+ {
+ const auto publish_hz = declare_parameter("publish_hz", 1.0);
+ const auto repeat_sequence = declare_parameter("repeat_sequence", true);
+ const auto track_file = declare_parameter("track_file", "");
+
+ auto config = antidrone_turret::TargetTrackSimulationConfig{};
+ config.repeat_sequence = repeat_sequence;
+
+ simulation_ = antidrone_turret::TargetTrackSimulation{
+ antidrone_turret::TargetSequence{load_samples_from_parameter(track_file, get_logger())},
+ config,
+ };
+
+ publisher_ = create_publisher(kTargetTopic, 10);
+ actuator_status_subscription_ = create_subscription(
+ kActuatorStatusTopic, 10, [this](const antidrone_turret::msg::ActuatorStatus& status) { on_actuator_status(status); });
+
+ const auto safe_hz = std::max(0.1, publish_hz);
+ const auto period = std::chrono::duration_cast(std::chrono::duration(1.0 / safe_hz));
+
+ timer_ = create_wall_timer(period, [this]() { publish_next_target(); });
+
+ RCLCPP_INFO(get_logger(), "publishing %zu target samples on %s at %.2f Hz", simulation_.size(), kTargetTopic, safe_hz);
+ }
+
+private:
+ void publish_next_target()
+ {
+ const auto target = simulation_.next_target();
+ if (!target.has_value()) {
+ return;
+ }
+
+ publisher_->publish(to_message(target->sample));
+
+ RCLCPP_INFO(get_logger(),
+ "target visible=%s x=%.1f y=%.1f distance_m=%.1f confidence=%.2f",
+ target->sample.visible ? "true" : "false",
+ target->sample.x,
+ target->sample.y,
+ target->sample.distance_m,
+ target->sample.confidence);
+
+ if (target->reached_protected_zone_while_reloading) {
+ RCLCPP_DEBUG(get_logger(), "target reached protected zone while actuator is reloading");
+ }
+ }
+
+ void on_actuator_status(const antidrone_turret::msg::ActuatorStatus& status)
+ {
+ const auto actuator_ready = status.state == antidrone_turret::msg::ActuatorStatus::READY;
+ const auto segment_ended = simulation_.apply_actuator_status(actuator_ready, status.trigger_count);
+
+ if (segment_ended) {
+ RCLCPP_DEBUG(get_logger(), "target track segment ended after accepted trigger");
+ }
+ }
+
+ antidrone_turret::TargetTrackSimulation simulation_;
+ rclcpp::Publisher::SharedPtr publisher_;
+ rclcpp::Subscription::SharedPtr actuator_status_subscription_;
+ rclcpp::TimerBase::SharedPtr timer_;
+};
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
diff --git a/homework_13/robot_ws/src/antidrone_turret/src/turret_controller_node.cpp b/homework_13/robot_ws/src/antidrone_turret/src/turret_controller_node.cpp
new file mode 100644
index 0000000..4e4e785
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/src/turret_controller_node.cpp
@@ -0,0 +1,238 @@
+#include
+#include
+
+#include
+
+#include "antidrone_turret/turret_controller.hpp"
+
+#include "antidrone_turret/msg/target.hpp"
+#include "antidrone_turret/msg/actuator_status.hpp"
+#include "antidrone_turret/msg/turret_status.hpp"
+#include "antidrone_turret/msg/gimbal_command.hpp"
+#include "antidrone_turret/msg/servo_command.hpp"
+#include "antidrone_turret/srv/trigger_actuator.hpp"
+
+using TurretStatus = antidrone_turret::msg::TurretStatus;
+using ServoCommand = antidrone_turret::msg::ServoCommand;
+using GimbalCommand = antidrone_turret::msg::GimbalCommand;
+using Target = antidrone_turret::msg::Target;
+using ActuatorStatus = antidrone_turret::msg::ActuatorStatus;
+using TriggerActuator = antidrone_turret::srv::TriggerActuator;
+
+namespace {
+
+constexpr auto kTargetTopic = "/perception/target";
+constexpr auto kActuatorStatusTopic = "/actuator/status";
+constexpr auto kTurretStatusTopic = "/turret/status";
+constexpr auto kGimbalTopic = "/gimbal/cmd";
+constexpr auto kServoTopic = "/servo/cmd";
+constexpr auto kTriggerService = "/actuator/trigger";
+
+std::int8_t to_msg_direction(turret_controller::ServoDirections direction)
+{
+ switch (direction) {
+ case turret_controller::ServoDirections::LEFT: {
+ return ServoCommand::LEFT;
+ }
+ case turret_controller::ServoDirections::RIGHT: {
+ return ServoCommand::RIGHT;
+ }
+ case turret_controller::ServoDirections::CENTER: {
+ return ServoCommand::CENTER;
+ }
+ }
+
+ return ServoCommand::CENTER;
+}
+
+std::int8_t to_msg_direction(turret_controller::GimbalDirections direction)
+{
+ switch (direction) {
+ case turret_controller::GimbalDirections::UP: {
+ return GimbalCommand::UP;
+ }
+ case turret_controller::GimbalDirections::DOWN: {
+ return GimbalCommand::DOWN;
+ }
+ case turret_controller::GimbalDirections::CENTER: {
+ return GimbalCommand::CENTER;
+ }
+ }
+
+ return GimbalCommand::CENTER;
+}
+
+std::uint8_t to_msg_target_state(turret_controller::TargetStates state)
+{
+ switch (state) {
+ case turret_controller::TargetStates::TARGET_NONE: {
+ return TurretStatus::TARGET_NONE;
+ }
+ case turret_controller::TargetStates::TARGET_LOW_CONFIDENCE: {
+ return TurretStatus::TARGET_LOW_CONFIDENCE;
+ }
+ case turret_controller::TargetStates::TARGET_LOCKED: {
+ return TurretStatus::TARGET_LOCKED;
+ }
+ }
+
+ return TurretStatus::TARGET_NONE;
+}
+
+std::uint8_t to_msg_action(turret_controller::ActionStates action)
+{
+ switch (action) {
+ case turret_controller::ActionStates::ACTION_IDLE: {
+ return TurretStatus::ACTION_IDLE;
+ }
+ case turret_controller::ActionStates::ACTION_TRACK: {
+ return TurretStatus::ACTION_TRACK;
+ }
+ }
+ return TurretStatus::ACTION_IDLE;
+}
+
+std::uint8_t to_msg_trigger_state(turret_controller::TriggerStates state)
+{
+ switch (state) {
+ case turret_controller::TriggerStates::TRIGGER_SKIP: {
+ return TurretStatus::TRIGGER_SKIP;
+ }
+ case turret_controller::TriggerStates::TRIGGER_REQUESTED: {
+ return TurretStatus::TRIGGER_REQUESTED;
+ }
+ case turret_controller::TriggerStates::TRIGGER_RELOADING: {
+ return TurretStatus::TRIGGER_RELOADING;
+ }
+ }
+
+ return TurretStatus::TRIGGER_SKIP;
+}
+} // namespace
+
+class TurretControllerNode final : public rclcpp::Node {
+public:
+ TurretControllerNode()
+ : Node("turret_controller_node")
+ {
+ confidence_threshold_ = declare_parameter("confidence_threshold", 0.80);
+ max_distance_m_ = declare_parameter("max_distance_m", 30.0);
+
+ turret_status_pub_ = create_publisher(kTurretStatusTopic, 10);
+ gimbal_pub_ = create_publisher(kGimbalTopic, 10);
+ servo_pub_ = create_publisher(kServoTopic, 10);
+
+ trigger_client_ = create_client(kTriggerService);
+
+ actuator_status_sub_ =
+ create_subscription(kActuatorStatusTopic, 10, [this](const ActuatorStatus& msg) { on_actuator_status(msg); });
+
+ target_sub_ = create_subscription(kTargetTopic, 10, [this](const Target& msg) { on_target(msg); });
+
+ RCLCPP_INFO(
+ get_logger(), "turret_controller_node ready: confidence_threshold=%.2f max_distance_m=%.1f", confidence_threshold_, max_distance_m_);
+ }
+
+private:
+ void on_actuator_status(const ActuatorStatus& msg)
+ {
+ this->actuator_ready_ = msg.state == ActuatorStatus::READY;
+ if (this->actuator_ready_) {
+ this->trigger_pending_ = false;
+ }
+ }
+
+ void on_target(const Target& msg)
+ {
+ const auto actuator_state = (this->actuator_ready_ && !this->trigger_pending_) ? turret_controller::ActuatorStatuses::READY
+ : turret_controller::ActuatorStatuses::RELOADING;
+
+ const auto status = turret_controller::build_turret_status(msg.visible,
+ msg.confidence,
+ msg.distance_m,
+ static_cast(this->confidence_threshold_),
+ static_cast(this->max_distance_m_),
+ actuator_state);
+
+ publish_turret_status(status);
+
+ if (status.action == turret_controller::ActionStates::ACTION_TRACK) {
+ this->publish_servo(turret_controller::make_servo_command(msg.x));
+ this->publish_gimbal(turret_controller::make_gimbal_command(msg.y));
+ }
+
+ if (status.trigger_state == turret_controller::TriggerStates::TRIGGER_REQUESTED) {
+ this->request_trigger(msg.confidence, msg.distance_m);
+ }
+ }
+
+ void publish_turret_status(const turret_controller::TurretStatus& status)
+ {
+ auto msg = TurretStatus{};
+ msg.target_state = to_msg_target_state(status.target_state);
+ msg.action = to_msg_action(status.action);
+ msg.trigger_state = to_msg_trigger_state(status.trigger_state);
+ msg.confidence = status.confidence;
+ msg.distance_m = status.distance_m;
+ turret_status_pub_->publish(msg);
+ }
+
+ void publish_servo(const turret_controller::ServoCommand& cmd)
+ {
+ auto msg = ServoCommand{};
+ msg.direction = to_msg_direction(cmd.direction);
+ msg.target_x = cmd.target_x;
+ msg.error_x = cmd.error_x;
+ servo_pub_->publish(msg);
+ }
+
+ void publish_gimbal(const turret_controller::GimbalCommand& cmd)
+ {
+ auto msg = GimbalCommand{};
+ msg.direction = to_msg_direction(cmd.direction);
+ msg.target_y = cmd.target_y;
+ msg.error_y = cmd.error_y;
+ gimbal_pub_->publish(msg);
+ }
+
+ void request_trigger(float confidence, float distance_m)
+ {
+ if (!this->trigger_client_->service_is_ready()) {
+ RCLCPP_WARN(get_logger(), "trigger service not available yet, skipping request");
+ return;
+ }
+
+ this->trigger_pending_ = true;
+
+ auto request = std::make_shared();
+ request->confidence = confidence;
+ request->distance_m = distance_m;
+
+ this->trigger_client_->async_send_request(request, [this](rclcpp::Client::SharedFuture future) {
+ const auto response = future.get();
+ RCLCPP_INFO(
+ get_logger(), "trigger response accepted=%s trigger_count=%u", response->accepted ? "true" : "false", response->trigger_count);
+ });
+ }
+
+ double confidence_threshold_{0.80};
+ double max_distance_m_{30.0};
+ bool actuator_ready_{false};
+ bool trigger_pending_{false};
+
+ rclcpp::Publisher::SharedPtr turret_status_pub_;
+ rclcpp::Publisher::SharedPtr gimbal_pub_;
+ rclcpp::Publisher::SharedPtr servo_pub_;
+ rclcpp::Client::SharedPtr trigger_client_;
+ rclcpp::Subscription::SharedPtr actuator_status_sub_;
+ rclcpp::Subscription::SharedPtr target_sub_;
+};
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+
+ return 0;
+}
\ No newline at end of file
diff --git a/homework_13/robot_ws/src/antidrone_turret/src/yaw_servo_driver_node.cpp b/homework_13/robot_ws/src/antidrone_turret/src/yaw_servo_driver_node.cpp
new file mode 100644
index 0000000..2a0a181
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/src/yaw_servo_driver_node.cpp
@@ -0,0 +1,60 @@
+#include
+#include
+
+#include
+#include "antidrone_turret/msg/servo_command.hpp"
+
+using ServoCommand = antidrone_turret::msg::ServoCommand;
+
+namespace {
+constexpr auto kServoTopic = "/servo/cmd";
+
+const char* direction_to_string(std::int8_t direction)
+{
+ switch (direction) {
+ case ServoCommand::LEFT: {
+ return "LEFT";
+ }
+ case ServoCommand::RIGHT: {
+ return "RIGHT";
+ }
+ case ServoCommand::CENTER: {
+ return "CENTER";
+ }
+ }
+
+ return "UNKNOWN";
+}
+} // namespace
+
+class YawServoDriverNode final : public rclcpp::Node {
+public:
+ YawServoDriverNode()
+ : Node("yaw_servo_driver_node")
+ {
+ subscription_ = create_subscription(kServoTopic, 10, [this](const ServoCommand& msg) { on_command(msg); });
+
+ RCLCPP_INFO(get_logger(), "yaw_servo_driver_node listening on %s", kServoTopic);
+ }
+
+private:
+ void on_command(const ServoCommand& msg)
+ {
+ RCLCPP_INFO(get_logger(),
+ "yaw_servo_driver_node got: direction=%s target_x=%.1f error_x=%.1f",
+ direction_to_string(msg.direction),
+ msg.target_x,
+ msg.error_x);
+ }
+
+ rclcpp::Subscription::SharedPtr subscription_;
+};
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+
+ return 0;
+}
\ No newline at end of file
diff --git a/homework_13/robot_ws/src/antidrone_turret/srv/TriggerActuator.srv b/homework_13/robot_ws/src/antidrone_turret/srv/TriggerActuator.srv
new file mode 100644
index 0000000..d329025
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/srv/TriggerActuator.srv
@@ -0,0 +1,5 @@
+float32 confidence
+float32 distance_m
+---
+bool accepted
+uint32 trigger_count
diff --git a/homework_13/robot_ws/src/antidrone_turret/test/provided_logic_test.cpp b/homework_13/robot_ws/src/antidrone_turret/test/provided_logic_test.cpp
new file mode 100644
index 0000000..bb18393
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/test/provided_logic_test.cpp
@@ -0,0 +1,274 @@
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include "antidrone_turret/actuator_model.hpp"
+#include "antidrone_turret/target_sequence.hpp"
+#include "antidrone_turret/target_track_loader.hpp"
+#include "antidrone_turret/target_track_simulation.hpp"
+
+namespace {
+
+TEST(TargetSequenceTest, DefaultSamplesMatchHomeworkScenario)
+{
+ const auto samples = antidrone_turret::default_target_samples();
+
+ ASSERT_EQ(samples.size(), 6U);
+
+ EXPECT_TRUE(samples[0].visible);
+ EXPECT_FLOAT_EQ(samples[0].x, 320.0F);
+ EXPECT_FLOAT_EQ(samples[0].y, 240.0F);
+ EXPECT_FLOAT_EQ(samples[0].distance_m, 70.0F);
+ EXPECT_FLOAT_EQ(samples[0].confidence, 0.70F);
+
+ EXPECT_TRUE(samples[5].visible);
+ EXPECT_FLOAT_EQ(samples[5].x, 445.0F);
+ EXPECT_FLOAT_EQ(samples[5].y, 170.0F);
+ EXPECT_FLOAT_EQ(samples[5].distance_m, 19.0F);
+ EXPECT_FLOAT_EQ(samples[5].confidence, 0.95F);
+}
+
+TEST(TargetSequenceTest, StopsWhenRepeatIsDisabled)
+{
+ auto sequence = antidrone_turret::TargetSequence{
+ antidrone_turret::default_target_samples(),
+ };
+
+ const auto sample_count = antidrone_turret::default_target_samples().size();
+ for (std::size_t index = 0; index < sample_count; ++index) {
+ EXPECT_FALSE(sequence.finished(false));
+ sequence.advance();
+ }
+ EXPECT_TRUE(sequence.finished(false));
+}
+
+TEST(TargetSequenceTest, WrapsWhenRepeatIsEnabled)
+{
+ auto sequence = antidrone_turret::TargetSequence{
+ antidrone_turret::default_target_samples(),
+ };
+
+ const auto& first = sequence.current();
+ EXPECT_FLOAT_EQ(first.x, 320.0F);
+
+ const auto sample_count = antidrone_turret::default_target_samples().size();
+ for (std::size_t index = 0; index < sample_count; ++index) {
+ sequence.advance();
+ }
+
+ EXPECT_FALSE(sequence.finished(true));
+ const auto& wrapped = sequence.current();
+ EXPECT_FLOAT_EQ(wrapped.x, 320.0F);
+}
+
+TEST(TargetSequenceTest, SkipMovesToNextVisibleSegment)
+{
+ auto sequence = antidrone_turret::TargetSequence{
+ std::vector{
+ antidrone_turret::TargetSample{true, 10.0F, 0.0F, 30.0F, 0.90F},
+ antidrone_turret::TargetSample{true, 20.0F, 0.0F, 20.0F, 0.92F},
+ antidrone_turret::TargetSample{false, 0.0F, 0.0F, 0.0F, 0.0F},
+ antidrone_turret::TargetSample{true, 100.0F, 0.0F, 80.0F, 0.80F},
+ antidrone_turret::TargetSample{true, 110.0F, 0.0F, 60.0F, 0.84F},
+ },
+ };
+
+ sequence.advance();
+ sequence.skip_to_next_track();
+
+ const auto& sample = sequence.current();
+ EXPECT_TRUE(sample.visible);
+ EXPECT_FLOAT_EQ(sample.x, 100.0F);
+}
+
+TEST(TargetTrackSimulationTest, AcceptedTriggerSkipsCurrentDrone)
+{
+ auto config = antidrone_turret::TargetTrackSimulationConfig{};
+ config.repeat_sequence = false;
+
+ auto simulation = antidrone_turret::TargetTrackSimulation{
+ antidrone_turret::TargetSequence{
+ std::vector{
+ antidrone_turret::TargetSample{true, 100.0F, 240.0F, 45.0F, 0.90F},
+ antidrone_turret::TargetSample{true, 120.0F, 238.0F, 25.0F, 0.92F},
+ antidrone_turret::TargetSample{true, 140.0F, 236.0F, 18.0F, 0.94F},
+ antidrone_turret::TargetSample{false, 0.0F, 0.0F, 0.0F, 0.0F},
+ antidrone_turret::TargetSample{true, 300.0F, 240.0F, 70.0F, 0.85F},
+ },
+ },
+ config,
+ };
+
+ const auto far_target = simulation.next_target();
+ ASSERT_TRUE(far_target.has_value());
+ EXPECT_FLOAT_EQ(far_target->sample.x, 100.0F);
+
+ const auto close_target = simulation.next_target();
+ ASSERT_TRUE(close_target.has_value());
+ EXPECT_FLOAT_EQ(close_target->sample.x, 120.0F);
+
+ EXPECT_TRUE(simulation.apply_actuator_status(false, 1U));
+
+ const auto next_drone = simulation.next_target();
+ ASSERT_TRUE(next_drone.has_value());
+ EXPECT_TRUE(next_drone->sample.visible);
+ EXPECT_FLOAT_EQ(next_drone->sample.x, 300.0F);
+}
+
+TEST(TargetTrackSimulationTest, ReachedProtectedZoneWhileReloadingSkipsCurrentDrone)
+{
+ auto config = antidrone_turret::TargetTrackSimulationConfig{};
+ config.repeat_sequence = false;
+ config.impact_distance_m = 8.0F;
+
+ auto simulation = antidrone_turret::TargetTrackSimulation{
+ antidrone_turret::TargetSequence{
+ std::vector{
+ antidrone_turret::TargetSample{true, 100.0F, 240.0F, 35.0F, 0.90F},
+ antidrone_turret::TargetSample{true, 120.0F, 238.0F, 7.0F, 0.92F},
+ antidrone_turret::TargetSample{true, 140.0F, 236.0F, 5.0F, 0.94F},
+ antidrone_turret::TargetSample{false, 0.0F, 0.0F, 0.0F, 0.0F},
+ antidrone_turret::TargetSample{true, 300.0F, 240.0F, 70.0F, 0.85F},
+ },
+ },
+ config,
+ };
+
+ EXPECT_FALSE(simulation.apply_actuator_status(false, 0U));
+
+ const auto first_target = simulation.next_target();
+ ASSERT_TRUE(first_target.has_value());
+ EXPECT_FALSE(first_target->reached_protected_zone_while_reloading);
+
+ const auto protected_zone_target = simulation.next_target();
+ ASSERT_TRUE(protected_zone_target.has_value());
+ EXPECT_TRUE(protected_zone_target->reached_protected_zone_while_reloading);
+ EXPECT_FLOAT_EQ(protected_zone_target->sample.distance_m, 7.0F);
+
+ const auto next_drone = simulation.next_target();
+ ASSERT_TRUE(next_drone.has_value());
+ EXPECT_TRUE(next_drone->sample.visible);
+ EXPECT_FLOAT_EQ(next_drone->sample.x, 300.0F);
+}
+
+TEST(TargetTrackLoaderTest, LoadsCsvRowsAndSkipsCommentsAndHeader)
+{
+ const auto path = std::filesystem::temp_directory_path() / "antidrone_turret_valid_track.csv";
+ {
+ std::ofstream output{path};
+ output << "# visible,x,y,distance_m,confidence\n";
+ output << "visible,x,y,distance_m,confidence\n";
+ output << "true,320,240,40,0.81\n";
+ output << "false,310,250,35,0.20\n";
+ }
+
+ const auto result = antidrone_turret::load_target_track_csv(path);
+ std::filesystem::remove(path);
+
+ ASSERT_TRUE(result.error.empty()) << result.error;
+ ASSERT_EQ(result.samples.size(), 2U);
+
+ EXPECT_TRUE(result.samples[0].visible);
+ EXPECT_FLOAT_EQ(result.samples[0].x, 320.0F);
+ EXPECT_FLOAT_EQ(result.samples[0].y, 240.0F);
+ EXPECT_FLOAT_EQ(result.samples[0].distance_m, 40.0F);
+ EXPECT_FLOAT_EQ(result.samples[0].confidence, 0.81F);
+
+ EXPECT_FALSE(result.samples[1].visible);
+}
+
+TEST(TargetTrackLoaderTest, ReportsInvalidRows)
+{
+ const auto path = std::filesystem::temp_directory_path() / "antidrone_turret_invalid_track.csv";
+ {
+ std::ofstream output{path};
+ output << "visible,x,y,distance_m,confidence\n";
+ output << "true,320,240,40\n";
+ }
+
+ const auto result = antidrone_turret::load_target_track_csv(path);
+ std::filesystem::remove(path);
+
+ EXPECT_TRUE(result.samples.empty());
+ EXPECT_FALSE(result.error.empty());
+}
+
+TEST(TargetTrackLoaderTest, LoadsAllProvidedTracks)
+{
+ const auto source_dir = std::filesystem::path{ANTIDRONE_TURRET_SOURCE_DIR};
+ const auto tracks_dir = source_dir / "tracks";
+
+ for (const auto& track_file : antidrone_turret::default_target_track_files()) {
+ const auto result = antidrone_turret::load_target_track_csv(tracks_dir / track_file);
+ EXPECT_TRUE(result.error.empty()) << track_file << ": " << result.error;
+ EXPECT_GT(result.samples.size(), 5U) << track_file;
+ }
+}
+
+TEST(TargetTrackLoaderTest, LoadsDefaultTracksAsOneSequenceWithGaps)
+{
+ const auto source_dir = std::filesystem::path{ANTIDRONE_TURRET_SOURCE_DIR};
+ const auto tracks_dir = source_dir / "tracks";
+
+ const auto result = antidrone_turret::load_target_track_csv_files(tracks_dir, antidrone_turret::default_target_track_files());
+
+ ASSERT_TRUE(result.error.empty()) << result.error;
+ EXPECT_GT(result.samples.size(), 50U);
+
+ const auto gap_count = std::count_if(
+ result.samples.begin(), result.samples.end(), [](const antidrone_turret::TargetSample& sample) { return !sample.visible; });
+ EXPECT_GE(gap_count, 4);
+}
+
+TEST(ActuatorModelTest, StartsReady)
+{
+ const auto actuator = antidrone_turret::ActuatorModel{};
+
+ EXPECT_TRUE(actuator.ready());
+ EXPECT_EQ(actuator.state(), antidrone_turret::ActuatorState::kReady);
+ EXPECT_EQ(actuator.trigger_count(), 0U);
+}
+
+TEST(ActuatorModelTest, AcceptedTriggerMovesToReloading)
+{
+ auto actuator = antidrone_turret::ActuatorModel{};
+
+ const auto result = actuator.trigger();
+
+ EXPECT_TRUE(result.accepted);
+ EXPECT_EQ(result.trigger_count, 1U);
+ EXPECT_FALSE(actuator.ready());
+ EXPECT_EQ(actuator.state(), antidrone_turret::ActuatorState::kReloading);
+ EXPECT_EQ(actuator.trigger_count(), 1U);
+}
+
+TEST(ActuatorModelTest, RejectsTriggerWhileReloading)
+{
+ auto actuator = antidrone_turret::ActuatorModel{};
+
+ ASSERT_TRUE(actuator.trigger().accepted);
+ const auto rejected = actuator.trigger();
+
+ EXPECT_FALSE(rejected.accepted);
+ EXPECT_EQ(rejected.trigger_count, 1U);
+ EXPECT_EQ(actuator.state(), antidrone_turret::ActuatorState::kReloading);
+}
+
+TEST(ActuatorModelTest, MarkReadyAllowsNextTrigger)
+{
+ auto actuator = antidrone_turret::ActuatorModel{};
+
+ ASSERT_TRUE(actuator.trigger().accepted);
+ actuator.mark_ready();
+ const auto second = actuator.trigger();
+
+ EXPECT_TRUE(second.accepted);
+ EXPECT_EQ(second.trigger_count, 2U);
+ EXPECT_EQ(actuator.trigger_count(), 2U);
+}
+
+} // namespace
diff --git a/homework_13/robot_ws/src/antidrone_turret/test/turret_controller_test.cpp b/homework_13/robot_ws/src/antidrone_turret/test/turret_controller_test.cpp
new file mode 100644
index 0000000..b89046a
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/test/turret_controller_test.cpp
@@ -0,0 +1,136 @@
+#include
+
+#include "antidrone_turret/turret_controller.hpp"
+
+namespace {
+
+constexpr float kConfidenceThreshold = 0.80f;
+constexpr float kMaxDistanceM = 30.0f;
+
+TEST(EvaluateTargetTest, NotVisibleIsTargetNone)
+{
+ EXPECT_EQ(turret_controller::evaluate_target(false, 0.99f, kConfidenceThreshold), turret_controller::TargetStates::TARGET_NONE);
+}
+
+TEST(EvaluateTargetTest, BelowThresholdIsLowConfidence)
+{
+ EXPECT_EQ(turret_controller::evaluate_target(true, 0.79f, kConfidenceThreshold), turret_controller::TargetStates::TARGET_LOW_CONFIDENCE);
+}
+
+TEST(EvaluateTargetTest, AtOrAboveThresholdIsLocked)
+{
+ EXPECT_EQ(turret_controller::evaluate_target(true, 0.80f, kConfidenceThreshold), turret_controller::TargetStates::TARGET_LOCKED);
+ EXPECT_EQ(turret_controller::evaluate_target(true, 0.95f, kConfidenceThreshold), turret_controller::TargetStates::TARGET_LOCKED);
+}
+
+TEST(ServoCommandTest, RightWhenXGreaterThanCenter)
+{
+ const auto cmd = turret_controller::make_servo_command(420.0f);
+ EXPECT_EQ(cmd.direction, turret_controller::ServoDirections::RIGHT);
+ EXPECT_FLOAT_EQ(cmd.target_x, 420.0f);
+ EXPECT_FLOAT_EQ(cmd.error_x, 100.0f);
+ EXPECT_GT(cmd.error_x, 0.0f);
+}
+
+TEST(ServoCommandTest, LeftWhenXLessThanCenter)
+{
+ const auto cmd = turret_controller::make_servo_command(200.0f);
+ EXPECT_EQ(cmd.direction, turret_controller::ServoDirections::LEFT);
+ EXPECT_LT(cmd.error_x, 0.0f);
+}
+
+TEST(ServoCommandTest, CenterWhenXEqualsCenter)
+{
+ const auto cmd = turret_controller::make_servo_command(320.0f);
+ EXPECT_EQ(cmd.direction, turret_controller::ServoDirections::CENTER);
+ EXPECT_FLOAT_EQ(cmd.error_x, 0.0f);
+}
+
+TEST(GimbalCommandTest, UpWhenYLessThanCenter)
+{
+ const auto cmd = turret_controller::make_gimbal_command(180.0f);
+ EXPECT_EQ(cmd.direction, turret_controller::GimbalDirections::UP);
+ EXPECT_FLOAT_EQ(cmd.target_y, 180.0f);
+ EXPECT_FLOAT_EQ(cmd.error_y, 60.0f);
+ EXPECT_GT(cmd.error_y, 0.0f);
+}
+
+TEST(GimbalCommandTest, DownWhenYGreaterThanCenter)
+{
+ const auto cmd = turret_controller::make_gimbal_command(300.0f);
+ EXPECT_EQ(cmd.direction, turret_controller::GimbalDirections::DOWN);
+ EXPECT_LT(cmd.error_y, 0.0f);
+}
+
+TEST(GimbalCommandTest, CenterWhenYEqualsCenter)
+{
+ const auto cmd = turret_controller::make_gimbal_command(240.0f);
+ EXPECT_EQ(cmd.direction, turret_controller::GimbalDirections::CENTER);
+ EXPECT_FLOAT_EQ(cmd.error_y, 0.0f);
+}
+
+TEST(TriggerDecisionTest, CloseAndReadyIsRequested)
+{
+ EXPECT_EQ(turret_controller::make_trigger_decision(25.0f, kMaxDistanceM, turret_controller::ActuatorStatuses::READY),
+ turret_controller::TriggerStates::TRIGGER_REQUESTED);
+}
+
+TEST(TriggerDecisionTest, CloseAndReloadingIsReloading)
+{
+ EXPECT_EQ(turret_controller::make_trigger_decision(25.0f, kMaxDistanceM, turret_controller::ActuatorStatuses::RELOADING),
+ turret_controller::TriggerStates::TRIGGER_RELOADING);
+}
+
+TEST(TriggerDecisionTest, FarIsSkipRegardlessOfActuator)
+{
+ EXPECT_EQ(turret_controller::make_trigger_decision(50.0f, kMaxDistanceM, turret_controller::ActuatorStatuses::READY),
+ turret_controller::TriggerStates::TRIGGER_SKIP);
+
+ EXPECT_EQ(turret_controller::make_trigger_decision(50.0f, kMaxDistanceM, turret_controller::ActuatorStatuses::RELOADING),
+ turret_controller::TriggerStates::TRIGGER_SKIP);
+}
+
+TEST(BuildTurretStatusTest, LowConfidenceIsIdleAndSkip)
+{
+ const auto status = turret_controller::build_turret_status(
+ true, 0.50f, 20.0f, kConfidenceThreshold, kMaxDistanceM, turret_controller::ActuatorStatuses::READY);
+ EXPECT_EQ(status.target_state, turret_controller::TargetStates::TARGET_LOW_CONFIDENCE);
+ EXPECT_EQ(status.action, turret_controller::ActionStates::ACTION_IDLE);
+ EXPECT_EQ(status.trigger_state, turret_controller::TriggerStates::TRIGGER_SKIP);
+}
+
+TEST(BuildTurretStatusTest, FarValidTargetIsLockedTrackSkip)
+{
+ const auto status = turret_controller::build_turret_status(
+ true, 0.90f, 45.0f, kConfidenceThreshold, kMaxDistanceM, turret_controller::ActuatorStatuses::READY);
+ EXPECT_EQ(status.target_state, turret_controller::TargetStates::TARGET_LOCKED);
+ EXPECT_EQ(status.action, turret_controller::ActionStates::ACTION_TRACK);
+ EXPECT_EQ(status.trigger_state, turret_controller::TriggerStates::TRIGGER_SKIP);
+}
+
+TEST(BuildTurretStatusTest, CloseValidReadyIsRequested)
+{
+ const auto status = turret_controller::build_turret_status(
+ true, 0.90f, 20.0f, kConfidenceThreshold, kMaxDistanceM, turret_controller::ActuatorStatuses::READY);
+ EXPECT_EQ(status.target_state, turret_controller::TargetStates::TARGET_LOCKED);
+ EXPECT_EQ(status.action, turret_controller::ActionStates::ACTION_TRACK);
+ EXPECT_EQ(status.trigger_state, turret_controller::TriggerStates::TRIGGER_REQUESTED);
+}
+
+TEST(BuildTurretStatusTest, CloseValidReloadingIsReloading)
+{
+ const auto status = turret_controller::build_turret_status(
+ true, 0.90f, 20.0f, kConfidenceThreshold, kMaxDistanceM, turret_controller::ActuatorStatuses::RELOADING);
+ EXPECT_EQ(status.trigger_state, turret_controller::TriggerStates::TRIGGER_RELOADING);
+}
+
+TEST(BuildTurretStatusTest, NotVisibleIsNoneIdleSkip)
+{
+ const auto status = turret_controller::build_turret_status(
+ false, 0.0f, 0.0f, kConfidenceThreshold, kMaxDistanceM, turret_controller::ActuatorStatuses::READY);
+ EXPECT_EQ(status.target_state, turret_controller::TargetStates::TARGET_NONE);
+ EXPECT_EQ(status.action, turret_controller::ActionStates::ACTION_IDLE);
+ EXPECT_EQ(status.trigger_state, turret_controller::TriggerStates::TRIGGER_SKIP);
+}
+
+} // namespace
\ No newline at end of file
diff --git a/homework_13/robot_ws/src/antidrone_turret/tracks/approach_trigger.csv b/homework_13/robot_ws/src/antidrone_turret/tracks/approach_trigger.csv
new file mode 100644
index 0000000..126d045
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/tracks/approach_trigger.csv
@@ -0,0 +1,24 @@
+# visible,x,y,distance_m,confidence
+# FPV approaches the turret. A controller should trigger when distance <= max_distance_m.
+visible,x,y,distance_m,confidence
+true,319,241,80,0.62
+true,324,239,72,0.68
+true,331,236,64,0.74
+true,338,232,56,0.81
+true,350,226,48,0.86
+true,365,218,40,0.90
+true,383,208,34,0.92
+true,401,197,29,0.94
+true,420,186,24,0.95
+true,439,176,19,0.96
+true,458,166,15,0.96
+true,476,158,12,0.95
+false,0,0,0,0
+true,210,248,90,0.76
+true,235,243,76,0.80
+true,270,236,62,0.86
+true,310,228,48,0.90
+true,350,218,36,0.93
+true,390,205,28,0.95
+true,430,190,22,0.96
+true,470,176,16,0.95
diff --git a/homework_13/robot_ws/src/antidrone_turret/tracks/far_flyby_no_trigger.csv b/homework_13/robot_ws/src/antidrone_turret/tracks/far_flyby_no_trigger.csv
new file mode 100644
index 0000000..62061cf
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/tracks/far_flyby_no_trigger.csv
@@ -0,0 +1,15 @@
+# visible,x,y,distance_m,confidence
+# FPV is visible and confident, but never close enough for trigger.
+visible,x,y,distance_m,confidence
+true,180,250,95,0.82
+true,205,246,88,0.85
+true,235,242,79,0.88
+true,270,239,68,0.90
+true,310,236,57,0.91
+true,350,234,49,0.92
+true,390,236,43,0.91
+true,430,240,46,0.90
+true,470,245,55,0.88
+true,505,250,67,0.86
+true,535,255,81,0.84
+true,560,260,98,0.82
diff --git a/homework_13/robot_ws/src/antidrone_turret/tracks/low_confidence_no_trigger.csv b/homework_13/robot_ws/src/antidrone_turret/tracks/low_confidence_no_trigger.csv
new file mode 100644
index 0000000..6197e08
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/tracks/low_confidence_no_trigger.csv
@@ -0,0 +1,14 @@
+# visible,x,y,distance_m,confidence
+# FPV gets close, but every confidence value stays below the 0.80 default threshold.
+visible,x,y,distance_m,confidence
+true,315,242,65,0.42
+true,322,239,55,0.48
+true,335,234,45,0.53
+true,352,226,36,0.58
+true,371,216,29,0.61
+true,392,205,23,0.64
+true,414,193,18,0.66
+true,438,181,14,0.68
+true,461,170,12,0.67
+true,482,162,11,0.65
+false,0,0,0,0
diff --git a/homework_13/robot_ws/src/antidrone_turret/tracks/reload_pressure.csv b/homework_13/robot_ws/src/antidrone_turret/tracks/reload_pressure.csv
new file mode 100644
index 0000000..1bfd99e
--- /dev/null
+++ b/homework_13/robot_ws/src/antidrone_turret/tracks/reload_pressure.csv
@@ -0,0 +1,23 @@
+# visible,x,y,distance_m,confidence
+# First close FPV can consume a shot. The next FPV reaches 7 m while actuator is reloading.
+visible,x,y,distance_m,confidence
+true,330,238,38,0.84
+true,342,232,32,0.88
+true,356,225,28,0.91
+true,370,218,24,0.93
+true,384,211,21,0.94
+true,398,204,19,0.95
+false,0,0,0,0
+true,300,238,26,0.92
+true,322,230,20,0.94
+true,348,220,15,0.95
+true,376,208,11,0.96
+true,404,196,7,0.96
+true,432,184,5,0.95
+false,0,0,0,0
+true,190,250,92,0.82
+true,230,244,73,0.86
+true,275,236,55,0.90
+true,325,226,38,0.93
+true,380,212,26,0.95
+true,435,198,18,0.96