first commit

README.md

@@ -0,0 +1,230 @@
+# Модуль В. Система технического зрения с использованием инструментов ИИ
+
+## Описание задачи
+
+На специальной полке на соревновательном поле расположены 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, высота) под конкретную установку.

README_bots.md

@@ -0,0 +1,540 @@
+# Требования
+## Требования к устройству
+* Процессор -  4 и более ядер
+* Оперативная память - 8 ГБ и более
+* Графика - Минимально: любая интегрированная графика (Intel HD * Graphics, AMD Radeon Vega), поддерживающая OpenGL 3.3. Чем мощнее, тем лучше
+* Место на диске - 50 ГБ или более
+## Требования к системе
+* Операционная система - Ubuntu 24.04 LTS Desktop (64 bit)  
+
+Рекомендуется установить на устройство отдельной системой в режиме dualboot или использовать виртуальную машину для установки. В случае виртуальной машины, важно выделить ей минимальные требуемые ресурсы на вашем устройстве из списка выше. Производительность в виртуальной машине может быть ниже, чем при нативной установке.
+
+# Установка
+### 1. Установка ROS2 Jazzy:
+Данная инструкция дублирует команды из [официальной документации ROS2](https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html) по установке.  
+
+Подготавливаем систему к установке
+``` bash
+sudo apt update && sudo apt install locales
+sudo locale-gen en_US en_US.UTF-8
+sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
+export LANG=en_US.UTF-8
+```
+
+Устанавливаем зависимости
+``` bash
+sudo apt install software-properties-common
+sudo add-apt-repository universe
+```
+``` bash
+sudo apt update && sudo apt install curl -y
+export ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}')
+curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo ${UBUNTU_CODENAME:-${VERSION_CODENAME}})_all.deb"
+sudo dpkg -i /tmp/ros2-apt-source.deb
+```
+
+Устанавливаем полную версию ROS2 Jazzy
+``` bash
+sudo apt install ros-jazzy-desctop-full
+```
+
+(Опционально) Можно добавить сурс (source) команду для ROS2 в ```~/.bashrc```, чтобы не нужно каждый раз в новом терминале вводить ```source /opt/ros/jazzy/setup.bash```
+``` bash
+echo 'source /opt/ros/jazzy/setup.bash' >> ~/.bashrc
+```
+
+Устанавливаем инструмент сборки colcon
+``` bash
+sudo apt install python3-colcon-common-extensions
+```
+
+### 2. Установка симулятора Webots:
+
+Данная инструкция дублирует команды из [официальной документации Webots](https://cyberbotics.com/doc/guide/installation-procedure#installing-the-debian-package-with-the-advanced-packaging-tool-apt) по установке.  
+
+Устанавливаем файл подписи Cyberbotics
+```bash
+sudo mkdir -p /etc/apt/keyrings
+
+cd /etc/apt/keyrings
+
+sudo wget -q https://cyberbotics.com/Cyberbotics.asc
+```
+
+Конфигурируем менеджер пакетов
+```bash
+echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/Cyberbotics.asc] https://cyberbotics.com/debian binary-amd64/" | sudo tee /etc/apt/sources.list.d/Cyberbotics.list
+
+sudo apt update
+```
+
+Устанаваливаем Webots
+```bash
+sudo apt install webots
+```
+
+Настраиваем переменную окружения
+```bash
+echo "export WEBOTS_HOME=/snap/webots/current/usr/share/webots" >> ~/.bashrc
+```
+
+### 3. Установка ROS2 пакетов симулятора
+
+Создаем папку рабочего пространства с любым именем в домашней директории, в данном примере используем папку `ros2_ws` 
+```bash
+mkdir ~/ros2_ws/src -p
+```
+
+Скачиваем требуемые пакеты
+```bash
+cd ~/ros2_ws/src
+
+git clone https://gitlab.mobird.dev/fms_group/ar_webots_fms_ros2.git
+
+git clone https://gitlab.mobird.dev/fms_group/ar_arm95_moveit_config.git
+
+git clone https://gitlab.mobird.dev/fms_group/ar_aruco_detect_ros2.git
+
+git clone https://gitlab.mobird.dev/fms_group/ar_dual_lidar_merge_ros2.git
+
+git clone https://gitlab.mobird.dev/fms_group/ar_nav_ros2.git
+```
+
+Устанавливаем зависимости через инструмент rosdep
+```bash
+cd ~/ros2_ws
+
+sudo rosdep init
+rosdep update
+rosdep install --from-paths src -y --ignore-src
+```
+
+Собираем пакеты
+
+```bash
+cd ~/ros2_ws
+colcon build --symlink-install
+```
+Перед запуском симулятора необходимо выполнить сурс (source) собранных файлов из папки `ros2_ws`, **это необходимо делать в каждом новом открытом терминале**
+```bash
+source /ros2_ws/install/setup.bash
+```
+
+(Опционально) данный сурс можно также добавить в ```~/.bashrc```
+
+### 4. Установка пакетов навигации и slam для RMC1
+Создайте отдельное рабочее пространство для пакетов навигации, в данном примере это будет `nav2_ws`. Клонируем в него файлы Nav2.
+```bash
+mkdir ~/nav2_ws/src -p
+git clone https://github.com/ros-planning/navigation2.git -b 1.3.10
+```
+Устанавливаем зависимости через инструмент rosdep
+```bash
+cd ~/nav2_ws
+
+sudo rosdep init
+rosdep update
+rosdep install -y -r -q --from-paths src --ignore-src --rosdistro jazzy
+```
+После этого собираем пакеты. Сборка может занять некоторое время, ~30 минут в зависимости от мощности ПК
+```bash
+cd ~/nav2_ws
+colcon build --symlink-install
+```
+Далее создаем новую папку, в данном примере это будет `slam_ws` и загружаем в нее файлы SLAM
+```bash
+mkdir ~/slam_ws/src -p
+git clone https://github.com/SteveMacenski/slam_toolbox.git -b 2.8.3
+```
+Устанавливаем зависимости через инструмент rosdep
+```bash
+cd ~/slam_ws
+
+sudo rosdep init
+rosdep update
+rosdep install -y -r -q --from-paths src --ignore-src --rosdistro jazzy
+```
+После этого собираем пакеты. Сборка может занять некоторое время, ~5 минут в зависимости от мощности ПК
+```bash
+cd ~/slam_ws
+colcon build --symlink-install
+```
+Перед запуском симулятора необходимо выполнить сурс (source) собранных файлов из папок `nav2_ws` и `nav2_ws`, **это необходимо делать в каждом новом открытом терминале**
+```bash
+source /nav2_ws/install/setup.bash
+source /slam_ws/install/setup.bash
+```
+(Опционально) данный сурс можно также добавить в ```~/.bashrc```
+
+# Запуск модулей
+## Модуль Б. Разработка и настройка навигационных алгоритмов для РМК
+```
+Задание: 
+1) РМК-2 стоит на точке старта над стартовым aruco-маркером.
+Необходимо по команде старта эксперта отправить роботу целевую точку, в которую должен
+приехать РМК-2.
+2) После прибытия к целевой точке, РМК-2 должен вернуться в точку
+старта, однако в этот раз перед стартом робота эксперт выставит на маршрут
+робота препятствие, которое необходимо обнаружить и объехать.
+```
+
+Чтобы запустить симуляцию данного задания, запустите launch файл:  
+**Не забудьте выполнить сурсы (source), если они не добавлены в ваш ~/.bashrc**
+```bash
+ros2 launch ar_webots_fms_ros2 module2.launch.py
+```
+
+При первом запуске мира могут долго скачиваться некоторые ассеты для карты, поэтому, если робот не появился, то перезапустите симулятор.
+
+При успешном запуске в симуляторе должна быть такая картина:
+![Симулятор с миром для модуля Б](docs/images/module2_view.png)
+
+В окне симулятора будут также отображаться изображения с камер робота. Предупреждения (warning) в консоли симулятора можно игнорировать. 
+
+Для визуальной оценки данных, которые получают датчики робота RMC2, можно использовать программу Rviz2. В пакете есть готовый скрипт для запуска настроенного под робота окна Rviz2.
+
+Для запуска откройте новый терминал и выполните:  
+**Не забудьте выполнить сурсы (source), если они не добавлены в ваш ~/.bashrc**
+```bash
+ros2 launch ar_webots_fms_ros2 rviz_rmc2.launch.py
+```
+Откроется следующее окно:
+![Rviz2 для модуля Б](docs/images/module2_rviz.png)
+
+Красные и синие точки отображают данные переднего и заднего лидара соответственно. Белые точки - объедененный скан переднего и заднего лидара.
+
+Слева снизу можно видеть изображение, которое передает камера, расположенная на дне робота. Поверх этого изображения также отрисовываются визуальные данные из Rviz2, которые попадают в поле зрения визуализируемой камеры. На скриншоте выше, например, видно, что камера "видит" некоторые оси координат робота. Но пусть вас не смущают эти визуализации, на самом изображении это все отсутствует.
+
+Чтобы управлять роботом RMC2 в ручном режиме, можно запустить скрипт телеопераций, который позволит задавать скорость и направление робота через клавиатуру. Для запуска откройте новый терминал и выполните:  
+**Не забудьте выполнить сурсы (source), если они не добавлены в ваш ~/.bashrc**
+```bash
+ros2 run ar_nav_ros2 teleop --ros-args -r __ns:=/RMC2
+```
+
+Перед вами появится инструкция с клавишами клавиатуры в следующем формате:
+
+```txt
+Control Your Robot!
+---------------------------
+Moving around:
+   q    w    e
+   a    s    d
+        x
+w/x : increase/decrease linear X velocity 
+q/e : increase/decrease linear Y velocity
+a/d : increase/decrease angular velocity 
+space key, s : force stop
+CTRL-C to quit
+```
+
+Чтобы заспавнить препятствие на карте, воспользуйтесь launch файлом в новом терминале:
+**Не забудьте выполнить сурсы (source), если они не добавлены в ваш ~/.bashrc**
+```bash
+ros2 launch ar_webots_fms_ros2 spawn_object.launch.py x:=-4.0 y:=1.0 angle_z:=0.0 object_name:=obstacle
+```
+В качестве аргументов `x`,`y` и `angle_z` в виде чисел с плавающей точкой можно задать координаты и ориентацию объекта. Чтобы задать уникальное имя объекта, укажите его в виде аргумента `name`.
+
+## Модуль В. Системы технического зрения с использованием инструментов искусственного интеллекта
+
+```
+Этот модуль направлен на обучение и интеграцию нейросетевой модели для автоматического распознавания. Конкурсанты должны самостоятельно собрать и разметить датасет, выбрать подходящую архитектуру нейросети, обучить ее и интегрировать в систему управления манипулятором.
+
+Задание:
+На специальной полке на соревновательном поле будут лежать N > 3 коробок. РМК-1 с манипулятором в стартовом положении около полки перпендикулярно ей (камера на манипуляторе смотрит вертикально вниз) смотрит на эту полку сверху. 
+С помощью генератора случайных чисел эксперт выбирает деталь, необходимую для захвата манипулятором. Конкурсанты должны запустить свой разработанный алгоритм, указать целевую деталь (1 - молоток, 2 - гаечный ключ, 3 - пассатижи) и начать выполнение. 
+РМК-1 должен автономно определить с помощью размеченного датасета, какая коробка его интересует, определить ее местоположение относительно базы манипулятора, направить манипулятор в нужную точку, схватить коробку схватом и поднять в стартовое положение. 
+После этого опустить коробку на пустое место на этой же полке.
+```
+
+Чтобы запустить симуляцию данного задания, запустите launch файл:  
+**Не забудьте выполнить сурсы (source), если они не добавлены в ваш ~/.bashrc**
+```bash
+ros2 launch ar_webots_fms_ros2 module3.launch.py
+```
+
+При первом запуске мира могут долго скачиваться некоторые ассеты для карты, поэтому, если робот не появился, то перезапустите симулятор.
+
+При успешном запуске в симуляторе должна быть такая картина:
+![Симулятор с миром для модуля В](docs/images/module3_view.png)
+
+Предупреждения (warning) в консоли симулятора можно игнорировать. 
+
+Для визуальной оценки данных, которые получают датчики робота RMC1, можно использовать программу Rviz2. В пакете есть готовый скрипт для запуска настроенного под робота окна Rviz2.
+
+Для запуска откройте новый терминал и выполните:  
+**Не забудьте выполнить сурсы (source), если они не добавлены в ваш ~/.bashrc**
+```bash
+ros2 launch ar_webots_fms_ros2 rviz_rmc1.launch.py
+```
+
+Откроется следующее окно:
+![Rviz2 для модуля Б](docs/images/module3_rviz.png)
+
+Данные для обучения нейросети можно собрать, перемещая камеру и объекты в симуляторе и делая скриншоты.
+
+Красные и синие точки отображают данные переднего и заднего лидара соответственно. Белые точки - объедененный скан переднего и заднего лидара.
+
+Слева снизу можно видеть изображение, которое передает камера, расположенная на захвате манипулятора. Поверх этого изображения также отрисовываются визуальные данные из Rviz2, которые попадают в поле зрения визуализируемой камеры. На скриншоте выше, например, видно, что камера "видит" некоторые оси координат робота. Но пусть вас не смущают эти визуализации, на самом изображении это все отсутствует.
+
+Чтобы управлять роботом RMC1 в ручном режиме, можно запустить скрипт телеопераций, который позволит задавать скорость и направление робота через клавиатуру. Для запуска откройте новый терминал и выполните:  
+**Не забудьте выполнить сурсы (source), если они не добавлены в ваш ~/.bashrc**
+```bash
+ros2 run ar_nav_ros2 teleop --ros-args -r __ns:=/RMC1
+```
+
+Перед вами появится инструкция с клавишами клавиатуры в следующем формате:
+
+```txt
+Control Your Robot!
+---------------------------
+Moving around:
+   q    w    e
+   a    s    d
+        x
+w/x : increase/decrease linear X velocity 
+q/e : increase/decrease linear Y velocity
+a/d : increase/decrease angular velocity 
+space key, s : force stop
+CTRL-C to quit
+```
+
+Чтобы запустить готовый пакет навигации Nav2, откройте новый терминал и выполните:  
+**Не забудьте выполнить сурсы (source), если они не добавлены в ваш ~/.bashrc**
+```bash
+ros2 launch ar_nav_ros2 bringup_launch.py
+```
+Запуск может занять около 15 секунд.   
+  
+После того, как стек навигации успешно запуститься, можно воспользоваться визуализацией Rviz2. Для запуска откройте новый терминал и выполните:  
+**Не забудьте выполнить сурсы (source), если они не добавлены в ваш ~/.bashrc**
+```bash
+ros2 launch ar_nav_ros2 rviz.launch.py 
+```
+Откроется следующее окно:  
+![Rviz2 для визуализации Nav2](docs/images/module3_nav2_rviz.png)
+
+В данном окне отображается 2D карта поля, показания лидара, локальная и глобальная карты занятости, границы робота и его траектории во время движения. Первоначально роботу необходимо задать начальную позицию, если его позиция на карте отличается от реальной. Для этого в верхней панели выберите инструмент `2D Pose Estimate`, кликните им в нужную точку и задайте ориентацию с помощью стрелки.  
+
+Чтобы задать целевую точку, выберите инструмент `2D Goal Pose`, кликните им в нужную точку и задайте ориентацию с помощью стрелки. Робот начнет движение по отрисованной траектории. 
+
+Для визуальной отладки манипулятора Arm95 и его управления через графический интерфейс, есть еще один готовый конфиг для Rviz2. Для запуска откройте новый терминал и выполните:  
+**Не забудьте выполнить сурсы (source), если они не добавлены в ваш ~/.bashrc**
+```bash
+ros2 launch ar_webots_fms_ros2 rviz_arm95.launch.py
+```
+
+Откроется следующее окно:
+![Rviz2 для манипулятора ARM95](docs/images/module3_rviz_arm95.png)
+
+Для ручного управления манипулятором в поле `Planning Group` выбираем *arm95_group*.
+
+Теперь в визуализации можно потянуть за стрелки или за голубую сферу и задать конечное положение манипулятора.
+Чтобы переместить манипулятор в заданное положение нажимаем кнопку `Plan`, затем `Execute`. Либо одной кнопкой сразу: `Plan & Execute`. Если удалось построить траекторию в заданну. точку, манипулятор должен переместиться. Можно проверить это в симуляторе Webots.
+
+Для ручного управления схватом в поле `Planning Group` выбираем *gripper*.
+Затем в поле `Goal State` выбираем `open` или `close` и затем нажимаем `Plan & Execute`.
+
+## Модуль Д. Решение складского кейса гетерогенной группой РМК
+
+```
+Конкурсанты должны обеспечить взаимодействие РМК-1 (оснащенного
+манипулятором) и РМК-2 (оснащенного подъемным механизмом) для
+выполнения последовательных (или параллельных) операций по
+транспортировке стеллажа и комплектации заказа
+
+Задание:
+
+1) РМК-2 выполняет транспортировку стеллажа из точки хранения в
+точку комплектации.
+2) РМК-1 выполняет операцию комплектации (picking) требуемой
+детали со стеллажа.
+3) РМК-1 доставляет деталь в точку сдачи.
+4) РМК-2 выполняет транспортировку стеллажа из точки комплектации
+в точку хранения.
+5) Оба РМК возвращаются в исходные позиции
+```
+
+Чтобы запустить симуляцию данного задания, запустите launch файл:  
+**Не забудьте выполнить сурсы (source), если они не добавлены в ваш ~/.bashrc**
+```bash
+ros2 launch ar_webots_fms_ros2 module5.launch.py
+```
+
+При первом запуске мира могут долго скачиваться некоторые ассеты для карты, поэтому, если робот не появился, то перезапустите симулятор.
+
+При успешном запуске в симуляторе должна быть такая картина:
+![Симулятор с миром для модуля В](docs/images/module5_view.png)
+
+Предупреждения (warning) в консоли симулятора можно игнорировать. 
+
+Далее запустите пакеты наыигации Nav2, откройте новый терминал и выполните:  
+**Не забудьте выполнить сурсы (source), если они не добавлены в ваш ~/.bashrc**
+```bash
+ros2 launch ar_nav_ros2 bringup_launch.py
+```
+
+В этом модуле можно использовать все предыдущие конфиги Rviz2 под каждого робота:  
+**Не забудьте выполнить сурсы (source), если они не добавлены в ваш ~/.bashrc**
+```bash
+# Для RMC1
+ros2 launch ar_webots_fms_ros2 rviz_rmc1.launch.py
+
+# Для манипулятора ARM95
+ros2 launch ar_webots_fms_ros2 rviz_arm95.launch.py
+
+# Для RMC2
+ros2 launch ar_webots_fms_ros2 rviz_rmc2.launch.py
+
+# Для Nav2
+ros2 launch ar_nav_ros2 rviz.launch.py
+```
+
+# Управление
+
+## Робот RMC1
+### Топики ROS2
+Робот RMC1 использует имя `RMC1` в поле `namespace` топиков ROS2. Ниже приведен список доступных топиков для взаимодействия:
+ * `/RMC1/odometry` [nav_msgs/msg/Odometry] - положение робота по колесной одометрии
+ * `/RMC1/scan` [sensor_msgs/msg/LaserScan] - объединенный скан переднего и заднего лидара
+ * `/RMC1/scan_back` [sensor_msgs/msg/LaserScan] - скан заднего лидара
+publisher
+ * `/RMC1/scan_front` [sensor_msgs/msg/LaserScan] - скан переднего лидара
+ * `/RMC1/cmd_vel` [geometry_msgs/msg/Twist] - целевая скорость платформы
+  * `/tf_static` [tf2_msgs/msg/TFMessage] - дерево статических преобразований между деталями робота
+  * `/tf` [tf2_msgs/msg/TFMessage] - дерево динамическиз преобразований между роботом и миром
+  * `/RMC1/arm95/camera_gripper/image_color` [sensor_msgs/msg/Image] - изображение с камеры манипулятора  
+
+### Управление манипулятором
+  Чтобы управлять манипулятором, используйте файл с примером из пакета `ar_webots_ros2/ar_webots_ros2/moveit_api_example.py`, его содержимое приведено ниже:
+``` python3
+import time
+import rclpy
+from rclpy.logging import get_logger
+from geometry_msgs.msg import PoseStamped
+from tf_transformations import quaternion_from_euler
+
+# moveit python library
+from moveit.planning import MoveItPy
+
+def plan_and_execute(robot, planning_component, logger, sleep_time=0.0):
+    plan_result = planning_component.plan()
+    if plan_result:
+        logger.info("Executing plan")
+        robot_trajectory = plan_result.trajectory
+        robot.execute(robot_trajectory, controllers=[])
+    else:
+        logger.error("Planning failed")
+    time.sleep(sleep_time)
+
+def main():
+    rclpy.init()
+    # Инициализация объектов MoveItPy
+    arm95 = MoveItPy(node_name="moveit_py", name_space="/RMC1/arm95")
+    arm95_arm = arm95.get_planning_component("arm95_group") # Объект - манипулятор
+    gripper = arm95.get_planning_component("gripper") # Объект - захват
+
+    # Пример открытия захвата
+    gripper.set_goal_state(configuration_name="open")
+    plan_and_execute(arm95, gripper, logger, sleep_time=0.0)
+
+    # Пример закрытия захвата
+    gripper.set_goal_state(configuration_name="closed")
+    plan_and_execute(arm95, gripper, logger, sleep_time=0.0)
+
+    # Задать текущее положение манипулятора как начальное
+    arm95_arm.set_start_state_to_current_state()
+
+    # Целевая ориентация схвата в углах Эйлера относительно основания "Base_link"
+    # Задается в виде roll, pitch, yaw
+    # Ориентация из углов Эцлера переводится в кватернион
+    q = quaternion_from_euler(3.14, 0.0, 1.57)
+
+    # Задать целевую позицию в формате PoseStamped сообщения
+    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 = -0.2   # Координата x захвата относительно основания "Base_link"
+    pose_goal.pose.position.y = -0.5   # Координата y захвата относительно основания "Base_link"
+    pose_goal.pose.position.z = 0.7    # Координата z захвата относительно основания "Base_link"
+    arm95_arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link="gripper_base") # Задается тип движения по заданной позиции
+    plan_and_execute(arm95, arm95_arm, logger, sleep_time=0.0) # Запуск перемещения в заданную позицию
+
+if __name__ == "__main__":
+    main()
+```
+
+Запускать данный файл необходимо через `ar_webots_ros2/launch/moveit_api_example.launch.py` файл, в котором в moveit контроллер передаются параметры манипулятоар и захвата.  
+
+### Управление навигацией Nav2
+Чтобы использовать навигацию Nav2, необходимо написать скрипт с использованием пакета `nav2_simple_commander`. Используйте его при запущенной навигации. Пример управляющего скрипта представлен ниже:
+``` python3
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
+import rclpy
+
+rclpy.init()
+nav = BasicNavigator() # Создание экземпляра контроллера
+
+# ...
+
+nav.setInitialPose(init_pose) # Задание исходной позиции в формате PoseStamped
+
+# ...
+
+path = nav.getPath(init_pose, goal_pose) # Построить маршрут из исходной точки в целевую (PoseStamped), проверка что путь существует
+
+# ...
+
+nav.goToPose(goal_pose) # Задать роботу цлевую позицию в формате PoseStamped
+while not nav.isTaskComplete(): # Цикл выполнения целевой точки, пока она не будет достигнута
+  feedback = nav.getFeedback() # Информация о выполнении задачи
+  if feedback.navigation_duration > 600: # Сравнение времени выполнения задачи с заданным временем
+    nav.cancelTask() # Остановить выполнение целевой точки
+
+# ...
+
+result = nav.getResult() # Получение результата о выполнении задачи
+if result == TaskResult.SUCCEEDED:
+    print('Goal succeeded!')
+elif result == TaskResult.CANCELED:
+    print('Goal was canceled!')
+elif result == TaskResult.FAILED:
+    print('Goal failed!')
+```
+
+Для запуска достаточно создать экземпляр контроллера, задать начальную и целевую точки. Робот начнет двиижение.
+
+Подробнее о работе с `nav2_simple_commander` можете уздать в [официальной документации](https://docs.nav2.org/commander_api/index.html). 
+
+## Робот RMC2
+### Топики ROS2
+Робот RMC2 использует имя `RMC2` в поле `namespace` топиков ROS2. Ниже приведен список доступных топиков для взаимодействия:
+* `/RMC2/aruco_id` [std_msgs/msg/String] - ID найденного нижней камерой ArUco маркера
+ * `/RMC2/camera_bottom/image_color` [sensor_msgs/msg/Image] - изображение с нижней камеры (геометрический центр робота)
+ * `/RMC2/depth_camera/image` [sensor_msgs/msg/Image] - изображение с передней камеры
+ * `/RMC2/depth_camera/point_cloud` [sensor_msgs/msg/PointCloud2] - облако точек с передней камеры глубины
+ * `/RMC2/lift_status` [std_msgs/msg/String] - состояние лифта (moving / success)
+ * `/RMC2/lift` [std_msgs/msg/Float64] - поднятие лифта на заданную высоту
+ * `/RMC2/odometry` [nav_msgs/msg/Odometry] колесная одометрия робота
+ * `/RMC2/scan` [sensor_msgs/msg/LaserScan] - объединенный скан переднего и заднего лидара
+ * `/RMC2/scan_back` [sensor_msgs/msg/LaserScan] - скан заднего лидара
+publisher
+ * `/RMC2/scan_front` [sensor_msgs/msg/LaserScan] - скан переднего лидара
+ * `/RMC2/cmd_vel` [geometry_msgs/msg/Twist] - целевая скорость платформы
+  * `/tf_static` [tf2_msgs/msg/TFMessage] - дерево статических преобразований между деталями робота
+  * `/tf` [tf2_msgs/msg/TFMessage] - дерево динамическиз преобразований между роботом и миром
+
+  Чтобы робот начал перемещение, необходиом начать публиковать в топик `/RMC2/cmd_vel` целевую скорость. Кинематика робота РМК2 позволяет задачать линейную скорость по оси `x` и угловую по оси `z` робота.
+
+  Чтобы управлять положением подъемного лифта, необходимо отправить в топик `/RMC2/lift` число с плавающей точкой с высотой, на которую поднимется лифт. Значение `0.0` соответствует полностью опущенному положению лифта. Значения `0.1` соответстует поднятому положению. Этого достаточно, чтобы поднять стеллаж с объектами. Лифт имеет 2 фиксированных положения, промежуточного не существует.
+
+  # Дополнительная информация
+  ## Поле 
+  Соревновательное поле размечено ArUco маркерами в соответствии со следующим изображением:
+  ![Поле ArUco маркеров](docs/images/aruco_graph.png)
+  
+  Маркеры ArUco имеют уникальные ID адреса от 0 до 35, верх маркера соответствует верху цифры на изображении выше. Инпче говоря, все маркеры ориентированы верхней гранью в одну сторону. 
+
+  Начало координат поля в Webots совпадает с маркером ID = 0, направление векторов координат показано на картинке выше: ось X смотрит вправо, ось Y смотрит вверх.