# Модуль В. Система технического зрения с использованием инструментов ИИ

## Описание задачи

На специальной полке на соревновательном поле расположены N > 3 коробок с рисунками инструментов. РМК-1 с манипулятором смотрит на полку сверху (камера на манипуляторе направлена вертикально вниз). Эксперт с помощью генератора случайных чисел выбирает целевую деталь. Алгоритм должен:

1. Распознать все коробки в кадре камеры
2. Определить координаты целевой коробки относительно базы манипулятора (Base_link)
3. Направить манипулятор к коробке, схватить её и поднять в стартовое положение
4. Опустить коробку на свободное место на полке

## Классы объектов

| ID | Название | Описание |
|----|----------|----------|
| 1  | Молоток (hammer) | Коробка с изображением молотка |
| 2  | Гаечный ключ (wrench) | Коробка с изображением гаечного ключа |
| 3  | Пассатижи (pliers) | Коробка с изображением пассатижей |

## Архитектура решения

- **Модель**: YOLOv8n-cls (nano classification) — легковесная модель для классификации изображений
- **Фреймворк**: Ultralytics / PyTorch
- **Размер входа**: 224x224 px
- **Размер модели**: ~5.3 MB (PyTorch), ~5.5 MB (ONNX)
- **Скорость инференса**: ~1.6 мс на CPU

### Пайплайн обработки кадра

```
Кадр с камеры манипулятора
        |
        v
Сегментация коробок (OpenCV: пороговая фильтрация + контурный анализ)
        |
        v
Классификация каждой коробки (YOLOv8n-cls)
        |
        v
Поиск целевой коробки (по ID класса + порог уверенности > 0.7)
        |
        v
Пересчёт пиксельных координат в координаты Base_link
        |
        v
Отправка координат манипулятору (MoveIt2 API)
```

## Структура файлов

```
module_v/
├── images/                      # Исходные изображения инструментов
│   ├── Hammer.jpg
│   ├── Wrench.jpg
│   └── Pliers.jpg
├── dataset/                     # Сгенерированный датасет
│   ├── train/                   # 900 изображений (300 на класс)
│   │   ├── hammer/
│   │   ├── wrench/
│   │   └── pliers/
│   └── val/                     # 180 изображений (60 на класс)
│       ├── hammer/
│       ├── wrench/
│       └── pliers/
├── runs/module_v_cls/weights/   # Результаты обучения
│   ├── best.pt
│   └── best.onnx
├── best.pt                      # Обученная модель (копия)
├── generate_dataset.py          # Скрипт генерации датасета
├── train_model.py               # Скрипт обучения модели
├── detect_box.py                # Основной скрипт инференса + ROS2
├── test_detect.py               # Тестовый скрипт с визуализацией
├── README.md                    # Общая инструкция по симулятору
└── README_MODULE_V.md           # Данный файл
```

## Установка зависимостей

```bash
pip install ultralytics opencv-python-headless Pillow albumentations
```

## Генерация датасета

Датасет генерируется из 3 исходных изображений с помощью аугментаций:
- Композитинг инструментов на различные фоны (имитация полки)
- Повороты, отражения, перспективные искажения
- Изменение яркости, контраста, насыщенности
- Размытие, шум, JPEG-компрессия

```bash
python3 generate_dataset.py
```

Результат: 900 train + 180 val изображений (по 300/60 на класс).

## Обучение модели

```bash
python3 train_model.py
```

Параметры обучения:
- Базовая модель: `yolov8n-cls.pt` (предобученная на ImageNet)
- Эпохи: до 80 (с early stopping, patience=15)
- Размер изображения: 224x224
- Batch size: 32

Результат: **100% top-1 accuracy** на валидации.

Модель автоматически экспортируется в ONNX формат.

## Запуск

### Тест на отдельном изображении

```bash
python3 detect_box.py --target 1 --image path/to/image.jpg
```

Где `--target` — ID целевой детали (1=молоток, 2=ключ, 3=пассатижи).

### Тест на всех исходных изображениях

```bash
python3 detect_box.py --target 2
```

### Тест с визуализацией на кадре с камеры

```bash
python3 test_detect.py
```

Результат сохраняется в `test_result.png` с аннотированными рамками и подписями классов.

### Запуск в режиме ROS2

Предварительно запустите симуляцию модуля В:

```bash
# Терминал 1: запуск симулятора
ros2 launch ar_webots_fms_ros2 module3.launch.py

# Терминал 2: запуск детектора (указать целевую деталь)
python3 detect_box.py --target 1 --ros
```

Скрипт подписывается на топик `/RMC1/arm95/camera_gripper/image_color`, распознаёт коробки и выводит координаты целевой коробки относительно Base_link.

## Результаты тестирования

### На исходных изображениях

| Изображение | Предсказание | Уверенность |
|-------------|-------------|-------------|
| Hammer.jpg | hammer (молоток) | 99.98% |
| Wrench.jpg | wrench (гаечный ключ) | 99.99% |
| Pliers.jpg | pliers (пассатижи) | 100.00% |

### На тестовом кадре с камеры (test.png)

5 коробок на полке (молоток, ключ, 2 пассатижей + объект на краю):

| Коробка | Класс | Уверенность |
|---------|-------|-------------|
| Box 1 | wrench | 99.51% |
| Box 2 | pliers | 100.00% |
| Box 3 | wrench | 65.81% (край кадра, фильтруется порогом) |
| Box 4 | hammer | 79.42% |
| Box 5 | pliers | 99.93% |

Рекомендуемый порог уверенности для фильтрации: **> 0.7**

## Интеграция с манипулятором

Пример полного цикла (распознавание + захват):

```python
import rclpy
from detect_box import load_model, find_target_box, pixel_to_base_link
from geometry_msgs.msg import PoseStamped
from tf_transformations import quaternion_from_euler
from moveit.planning import MoveItPy

# Инициализация
rclpy.init()
model = load_model("best.pt")
arm95 = MoveItPy(node_name="moveit_py", name_space="/RMC1/arm95")
arm95_arm = arm95.get_planning_component("arm95_group")
gripper = arm95.get_planning_component("gripper")

# 1. Получить кадр с камеры (через ROS2 subscriber)
# frame = ... (cv2 image from /RMC1/arm95/camera_gripper/image_color)

# 2. Найти целевую коробку
target_id = 1  # молоток
result = find_target_box(model, frame, target_id)
if result:
    cx, cy, conf, bbox = result
    h, w = frame.shape[:2]
    wx, wy, wz = pixel_to_base_link(cx, cy, w, h)

    # 3. Переместить манипулятор к коробке
    q = quaternion_from_euler(3.14, 0.0, 1.57)
    pose_goal = PoseStamped()
    pose_goal.header.frame_id = "Base_link"
    pose_goal.pose.orientation.x = q[0]
    pose_goal.pose.orientation.y = q[1]
    pose_goal.pose.orientation.z = q[2]
    pose_goal.pose.orientation.w = q[3]
    pose_goal.pose.position.x = wx
    pose_goal.pose.position.y = wy
    pose_goal.pose.position.z = 0.3  # высота над коробкой

    arm95_arm.set_start_state_to_current_state()
    arm95_arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link="gripper_base")
    plan_result = arm95_arm.plan()
    if plan_result:
        arm95.execute(plan_result.trajectory, controllers=[])

    # 4. Захватить коробку
    gripper.set_goal_state(configuration_name="closed")
    plan_result = gripper.plan()
    if plan_result:
        arm95.execute(plan_result.trajectory, controllers=[])
```

**Важно**: координаты `pixel_to_base_link()` требуют калибровки параметров камеры (FOV, высота) под конкретную установку.