From 0ca59432d0dc30133d726e11ab907de79895ecc8 Mon Sep 17 00:00:00 2001
From: mavxa <mafvacurse@gmail.com>
Date: Mon, 14 Sep 2026 01:12:30 +0300
Subject: [PATCH] feat: add basic ROS rover scripts

---
 basic/README.md         |  86 +++++++++
 basic/drive_to_aruco.py | 409 ++++++++++++++++++++++++++++++++++++++++
 basic/grid_route.py     |  69 +++++++
 basic/lift_rmc2.py      |  65 +++++++
 basic/move_rmc.py       |  93 +++++++++
 basic/stop_all.py       |  31 +++
 basic/watch_robot.py    |  70 +++++++
 7 files changed, 823 insertions(+)
 create mode 100644 basic/README.md
 create mode 100644 basic/drive_to_aruco.py
 create mode 100644 basic/grid_route.py
 create mode 100644 basic/lift_rmc2.py
 create mode 100644 basic/move_rmc.py
 create mode 100644 basic/stop_all.py
 create mode 100644 basic/watch_robot.py

diff --git a/basic/README.md b/basic/README.md
new file mode 100644
index 0000000..32c3d1f
--- /dev/null
+++ b/basic/README.md
@@ -0,0 +1,86 @@
+# Базовые примеры для реального ROS 2
+
+Это короткие учебные скрипты, которые работают через штатные ROS 2 интерфейсы
+роверов: `cmd_vel`, `odometry`, `scan_front`, `aruco_id`, TF и `lift`. Имена
+сверены с выданной документацией физических РМК от 08.09.2026. В них нет
+прямого управления моторами и нет подмены симулятора. Один и тот же ROS API можно
+проверять в Webots, а затем перенести на физический РМК после сверки имён топиков,
+TF, размеров поля и безопасных скоростей.
+
+Это не готовые конкурсные решения и не замена аппаратному аварийному STOP.
+
+Перед запуском:
+
+```bash
+source /opt/ros/jazzy/setup.bash
+source ~/ros2_ws/install/setup.bash
+export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+```
+
+Сначала проверьте реальные интерфейсы:
+
+```bash
+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 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
+```
+
+Практические примеры:
+
+```bash
+# Проехать RMC2 от текущей метки до ArUco 14 на физическом ROS
+python3 basic/drive_to_aruco.py 14
+
+# То же в Webots; только здесь нужен --sim
+python3 basic/drive_to_aruco.py 14 --sim
+
+# Равнозначная форма с именованным аргументом
+python3 basic/drive_to_aruco.py --sim --target 14
+
+# Закрытые клетки и уменьшенная реальная скорость
+python3 basic/drive_to_aruco.py 14 --blocked 8 9 --max-speed 0.08
+
+# Короткая проверка движения; lidar обязателен, после 1 с будет stop
+python3 basic/move_rmc.py --robot RMC1 --x 0.15 --seconds 1
+
+# Поворот RMC2 на месте
+python3 basic/move_rmc.py --robot RMC2 --yaw 0.3 --seconds 1
+
+# Статус положения и лидара
+python3 basic/watch_robot.py --robot RMC1
+
+# Управление лифтом RMC2
+python3 basic/lift_rmc2.py up
+python3 basic/lift_rmc2.py down
+
+# Остановить оба ровера
+python3 basic/stop_all.py
+```
+
+`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` он отправляет
+нулевой `Twist` и завершает работу.
+
+На реальном поле сначала измерьте шаг сетки и при необходимости задайте
+`--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
new file mode 100644
index 0000000..d494450
--- /dev/null
+++ b/basic/drive_to_aruco.py
@@ -0,0 +1,409 @@
+#!/usr/bin/env python3
+"""Безопасно проехать на RMC по сетке до указанной ArUco-метки.
+
+Скрипт использует штатные ROS 2 топики и TF. По умолчанию рассчитан на
+физический робот; для Webots явно передайте --sim.
+"""
+
+import argparse
+import math
+import re
+import time
+from collections import deque
+
+import rclpy
+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 (
+    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
+
+
+def clamp(value, low, high):
+    return max(low, min(high, value))
+
+
+def wrap(angle):
+    return math.atan2(math.sin(angle), math.cos(angle))
+
+
+def yaw(quaternion):
+    return math.atan2(
+        2 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y),
+        1 - 2 * (quaternion.y**2 + quaternion.z**2),
+    )
+
+
+def compose(a, b):
+    """Перенести позу b из системы a в родительскую систему a."""
+    cosine, sine = math.cos(a[2]), math.sin(a[2])
+    return (
+        a[0] + cosine * b[0] - sine * b[1],
+        a[1] + sine * b[0] + cosine * b[1],
+        wrap(a[2] + b[2]),
+    )
+
+
+def inverse(pose):
+    cosine, sine = math.cos(pose[2]), math.sin(pose[2])
+    return (
+        -cosine * pose[0] - sine * pose[1],
+        sine * pose[0] - cosine * pose[1],
+        -pose[2],
+    )
+
+
+def marker_pose(marker, columns, spacing):
+    """Штатная схема ЧВТ: 0=(0,0), 1=(0,+S), COLS=(-S,0)."""
+    row, column = divmod(marker, columns)
+    return -row * spacing, column * spacing, 0.0
+
+
+def build_route(start, target, rows, columns, blocked):
+    queue = deque([start])
+    previous = {start: None}
+
+    while queue:
+        current = queue.popleft()
+        if current == target:
+            break
+        row, column = divmod(current, columns)
+        for next_row, next_column in (
+            (row - 1, column),
+            (row + 1, column),
+            (row, column - 1),
+            (row, column + 1),
+        ):
+            candidate = next_row * columns + next_column
+            if not (0 <= next_row < rows and 0 <= next_column < columns):
+                continue
+            if candidate in blocked or candidate in previous:
+                continue
+            previous[candidate] = current
+            queue.append(candidate)
+
+    if target not in previous:
+        raise RuntimeError(f"Нет маршрута {start} -> {target}")
+    route = []
+    current = target
+    while current is not None:
+        route.append(current)
+        current = previous[current]
+    return list(reversed(route))
+
+
+class ArucoDrive(Node):
+    def __init__(self, args):
+        super().__init__("mvch_basic_aruco_drive")
+        self.args = args
+        self.robot = args.robot.strip("/")
+        if args.sim:
+            self.set_parameters([Parameter("use_sim_time", value=True)])
+
+        self.odom = None
+        self.pose = None
+        self.map_from_odom = None
+        self.marker = None
+        self.marker_error = math.inf
+        self.front = None
+        self.odom_wall = 0.0
+        self.marker_wall = 0.0
+        self.scan_wall = 0.0
+
+        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
+        )
+        self.create_subscription(
+            Odometry,
+            f"/{self.robot}/odometry",
+            self.on_odometry,
+            qos_profile_sensor_data,
+        )
+        self.create_subscription(
+            LaserScan,
+            args.scan_topic or f"/{self.robot}/scan_front",
+            self.on_scan,
+            qos_profile_sensor_data,
+        )
+        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):
+        frame = message.header.frame_id.strip("/")
+        if frame:
+            self.odom_frame = frame
+        pose = message.pose.pose
+        values = (pose.position.x, pose.position.y, yaw(pose.orientation))
+        if all(math.isfinite(value) for value in values):
+            self.odom = values
+            self.odom_wall = time.monotonic()
+
+    def on_marker(self, message):
+        match = re.fullmatch(r"(?:aruco_)?(\d+)", message.data.strip())
+        if match:
+            marker = int(match.group(1))
+            if 0 <= marker < self.args.rows * self.args.columns:
+                self.marker = marker
+                self.marker_wall = time.monotonic()
+
+    def on_scan(self, message):
+        if not message.ranges or message.angle_increment == 0:
+            return
+        distances = []
+        for index, distance in enumerate(message.ranges):
+            angle = message.angle_min + index * message.angle_increment
+            if abs(angle) > math.radians(25):
+                continue
+            if math.isfinite(distance) and message.range_min <= distance <= message.range_max:
+                distances.append(distance)
+        # Пустое пространство lidar часто кодирует как inf. Это свежий скан,
+        # просто без препятствий в выбранном секторе.
+        self.front = min(distances, default=math.inf)
+        self.scan_wall = time.monotonic()
+
+    def localize(self):
+        """Привязать odometry к сетке по последней видимой метке."""
+        if self.odom is None or self.marker is None:
+            return
+        try:
+            transform = self.tf_buffer.lookup_transform(
+                self.odom_frame, f"aruco_{self.marker}", Time()
+            ).transform
+        except TransformException:
+            return
+
+        odom_marker = (
+            transform.translation.x,
+            transform.translation.y,
+            yaw(transform.rotation),
+        )
+        correction = compose(
+            marker_pose(self.marker, self.args.columns, self.args.spacing),
+            inverse(odom_marker),
+        )
+        new_pose = compose(correction, self.odom)
+        if self.pose is not None:
+            jump = math.hypot(new_pose[0] - self.pose[0], new_pose[1] - self.pose[1])
+            if jump > 0.35:
+                return
+        self.map_from_odom = correction
+        self.pose = new_pose
+        self.marker_error = math.hypot(
+            odom_marker[0] - self.odom[0], odom_marker[1] - self.odom[1]
+        )
+
+    def update(self):
+        rclpy.spin_once(self, timeout_sec=0.05)
+        self.localize()
+        if self.map_from_odom is not None and self.odom is not None:
+            self.pose = compose(self.map_from_odom, self.odom)
+
+    def stop(self):
+        self.publisher.publish(Twist())
+
+    def wait_until_ready(self):
+        print("Жду odometry, scan_front, aruco_id и TF метки...")
+        deadline = time.monotonic() + self.args.ready_timeout
+        while rclpy.ok() and time.monotonic() < deadline:
+            self.update()
+            self.stop()
+            now = time.monotonic()
+            if (
+                self.pose is not None
+                and self.front is not None
+                and now - self.odom_wall < 1.0
+                and now - self.scan_wall < 1.0
+                and now - self.marker_wall < 1.0
+            ):
+                return
+        raise RuntimeError(
+            "Датчики не готовы. Проверьте ros2 topic list, aruco_id и TF aruco_<ID>."
+        )
+
+    def check_safety(self):
+        now = time.monotonic()
+        if now - self.odom_wall > 1.0:
+            raise RuntimeError("Потеря odometry")
+        if now - self.scan_wall > 1.0:
+            raise RuntimeError("Потеря scan_front")
+        if self.front < self.args.stop_distance:
+            raise RuntimeError(
+                f"Препятствие спереди: {self.front:.2f} м "
+                f"(< {self.args.stop_distance:.2f} м)"
+            )
+
+    def drive_to(self, target):
+        target_pose = marker_pose(target, self.args.columns, self.args.spacing)
+        deadline = time.monotonic() + self.args.waypoint_timeout
+
+        while rclpy.ok() and time.monotonic() < deadline:
+            self.update()
+            self.check_safety()
+            dx = target_pose[0] - self.pose[0]
+            dy = target_pose[1] - self.pose[1]
+            distance = math.hypot(dx, dy)
+            marker_is_fresh = (
+                self.marker == target
+                and time.monotonic() - self.marker_wall < 1.0
+                and self.marker_error <= self.args.tolerance
+            )
+            if distance <= self.args.tolerance and marker_is_fresh:
+                self.stop()
+                print(f"Метка {target} достигнута, ошибка {self.marker_error:.3f} м")
+                return
+
+            angle_error = wrap(math.atan2(dy, dx) - self.pose[2])
+            command = Twist()
+            if abs(angle_error) > 0.15:
+                command.angular.z = clamp(
+                    1.4 * angle_error,
+                    -self.args.max_angular,
+                    self.args.max_angular,
+                )
+            else:
+                command.linear.x = min(self.args.max_speed, 0.5 * distance)
+                command.angular.z = clamp(
+                    1.2 * angle_error,
+                    -self.args.max_angular,
+                    self.args.max_angular,
+                )
+            self.publisher.publish(command)
+
+        raise RuntimeError(f"Таймаут движения к метке {target}")
+
+
+def arguments():
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("target", nargs="?", type=int, help="ID целевой ArUco")
+    parser.add_argument(
+        "--target",
+        dest="target_option",
+        type=int,
+        help="ID целевой ArUco (альтернатива позиционному аргументу)",
+    )
+    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="м/с")
+    parser.add_argument("--max-angular", type=float, default=0.30, help="рад/с")
+    parser.add_argument("--stop-distance", type=float, default=0.55, help="м")
+    parser.add_argument("--tolerance", type=float, default=0.06, help="м")
+    parser.add_argument("--ready-timeout", type=float, default=15.0, help="с")
+    parser.add_argument("--waypoint-timeout", type=float, default=45.0, help="с")
+    parser.add_argument("--scan-topic", help="Например /RMC2/scan_front")
+    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.target is not None and args.target_option is not None:
+        parser.error("укажите target один раз: позиционно или через --target")
+    if args.target is None:
+        args.target = args.target_option
+    if args.target is None:
+        parser.error("укажите ID цели: TARGET или --target TARGET")
+    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}")
+    if args.spacing <= 0 or args.max_speed <= 0 or args.max_angular <= 0:
+        parser.error("spacing и скорости должны быть больше нуля")
+    return args
+
+
+def main():
+    args = arguments()
+    rclpy.init()
+    node = ArucoDrive(args)
+    exit_code = 0
+    try:
+        node.wait_until_ready()
+        start = node.marker
+        blocked = set(args.blocked) - {start, args.target}
+        route = build_route(
+            start, args.target, args.rows, args.columns, blocked
+        )
+        print(f"Стартовая метка: {start}")
+        print("Маршрут:", " -> ".join(map(str, route)))
+        print(
+            f"Топики: /{node.robot}/cmd_vel, /{node.robot}/odometry, "
+            f"{node.aruco_topic}, {args.scan_topic or f'/{node.robot}/scan_front'}"
+        )
+        if not args.yes:
+            input("Проверьте поле и держите аппаратный STOP. Enter — начать: ")
+            # Во время input ROS не обрабатывал сообщения: перед движением снова
+            # получить свежие odometry/lidar/ArUco.
+            node.wait_until_ready()
+        for marker in route[1:]:
+            node.drive_to(marker)
+        node.stop()
+        print(f"FINISHED: ровер стоит над ArUco {args.target}")
+    except (KeyboardInterrupt, RuntimeError) as error:
+        exit_code = 1
+        print(f"STOP: {error}")
+    finally:
+        for _ in range(10):
+            node.stop()
+            rclpy.spin_once(node, timeout_sec=0.02)
+        node.destroy_node()
+        rclpy.shutdown()
+    raise SystemExit(exit_code)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/basic/grid_route.py b/basic/grid_route.py
new file mode 100644
index 0000000..867b3f7
--- /dev/null
+++ b/basic/grid_route.py
@@ -0,0 +1,69 @@
+#!/usr/bin/env python3
+"""Строит кратчайший маршрут по регулярной сетке ArUco без ROS."""
+
+import argparse
+from collections import deque
+
+
+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
+
+
+def build_route(start: int, target: int, blocked: set[int], size: int):
+    queue = deque([start])
+    previous = {start: None}
+
+    while queue:
+        current = queue.popleft()
+        if current == target:
+            break
+        for candidate in neighbors(current, size):
+            if candidate not in blocked and candidate not in previous:
+                previous[candidate] = current
+                queue.append(candidate)
+
+    if target not in previous:
+        return None
+
+    route = []
+    current = target
+    while current is not None:
+        route.append(current)
+        current = previous[current]
+    return list(reversed(route))
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__)
+    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,
+        nargs="*",
+        default=[],
+        help="Маркеры, через которые нельзя ехать",
+    )
+    args = parser.parse_args()
+
+    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, args.size)
+    if route is None:
+        raise SystemExit("Маршрут не найден")
+    print(" -> ".join(map(str, route)))
+
+
+if __name__ == "__main__":
+    main()
diff --git a/basic/lift_rmc2.py b/basic/lift_rmc2.py
new file mode 100644
index 0000000..b49b182
--- /dev/null
+++ b/basic/lift_rmc2.py
@@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+"""Поднимает или опускает лифт RMC2 через штатный ROS-топик."""
+
+import argparse
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
+from std_msgs.msg import Float64, String
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("state", choices=("up", "down"))
+    parser.add_argument("--wait", type=float, default=6.0, help="Сколько ждать статус, с")
+    args = parser.parse_args()
+
+    rclpy.init()
+    node = Node("mvch_basic_lift")
+    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.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:
+            rclpy.spin_once(node, timeout_sec=0.1)
+            if statuses and statuses[-1].strip().lower() == expected:
+                break
+
+        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/basic/move_rmc.py b/basic/move_rmc.py
new file mode 100644
index 0000000..5efc50e
--- /dev/null
+++ b/basic/move_rmc.py
@@ -0,0 +1,93 @@
+#!/usr/bin/env python3
+"""Подаёт ограниченную по времени ROS-команду скорости и затем останавливает RMC."""
+
+import argparse
+import math
+import time
+
+import rclpy
+from geometry_msgs.msg import Twist
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import LaserScan
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--robot", choices=("RMC1", "RMC2"), required=True)
+    parser.add_argument("--x", type=float, default=0.0, help="Скорость вперёд, м/с")
+    parser.add_argument("--y", type=float, default=0.0, help="Боковая скорость RMC1, м/с")
+    parser.add_argument("--yaw", type=float, default=0.0, help="Угловая скорость, рад/с")
+    parser.add_argument("--seconds", type=float, default=1.0)
+    parser.add_argument("--stop-distance", type=float, default=0.55, help="Стоп по lidar, м")
+    parser.add_argument("--no-lidar", action="store_true", help="Только стенд без препятствий")
+    args = parser.parse_args()
+
+    if args.seconds <= 0:
+        parser.error("--seconds должен быть больше нуля")
+    if args.robot == "RMC2" and args.y != 0:
+        parser.error("RMC2 не поддерживает боковое движение: используйте --y 0")
+
+    rclpy.init()
+    node = Node("mvch_basic_move")
+    publisher = node.create_publisher(Twist, f"/{args.robot}/cmd_vel", 10)
+    front_distance = [None]
+
+    def on_scan(message):
+        if not message.ranges or message.angle_increment == 0:
+            return
+        front = []
+        for index, distance in enumerate(message.ranges):
+            angle = message.angle_min + index * message.angle_increment
+            if abs(angle) <= 0.44 and math.isfinite(distance):
+                if message.range_min <= distance <= message.range_max:
+                    front.append(distance)
+        front_distance[0] = min(front, default=math.inf)
+
+    if not args.no_lidar:
+        node.create_subscription(
+            LaserScan,
+            f"/{args.robot}/scan_front",
+            on_scan,
+            qos_profile_sensor_data,
+        )
+    command = Twist()
+    command.linear.x = args.x
+    command.linear.y = args.y
+    command.angular.z = args.yaw
+
+    print(f"{args.robot}: cmd_vel x={args.x}, y={args.y}, yaw={args.yaw}")
+    if not args.no_lidar:
+        print(f"Жду /{args.robot}/scan_front...")
+        deadline = time.monotonic() + 3.0
+        while front_distance[0] is None and time.monotonic() < deadline:
+            rclpy.spin_once(node, timeout_sec=0.1)
+        if front_distance[0] is None:
+            node.destroy_node()
+            rclpy.shutdown()
+            raise SystemExit("Нет scan_front: движение запрещено. Для стенда: --no-lidar")
+    started = time.monotonic()
+    exit_code = 0
+    try:
+        while time.monotonic() - started < args.seconds:
+            if front_distance[0] is not None and front_distance[0] < args.stop_distance:
+                raise RuntimeError(
+                    f"Препятствие {front_distance[0]:.2f} м — аварийный стоп"
+                )
+            publisher.publish(command)
+            rclpy.spin_once(node, timeout_sec=0.05)
+    except RuntimeError as error:
+        exit_code = 1
+        print(f"STOP: {error}")
+    finally:
+        stop = Twist()
+        for _ in range(5):
+            publisher.publish(stop)
+            rclpy.spin_once(node, timeout_sec=0.05)
+        node.destroy_node()
+        rclpy.shutdown()
+    raise SystemExit(exit_code)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/basic/stop_all.py b/basic/stop_all.py
new file mode 100644
index 0000000..43c1a60
--- /dev/null
+++ b/basic/stop_all.py
@@ -0,0 +1,31 @@
+#!/usr/bin/env python3
+"""Публикует нулевую скорость обоим роверам."""
+
+import time
+
+import rclpy
+from geometry_msgs.msg import Twist
+from rclpy.node import Node
+
+
+def main():
+    rclpy.init()
+    node = Node("mvch_basic_stop_all")
+    publishers = [
+        node.create_publisher(Twist, "/RMC1/cmd_vel", 10),
+        node.create_publisher(Twist, "/RMC2/cmd_vel", 10),
+    ]
+    stop = Twist()
+    for _ in range(10):
+        for publisher in publishers:
+            publisher.publish(stop)
+        rclpy.spin_once(node, timeout_sec=0.05)
+        time.sleep(0.02)
+    print("Нулевая скорость отправлена RMC1 и RMC2")
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
+
diff --git a/basic/watch_robot.py b/basic/watch_robot.py
new file mode 100644
index 0000000..6c9bc7d
--- /dev/null
+++ b/basic/watch_robot.py
@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+"""Показывает одометрию и минимальную дистанцию перед ровером."""
+
+import argparse
+import math
+import time
+
+import rclpy
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import LaserScan
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--robot", choices=("RMC1", "RMC2"), required=True)
+    args = parser.parse_args()
+
+    rclpy.init()
+    node = Node("mvch_basic_watch")
+    state = {"pose": None, "front": None}
+
+    def on_odom(message):
+        position = message.pose.pose.position
+        orientation = message.pose.pose.orientation
+        siny = 2.0 * (orientation.w * orientation.z + orientation.x * orientation.y)
+        cosy = 1.0 - 2.0 * (orientation.y**2 + orientation.z**2)
+        state["pose"] = (position.x, position.y, math.atan2(siny, cosy))
+
+    def on_scan(message):
+        front = []
+        for index, distance in enumerate(message.ranges):
+            angle = message.angle_min + index * message.angle_increment
+            if abs(angle) <= math.radians(20) and math.isfinite(distance):
+                if message.range_min <= distance <= message.range_max:
+                    front.append(distance)
+        state["front"] = min(front, default=math.inf)
+
+    node.create_subscription(
+        Odometry, f"/{args.robot}/odometry", on_odom, qos_profile_sensor_data
+    )
+    node.create_subscription(
+        LaserScan, f"/{args.robot}/scan_front", on_scan, qos_profile_sensor_data
+    )
+
+    print(f"Слушаю {args.robot}. Ctrl+C — выход.")
+    last_print = 0.0
+    try:
+        while rclpy.ok():
+            rclpy.spin_once(node, timeout_sec=0.1)
+            if time.monotonic() - last_print >= 0.5:
+                pose = state["pose"]
+                front = state["front"]
+                pose_text = "нет odometry" if pose is None else (
+                    f"x={pose[0]:.2f}, y={pose[1]:.2f}, yaw={math.degrees(pose[2]):.1f}°"
+                )
+                front_text = "нет scan_front" if front is None else f"{front:.2f} м"
+                print(f"{pose_text}; спереди: {front_text}")
+                last_print = time.monotonic()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
+