diff --git a/README.md b/README.md index 56aa5e3..64d1afb 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ - [`materials/presentations`](materials/presentations) — пять учебных презентаций (введение, документация, Python/ROS, симулятор, развитие компетенции). - [`materials/pdf`](materials/pdf) — шестая презентация про датасет и модели, расписание. -- [`materials/final`](materials/final) — полный очный финал: инфраструктурный лист, КЗ модулей А–Д, критерии и приложения. +- [`materials/final`](materials/final) — полный очный финал: инфраструктурный лист, КЗ модулей А–Д, критерии, приложения и выданная документация физических РМК. - [`materials/tasks`](materials/tasks) — отдельный ранний/отборочный комплект по навигации; не путать с полным финалом. - [`basic`](basic) — примеры для реального ROS 2: движение к ArUco с odometry/TF/lidar, ручной `cmd_vel`, остановка, лифт, мониторинг и BFS-маршрут. - [`setup/FROM_ZERO.md`](setup/FROM_ZERO.md) — подготовка полностью чистого Windows/Ubuntu и день настройки оборудования. @@ -22,12 +22,13 @@ ## Порядок чтения 1. `materials/final/04-final-task.docx` и `materials/final/05-final-criteria.xlsx` — сначала понять конечный результат и баллы очного финала. -2. `1. Введение...` и `2. Документация...` — термины, РМК и структура компетенции. -3. `3. Разбор...` — Python, ROS 2, топики и сервисы. -4. `4. Практика... (симулятор)` — установка и работа с Webots. -5. `5. Практика... (датасет + модели)` — разметка и обучение детектора. -6. `setup/FROM_ZERO.md` — подготовка ПК и оборудования. -7. README каждого модуля в `mvch` — точные команды текущего решения. +2. `materials/final/09-robot-documentation.pdf` — реальные размеры, топики, камеры, лифт и сеть РМК. +3. `1. Введение...` и `2. Документация...` — термины, РМК и структура компетенции. +4. `3. Разбор...` — Python, ROS 2, топики и сервисы. +5. `4. Практика... (симулятор)` — установка и работа с Webots. +6. `5. Практика... (датасет + модели)` — разметка и обучение детектора. +7. `setup/FROM_ZERO.md` — подготовка ПК и оборудования. +8. README каждого модуля в `mvch` — точные команды текущего решения. ## Быстрый старт на подготовленной VM @@ -177,6 +178,29 @@ ros2 topic echo /RMC2/scan_front --once Точный список топиков зависит от активного launch-файла. Если `scan_front` отсутствует, сначала посмотрите `ros2 topic list | sort`, а не меняйте код вслепую. +## ROS API физических роверов из новой документации + +Физическое поле на схеме имеет сетку 5 × 5 и ID 0–24. Система координат та же, +что использует код: `0=(0,0)`, `1=(0,+1 м)`, `5=(-1 м,0)`. Webots остаётся +отдельным профилем 6 × 6 с ID 0–35. + +| Назначение | RMC1 | RMC2 | +| --- | --- | --- | +| Скорость | `/RMC1/cmd_vel` (`Twist`) | `/RMC2/cmd_vel` (`Twist`) | +| Одометрия | `/RMC1/odometry` | `/RMC2/odometry` | +| Лидары | `/RMC1/scan_front`, `/scan_back`, `/scan` | `/RMC2/scan_front`, `/scan_back`, `/scan` | +| TF | `/RMC1/tf`, `/RMC1/tf_static` | `/RMC2/tf`, `/RMC2/tf_static` | +| Камера | `/RMC1/arm95/svcam/right/image/compressed` | `/RMC2/camera_bottom/image` | +| ArUco ID | в PDF отдельный топик не указан | `/RMC2/camera_bottom/aruco_id` (`String`) | +| Лифт | — | `/RMC2/lift` (`Float64`), `/RMC2/lift_status` (`String`, Transient Local) | + +Лифт принимает `0.0` для нижнего положения и `0.05` для верхнего; статусы: +`lowered`, `moving`, `raised`. В PDF у RMC1 камера с суффиксом `/compressed` +помечена типом `sensor_msgs/Image`, а не `CompressedImage`, и топик `CameraInfo` +не перечислен. Это нужно обязательно проверить командой `ros2 topic info -v` на +оборудовании: код умеет работать с обоими типами, а калибровку при отсутствии +`CameraInfo` можно передать аргументами. + ## Если на реальном ровере другие датчики или ROS API Успешный запуск Python-процесса ещё не означает, что ровер готов ехать. Текущие @@ -208,12 +232,14 @@ ros2 topic list -t | tee ~/equipment-audit/topics.txt ros2 topic info /RMC2/cmd_vel -v | tee ~/equipment-audit/cmd_vel.txt ros2 topic info /RMC2/odometry -v | tee ~/equipment-audit/odometry.txt ros2 topic info /RMC2/scan_front -v | tee ~/equipment-audit/scan_front.txt -ros2 topic info /RMC2/aruco_id -v | tee ~/equipment-audit/aruco_id.txt +ros2 topic info /RMC2/camera_bottom/aruco_id -v | tee ~/equipment-audit/aruco_id.txt +ros2 topic info /RMC2/lift_status -v | tee ~/equipment-audit/lift_status.txt +ros2 topic info /RMC1/arm95/svcam/right/image/compressed -v | tee ~/equipment-audit/rmc1_camera.txt timeout 5 ros2 topic hz /RMC2/odometry timeout 5 ros2 topic hz /RMC2/scan_front ros2 topic echo /RMC2/odometry --once -ros2 topic echo /RMC2/aruco_id --once +ros2 topic echo /RMC2/camera_bottom/aruco_id --once ros2 run tf2_ros tf2_echo RMC2/odom RMC2/base_link ros2 run tf2_ros tf2_echo RMC2/odom aruco_0 @@ -257,8 +283,8 @@ python3 basic/watch_robot.py --robot RMC2 | Модуль | Решение | Основной запуск | | --- | --- | --- | -| Б | навигация RMC2 по ArUco | `python3 module2/module_b.py --target 14 --sim` | -| В | YOLO + ARM95 | `module3/.venv/bin/python module3/module3.py --target hammer` | +| Б | навигация RMC2 по ArUco | реальный: `python3 module2/module_b.py --target 14`; Webots: добавить `--sim` | +| В | YOLO + ARM95 | реальный: `module3/.venv/bin/python module3/module3.py --target hammer`; Webots: добавить `--sim` | | Г | веб-FMS двух роверов | `cd module4 && bun run start` | | Д | общий сценарий RMC1 + RMC2 | `module3/.venv/bin/python module5/module5.py --target hammer` | diff --git a/basic/README.md b/basic/README.md index 3dae951..bc12150 100644 --- a/basic/README.md +++ b/basic/README.md @@ -1,7 +1,8 @@ # Базовые примеры для реального ROS 2 Это короткие учебные скрипты, которые работают через штатные ROS 2 интерфейсы -роверов: `cmd_vel`, `odometry`, `scan_front`, `aruco_id`, TF и `lift`. В них нет +роверов: `cmd_vel`, `odometry`, `scan_front`, `aruco_id`, TF и `lift`. Имена +сверены с выданной документацией физических РМК от 08.09.2026. В них нет прямого управления моторами и нет подмены симулятора. Один и тот же ROS API можно проверять в Webots, а затем перенести на физический РМК после сверки имён топиков, TF, размеров поля и безопасных скоростей. @@ -23,7 +24,8 @@ ros2 topic list | sort ros2 topic info /RMC2/cmd_vel -v ros2 topic info /RMC2/odometry -v ros2 topic info /RMC2/scan_front -v -ros2 topic echo /RMC2/aruco_id +ros2 topic info /RMC2/camera_bottom/aruco_id -v +ros2 topic echo /RMC2/camera_bottom/aruco_id --once ros2 run tf2_ros tf2_echo RMC2/odom aruco_0 ``` @@ -56,8 +58,9 @@ python3 basic/lift_rmc2.py down python3 basic/stop_all.py ``` -`drive_to_aruco.py` рассчитан на регулярную сетку 6×6: `0=(0,0)`, -`1=(0,+1 м)`, `6=(-1 м,0)`. Ровер должен стартовать центром над видимой меткой. +`drive_to_aruco.py` по умолчанию использует реальную сетку 5×5: `0=(0,0)`, +`1=(0,+1 м)`, `5=(-1 м,0)`. С `--sim` выбирается сетка Webots 6×6, где +`6=(-1 м,0)`. Ровер должен стартовать центром над видимой меткой. Скрипт сам определяет старт, строит BFS-маршрут, печатает его до движения, затем едет по odometry и уточняет положение через TF каждой ArUco. Без свежих odometry, лидара, метки на финише или при препятствии ближе `--stop-distance` он отправляет @@ -67,8 +70,14 @@ python3 basic/stop_all.py `--spacing`. Если организаторы используют другой топик лидара, передайте `--scan-topic /нужный/топик`. Не используйте `--yes` в первом физическом заезде. +По выданной документации нижняя камера и `aruco_id` есть у RMC2. Поэтому реальный +запуск `drive_to_aruco.py` разрешён только для RMC2. Подъём лифта публикует `0.05`, +опускание — `0.0`, а статус читается с обязательным QoS `Transient Local`. + У RMC1 доступно боковое движение (`linear.y`), у RMC2 оно принудительно запрещено в `move_rmc.py`. Режим `--no-lidar` предназначен только для вывешенного стенда, где колёса не могут переместить робот. `grid_route.py` оставлен как офлайн-проверка графа. Он ничего не публикует в ROS. +Для реального поля используется его дефолт `--size 5`, для Webots добавьте +`--size 6`. diff --git a/basic/drive_to_aruco.py b/basic/drive_to_aruco.py index 5fe776b..61440cc 100755 --- a/basic/drive_to_aruco.py +++ b/basic/drive_to_aruco.py @@ -16,10 +16,16 @@ from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry from rclpy.node import Node from rclpy.parameter import Parameter -from rclpy.qos import qos_profile_sensor_data +from rclpy.qos import ( + DurabilityPolicy, + QoSProfile, + ReliabilityPolicy, + qos_profile_sensor_data, +) from rclpy.time import Time from sensor_msgs.msg import LaserScan from std_msgs.msg import String +from tf2_msgs.msg import TFMessage from tf2_ros import Buffer, TransformException, TransformListener @@ -117,6 +123,26 @@ class ArucoDrive(Node): self.odom_frame = f"{self.robot}/odom" self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) + if not args.sim: + tf_qos = QoSProfile( + depth=100, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + ) + static_tf_qos = QoSProfile( + depth=100, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + self.create_subscription( + TFMessage, f"/{self.robot}/tf", self.on_tf, tf_qos + ) + self.create_subscription( + TFMessage, + f"/{self.robot}/tf_static", + self.on_tf_static, + static_tf_qos, + ) self.publisher = self.create_publisher( Twist, f"/{self.robot}/cmd_vel", 10 ) @@ -132,9 +158,22 @@ class ArucoDrive(Node): self.on_scan, qos_profile_sensor_data, ) - self.create_subscription( - String, f"/{self.robot}/aruco_id", self.on_marker, 10 + self.aruco_topic = ( + f"/{self.robot}/aruco_id" + if args.sim + else f"/{self.robot}/camera_bottom/aruco_id" ) + self.create_subscription(String, self.aruco_topic, self.on_marker, 10) + + def on_tf(self, message): + for transform in message.transforms: + self.tf_buffer.set_transform(transform, f"{self.robot} namespaced tf") + + def on_tf_static(self, message): + for transform in message.transforms: + self.tf_buffer.set_transform_static( + transform, f"{self.robot} namespaced tf_static" + ) def on_odometry(self, message): pose = message.pose.pose @@ -280,9 +319,9 @@ class ArucoDrive(Node): def arguments(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("target", type=int, help="ID целевой ArUco") - parser.add_argument("--robot", default="RMC2", help="RMC1 или RMC2") - parser.add_argument("--rows", type=int, default=6) - parser.add_argument("--columns", type=int, default=6) + parser.add_argument("--robot", choices=("RMC1", "RMC2"), default="RMC2") + parser.add_argument("--rows", type=int) + parser.add_argument("--columns", type=int) parser.add_argument("--spacing", type=float, default=1.0, help="Шаг сетки, м") parser.add_argument("--blocked", type=int, nargs="*", default=[]) parser.add_argument("--max-speed", type=float, default=0.12, help="м/с") @@ -295,6 +334,15 @@ def arguments(): parser.add_argument("--yes", action="store_true", help="Не ждать Enter") parser.add_argument("--sim", action="store_true", help="Использовать /clock Webots") args = parser.parse_args() + if args.rows is None: + args.rows = 6 if args.sim else 5 + if args.columns is None: + args.columns = 6 if args.sim else 5 + if not args.sim and args.robot != "RMC2": + parser.error( + "Документация физического RMC1 не содержит нижнюю ArUco-камеру; " + "drive_to_aruco поддерживает реальный RMC2" + ) count = args.rows * args.columns if args.rows <= 0 or args.columns <= 0 or not 0 <= args.target < count: parser.error(f"target должен быть в диапазоне 0..{count - 1}") @@ -319,7 +367,7 @@ def main(): print("Маршрут:", " -> ".join(map(str, route))) print( f"Топики: /{node.robot}/cmd_vel, /{node.robot}/odometry, " - f"/{node.robot}/aruco_id, {args.scan_topic or f'/{node.robot}/scan_front'}" + f"{node.aruco_topic}, {args.scan_topic or f'/{node.robot}/scan_front'}" ) if not args.yes: input("Проверьте поле и держите аппаратный STOP. Enter — начать: ") diff --git a/basic/grid_route.py b/basic/grid_route.py index 4763beb..867b3f7 100755 --- a/basic/grid_route.py +++ b/basic/grid_route.py @@ -1,22 +1,19 @@ #!/usr/bin/env python3 -"""Строит кратчайший маршрут по сетке маркеров 6x6 без ROS.""" +"""Строит кратчайший маршрут по регулярной сетке ArUco без ROS.""" import argparse from collections import deque -SIZE = 6 - - -def neighbors(marker: int): - row, column = divmod(marker, SIZE) +def neighbors(marker: int, size: int): + row, column = divmod(marker, size) for dr, dc in ((-1, 0), (1, 0), (0, -1), (0, 1)): next_row, next_column = row + dr, column + dc - if 0 <= next_row < SIZE and 0 <= next_column < SIZE: - yield next_row * SIZE + next_column + if 0 <= next_row < size and 0 <= next_column < size: + yield next_row * size + next_column -def build_route(start: int, target: int, blocked: set[int]): +def build_route(start: int, target: int, blocked: set[int], size: int): queue = deque([start]) previous = {start: None} @@ -24,7 +21,7 @@ def build_route(start: int, target: int, blocked: set[int]): current = queue.popleft() if current == target: break - for candidate in neighbors(current): + for candidate in neighbors(current, size): if candidate not in blocked and candidate not in previous: previous[candidate] = current queue.append(candidate) @@ -42,8 +39,9 @@ def build_route(start: int, target: int, blocked: set[int]): def main(): parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument("start", type=int, help="Стартовый маркер 0..35") - parser.add_argument("target", type=int, help="Целевой маркер 0..35") + parser.add_argument("start", type=int, help="ID стартового маркера") + parser.add_argument("target", type=int, help="ID целевого маркера") + parser.add_argument("--size", type=int, default=5, help="5 для реального поля, 6 для Webots") parser.add_argument( "--blocked", type=int, @@ -53,13 +51,15 @@ def main(): ) args = parser.parse_args() - if not 0 <= args.start < SIZE * SIZE or not 0 <= args.target < SIZE * SIZE: - parser.error("start и target должны быть от 0 до 35") + if args.size < 2: + parser.error("--size должен быть не меньше 2") + if not 0 <= args.start < args.size**2 or not 0 <= args.target < args.size**2: + parser.error(f"start и target должны быть от 0 до {args.size**2 - 1}") blocked = set(args.blocked) blocked.discard(args.start) blocked.discard(args.target) - route = build_route(args.start, args.target, blocked) + route = build_route(args.start, args.target, blocked, args.size) if route is None: raise SystemExit("Маршрут не найден") print(" -> ".join(map(str, route))) @@ -67,4 +67,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/basic/lift_rmc2.py b/basic/lift_rmc2.py index 578ca24..b49b182 100755 --- a/basic/lift_rmc2.py +++ b/basic/lift_rmc2.py @@ -6,6 +6,7 @@ import time import rclpy from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy from std_msgs.msg import Float64, String @@ -17,29 +18,48 @@ def main(): rclpy.init() node = Node("mvch_basic_lift") - publisher = node.create_publisher(Float64, "/RMC2/lift", 10) - statuses = [] - node.create_subscription(String, "/RMC2/lift_status", lambda msg: statuses.append(msg.data), 10) + exit_message = None + try: + publisher = node.create_publisher(Float64, "/RMC2/lift", 10) + statuses = [] + status_qos = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + node.create_subscription( + String, + "/RMC2/lift_status", + lambda msg: statuses.append(msg.data), + status_qos, + ) - height = 0.1 if args.state == "up" else 0.0 - print(f"RMC2 lift: {args.state}, команда {height}") - for _ in range(5): - publisher.publish(Float64(data=height)) - rclpy.spin_once(node, timeout_sec=0.1) + height = 0.05 if args.state == "up" else 0.0 + expected = "raised" if args.state == "up" else "lowered" + print(f"RMC2 lift: {args.state}, команда {height}") + for _ in range(5): + publisher.publish(Float64(data=height)) + rclpy.spin_once(node, timeout_sec=0.1) - deadline = time.monotonic() + args.wait - while time.monotonic() < deadline and not statuses: - rclpy.spin_once(node, timeout_sec=0.1) + deadline = time.monotonic() + args.wait + while time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + if statuses and statuses[-1].strip().lower() == expected: + break - if statuses: - print(f"Статус: {statuses[-1]}") - else: - print("Нового lift_status нет. Это возможно, если лифт уже был в нужном положении.") - - node.destroy_node() - rclpy.shutdown() + if statuses and statuses[-1].strip().lower() == expected: + print(f"Статус: {statuses[-1]}") + else: + current = statuses[-1] if statuses else "нет данных" + exit_message = ( + f"Лифт не подтвердил {expected}; текущий статус: {current}" + ) + finally: + node.destroy_node() + rclpy.shutdown() + if exit_message: + raise SystemExit(exit_message) if __name__ == "__main__": main() - diff --git a/materials/final/09-robot-documentation.pdf b/materials/final/09-robot-documentation.pdf new file mode 100644 index 0000000..ce1bde6 Binary files /dev/null and b/materials/final/09-robot-documentation.pdf differ diff --git a/setup/COMMANDS.md b/setup/COMMANDS.md index e4f752f..67f2d74 100644 --- a/setup/COMMANDS.md +++ b/setup/COMMANDS.md @@ -202,6 +202,10 @@ ros2 topic list | sort ros2 topic echo /RMC1/odometry --once ros2 topic echo /RMC2/odometry --once ros2 topic echo /RMC2/scan_front --once +ros2 topic info /RMC2/camera_bottom/aruco_id -v +ros2 topic echo /RMC2/camera_bottom/aruco_id --once +ros2 topic info /RMC2/lift_status -v +ros2 topic info /RMC1/arm95/svcam/right/image/compressed -v ``` ## Git bundle на флешку