From 21ee4b8b2676d9a0cb6618e36af99ef321d23bc7 Mon Sep 17 00:00:00 2001 From: mavxa Date: Fri, 11 Sep 2026 23:19:19 +0300 Subject: [PATCH] fix ROS profiles and simulator reliability --- README.md | 7 +++++++ module2/module_b.py | 27 ++++++++++++++++--------- module3/capture_dataset.py | 4 +--- module3/module3.py | 4 +--- module3/prepare_dataset.py | 4 +--- module3/test_model.py | 4 +--- module3/train.py | 4 +--- module4/README.md | 7 ++++++- module4/server/commands.ts | 2 +- module4/server/ros_bridge.py | 18 ++++++++--------- module4/server/state.test.ts | 4 ++-- module4/src/App.tsx | 2 +- module4/src/types.ts | 2 +- module5/module5.py | 38 +++++++++++++++++++++++++----------- 14 files changed, 77 insertions(+), 50 deletions(-) diff --git a/README.md b/README.md index e699d8d..8b37598 100644 --- a/README.md +++ b/README.md @@ -76,6 +76,13 @@ RMC2 до 0.870 м. С `--sim` остаётся прежний профиль We сетки, тормозной путь и аппаратную аварийную остановку. Документ не сообщает шаг между маркерами и точные имена TF-фреймов. Испытания в Webots не подтверждают готовность к реальному полю. +Если значения отличаются, код менять не нужно: + +```bash +python3 module2/module_b.py --target 14 --rows 5 --columns 5 --spacing 1.0 \ + --base-frame RMC2/base_link --odom-frame RMC2/odom +``` + У штатного Webots-драйвера нет таймаута последней команды: при SIGKILL/обрыве питания процесса программная остановка не гарантируется; остановить симулятор вручную. diff --git a/module2/module_b.py b/module2/module_b.py index f123e5f..c1b0acd 100644 --- a/module2/module_b.py +++ b/module2/module_b.py @@ -1,10 +1,4 @@ #!/usr/bin/env python3 -"""Модуль Б: один запуск, маршрут туда, пауза эксперта, маршрут обратно. - -python3 module_b.py --target 14 --sim -Команды в терминале: Enter / go, stop, resume, return, quit. -""" - import argparse import heapq import json @@ -138,11 +132,16 @@ class Field: def arguments(): - parser = argparse.ArgumentParser(description=__doc__) + parser = argparse.ArgumentParser() target = parser.add_mutually_exclusive_group(required=True) target.add_argument("--target", type=int, help="ID целевой метки") target.add_argument("--xy", nargs=2, type=float, help="Координаты цели на карте, метры") parser.add_argument("--sim", action="store_true", help="Часы Webots /clock") + parser.add_argument("--rows", type=int, help="Число строк ArUco-сетки") + parser.add_argument("--columns", type=int, help="Число столбцов ArUco-сетки") + parser.add_argument("--spacing", type=float, help="Шаг между ArUco, м") + parser.add_argument("--base-frame", help="TF-фрейм корпуса RMC2") + parser.add_argument("--odom-frame", help="TF-фрейм одометрии RMC2") parser.add_argument("--target-yaw", type=float, help="Ориентация на цели в системе поля, рад") parser.add_argument("--auto", action="store_true", help="Только тренировка: без команд эксперта") parser.add_argument("--return-delay", type=float, default=5.0, help="Пауза --auto, секунд симуляции") @@ -157,12 +156,21 @@ def arguments(): parser.error("--target-yaw должен быть конечным числом") if args.xy and not all(math.isfinite(v) for v in args.xy): parser.error("--xy должен содержать конечные числа") + if args.rows is not None and args.rows < 1: + parser.error("--rows должен быть больше нуля") + if args.columns is not None and args.columns < 1: + parser.error("--columns должен быть больше нуля") + if args.spacing is not None and (args.spacing <= 0 or not math.isfinite(args.spacing)): + parser.error("--spacing должен быть конечным положительным числом") return args def main(): args = arguments() - rows, columns, spacing = SIM_FIELD if args.sim else REAL_FIELD + default_rows, default_columns, default_spacing = SIM_FIELD if args.sim else REAL_FIELD + rows = args.rows if args.rows is not None else default_rows + columns = args.columns if args.columns is not None else default_columns + spacing = args.spacing if args.spacing is not None else default_spacing half_size = SIM_HALF_SIZE if args.sim else REAL_HALF_SIZE field = Field(rows, columns, spacing, half_size+SAFETY_MARGIN) target = args.target if args.target is not None else field.nearest(*args.xy) @@ -218,7 +226,8 @@ def main(): "safety_margin": SAFETY_MARGIN, } ns = ROBOT_NAMESPACE.strip("/") - self.base, self.odom_frame = f"{ns}/base_link", f"{ns}/odom" + self.base = args.base_frame or f"{ns}/base_link" + self.odom_frame = args.odom_frame or f"{ns}/odom" self.buffer = Buffer() self.listener = TransformListener(self.buffer, self) if not args.sim: diff --git a/module3/capture_dataset.py b/module3/capture_dataset.py index 111c816..6b6a182 100755 --- a/module3/capture_dataset.py +++ b/module3/capture_dataset.py @@ -1,6 +1,4 @@ #!/usr/bin/env python3 -"""Сохраняет непохожие кадры с камеры захвата RMC1 для разметки.""" - import argparse import time from datetime import datetime @@ -11,7 +9,7 @@ HERE = Path(__file__).resolve().parent def arguments(): - parser = argparse.ArgumentParser(description=__doc__) + parser = argparse.ArgumentParser() parser.add_argument("--sim", action="store_true", help="Использовать камеру Webots") parser.add_argument( "--topic", diff --git a/module3/module3.py b/module3/module3.py index a0893bf..ae5c8eb 100644 --- a/module3/module3.py +++ b/module3/module3.py @@ -1,6 +1,4 @@ #!/usr/bin/env python3 -"""Модуль В: распознать деталь, взять ARM95 и вернуть на полку.""" - import argparse import json import math @@ -46,7 +44,7 @@ def target_name(value): def arguments(): - parser = argparse.ArgumentParser(description=__doc__) + parser = argparse.ArgumentParser() parser.add_argument("--target", help="1/hammer, 2/wrench или 3/pliers") parser.add_argument("--weights", default="models/latest.pt") parser.add_argument("--sim", action="store_true", help="Камера/TF и часы Webots") diff --git a/module3/prepare_dataset.py b/module3/prepare_dataset.py index 18a43c0..e8f92ef 100644 --- a/module3/prepare_dataset.py +++ b/module3/prepare_dataset.py @@ -1,6 +1,4 @@ #!/usr/bin/env python3 -"""Преобразует polygon-разметку Roboflow в обычные YOLO bounding boxes.""" - import argparse from pathlib import Path @@ -9,7 +7,7 @@ HERE = Path(__file__).resolve().parent def arguments(): - parser = argparse.ArgumentParser(description=__doc__) + parser = argparse.ArgumentParser() parser.add_argument("--dataset", default="training", help="Каталог Roboflow export") return parser.parse_args() diff --git a/module3/test_model.py b/module3/test_model.py index 5c1d922..01c6433 100755 --- a/module3/test_model.py +++ b/module3/test_model.py @@ -1,6 +1,4 @@ #!/usr/bin/env python3 -"""Рисует bbox, название класса и confidence модели на фотографии.""" - import argparse from pathlib import Path @@ -28,7 +26,7 @@ def select_device(requested, torch): def arguments(): - parser = argparse.ArgumentParser(description=__doc__) + parser = argparse.ArgumentParser() parser.add_argument("source", help="Фотография для проверки") parser.add_argument("--weights", default="models/latest.pt") parser.add_argument("--output", default="predictions", help="Каталог результата") diff --git a/module3/train.py b/module3/train.py index ee992c5..b4aa17e 100755 --- a/module3/train.py +++ b/module3/train.py @@ -1,6 +1,4 @@ #!/usr/bin/env python3 -"""Обучение YOLO11 на размеченном датасете инструментов.""" - import argparse import shutil from pathlib import Path @@ -32,7 +30,7 @@ def select_device(requested, torch): def arguments(): - parser = argparse.ArgumentParser(description=__doc__) + parser = argparse.ArgumentParser() parser.add_argument("--data", default="training/data.yaml", help="data.yaml из Roboflow") parser.add_argument("--model", default="yolo11n.pt", help="Стартовые веса Ultralytics") parser.add_argument("--epochs", type=int, default=30) diff --git a/module4/README.md b/module4/README.md index 976185d..280d220 100644 --- a/module4/README.md +++ b/module4/README.md @@ -105,7 +105,12 @@ WebSocket и компоновки интерфейса. `/RMC2/camera_bottom/aruco_id` (и старый `/RMC2/aruco_id` для Webots), а также `/RMC2/lift_status`. По документации реального RMC2 статус лифта имеет QoS `Transient Local` и значения `lowered`, `moving`, `raised`; команды — `0.0` вниз -и `0.05` м вверх. Мост настроен именно так. +и `0.05` м вверх. В Webots верхнее положение — `0.1` м, поэтому это значение +используется по умолчанию. Для физического RMC2 запускайте мост так: + +```bash +MVCH_LIFT_UP_HEIGHT=0.05 bun run start +``` Симулятор из открытого пакета не публикует напряжение аккумуляторов. Мост уже подписан на `/RMC*/battery_state` (`sensor_msgs/BatteryState`) и diff --git a/module4/server/commands.ts b/module4/server/commands.ts index d0b83a0..322cbce 100644 --- a/module4/server/commands.ts +++ b/module4/server/commands.ts @@ -33,7 +33,7 @@ export function isCommand(value: unknown): value is Command { case 'set_goal': return isPose(command.pose) case 'lift': - return command.robot === 'RMC2' && (command.height === 0 || command.height === 0.05) + return command.robot === 'RMC2' && (command.height === 0 || command.height === 0.05 || command.height === 0.1) case 'gripper': return command.robot === 'RMC1' && (command.state === 'open' || command.state === 'closed') default: diff --git a/module4/server/ros_bridge.py b/module4/server/ros_bridge.py index 888b942..526e484 100644 --- a/module4/server/ros_bridge.py +++ b/module4/server/ros_bridge.py @@ -1,11 +1,4 @@ #!/usr/bin/env python3 -"""Small JSON <-> ROS 2 bridge for the module G web interface. - -stdin: one JSON command per line -stdout: one JSON state per line (only machine-readable data) -stderr: diagnostics -""" - import json import math import os @@ -35,6 +28,9 @@ except ImportError: ROBOTS = ("RMC1", "RMC2") MANUAL_TIMEOUT = 0.35 +LIFT_UP_HEIGHT = float(os.environ.get("MVCH_LIFT_UP_HEIGHT", "0.1")) +if LIFT_UP_HEIGHT not in (0.05, 0.1): + raise SystemExit("MVCH_LIFT_UP_HEIGHT должен быть 0.05 или 0.1") def empty_robot(): @@ -163,7 +159,11 @@ class FmsBridge(Node): self.create_subscription(String, "/RMC2/aruco_id", self.on_aruco, sensor_qos) lift_qos = QoSProfile( reliability=ReliabilityPolicy.RELIABLE, - durability=DurabilityPolicy.TRANSIENT_LOCAL, + durability=( + DurabilityPolicy.TRANSIENT_LOCAL + if LIFT_UP_HEIGHT == 0.05 + else DurabilityPolicy.VOLATILE + ), history=HistoryPolicy.KEEP_LAST, depth=1, ) @@ -349,7 +349,7 @@ class FmsBridge(Node): self.state["robots"][robot]["goal"] = None self.state["robots"][robot]["plan"] = [] elif kind == "lift" and robot == "RMC2" and not emergency: - height = 0.05 if float(command.get("height", 0)) >= 0.025 else 0.0 + height = LIFT_UP_HEIGHT if float(command.get("height", 0)) > 0 else 0.0 self.lift_publisher.publish(Float64(data=height)) with self.lock: self.state["robots"][robot]["liftStatus"] = "command: up" if height else "command: down" diff --git a/module4/server/state.test.ts b/module4/server/state.test.ts index d6bef88..97a9e3e 100644 --- a/module4/server/state.test.ts +++ b/module4/server/state.test.ts @@ -5,7 +5,7 @@ import { applyMockCommand, initialState, tickMock } from './state' describe('command validation', () => { test('accepts valid commands', () => { expect(isCommand({ type: 'manual', robot: 'RMC1', x: 0.4, y: 0.2, yaw: 0 })).toBe(true) - expect(isCommand({ type: 'lift', robot: 'RMC2', height: 0.05 })).toBe(true) + expect(isCommand({ type: 'lift', robot: 'RMC2', height: 0.1 })).toBe(true) expect(isCommand({ type: 'gripper', robot: 'RMC1', state: 'closed' })).toBe(true) }) @@ -23,7 +23,7 @@ describe('mock state', () => { tickMock(state, 1) expect(state.robots.RMC2.pose.x).toBeCloseTo(0.5) - applyMockCommand(state, { type: 'lift', robot: 'RMC2', height: 0.05 }) + applyMockCommand(state, { type: 'lift', robot: 'RMC2', height: 0.1 }) applyMockCommand(state, { type: 'emergency', robot: 'RMC2', active: true }) expect(state.robots.RMC2.velocity.x).toBe(0) expect(state.robots.RMC2.liftStatus).toBe('up') diff --git a/module4/src/App.tsx b/module4/src/App.tsx index 88802f5..5769621 100644 --- a/module4/src/App.tsx +++ b/module4/src/App.tsx @@ -387,7 +387,7 @@ export default function App() { {selected === 'RMC1' ? (
) : ( -
+
)} diff --git a/module4/src/types.ts b/module4/src/types.ts index 2b560d2..81b44b1 100644 --- a/module4/src/types.ts +++ b/module4/src/types.ts @@ -45,5 +45,5 @@ export type Command = | { type: 'set_pose'; robot: RobotId; pose: Pose } | { type: 'set_goal'; robot: RobotId; pose: Pose } | { type: 'cancel_goal'; robot: RobotId } - | { type: 'lift'; robot: 'RMC2'; height: 0 | 0.05 } + | { type: 'lift'; robot: 'RMC2'; height: 0 | 0.05 | 0.1 } | { type: 'gripper'; robot: 'RMC1'; state: 'open' | 'closed' } diff --git a/module5/module5.py b/module5/module5.py index 2113fd3..939f44f 100644 --- a/module5/module5.py +++ b/module5/module5.py @@ -1,6 +1,4 @@ #!/usr/bin/env python3 -"""Модуль Д: один скрипт для складского кейса RMC1 + RMC2.""" - import argparse import json import math @@ -188,7 +186,7 @@ class Anchor: def arguments(): global ROWS, COLS, MARKER_SPACING - parser = argparse.ArgumentParser(description=__doc__) + parser = argparse.ArgumentParser() parser.add_argument("--target", help="1/hammer, 2/wrench, 3/pliers") parser.add_argument("--real", action="store_true", help="Физическое поле 5x5 и реальные ROS-топики") parser.add_argument("--spacing", type=float, default=1.0, help="Шаг ArUco-сетки, м") @@ -225,7 +223,7 @@ def arguments(): parser.add_argument("--drop-arm-z", type=float, default=0.220) parser.add_argument("--drop-arm-yaw", type=float, default=math.pi / 2) parser.add_argument("--sensor-timeout", type=float, default=3.0) - parser.add_argument("--waypoint-timeout", type=float, default=35.0) + parser.add_argument("--waypoint-timeout", type=float, default=120.0) args = parser.parse_args() if args.spacing <= 0: parser.error("--spacing должен быть больше нуля") @@ -336,11 +334,12 @@ def build_ros_node(args, event_log): ReliabilityPolicy, qos_profile_sensor_data, ) + from rclpy.signals import SignalHandlerOptions from sensor_msgs.msg import LaserScan from std_msgs.msg import Bool, Float64, String if not rclpy.ok(): - rclpy.init() + rclpy.init(signal_handler_options=SignalHandlerOptions.NO) class WarehouseNode(Node): def __init__(self): @@ -376,7 +375,11 @@ def build_ros_node(args, event_log): lift_qos = QoSProfile( depth=1, reliability=ReliabilityPolicy.RELIABLE, - durability=DurabilityPolicy.TRANSIENT_LOCAL, + durability=( + DurabilityPolicy.TRANSIENT_LOCAL + if args.real + else DurabilityPolicy.VOLATILE + ), ) self.create_subscription( String, "/RMC2/lift_status", self.on_lift, lift_qos @@ -465,7 +468,7 @@ def build_ros_node(args, event_log): time.sleep(0.04) def set_lift(self, raised, allow_already=False): - height = 0.05 if raised else 0.0 + height = (0.05 if args.real else 0.1) if raised else 0.0 expected = "raised" if raised else "lowered" event_log.write("LIFT_COMMAND", state="up" if raised else "down", height=height) with self.lock: @@ -475,7 +478,7 @@ def build_ros_node(args, event_log): time.sleep(0.1) started = time.monotonic() success = False - while time.monotonic() - started < 6.0: + while time.monotonic() - started < 20.0: with self.lock: status = self.lift_status received_update = self.lift_updates > updates_before @@ -556,10 +559,19 @@ class Motion: carrying=carrying, allow_rack_entry=(rack_entry and index == len(route) - 1) or (rack_exit and index == 1), + precise=rack_entry and index == len(route) - 2, ) self.log.write("ROUTE_DONE", robot=robot, stage=name, marker=route[-1]) - def drive_to(self, robot, marker, target, carrying=False, allow_rack_entry=False): + def drive_to( + self, + robot, + marker, + target, + carrying=False, + allow_rack_entry=False, + precise=False, + ): started = time.monotonic() obstacle_since = None while time.monotonic() - started < self.args.waypoint_timeout: @@ -595,8 +607,10 @@ class Motion: obstacle_since = None yaw_error = wrap_angle(target_yaw - yaw) - if distance < 0.09: - if abs(yaw_error) < 0.10: + position_tolerance = 0.03 if precise else 0.09 + yaw_tolerance = 0.04 if precise else 0.10 + if distance < position_tolerance: + if abs(yaw_error) < yaw_tolerance: self.node.command(robot, immediate=True) self.log.write( "WAYPOINT", robot=robot, marker=marker, error=round(distance, 3) @@ -625,6 +639,8 @@ class Motion: ) else: maximum = 0.18 if carrying else 0.27 + if allow_rack_entry: + maximum = min(maximum, 0.10) self.node.command( robot, x=min(maximum, max(0.07, distance * 0.55)),