Лабораторна робота №15. Інтеграція MG400 з ROS 2

Мета: навчитися читати URDF/Xacro-модель MG400, розрізняти topics, services та actions у ROS 2, перевірити TF-граф і display.launch.py без обладнання, реалізувати mock-чергу довготривалих команд із скасуванням і безпечною реакцією на втрату вузла; чітко відокремити таку перевірку від фізичного bringup.

Результати навчання та передумови

Після роботи студент уміє:

1. пояснити ролі robot_state_publisher, /joint_states, TF, service та action;
2. знайти опис ланок, суглобів і action-інтерфейсу в локальному пакеті;
3. запустити статичну перевірку репозиторію та, за наявності узгодженого ROS 2-середовища, лише апаратно незалежний display;
4. відтворити прийняття, feedback, скасування та помилку mock-action;
5. перейти у FAULT при втраті heartbeat і не відновлювати чергу автоматично.

Передумови: Python, базові команди Linux, графи, поняття publish/subscribe та виконані ЛР-01, ЛР-13. Фізична апробація не входить до цієї редакції.

Середовище виконання

Необхідні знання та матеріали

• локальний каталог MG400_ROS2-humble, зокрема display.launch.py, mg400.urdf.xacro та CommandQueue.action;
• Python зі стандартною бібліотекою;
• для необов'язкового display — окреме узгоджене середовище ROS 2 Humble з RViz2 та зібраним workspace;
• журнальна таблиця з цієї роботи; мережеві адреси, паролі й токени не потрібні.

Ризики та правила безпеки

Ризики

display.launch.py створює лише опис і візуалізацію. Натомість main.launch.py включає mg400_node, який відкриває мережеві інтерфейси реального контролера; service/action-виклики можуть активувати або перемістити робот. Втрата клієнта не доводить, що черга контролера порожня.

Заборонені дії

• не запускати main.launch.py, mg400.launch.py, sample.bash або command_queue_client;
• не викликати EnableRobot, MovJ, MovL, JointMovJ, Continue, I/O чи action фізичного робота;
• не підставляти заводську IP-адресу як адресу MERC-I5;
• не вважати RViz симулятором динаміки, перевіркою колізій або доказом фізичної безпеки.

Умови негайної зупинки

У mock/display припинити сценарій при появі неочікуваного мережевого вузла, невідомого namespace, зависанні процесу, втраті heartbeat, невдалому скасуванні або невідомому стані action. На фізичній системі додатково діють усі умови з правил експлуатації.

Безпечний стан

Для цієї роботи: жодного з'єднання з MG400, жодного апаратного виходу, mock-черга порожня або скасована, стан IDLE чи зафіксований FAULT, автоматичне відновлення вимкнене.

Передпусковий чекліст

• Відкрито саме display.launch.py, а не main.launch.py.
• Ethernet/Wi-Fi до контролера MG400 не використовується.
• У команді немає IP-адреси, service/action керування або sample.bash.
• Workspace містить лише перевірену локальну копію пакета.
• Mock-тести завершуються без мережі.
• Відомо, як завершити процес через Ctrl+C.

Хід виконання роботи

1. Звірити локальну структуру ROS-пакета.
2. Дослідити модель, TF та інтерфейси.
3. Виконати safe-smoke для action-черги.
4. Запустити статичний аудитор.
5. За наявності підготовленого ROS 2-середовища запустити тільки display.
6. Перевірити скасування й втрату heartbeat, оформити метрики.

Методичні вказівки й теоретичні відомості

1. URDF/Xacro, TF і joint states

URDF задає дерево ланок і суглобів; Xacro додає макроси й параметри, після розгортання яких утворюється URDF. robot_state_publisher поєднує геометрію моделі зі значеннями /joint_states та публікує перетворення TF. RViz відображає отримане дерево, але не моделює сили, гальмування чи реальний стан контролера.

У локальному display.launch.py запускаються rsp.launch.py, власний joint_state_publisher_gui і RViz. README прямо позначає цей display як апаратно незалежний. main.launch.py, навпаки, включає мережевий mg400_node; ці маршрути не взаємозамінні.

2. Topics, services та actions

Локальний CommandQueue.action приймає масив команд, повертає result та error_id, а feedback містить позу й чотири кути. Наявність cancel у протоколі action не гарантує миттєвої фізичної зупинки.

Текстовий опис рисунка: URDF/Xacro і mock joint states надходять до robot_state_publisher, який передає TF у RViz; окремо mock action client обмінюється goal/cancel/feedback/result із чергою, а heartbeat monitor переводить її у FAULT після тайм-ауту.

