docs: add physical rover setup reference

2026-09-10 22:06:15 +03:00
parent 19d08523eb
commit 1b0ebe1b5a
7 changed files with 163 additions and 57 deletions
@@ -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`.
@@ -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 — начать: ")
@@ -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()
-
@@ -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()
-