diff --git a/README.md b/README.md index 34f4a99..e699d8d 100644 --- a/README.md +++ b/README.md @@ -61,9 +61,21 @@ python3 module2/module_b.py --target 14 --sim --auto --return-delay 10 python3 module2/module_b.py --xy -2 2 --target-yaw 1.57 --sim ``` -На физическом роботе `--sim` не указывать. Перед переносом обязательно проверить -размеры корпуса, тормозной путь, TF, шаг и ориентации графа, датчики и аппаратную -аварийную остановку. Испытания в Webots не подтверждают готовность к реальному полю. +На физическом роботе `--sim` не указывать: + +```bash +python3 module2/module_b.py --target 14 +``` + +Тогда используется подтверждённый документацией профиль: поле 5×5 (ID 0–24), +ArUco `/RMC2/camera_bottom/aruco_id`, TF `/RMC2/tf` и `/RMC2/tf_static`, габарит +RMC2 до 0.870 м. С `--sim` остаётся прежний профиль Webots 6×6 (ID 0–35), +`/RMC2/aruco_id` и глобальные `/tf`/`/tf_static`. + +Перед физическим заездом всё равно проверить частоту топиков, имена frame_id, шаг +сетки, тормозной путь и аппаратную аварийную остановку. Документ не сообщает шаг +между маркерами и точные имена TF-фреймов. Испытания в Webots не подтверждают +готовность к реальному полю. У штатного Webots-драйвера нет таймаута последней команды: при SIGKILL/обрыве питания процесса программная остановка не гарантируется; остановить симулятор вручную. @@ -74,7 +86,8 @@ python3 module2/module_b.py --xy -2 2 --target-yaw 1.57 --sim Читайте в таком порядке: `Field.route` → `localize` → `scan_points` → `plan` → `drive` → `tick`. -Координаты штатного поля: `0=(0,0)`, `6=(-1,0)`, `1=(0,1)`. +Координаты реального поля: `0=(0,0)`, `5=(-1,0)`, `1=(0,1)`; в Webots 6×6: +`0=(0,0)`, `6=(-1,0)`, `1=(0,1)`. Поворот `yaw` каждой метки — ориентация её TF-системы относительно поля. Измеренное положение метки в odom сравнивается с её известным положением на поле; так получаем преобразование `map_from_odom`. Между метками движение идёт по одометрии. @@ -88,13 +101,14 @@ python3 module2/module_b.py --xy -2 2 --target-yaw 1.57 --sim к предыдущему маркеру по своему ребру и публикует новый маршрут `ROUTE_REPLANNED`. Если отступить безопасно нельзя, он стоит и завершает попытку по таймауту. -Размеры сетки, шаг и известные занятые клетки задаются константами `ROWS`, `COLS`, -`MARKER_SPACING`, `BLOCKED_MARKERS` в начале скрипта. Все длины — метры. +Профили сетки находятся в `REAL_FIELD` и `SIM_FIELD`, известные занятые клетки — +в `BLOCKED_MARKERS`. Все длины — метры. ## Проверки и логи ```bash -python3 module2/module_b.py --target 14 --dry-run +python3 module2/module_b.py --target 14 --dry-run # реальное поле 5x5 +python3 module2/module_b.py --target 35 --sim --dry-run # Webots 6x6 ``` `reports/module_b_*.jsonl` содержат маршрут, движение, ошибки, препятствия, diff --git a/module2/module_b.py b/module2/module_b.py index 0001f4f..f123e5f 100644 --- a/module2/module_b.py +++ b/module2/module_b.py @@ -19,20 +19,17 @@ from datetime import datetime from pathlib import Path -# Все параметры модуля Б находятся здесь. Перед реальным полем меняются в одном месте. -ROWS = 6 -COLS = 6 -MARKER_SPACING = 1.0 +# Профили поля из симулятора и документации физических РМК от 08.09.2026. +SIM_FIELD = (6, 6, 1.0) +REAL_FIELD = (5, 5, 1.0) BLOCKED_MARKERS = set() ROBOT_NAMESPACE = "RMC2" MARKER_PREFIX = "aruco_" -# RMC2.proto: край корпуса находится примерно в 0.373 м от base_link. -# Четыре сантиметра запаса оставляют проходимым узкий коридор у marker 33. -ROBOT_HALF_LENGTH = 0.375 -ROBOT_HALF_WIDTH = 0.375 +# В Webots край корпуса около 0.375 м. Реальный RMC2 имеет габарит до 0.870 м. +SIM_HALF_SIZE = 0.375 +REAL_HALF_SIZE = 0.435 SAFETY_MARGIN = 0.04 -ROUTE_CLEARANCE = 0.44 MAX_LINEAR = 0.25 MAX_ANGULAR = 0.45 @@ -84,17 +81,21 @@ def segment_distance(point, a, b): class Field: """Регулярная сетка ArUco и кратчайший путь по ней.""" - def __init__(self): + def __init__(self, rows, columns, spacing, clearance): + self.rows = rows + self.columns = columns + self.spacing = spacing + self.clearance = clearance self.poses = {} self.graph = {} - for row in range(ROWS): - for col in range(COLS): - marker = row*COLS+col - self.poses[marker] = (-row*MARKER_SPACING, col*MARKER_SPACING, 0.0) + for row in range(rows): + for col in range(columns): + marker = row*columns+col + self.poses[marker] = (-row*spacing, col*spacing, 0.0) self.graph[marker] = [ - r*COLS+c for r, c in + r*columns+c for r, c in ((row+1, col), (row, col+1), (row-1, col), (row, col-1)) - if 0 <= r < ROWS and 0 <= c < COLS + if 0 <= r < rows and 0 <= c < columns ] def nearest(self, x, y): @@ -126,7 +127,7 @@ class Field: if neighbor in BLOCKED_MARKERS or edge in forbidden: continue a, b = self.poses[current], self.poses[neighbor] - if any(segment_distance(p, a, b) < ROUTE_CLEARANCE for p in obstacle_points): + if any(segment_distance(p, a, b) < self.clearance for p in obstacle_points): continue new_cost = cost+distance(a, b) if new_cost < costs.get(neighbor, math.inf): @@ -161,7 +162,9 @@ def arguments(): def main(): args = arguments() - field = Field() + rows, columns, spacing = SIM_FIELD if args.sim else REAL_FIELD + 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) if target not in field.poses or target in BLOCKED_MARKERS: raise SystemExit("Целевая метка отсутствует в графе или заблокирована") @@ -174,11 +177,17 @@ def main(): from nav_msgs.msg import Odometry from rclpy.clock import Clock, ClockType from rclpy.node import Node - from rclpy.qos import qos_profile_sensor_data + from rclpy.qos import ( + DurabilityPolicy, + QoSProfile, + ReliabilityPolicy, + qos_profile_sensor_data, + ) from rclpy.signals import SignalHandlerOptions from rclpy.time import Time from sensor_msgs.msg import LaserScan from std_msgs.msg import Bool, String + from tf2_msgs.msg import TFMessage from tf2_ros import Buffer, TransformListener, TransformException def yaw(q): @@ -203,18 +212,41 @@ def main(): } self.robot = { "namespace": ROBOT_NAMESPACE, "marker_prefix": MARKER_PREFIX, - "clearance": ROUTE_CLEARANCE, "half_length": ROBOT_HALF_LENGTH, - "half_width": ROBOT_HALF_WIDTH, "safety_margin": SAFETY_MARGIN, + "clearance": half_size+SAFETY_MARGIN, + "half_length": half_size, + "half_width": half_size, + "safety_margin": SAFETY_MARGIN, } ns = ROBOT_NAMESPACE.strip("/") self.base, self.odom_frame = f"{ns}/base_link", f"{ns}/odom" self.buffer = Buffer() self.listener = TransformListener(self.buffer, self) + if not args.sim: + # На физических РМК TF опубликован в namespaced-топиках. + 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"/{ns}/tf", self.on_tf, tf_qos + ) + self.create_subscription( + TFMessage, f"/{ns}/tf_static", self.on_tf_static, static_tf_qos + ) self.pub = self.create_publisher(Twist, f"/{ns}/cmd_vel", 1) self.status_pub = self.create_publisher(String, "/mvch/status", 10) self.create_subscription(Odometry, f"/{ns}/odometry", self.on_odom, qos_profile_sensor_data) self.create_subscription(LaserScan, f"/{ns}/scan", self.on_scan, qos_profile_sensor_data) - self.create_subscription(String, f"/{ns}/aruco_id", self.on_marker, 10) + aruco_topic = ( + f"/{ns}/aruco_id" if args.sim else f"/{ns}/camera_bottom/aruco_id" + ) + self.create_subscription(String, aruco_topic, self.on_marker, 10) self.create_subscription(Bool, "/mvch/emergency_stop", self.on_stop, 10) self.create_subscription(String, "/mvch/command", lambda m: self.commands.put(m.data), 10) self.commands = queue.Queue() @@ -250,8 +282,17 @@ def main(): # Таймер на монотонных часах: остановка сработает даже при паузе /clock. self.timer = self.create_timer(0.05, self.tick, clock=Clock(clock_type=ClockType.STEADY_TIME)) self.log("READY", target=target, config="constants in module_b.py", sim=args.sim, + field=f"{rows}x{columns}", aruco_topic=aruco_topic, instructions="Enter/go: старт; return: построить возврат; stop/resume; quit") + def on_tf(self, message): + for transform in message.transforms: + self.buffer.set_transform(transform, "RMC2 namespaced tf") + + def on_tf_static(self, message): + for transform in message.transforms: + self.buffer.set_transform_static(transform, "RMC2 namespaced tf_static") + def now(self): return self.get_clock().now().nanoseconds / 1e9 diff --git a/module3/README.md b/module3/README.md index ba152b7..7226688 100644 --- a/module3/README.md +++ b/module3/README.md @@ -1,6 +1,8 @@ # Модуль В: датасет и YOLO -Камера: `/RMC1/arm95/camera_gripper/image_color` (`1600x1200`). +Камера Webots: `/RMC1/arm95/camera_gripper/image_color`. На физическом RMC1 новая +документация указывает `/RMC1/arm95/svcam/right/image/compressed` с типом +`sensor_msgs/msg/Image` — несмотря на суффикс `compressed`. Основной зачетный скрипт: `module3.py`. Он показывает обработанный видеопоток, определяет координаты выбранной детали относительно `Base_link`, захватывает ее, @@ -47,9 +49,40 @@ source .venv/bin/activate source /opt/ros/jazzy/setup.bash source ~/ros2_ws/install/setup.bash cd ~/scripts/mvch -module3/.venv/bin/python module3/module3.py --target 1 +module3/.venv/bin/python module3/module3.py --target 1 --sim ``` +Для Webots `--sim` обязателен: он выбирает симуляторную камеру, глобальные TF и +`use_sim_time`. + +На физическом RMC1 `--sim` не нужен. До запуска проверьте фактический тип камеры: + +```bash +ros2 topic info /RMC1/arm95/svcam/right/image/compressed -v +ros2 topic echo /RMC1/arm95/svcam/right/camera_info --once +ros2 run tf2_ros tf2_echo Base_link ИМЯ_FRAME_КАМЕРЫ +``` + +Если `ros2 topic info` вопреки PDF показывает +`sensor_msgs/msg/CompressedImage`, добавьте `--compressed`. Если `CameraInfo` +публикуется под другим именем, задайте `--camera-info-topic`. При полном отсутствии +`CameraInfo` можно передать калибровку камеры вручную: + +```bash +module3/.venv/bin/python module3/module3.py --target hammer \ + --fx FX --fy FY --cx CX --cy CY +``` + +Без CameraInfo либо четырёх параметров калибровки скрипт распознает класс, но +намеренно не двигает ARM95: по одному bounding box нельзя получить метрические +координаты детали. + +MoveIt-конфигурация в скрипте повторяет выданный +`moveit_api_example.launch.py`: `arm95_webots.urdf`, группы `arm95_group` и +`gripper`, namespace `/RMC1/arm95`. Для физического запуска меняется только +`use_sim_time=false`. Если на площадке организаторы выдадут обновлённый launch или +URDF для железа, он имеет приоритет — сначала сравните его с этим примером. + Цели: `1/hammer`, `2/wrench`, `3/pliers`. После того как эксперт зафиксировал видеопоток, нажмите `G` или `Space` в окне либо введите `g` и нажмите Enter в терминале. `Esc` останавливает скрипт до начала движения. @@ -57,7 +90,7 @@ module3/.venv/bin/python module3/module3.py --target 1 Сначала обязательно проверьте только координаты: ```bash -module3/.venv/bin/python module3/module3.py --target hammer --dry-run +module3/.venv/bin/python module3/module3.py --target hammer --sim --dry-run ``` В режиме `--dry-run` подтверждать запуск клавишей не нужно: скрипт завершится @@ -66,7 +99,7 @@ module3/.venv/bin/python module3/module3.py --target hammer --dry-run Для проверки через SSH без окна: ```bash -module3/.venv/bin/python module3/module3.py --target hammer --dry-run --no-window +module3/.venv/bin/python module3/module3.py --target hammer --sim --dry-run --no-window ``` Геометрию реального стенда нельзя брать из симулятора вслепую. Перед зачетом @@ -110,9 +143,12 @@ wrench ```bash source /opt/ros/jazzy/setup.bash source ~/ros2_ws/install/setup.bash -python3 module3/capture_dataset.py --count 30 +python3 module3/capture_dataset.py --sim --count 30 ``` +На реальном RMC1 `capture_dataset.py` по умолчанию использует новую камеру. Если +её реальный тип окажется `CompressedImage`, добавьте `--compressed`. + Во время сбора плавно меняйте положение RMC1/манипулятора. Скрипт пропускает почти одинаковые кадры. Для одиночного снимка используйте `--count 1 --min-change 0`. diff --git a/module3/capture_dataset.py b/module3/capture_dataset.py index 96a6490..111c816 100755 --- a/module3/capture_dataset.py +++ b/module3/capture_dataset.py @@ -12,11 +12,12 @@ HERE = Path(__file__).resolve().parent def arguments(): parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--sim", action="store_true", help="Использовать камеру Webots") parser.add_argument( "--topic", - default="/RMC1/arm95/camera_gripper/image_color", - help="ROS-топик sensor_msgs/Image", + help="ROS-топик камеры; по умолчанию выбирается по --sim", ) + parser.add_argument("--compressed", action="store_true", help="Тип CompressedImage") parser.add_argument("--output", default="dataset/raw", help="Каталог для JPG") parser.add_argument("--count", type=int, default=30, help="Сколько кадров сохранить") parser.add_argument("--interval", type=float, default=0.5, help="Минимум секунд между JPG") @@ -29,6 +30,12 @@ def arguments(): parser.add_argument("--timeout", type=float, default=180.0, help="Общий таймаут") parser.add_argument("--jpeg-quality", type=int, default=95) args = parser.parse_args() + if args.topic is None: + args.topic = ( + "/RMC1/arm95/camera_gripper/image_color" + if args.sim + else "/RMC1/arm95/svcam/right/image/compressed" + ) if args.count < 1 or args.interval < 0 or args.timeout <= 0: parser.error("count >= 1, interval >= 0, timeout > 0") if not 1 <= args.jpeg_quality <= 100 or args.min_change < 0: @@ -45,7 +52,7 @@ def main(): from cv_bridge import CvBridge from rclpy.node import Node from rclpy.qos import qos_profile_sensor_data - from sensor_msgs.msg import Image + from sensor_msgs.msg import CompressedImage, Image except ImportError as error: raise SystemExit( f"Нет ROS/OpenCV зависимости: {error}. Запускайте после source ROS 2 в VM." @@ -63,14 +70,22 @@ def main(): self.saved = 0 self.previous = None self.last_save = 0.0 - self.create_subscription(Image, args.topic, self.on_image, qos_profile_sensor_data) + image_type = CompressedImage if args.compressed else Image + self.create_subscription(image_type, args.topic, self.on_image, qos_profile_sensor_data) def on_image(self, message): now = time.monotonic() if now - self.last_save < args.interval: return try: - frame = self.bridge.imgmsg_to_cv2(message, desired_encoding="bgr8") + if args.compressed: + frame = self.bridge.compressed_imgmsg_to_cv2( + message, desired_encoding="bgr8" + ) + else: + frame = self.bridge.imgmsg_to_cv2( + message, desired_encoding="bgr8" + ) except Exception as error: self.get_logger().error(f"Не удалось преобразовать кадр: {error}") return diff --git a/module3/module3.py b/module3/module3.py index 1559ebd..a0893bf 100644 --- a/module3/module3.py +++ b/module3/module3.py @@ -49,7 +49,19 @@ def arguments(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--target", help="1/hammer, 2/wrench или 3/pliers") parser.add_argument("--weights", default="models/latest.pt") - parser.add_argument("--topic", default="/RMC1/arm95/camera_gripper/image_color") + parser.add_argument("--sim", action="store_true", help="Камера/TF и часы Webots") + parser.add_argument("--topic", help="ROS-топик камеры; по умолчанию выбирается по --sim") + parser.add_argument("--camera-info-topic") + parser.add_argument( + "--compressed", + action="store_true", + help="Топик имеет тип sensor_msgs/CompressedImage, а не Image", + ) + parser.add_argument("--base-frame", default="Base_link") + parser.add_argument("--fx", type=float, help="Фокус X, если CameraInfo отсутствует") + parser.add_argument("--fy", type=float, help="Фокус Y, если CameraInfo отсутствует") + parser.add_argument("--cx", type=float, help="Главная точка X") + parser.add_argument("--cy", type=float, help="Главная точка Y") parser.add_argument("--conf", type=float, default=0.20) parser.add_argument("--imgsz", type=int, default=512) parser.add_argument("--device", default="cpu") @@ -70,12 +82,26 @@ def arguments(): parser.add_argument("--plan-only", action="store_true", help="Спланировать подход, не двигать руку") args = parser.parse_args() + if args.topic is None: + args.topic = ( + "/RMC1/arm95/camera_gripper/image_color" + if args.sim + else "/RMC1/arm95/svcam/right/image/compressed" + ) + if not 0.0 <= args.conf <= 1.0: parser.error("--conf должен быть от 0 до 1") if args.timeout <= 0 or args.approach_z <= args.pick_z: parser.error("timeout > 0, approach-z должен быть выше pick-z") if (args.drop_x is None) != (args.drop_y is None): parser.error("--drop-x и --drop-y задаются вместе") + intrinsics = (args.fx, args.fy, args.cx, args.cy) + if any(value is not None for value in intrinsics) and not all( + value is not None and math.isfinite(value) for value in intrinsics + ): + parser.error("--fx, --fy, --cx и --cy задаются только вместе") + if args.fx is not None and (args.fx <= 0 or args.fy <= 0): + parser.error("--fx и --fy должны быть больше нуля") return args @@ -213,18 +239,24 @@ def card_near_center(frame, requested, cv2, np): def estimate_pose(node, detection, plane_z): - if node.camera_info is None: - raise RuntimeError("Нет CameraInfo") - frame = node.camera_frame or node.camera_info.header.frame_id + if node.camera_info is None and node.manual_k is None: + raise RuntimeError( + f"Нет CameraInfo с {node.info_topic}; задайте калибровку --fx/--fy/--cx/--cy" + ) + frame = node.camera_frame or ( + node.camera_info.header.frame_id if node.camera_info is not None else "" + ) + if not frame: + raise RuntimeError("В сообщении камеры не указан frame_id") transform = node.tf_buffer.lookup_transform( - "Base_link", frame, node.Time(), timeout=node.Duration(seconds=1.0) + node.base_frame, frame, node.Time(), timeout=node.Duration(seconds=1.0) ) translation = transform.transform.translation rotation = transform.transform.rotation quaternion = (rotation.x, rotation.y, rotation.z, rotation.w) u, v = detection.center - matrix = node.camera_info.k + matrix = node.camera_info.k if node.camera_info is not None else node.manual_k fx, fy, cx, cy = matrix[0], matrix[4], matrix[2], matrix[5] ray_camera = ((u - cx) / fx, (v - cy) / fy, 1.0) ray_base = rotate_vector(ray_camera, quaternion) @@ -249,27 +281,93 @@ def estimate_pose(node, detection, plane_z): def build_vision_node(args, imports): - Node, CvBridge, Image, CameraInfo, qos, Buffer, TransformListener, Duration, Time = imports + ( + Node, + CvBridge, + Image, + CompressedImage, + CameraInfo, + qos, + Buffer, + TransformListener, + Duration, + Time, + TFMessage, + Parameter, + QoSProfile, + ReliabilityPolicy, + DurabilityPolicy, + ) = imports class VisionNode(Node): def __init__(self): super().__init__("mvch_module3_vision") + self.set_parameters( + [Parameter("use_sim_time", value=args.sim)] + ) self.Duration = Duration self.Time = Time + self.base_frame = args.base_frame self.bridge = CvBridge() self.frame = None self.camera_info = None + self.manual_k = ( + (args.fx, 0.0, args.cx, 0.0, args.fy, args.cy, 0.0, 0.0, 1.0) + if args.fx is not None + else None + ) self.camera_frame = None self.lock = threading.Lock() self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) - self.create_subscription(Image, args.topic, self.on_image, qos) - info_topic = args.topic.rsplit("/", 1)[0] + "/camera_info" + 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, "/RMC1/tf", self.on_tf, tf_qos + ) + self.create_subscription( + TFMessage, "/RMC1/tf_static", self.on_tf_static, static_tf_qos + ) + image_type = CompressedImage if args.compressed else Image + self.create_subscription(image_type, args.topic, self.on_image, qos) + info_topic = args.camera_info_topic + if info_topic is None: + if args.topic.endswith("/image/compressed"): + info_topic = args.topic.removesuffix("/image/compressed") + "/camera_info" + else: + info_topic = args.topic.rsplit("/", 1)[0] + "/camera_info" self.create_subscription(CameraInfo, info_topic, self.on_info, qos) + self.info_topic = info_topic + + def on_tf(self, message): + for transform in message.transforms: + self.tf_buffer.set_transform(transform, "RMC1 namespaced tf") + + def on_tf_static(self, message): + for transform in message.transforms: + self.tf_buffer.set_transform_static( + transform, "RMC1 namespaced tf_static" + ) def on_image(self, message): try: - frame = self.bridge.imgmsg_to_cv2(message, desired_encoding="bgr8") + if args.compressed: + frame = self.bridge.compressed_imgmsg_to_cv2( + message, desired_encoding="bgr8" + ) + else: + frame = self.bridge.imgmsg_to_cv2( + message, desired_encoding="bgr8" + ) except Exception as error: self.get_logger().error(f"Ошибка кадра: {error}") return @@ -296,6 +394,7 @@ class Arm: quaternion_from_euler, event_log, plan_only=False, + base_frame="Base_link", ): self.robot = moveit( node_name="mvch_module3_moveit", @@ -309,6 +408,7 @@ class Arm: self.quaternion_from_euler = quaternion_from_euler self.log = event_log self.plan_only = plan_only + self.base_frame = base_frame def execute(self, component, stage): plan = component.plan() @@ -327,7 +427,7 @@ class Arm: def pose(self, x, y, z, yaw, stage): goal = self.PoseStamped() - goal.header.frame_id = "Base_link" + goal.header.frame_id = self.base_frame qx, qy, qz, qw = self.quaternion_from_euler(math.pi, 0.0, yaw) goal.pose.orientation.x = qx goal.pose.orientation.y = qy @@ -529,9 +629,16 @@ def main(): from rclpy.duration import Duration from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node - from rclpy.qos import qos_profile_sensor_data + 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 CameraInfo, Image + from sensor_msgs.msg import CameraInfo, CompressedImage, Image + from tf2_msgs.msg import TFMessage from tf2_ros import Buffer, TransformListener from tf_transformations import quaternion_from_euler from ultralytics import YOLO @@ -554,12 +661,18 @@ def main(): Node, CvBridge, Image, + CompressedImage, CameraInfo, qos_profile_sensor_data, Buffer, TransformListener, Duration, Time, + TFMessage, + Parameter, + QoSProfile, + ReliabilityPolicy, + DurabilityPolicy, ) node = build_vision_node(args, imports) executor = SingleThreadedExecutor() @@ -594,7 +707,7 @@ def main(): .to_moveit_configs() .to_dict() ) - moveit_config["use_sim_time"] = True + moveit_config["use_sim_time"] = args.sim # Wildcard нужен из-за namespace /RMC1/arm95: обычный config_dict # записывает параметры только для имени узла без namespace. with NamedTemporaryFile("w", suffix=".yaml", delete=False) as stream: @@ -608,6 +721,7 @@ def main(): quaternion_from_euler, event_log, args.plan_only, + args.base_frame, ) moveit_active = True finally: diff --git a/module4/README.md b/module4/README.md index 6fe1030..976185d 100644 --- a/module4/README.md +++ b/module4/README.md @@ -7,7 +7,7 @@ Tailwind CSS 4, Bun и небольшой ROS 2-мост на Python. - выбор RMC1 или RMC2; - положение, скорость и история движения обоих роверов; -- ArUco-граф 6 × 6 с координатами маркеров 0–35; +- ArUco-граф 5 × 5 для физического поля или 6 × 6 для Webots; - карта занятости RMC1 из `/map`; - облако точек `/RMC*/scan`, повернутое и перенесённое вместе с ровером; - план Nav2 из `/RMC*/plan` и текущая целевая точка; @@ -54,6 +54,13 @@ ros2 launch ar_nav_ros2 bringup_launch.py namespace:=/RMC1 robot_type:=rmc1 использует простой контроллер по одометрии. Он доводит оба ровера до x/y/yaw, но не строит обход сложных препятствий, поэтому Nav2 для RMC1 предпочтительнее. +Для показанной в PDF физической площадки ничего задавать не нужно: используется +сетка 5 × 5, ID 0–24. В Webots перед запуском моста задайте его сетку 6 × 6: + +```bash +export MVCH_FIELD_SIZE=6 +``` + 3. В третьем, также ROS-sourced терминале запустить FMS: ```bash @@ -94,8 +101,11 @@ WebSocket и компоновки интерфейса. | План | `/RMC1/plan` | `/RMC2/plan` | | Рабочий инструмент | `.../gripper_trajectory_controller/joint_trajectory` | `/RMC2/lift` | -Дополнительно используются `/map`, `/RMC2/aruco_id` и -`/RMC2/lift_status`. +Дополнительно используются `/map`, реальный +`/RMC2/camera_bottom/aruco_id` (и старый `/RMC2/aruco_id` для Webots), а также +`/RMC2/lift_status`. По документации реального RMC2 статус лифта имеет QoS +`Transient Local` и значения `lowered`, `moving`, `raised`; команды — `0.0` вниз +и `0.05` м вверх. Мост настроен именно так. Симулятор из открытого пакета не публикует напряжение аккумуляторов. Мост уже подписан на `/RMC*/battery_state` (`sensor_msgs/BatteryState`) и diff --git a/module4/server/commands.ts b/module4/server/commands.ts index 1b395f3..d0b83a0 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.1) + return command.robot === 'RMC2' && (command.height === 0 || command.height === 0.05) 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 4aa797e..888b942 100644 --- a/module4/server/ros_bridge.py +++ b/module4/server/ros_bridge.py @@ -8,6 +8,7 @@ stderr: diagnostics import json import math +import os import queue import sys import threading @@ -81,6 +82,7 @@ class FmsBridge(Node): "type": "state", "timestamp": int(time.time() * 1000), "mode": "ros", + "fieldSize": max(2, min(10, int(os.environ.get("MVCH_FIELD_SIZE", "5")))), "bridgeOnline": True, "bridgeError": None, "map": None, @@ -154,8 +156,20 @@ class FmsBridge(Node): self.create_subscription(OccupancyGrid, "/map", self.on_map, map_qos) self.create_subscription(OccupancyGrid, "/RMC1/map", self.on_map, map_qos) + # Первый топик физического RMC2, второй оставлен для Webots. + self.create_subscription( + String, "/RMC2/camera_bottom/aruco_id", self.on_aruco, sensor_qos + ) self.create_subscription(String, "/RMC2/aruco_id", self.on_aruco, sensor_qos) - self.create_subscription(String, "/RMC2/lift_status", self.on_lift_status, 10) + lift_qos = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + depth=1, + ) + self.create_subscription( + String, "/RMC2/lift_status", self.on_lift_status, lift_qos + ) self.lift_publisher = self.create_publisher(Float64, "/RMC2/lift", 10) self.gripper_publisher = self.create_publisher( JointTrajectory, @@ -335,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.1 if float(command.get("height", 0)) >= 0.05 else 0.0 + height = 0.05 if float(command.get("height", 0)) >= 0.025 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 5038a4e..d6bef88 100644 --- a/module4/server/state.test.ts +++ b/module4/server/state.test.ts @@ -5,12 +5,12 @@ 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.1 })).toBe(true) + expect(isCommand({ type: 'lift', robot: 'RMC2', height: 0.05 })).toBe(true) expect(isCommand({ type: 'gripper', robot: 'RMC1', state: 'closed' })).toBe(true) }) test('rejects wrong robot tools and non-finite coordinates', () => { - expect(isCommand({ type: 'lift', robot: 'RMC1', height: 0.1 })).toBe(false) + expect(isCommand({ type: 'lift', robot: 'RMC1', height: 0.05 })).toBe(false) expect(isCommand({ type: 'gripper', robot: 'RMC2', state: 'open' })).toBe(false) expect(isCommand({ type: 'set_goal', robot: 'RMC1', pose: { x: Number.NaN, y: 0, yaw: 0 } })).toBe(false) }) @@ -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.1 }) + applyMockCommand(state, { type: 'lift', robot: 'RMC2', height: 0.05 }) 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/server/state.ts b/module4/server/state.ts index 8eff042..ea56342 100644 --- a/module4/server/state.ts +++ b/module4/server/state.ts @@ -22,6 +22,7 @@ export function initialState(mode: 'ros' | 'mock'): FmsState { type: 'state', timestamp: Date.now(), mode, + fieldSize: mode === 'mock' ? 6 : 5, bridgeOnline: mode === 'mock', bridgeError: null, map: mockMap, diff --git a/module4/src/App.tsx b/module4/src/App.tsx index 949980e..88802f5 100644 --- a/module4/src/App.tsx +++ b/module4/src/App.tsx @@ -24,6 +24,7 @@ const initialState: FmsState = { type: 'state', timestamp: 0, mode: 'ros', + fieldSize: 5, bridgeOnline: false, bridgeError: null, map: null, @@ -89,10 +90,11 @@ function FieldMap({ state, selected }: { state: FmsState; selected: RobotId }) { return result.slice(0, 5000) }, [state.map]) - const markers = Array.from({ length: 36 }, (_, id) => ({ + const fieldSize = state.fieldSize || 5 + const markers = Array.from({ length: fieldSize ** 2 }, (_, id) => ({ id, - x: -Math.floor(id / 6), - y: id % 6, + x: -Math.floor(id / fieldSize), + y: id % fieldSize, })) return ( @@ -115,10 +117,10 @@ function FieldMap({ state, selected }: { state: FmsState; selected: RobotId }) { ) })} - {Array.from({ length: 6 }, (_, row) => Array.from({ length: 6 }, (_, col) => { + {Array.from({ length: fieldSize }, (_, row) => Array.from({ length: fieldSize }, (_, col) => { const here = worldToSvg({ x: -row, y: col }) - const right = col < 5 ? worldToSvg({ x: -row, y: col + 1 }) : null - const down = row < 5 ? worldToSvg({ x: -(row + 1), y: col }) : null + const right = col < fieldSize - 1 ? worldToSvg({ x: -row, y: col + 1 }) : null + const down = row < fieldSize - 1 ? worldToSvg({ x: -(row + 1), y: col }) : null return ( {right && } @@ -165,7 +167,7 @@ function FieldMap({ state, selected }: { state: FmsState; selected: RobotId }) { })}
- ArUco: шаг 1 м · X вниз · Y вправо + ArUco: поле {fieldSize}×{fieldSize} · шаг 1 м · X вниз · Y вправо
● RMC1● RMC2-- маршрут @@ -385,7 +387,7 @@ export default function App() { {selected === 'RMC1' ? (
) : ( -
+
)} diff --git a/module4/src/types.ts b/module4/src/types.ts index 0e08e90..2b560d2 100644 --- a/module4/src/types.ts +++ b/module4/src/types.ts @@ -31,6 +31,7 @@ export type FmsState = { type: 'state' timestamp: number mode: 'ros' | 'mock' + fieldSize: number bridgeOnline: boolean bridgeError: string | null map: MapState | null @@ -44,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.1 } + | { type: 'lift'; robot: 'RMC2'; height: 0 | 0.05 } | { type: 'gripper'; robot: 'RMC1'; state: 'open' | 'closed' } diff --git a/module5/README.md b/module5/README.md index aa47704..f7aafe5 100644 --- a/module5/README.md +++ b/module5/README.md @@ -13,7 +13,8 @@ Решение специально последовательное: это медленнее параллельного управления, зато уменьшает риск столкновения RMC1 и RMC2. Маршруты строятся BFS по графу -ArUco 6 × 6, занятые клетки исключаются. Движение замыкается по odometry, +ArUco, занятые клетки исключаются. Для Webots используется сетка 6 × 6, для +физического поля из документации — 5 × 5. Движение замыкается по odometry, `scan_front` вызывает остановку перед препятствием, команды скорости сглаживаются. Соседняя парковочная точка RMC2 автоматически берётся из построенного пути к комплектации, отдельный параметр для неё не нужен. @@ -63,7 +64,7 @@ python3 module5/module5.py --target 1 Перед движением программа показывает все точки и маршруты. После проверки нажмите Enter. Классы цели: `1=hammer`, `2=wrench`, `3=pliers`. -Пример с точками эксперта: +Пример с точками эксперта для Webots: ```bash python3 module5/module5.py --target wrench \ @@ -72,6 +73,26 @@ python3 module5/module5.py --target wrench \ --delivery 5 --delivery-approach 4 --blocked 31,35 ``` +На физическом поле обязательно добавьте `--real` и явно передайте все семь точек +эксперта. Без них скрипт откажется запускаться, чтобы случайно не применить +симуляторный пресет 0–35 к реальным ID 0–24: + +```bash +python3 module5/module5.py --real --target wrench \ + --rmc1-start "$RMC1_START" --rmc2-start "$RMC2_START" \ + --rack "$RACK" --assembly "$ASSEMBLY" \ + --assembly-approach "$ASSEMBLY_APPROACH" \ + --delivery "$DELIVERY" --delivery-approach "$DELIVERY_APPROACH" \ + --blocked "$BLOCKED" --dry-run +``` + +Сначала присвойте переменным фактические ID и проверьте этот `--dry-run`. Затем +уберите только `--dry-run`. Реальный профиль также выбирает камеру +`/RMC1/arm95/svcam/right/image/compressed`, лифт `0.05` м и системное время. +Суффикс камеры противоречит указанному в PDF типу `sensor_msgs/Image`, поэтому +сначала выполните `ros2 topic info ... -v`; если фактический тип +`CompressedImage`, добавьте `--camera-compressed`. + Только маршруты, без ROS и движения: ```bash @@ -118,6 +139,11 @@ Webots не имеет аппаратного watchdog: после SIGKILL до поле: геометрия полки может отличаться. Если ARM95 пока не откалиброван, `--skip-arm` позволяет отдельно проверить всю логистику RMC1/RMC2. +Для расчёта координат детали нужен `CameraInfo`. Если его нет в списке топиков, +передайте калибровку как `--camera-fx`, `--camera-fy`, `--camera-cx`, +`--camera-cy`. Имена frame камеры и базы проверьте через TF; базу можно заменить +аргументом `--base-frame`. + ## Логи и критерии Логи сохраняются в `module5/logs/module5_*.jsonl` и не попадают в Git. В них diff --git a/module5/module5.py b/module5/module5.py index 631ee59..2113fd3 100644 --- a/module5/module5.py +++ b/module5/module5.py @@ -33,7 +33,7 @@ def wrap_angle(value): def marker_xy(marker_id): - """Координаты поля: 0=(0,0), 6=(-1,0), 1=(0,1).""" + """Координаты поля: 0=(0,0), COLS=(-1,0), 1=(0,1).""" row, column = divmod(marker_id, COLS) return -row * MARKER_SPACING, column * MARKER_SPACING @@ -128,7 +128,9 @@ class Scenario: } for name, marker in points.items(): if marker not in range(ROWS * COLS): - raise ValueError(f"{name}: ID {marker} вне диапазона 0..35") + raise ValueError( + f"{name}: ID {marker} вне диапазона 0..{ROWS * COLS - 1}" + ) if self.assembly_approach not in neighbours(self.assembly): raise ValueError("assembly-approach должен быть соседней ячейкой") if self.delivery_approach not in neighbours(self.delivery): @@ -185,16 +187,19 @@ class Anchor: def arguments(): + global ROWS, COLS, MARKER_SPACING parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--target", help="1/hammer, 2/wrench, 3/pliers") - parser.add_argument("--rmc1-start", type=int, default=15) - parser.add_argument("--rmc2-start", type=int, default=0) - parser.add_argument("--rack", type=int, default=34) - parser.add_argument("--assembly", type=int, default=16) - parser.add_argument("--assembly-approach", type=int, default=10) - parser.add_argument("--delivery", type=int, default=5) - parser.add_argument("--delivery-approach", type=int, default=4) - parser.add_argument("--blocked", type=parse_markers, default={31, 35}) + parser.add_argument("--real", action="store_true", help="Физическое поле 5x5 и реальные ROS-топики") + parser.add_argument("--spacing", type=float, default=1.0, help="Шаг ArUco-сетки, м") + parser.add_argument("--rmc1-start", type=int) + parser.add_argument("--rmc2-start", type=int) + parser.add_argument("--rack", type=int) + parser.add_argument("--assembly", type=int) + parser.add_argument("--assembly-approach", type=int) + parser.add_argument("--delivery", type=int) + parser.add_argument("--delivery-approach", type=int) + parser.add_argument("--blocked", type=parse_markers) parser.add_argument("--rmc1-start-yaw", type=float, default=math.pi) parser.add_argument("--rmc2-start-yaw", type=float, default=math.pi) parser.add_argument("--rack-yaw", type=float, default=math.pi) @@ -202,9 +207,14 @@ def arguments(): parser.add_argument("--dry-run", action="store_true", help="Показать сценарий без ROS") parser.add_argument("--skip-arm", action="store_true", help="Только роверы и лифт") parser.add_argument("--weights", default="module3/models/latest.pt") - parser.add_argument( - "--camera-topic", default="/RMC1/arm95/camera_gripper/image_color" - ) + parser.add_argument("--camera-topic") + parser.add_argument("--camera-info-topic") + parser.add_argument("--camera-compressed", action="store_true") + parser.add_argument("--base-frame", default="Base_link") + parser.add_argument("--camera-fx", type=float) + parser.add_argument("--camera-fy", type=float) + parser.add_argument("--camera-cx", type=float) + parser.add_argument("--camera-cy", type=float) parser.add_argument("--conf", type=float, default=0.20) parser.add_argument("--vision-timeout", type=float, default=60.0) parser.add_argument("--plane-z", type=float, default=0.128) @@ -217,10 +227,65 @@ def arguments(): parser.add_argument("--sensor-timeout", type=float, default=3.0) parser.add_argument("--waypoint-timeout", type=float, default=35.0) args = parser.parse_args() + if args.spacing <= 0: + parser.error("--spacing должен быть больше нуля") + + ROWS = COLS = 5 if args.real else 6 + MARKER_SPACING = args.spacing + point_names = ( + "rmc1_start", + "rmc2_start", + "rack", + "assembly", + "assembly_approach", + "delivery", + "delivery_approach", + ) + if args.real: + missing = [ + name.replace("_", "-") + for name in point_names + if getattr(args, name) is None + ] + if missing: + parser.error( + "Для --real эксперт должен сообщить все точки 0..24; не заданы: " + + ", ".join(missing) + ) + if args.blocked is None: + args.blocked = set() + else: + simulator_defaults = { + "rmc1_start": 15, + "rmc2_start": 0, + "rack": 34, + "assembly": 16, + "assembly_approach": 10, + "delivery": 5, + "delivery_approach": 4, + } + for name, value in simulator_defaults.items(): + if getattr(args, name) is None: + setattr(args, name, value) + if args.blocked is None: + args.blocked = {31, 35} + if args.camera_topic is None: + args.camera_topic = ( + "/RMC1/arm95/svcam/right/image/compressed" + if args.real + else "/RMC1/arm95/camera_gripper/image_color" + ) if not 0.0 <= args.conf <= 1.0: parser.error("--conf должен быть от 0 до 1") if args.approach_z <= max(args.pick_z, args.drop_arm_z): parser.error("--approach-z должен быть выше pick/drop") + intrinsics = (args.camera_fx, args.camera_fy, args.camera_cx, args.camera_cy) + if any(value is not None for value in intrinsics) and not all( + value is not None and math.isfinite(value) for value in intrinsics + ): + parser.error("--camera-fx/fy/cx/cy задаются только вместе") + if args.camera_fx is not None and (args.camera_fx <= 0 or args.camera_fy <= 0): + parser.error("--camera-fx и --camera-fy должны быть больше нуля") if min(args.sensor_timeout, args.waypoint_timeout, args.vision_timeout) <= 0: parser.error("таймауты должны быть положительными") return args @@ -265,7 +330,12 @@ def build_ros_node(args, event_log): from nav_msgs.msg import Odometry from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node - from rclpy.qos import qos_profile_sensor_data + from rclpy.qos import ( + DurabilityPolicy, + QoSProfile, + ReliabilityPolicy, + qos_profile_sensor_data, + ) from sensor_msgs.msg import LaserScan from std_msgs.msg import Bool, Float64, String @@ -303,7 +373,14 @@ def build_ros_node(args, event_log): lambda message, robot=name: self.on_scan(robot, message), qos_profile_sensor_data, ) - self.create_subscription(String, "/RMC2/lift_status", self.on_lift, 10) + lift_qos = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + self.create_subscription( + String, "/RMC2/lift_status", self.on_lift, lift_qos + ) self.create_subscription(Bool, "/mvch/module5/emergency_stop", self.on_stop, 10) self.create_subscription(String, "/mvch/module5/command", self.on_command, 10) @@ -388,7 +465,8 @@ def build_ros_node(args, event_log): time.sleep(0.04) def set_lift(self, raised, allow_already=False): - height = 0.1 if raised else 0.0 + height = 0.05 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: updates_before = self.lift_updates @@ -401,9 +479,10 @@ def build_ros_node(args, event_log): with self.lock: status = self.lift_status received_update = self.lift_updates > updates_before - # Драйвер публикует status только при его изменении. Если лифт уже - # был внизу до нашей подписки, команда down не создаст сообщения. - if time.monotonic() - started >= 1.1 and received_update and "success" in status.lower(): + # Реальный драйвер публикует lowered/moving/raised с + # Transient Local. Webots старых версий мог отвечать success. + normalized = status.strip().lower() + if normalized == expected or (received_update and "success" in normalized): success = True break if time.monotonic() - started >= 1.3 and allow_already and not received_update: @@ -573,9 +652,16 @@ class ArmWorkflow: from moveit_configs_utils import MoveItConfigsBuilder from rclpy.duration import Duration from rclpy.node import Node - from rclpy.qos import qos_profile_sensor_data + 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 CameraInfo, Image + from sensor_msgs.msg import CameraInfo, CompressedImage, Image + from tf2_msgs.msg import TFMessage from tf2_ros import Buffer, TransformListener from tf_transformations import quaternion_from_euler from ultralytics import YOLO @@ -615,14 +701,31 @@ class ArmWorkflow: Node, CvBridge, Image, + CompressedImage, CameraInfo, qos_profile_sensor_data, Buffer, TransformListener, Duration, Time, + TFMessage, + Parameter, + QoSProfile, + ReliabilityPolicy, + DurabilityPolicy, ) - self.vision = build_vision_node(SimpleNamespace(topic=args.camera_topic), imports) + vision_args = SimpleNamespace( + topic=args.camera_topic, + camera_info_topic=args.camera_info_topic, + compressed=args.camera_compressed, + base_frame=args.base_frame, + sim=not args.real, + fx=args.camera_fx, + fy=args.camera_fy, + cx=args.camera_cx, + cy=args.camera_cy, + ) + self.vision = build_vision_node(vision_args, imports) executor.add_node(self.vision) self.model = YOLO(str(weights)) self.log.write("VISION_READY", target=target, weights=str(weights)) @@ -638,7 +741,7 @@ class ArmWorkflow: .to_moveit_configs() .to_dict() ) - moveit_config["use_sim_time"] = True + moveit_config["use_sim_time"] = not args.real with NamedTemporaryFile("w", suffix=".yaml", delete=False) as stream: yaml.safe_dump({"/**": {"ros__parameters": moveit_config}}, stream) params_file = stream.name @@ -649,6 +752,7 @@ class ArmWorkflow: PoseStamped, quaternion_from_euler, event_log, + base_frame=args.base_frame, ) finally: Path(params_file).unlink(missing_ok=True)