flowchart LR
    JSP["Mock joint-state publisher"] -->|"/joint_states"| RSP["robot_state_publisher"]
    URDF["URDF/Xacro"] --> RSP
    RSP -->|"/tf, /tf_static"| RVIZ["RViz2 display"]
    CLIENT["Mock action client"] -->|"goal / cancel"| SERVER["Mock command queue"]
    SERVER -->|"feedback / result"| CLIENT
    HEART["Heartbeat monitor"] -->|"timeout"| FAULT["FAULT + manual recovery"]

Рис. 1. Апаратно незалежні потоки даних ЛР-15

3. Джерела

• офіційний опис інтерфейсів ROS 2 Humble;
• офіційні навчальні матеріали URDF для ROS 2 Humble;
• офіційний опис tf2;
• локальні README, display.launch.py і CommandQueue-опис;
• базова лінія ПЗ MERC-I5 і правила допуску.

Виконання лабораторної роботи

Крок 1. Перевірка меж завдання

Випишіть два дозволені артефакти (display.launch.py, mock) і щонайменше три заборонені (main.launch.py, sample.bash, service/action фізичного робота). Перевірте, що контролер не використовується.

Очікуваний результат: заповнений чекліст і словесне розділення display/bringup.

Критерій правильності: студент пояснює, який launch-файл створює мережевий вузол, не виконуючи його.

Якщо результат не отримано: повернутися до локальних launch-файлів; нічого не запускати.

Крок 2. Safe-smoke черги та скасування

Виконайте блок без ROS 2, мережі та обладнання.

# safe-smoke
from dataclasses import dataclass
from enum import Enum, auto

class State(Enum):
    IDLE = auto()
    ACTIVE = auto()
    CANCELLED = auto()
    FAULT = auto()

@dataclass
class MockActionQueue:
    state: State = State.IDLE
    heartbeat_age: int = 0
    completed: int = 0

    def send(self, commands: tuple[str, ...]) -> None:
        if self.state is not State.IDLE or not commands:
            raise RuntimeError("goal rejected")
        self.state = State.ACTIVE

    def feedback(self) -> None:
        if self.state is not State.ACTIVE:
            raise RuntimeError("no active goal")
        self.completed += 1
        self.heartbeat_age = 0

    def cancel(self) -> None:
        if self.state is State.ACTIVE:
            self.state = State.CANCELLED

    def tick_without_heartbeat(self, limit: int = 2) -> None:
        self.heartbeat_age += 1
        if self.heartbeat_age > limit:
            self.state = State.FAULT

q = MockActionQueue()
q.send(("PLAN_A", "PLAN_B"))
q.feedback()
q.cancel()
assert q.state is State.CANCELLED and q.completed == 1
lost = MockActionQueue()
lost.send(("PLAN",))
for _ in range(3):
    lost.tick_without_heartbeat()
assert lost.state is State.FAULT
print("safe-smoke: cancel and heartbeat timeout PASS")

Очікуваний результат: safe-smoke: cancel and heartbeat timeout PASS.

Критерій правильності: код завершується з кодом 0; після timeout стан лише FAULT.

Якщо результат не отримано: не переходити до ROS; перевірити незмінність коду й версію Python.

Крок 3. Статичний аудит локального пакета

Збережіть код як ros_static_check.py. Він лише читає файли.

from __future__ import annotations

import argparse
import re
import xml.etree.ElementTree as ET
from pathlib import Path


def require(path: Path) -> str:
    if not path.is_file():
        raise FileNotFoundError(path)
    return path.read_text(encoding="utf-8")


def main() -> int:
    parser = argparse.ArgumentParser()
    parser.add_argument("--repo", type=Path, default=Path("MG400_ROS2-humble"))
    args = parser.parse_args()
    repo = args.repo
    xacro = repo / "mg400_description/urdf/mg400.urdf.xacro"
    macro = repo / "mg400_description/urdf/mg400.xacro"
    display = repo / "mg400_bringup/launch/display.launch.py"
    action = repo / "mg400_msgs/action/CommandQueue.action"

    ET.parse(xacro)
    ET.parse(macro)
    macro_text = require(macro)
    display_text = require(display)
    action_text = require(action)

    links = sorted(set(re.findall(r'<link\s+name="\$\{prefix\}([^"$]+)"', macro_text)))
    joints = sorted(set(re.findall(r'<joint\s+name="\$\{prefix\}([^"$]+)"', macro_text)))
    sections = action_text.split("---")
    checks = {
        "xml_ok": True,
        "has_links": bool(links),
        "has_joints": bool(joints),
        "action_goal_result_feedback": len(sections) == 3,
        "display_has_rviz": "rviz.launch.py" in display_text,
        "display_has_rsp": "rsp.launch.py" in display_text,
        "display_has_network_node": "mg400.launch.py" in display_text,
    }
    for key, value in checks.items():
        print(f"{key}={value}")
    print(f"links={len(links)} joints={len(joints)}")
    assert all(value for key, value in checks.items() if key != "display_has_network_node")
    assert checks["display_has_network_node"] is False
    return 0


if __name__ == "__main__":
    raise SystemExit(main())

python ros_static_check.py --repo MG400_ROS2-humble

Очікуваний результат: усі позитивні перевірки True, display_has_network_node=False, кількість ланок і суглобів більша нуля.

Критерій правильності: XML розбирається; action має три секції; display не включає mg400.launch.py.

Якщо результат не отримано: зафіксувати файл і помилку; не виправляти модель довільно й не переходити до bringup.

Крок 4. Інспекція графа без запуску обладнання

За вихідним кодом заповніть таблицю.

Очікуваний результат: усі чотири рядки пояснено власними словами.

Критерій правильності: topic/service/action не змішані; result, feedback, cancel мають різні ролі.

Якщо результат не отримано: повторно прочитати .msg/.srv/.action; не робити пробний action-виклик.

Крок 5. Необов'язковий hardware-free display

Крок дозволено лише в уже зібраному контрольному Ubuntu 22.04/ROS 2 Humble workspace. Команда не підключає MG400 за структурою локального launch-файла.

source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch mg400_bringup display.launch.py workspace_visible:=False

В іншому терміналі допускається лише read-only інспекція:

ros2 node list
ros2 topic list
ros2 topic echo /joint_states --once

Очікуваний результат: RViz показує модель, GUI змінює візуальну конфігурацію, /joint_states має повідомлення.

Критерій правильності: у графі немає mg400_node; Ethernet/IP контролера не використовуються.

Якщо результат не отримано: завершити Ctrl+C, зберегти журнал; не запускати main.launch.py як «спосіб виправлення».

Крок 6. Аналіз негативних і граничних сценаріїв

Повторіть safe-smoke, змінюючи лише тестову копію: порожній goal має бути відхилений; cancel після IDLE не створює руху; heartbeat на межі limit ще не дає FAULT, після перевищення — дає.

Очікуваний результат: детерміновані стани й пояснення off-by-one межі.

Критерій правильності: жоден сценарій не переходить із FAULT в ACTIVE без окремої ручної процедури.

Якщо результат не отримано: зупинити сценарій, додати трасування станів і усунути неоднозначний перехід.

Фізичний bringup: еталон і межа поточної роботи

Еталонний фізичний етап потребує затвердженої конфігурації, локального оператора, перевіреного E-Stop, вільної зони, меж, навантаження, Tool/User frames, мінімальної швидкості, heartbeat і журналу. Спершу виконують read-only перевірку стану; будь-яка активація або action потребує нового явного дозволу. У цьому запуску етап не виконується.

Сценарії перевірки

Таблиці вимірювань і метрики

Обов'язкові метрики: частка пройдених assertions, кількість неочікуваних вузлів, latency mock-feedback (за time.perf_counter) і однозначність кінцевого стану. Цільові апаратні значення не задаються без вимірювань.

Вимоги до звіту

Див. загальні вимоги. Додайте: карту ROS-графа, таблицю артефактів, вивід аудитора, журнал позитивного/негативного/граничного тестів, пояснення display проти bringup та явну фразу «фізичний MG400 не підключався і не рухався».

Критерії оцінювання

Контрольні питання

1. Чому RViz не доводить відсутність колізії?
2. Які дані входять до goal, feedback і result CommandQueue.action?
3. Коли застосовують topic, service та action?
4. Чому cancel-відповідь треба звірити з фактичним станом?
5. Яка різниця між display.launch.py і main.launch.py?
6. Чому відновлення після втрати heartbeat має бути ручним?

Висновки

Сформулюйте, які частини інтеграції підтверджені статично й у mock, які властивості показує TF/RViz, як система обробляє cancel і втрату вузла та чому ця робота не є фізичною апробацією MG400.