#!/usr/bin/env python3 import argparse import json import math import os import sys import threading import time from collections import deque from dataclasses import dataclass from datetime import datetime from pathlib import Path from tempfile import NamedTemporaryFile from types import SimpleNamespace HERE = Path(__file__).resolve().parent ROOT = HERE.parent ROWS = 6 COLS = 6 MARKER_SPACING = 1.0 ROBOT_NAMES = ("RMC1", "RMC2") def clamp(value, limit): return max(-limit, min(limit, value)) def wrap_angle(value): return math.atan2(math.sin(value), math.cos(value)) def marker_xy(marker_id): """Координаты поля: 0=(0,0), COLS=(-1,0), 1=(0,1).""" row, column = divmod(marker_id, COLS) return -row * MARKER_SPACING, column * MARKER_SPACING def neighbours(marker_id): row, column = divmod(marker_id, COLS) result = [] for dr, dc in ((-1, 0), (0, -1), (0, 1), (1, 0)): nr, nc = row + dr, column + dc if 0 <= nr < ROWS and 0 <= nc < COLS: result.append(nr * COLS + nc) return result def shortest_path(start, goal, blocked=()): """BFS по регулярному графу ArUco.""" forbidden = set(blocked) - {start, goal} queue = deque([start]) previous = {start: None} while queue: current = queue.popleft() if current == goal: break for candidate in neighbours(current): if candidate not in forbidden and candidate not in previous: previous[candidate] = current queue.append(candidate) if goal not in previous: raise ValueError(f"Нет маршрута {start} -> {goal}; занято: {sorted(forbidden)}") route = [] current = goal while current is not None: route.append(current) current = previous[current] return list(reversed(route)) def face_marker(position, target): x1, y1 = marker_xy(position) x2, y2 = marker_xy(target) return math.atan2(y2 - y1, x2 - x1) def parse_markers(value): if not value.strip(): return set() try: return {int(item.strip()) for item in value.split(",")} except ValueError as error: raise argparse.ArgumentTypeError("Нужны ID через запятую, например 31,35") from error class EventLog: def __init__(self): directory = HERE / "logs" directory.mkdir(parents=True, exist_ok=True) self.path = directory / f"module5_{datetime.now():%Y%m%d_%H%M%S}.jsonl" self.lock = threading.Lock() def write(self, event, **fields): record = {"time": datetime.now().isoformat(timespec="milliseconds"), "event": event} record.update(fields) line = json.dumps(record, ensure_ascii=False) with self.lock: print(line, flush=True) with self.path.open("a", encoding="utf-8") as stream: stream.write(line + "\n") stream.flush() os.fsync(stream.fileno()) @dataclass class Scenario: rmc1_start: int rmc2_start: int rack: int assembly: int assembly_approach: int delivery: int delivery_approach: int blocked: set def validate(self): points = { "rmc1-start": self.rmc1_start, "rmc2-start": self.rmc2_start, "rack": self.rack, "assembly": self.assembly, "assembly-approach": self.assembly_approach, "delivery": self.delivery, "delivery-approach": self.delivery_approach, } for name, marker in points.items(): if marker not in range(ROWS * COLS): 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): raise ValueError("delivery-approach должен быть соседней ячейкой") if self.assembly in self.blocked or self.rack in self.blocked: raise ValueError("Активный стеллаж и точка комплектации не должны быть в --blocked") if self.rack == self.assembly: raise ValueError("Стеллаж и точка комплектации должны отличаться") def routes(self): # Полка сдачи и чужие стеллажи считаются препятствиями для RMC2. rmc2_blocked = self.blocked | {self.delivery} # Для RMC1 стеллаж уже стоит в комплектации, а полка занята физически. rmc1_blocked = self.blocked | {self.assembly, self.delivery} to_assembly = shortest_path(self.rack, self.assembly, rmc2_blocked) parking = to_assembly[-2] return { "rmc2_to_rack": shortest_path(self.rmc2_start, self.rack, rmc2_blocked), "rmc2_to_assembly": to_assembly, "rmc2_clear_assembly": [self.assembly, parking], "rmc2_back_to_assembly": [parking, self.assembly], "rmc1_to_assembly": shortest_path( self.rmc1_start, self.assembly_approach, rmc1_blocked ), "rmc1_to_delivery": shortest_path( self.assembly_approach, self.delivery_approach, rmc1_blocked ), "rmc2_return_rack": shortest_path(self.assembly, self.rack, rmc2_blocked), "rmc1_return": shortest_path( self.delivery_approach, self.rmc1_start, rmc1_blocked ), "rmc2_return": shortest_path(self.rack, self.rmc2_start, rmc2_blocked), } @dataclass class Anchor: world_x: float world_y: float world_yaw: float odom_x: float odom_y: float odom_yaw: float def to_odom(self, world_x, world_y, world_yaw): dx, dy = world_x - self.world_x, world_y - self.world_y rotation = self.odom_yaw - self.world_yaw cosine, sine = math.cos(rotation), math.sin(rotation) return ( self.odom_x + cosine * dx - sine * dy, self.odom_y + sine * dx + cosine * dy, wrap_angle(self.odom_yaw + world_yaw - self.world_yaw), ) def arguments(): global ROWS, COLS, MARKER_SPACING 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-сетки, м") 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) parser.add_argument("--auto", action="store_true", help="Не ждать Enter перед стартом") 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") 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) parser.add_argument("--pick-z", type=float, default=0.210) parser.add_argument("--approach-z", type=float, default=0.38) parser.add_argument("--drop-arm-x", type=float, default=0.55) parser.add_argument("--drop-arm-y", type=float, default=0.0) 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=120.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 def make_scenario(args): scenario = Scenario( args.rmc1_start, args.rmc2_start, args.rack, args.assembly, args.assembly_approach, args.delivery, args.delivery_approach, set(args.blocked), ) scenario.validate() return scenario def print_plan(scenario, routes, target): print("\n=== МОДУЛЬ Д: ПЛАН ДО ЗАПУСКА ===") print(f"Целевая деталь: {target}") print( f"RMC2: {scenario.rmc2_start} -> стеллаж {scenario.rack} -> " f"комплектация {scenario.assembly} -> {scenario.rack} -> {scenario.rmc2_start}" ) print( f"RMC1: {scenario.rmc1_start} -> подход {scenario.assembly_approach} -> " f"сдача через {scenario.delivery_approach} -> {scenario.rmc1_start}" ) for name, route in routes.items(): print(f"{name}: {route}") print("Приоритет: STOP -> безопасность -> точность -> скорость") print("Команды: stop, resume, abort. Внешний стоп: /mvch/module5/emergency_stop") print("====================================\n") def build_ros_node(args, event_log): import rclpy from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from rclpy.qos import ( DurabilityPolicy, QoSProfile, 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(signal_handler_options=SignalHandlerOptions.NO) class WarehouseNode(Node): def __init__(self): super().__init__("mvch_module5") self.lock = threading.Lock() self.poses = {name: None for name in ROBOT_NAMES} self.odom_seen = {name: 0.0 for name in ROBOT_NAMES} self.front_distance = {name: math.inf for name in ROBOT_NAMES} self.scan_seen = {name: 0.0 for name in ROBOT_NAMES} self.last_command = {name: (0.0, 0.0, 0.0, time.monotonic()) for name in ROBOT_NAMES} self.emergency = threading.Event() self.abort = threading.Event() self.lift_status = "unknown" self.lift_updates = 0 self.cmd_publishers = { name: self.create_publisher(Twist, f"/{name}/cmd_vel", 10) for name in ROBOT_NAMES } self.lift_publisher = self.create_publisher(Float64, "/RMC2/lift", 10) for name in ROBOT_NAMES: self.create_subscription( Odometry, f"/{name}/odometry", lambda message, robot=name: self.on_odom(robot, message), qos_profile_sensor_data, ) self.create_subscription( LaserScan, f"/{name}/scan_front", lambda message, robot=name: self.on_scan(robot, message), qos_profile_sensor_data, ) lift_qos = QoSProfile( depth=1, reliability=ReliabilityPolicy.RELIABLE, durability=( DurabilityPolicy.TRANSIENT_LOCAL if args.real else DurabilityPolicy.VOLATILE ), ) 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) def on_odom(self, robot, message): 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) pose = ( float(message.pose.pose.position.x), float(message.pose.pose.position.y), math.atan2(siny, cosy), ) with self.lock: self.poses[robot] = pose self.odom_seen[robot] = time.monotonic() def on_scan(self, robot, message): distances = [] for index, distance in enumerate(message.ranges): angle = message.angle_min + index * message.angle_increment if abs(angle) <= math.radians(24) and math.isfinite(distance): if message.range_min <= distance <= message.range_max: distances.append(float(distance)) with self.lock: self.front_distance[robot] = min(distances, default=math.inf) self.scan_seen[robot] = time.monotonic() def on_lift(self, message): with self.lock: self.lift_status = str(message.data) self.lift_updates += 1 def on_stop(self, message): if message.data: self.emergency.set() event_log.write("EMERGENCY_STOP", source="topic") else: self.emergency.clear() event_log.write("RESUME", source="topic") def on_command(self, message): command = message.data.strip().lower() if command == "stop": self.emergency.set() event_log.write("EMERGENCY_STOP", source="command") elif command == "resume": self.emergency.clear() event_log.write("RESUME", source="command") elif command in {"abort", "quit"}: self.abort.set() def pose(self, robot): with self.lock: return self.poses[robot], self.odom_seen[robot] def scan(self, robot): with self.lock: return self.front_distance[robot], self.scan_seen[robot] def command(self, robot, x=0.0, y=0.0, yaw=0.0, immediate=False): now = time.monotonic() old_x, old_y, old_yaw, old_time = self.last_command[robot] if immediate: next_x, next_y, next_yaw = x, y, yaw else: dt = min(0.15, max(0.02, now - old_time)) linear_step, angular_step = 0.65 * dt, 1.2 * dt next_x = old_x + clamp(x - old_x, linear_step) next_y = old_y + clamp(y - old_y, linear_step) next_yaw = old_yaw + clamp(yaw - old_yaw, angular_step) message = Twist() message.linear.x = next_x message.linear.y = next_y if robot == "RMC1" else 0.0 message.angular.z = next_yaw self.cmd_publishers[robot].publish(message) self.last_command[robot] = (next_x, next_y, next_yaw, now) def stop_all(self): for _ in range(3): for robot in ROBOT_NAMES: self.command(robot, immediate=True) time.sleep(0.04) def set_lift(self, raised, allow_already=False): 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: updates_before = self.lift_updates for _ in range(5): self.lift_publisher.publish(Float64(data=height)) time.sleep(0.1) started = time.monotonic() success = False while time.monotonic() - started < 20.0: with self.lock: status = self.lift_status received_update = self.lift_updates > updates_before # Реальный драйвер публикует 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: event_log.write( "LIFT_ALREADY_AT_TARGET", state="up" if raised else "down", status=status, ) success = True break time.sleep(0.1) if not success: raise RuntimeError(f"RMC2: лифт не завершил движение, status={status}") event_log.write("LIFT_DONE", state="up" if raised else "down", status=status) return WarehouseNode(), rclpy, MultiThreadedExecutor class Motion: def __init__(self, node, args, event_log, anchors): self.node = node self.args = args self.log = event_log self.anchors = anchors def wait_ready(self): deadline = time.monotonic() + 20.0 while time.monotonic() < deadline: odometry_ready = all( self.node.pose(robot)[0] is not None for robot in ROBOT_NAMES ) lidar_ready = all(self.node.scan(robot)[1] > 0.0 for robot in ROBOT_NAMES) if odometry_ready and lidar_ready: return time.sleep(0.1) missing_odom = [robot for robot in ROBOT_NAMES if self.node.pose(robot)[0] is None] missing_lidar = [robot for robot in ROBOT_NAMES if self.node.scan(robot)[1] == 0.0] raise RuntimeError( f"Датчики не готовы; odometry={missing_odom or 'ok'}, " f"scan_front={missing_lidar or 'ok'}" ) def wait_if_stopped(self): announced = False while self.node.emergency.is_set(): self.node.stop_all() if not announced: self.log.write("PAUSED") announced = True if self.node.abort.is_set(): raise RuntimeError("Остановлено оператором") time.sleep(0.1) def drive_route( self, robot, name, route, final_world_yaw, carrying=False, rack_entry=False, rack_exit=False, ): self.log.write("ROUTE", robot=robot, stage=name, markers=route, carrying=carrying) for index, marker in enumerate(route[1:], start=1): x, y = marker_xy(marker) if index == len(route) - 1: world_yaw = final_world_yaw else: nx, ny = marker_xy(route[index + 1]) world_yaw = math.atan2(ny - y, nx - x) target = self.anchors[robot].to_odom(x, y, world_yaw) self.drive_to( robot, marker, target, 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, precise=False, ): started = time.monotonic() obstacle_since = None while time.monotonic() - started < self.args.waypoint_timeout: if self.node.abort.is_set(): raise RuntimeError("Остановлено оператором") self.wait_if_stopped() pose, seen = self.node.pose(robot) if pose is None or time.monotonic() - seen > self.args.sensor_timeout: self.node.command(robot, immediate=True) raise RuntimeError(f"{robot}: потеря odometry") x, y, yaw = pose tx, ty, target_yaw = target dx, dy = tx - x, ty - y distance = math.hypot(dx, dy) front, scan_seen = self.node.scan(robot) fresh_scan = time.monotonic() - scan_seen <= self.args.sensor_timeout stop_distance = 0.34 if robot == "RMC2" else 0.30 # Стеллаж имеет размер около 0.95 м и стоит центром на ArUco. # На финальном въезде RMC2 штатно едет под его платформу. rack_zone = allow_rack_entry and distance < 1.10 blocked = fresh_scan and front < stop_distance and not rack_zone if blocked: self.node.command(robot, immediate=True) if obstacle_since is None: obstacle_since = time.monotonic() self.log.write( "OBSTACLE_STOP", robot=robot, marker=marker, distance=round(front, 3) ) if time.monotonic() - obstacle_since > 12.0: raise RuntimeError(f"{robot}: препятствие не освободило путь") time.sleep(0.08) continue obstacle_since = None yaw_error = wrap_angle(target_yaw - yaw) 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) ) return self.node.command(robot, yaw=clamp(yaw_error * 1.5, 0.42 if carrying else 0.55)) time.sleep(0.05) continue if robot == "RMC1": cosine, sine = math.cos(yaw), math.sin(yaw) body_x = cosine * dx + sine * dy body_y = -sine * dx + cosine * dy speed = 0.23 if carrying else 0.32 self.node.command( robot, x=clamp(body_x * 0.75, speed), y=clamp(body_y * 0.75, speed), yaw=clamp(yaw_error * 1.2, 0.40), ) else: heading_error = wrap_angle(math.atan2(dy, dx) - yaw) if abs(heading_error) > 0.18: self.node.command( robot, yaw=clamp(heading_error * 1.45, 0.42 if carrying else 0.58) ) 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)), yaw=clamp(heading_error * 1.2, 0.30), ) time.sleep(0.05) self.node.command(robot, immediate=True) raise RuntimeError(f"{robot}: таймаут движения к marker {marker}") class ArmWorkflow: """Тонкая обвязка проверенного зрения и MoveIt из модуля В.""" def __init__(self, args, event_log, executor, target): if str(ROOT) not in sys.path: sys.path.insert(0, str(ROOT)) try: import cv2 import numpy as np import yaml from ament_index_python.packages import get_package_share_directory from cv_bridge import CvBridge from geometry_msgs.msg import PoseStamped from moveit.planning import MoveItPy from moveit_configs_utils import MoveItConfigsBuilder from rclpy.duration import Duration 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 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 from module3.module3 import ( Arm, build_vision_node, card_near_center, choose_target, estimate_pose, find_targets, ) except ImportError as error: raise RuntimeError( f"Нет зависимости {error.name}: запустите из module3/.venv " "после source ROS или используйте --skip-arm" ) from error if int(np.__version__.split(".", 1)[0]) >= 2: raise RuntimeError("Для cv_bridge нужен numpy<2 в module3/.venv") weights = Path(args.weights).expanduser() if not weights.is_absolute(): weights = ROOT / weights if not weights.is_file(): raise RuntimeError(f"Не найдены веса YOLO: {weights}") self.args = args self.log = event_log self.executor = executor self.target = target self.cv2 = cv2 self.np = np self.find_targets = find_targets self.choose_target = choose_target self.estimate_pose = estimate_pose self.card_near_center = card_near_center imports = ( Node, CvBridge, Image, CompressedImage, CameraInfo, qos_profile_sensor_data, Buffer, TransformListener, Duration, Time, TFMessage, Parameter, QoSProfile, ReliabilityPolicy, DurabilityPolicy, ) 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)) package_dir = Path(get_package_share_directory("ar_webots_fms_ros2")) moveit_config = ( MoveItConfigsBuilder("arm95") .robot_description(file_path=str(package_dir / "resource/urdf/arm95_webots.urdf")) .trajectory_execution( file_path=str(package_dir / "resource/config/moveit_controllers.yaml") ) .moveit_cpp(file_path=str(package_dir / "resource/config/moveit_cpp.yaml")) .to_moveit_configs() .to_dict() ) 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 try: self.arm = Arm( MoveItPy, params_file, PoseStamped, quaternion_from_euler, event_log, base_frame=args.base_frame, ) finally: Path(params_file).unlink(missing_ok=True) self.log.write("MOVEIT_READY") def detect(self): started = time.monotonic() last_report = 0.0 seen = {} while time.monotonic() - started < self.args.vision_timeout: frame = self.vision.latest_frame() if frame is None: time.sleep(0.05) continue result = self.model.predict( source=frame, conf=self.args.conf, imgsz=512, device="cpu", verbose=False, )[0] if result.boxes is not None: for box in result.boxes: name = str(result.names[int(box.cls.item())]).lower() seen[name] = max(seen.get(name, 0.0), float(box.conf.item())) detections = self.find_targets( result, frame, self.target, self.cv2, self.np ) detection = self.choose_target(self.vision, detections, self.args.plane_z) if detection is not None: self.log.write( "DETECTION", target=self.target, confidence=round(detection.confidence, 4), ) return detection if time.monotonic() - last_report > 5.0: self.log.write( "VISION_WAIT", target=self.target, seen={key: round(value, 3) for key, value in seen.items()}, ) last_report = time.monotonic() raise RuntimeError( f"Класс {self.target} не найден за {self.args.vision_timeout:.0f} с; " f"видел: {seen or 'ничего'}" ) def pick(self): self.log.write("ARM_STAGE", stage="pick_start") self.arm.open() self.arm.initial("транспортировочное положение перед захватом") detection = self.detect() x, y, yaw = self.estimate_pose(self.vision, detection, self.args.plane_z) self.log.write("PICK_TARGET", x=round(x, 4), y=round(y, 4), yaw=round(yaw, 4)) self.arm.pose(x, y, self.args.approach_z, yaw, "подход над деталью") # Камера приблизилась: уточняем положение по контуру карточки. time.sleep(0.6) frame = self.vision.latest_frame() if frame is not None: try: close = self.card_near_center(frame, self.target, self.cv2, self.np) refined_x, refined_y, refined_yaw = self.estimate_pose( self.vision, close, self.args.plane_z ) correction = math.hypot(refined_x - x, refined_y - y) if correction <= 0.12: x, y, yaw = refined_x, refined_y, refined_yaw self.log.write("PICK_REFINED", correction=round(correction, 4)) except Exception as error: self.log.write("PICK_REFINE_SKIPPED", reason=str(error)) self.arm.pose(x, y, self.args.approach_z, yaw, "точный подход") self.arm.pose(x, y, self.args.pick_z, yaw, "опустить схват") self.arm.close() self.arm.pose(x, y, self.args.approach_z, yaw, "поднять деталь") self.arm.initial("транспортировочное положение с деталью") self.log.write("DETAIL_PICKED", target=self.target) def place(self): self.log.write("ARM_STAGE", stage="place_start") x, y = self.args.drop_arm_x, self.args.drop_arm_y yaw = self.args.drop_arm_yaw self.arm.pose(x, y, self.args.approach_z, yaw, "подход к полке сдачи") self.arm.pose(x, y, self.args.drop_arm_z, yaw, "опустить деталь на полку") self.arm.open() self.arm.pose(x, y, self.args.approach_z, yaw, "отойти от детали") self.arm.initial("транспортировочное положение без детали") self.log.write("DETAIL_PLACED", target=self.target) def close(self): try: self.executor.remove_node(self.vision) self.vision.destroy_node() except Exception: pass def terminal_commands(node): while not node.abort.is_set(): try: command = input().strip().lower() except EOFError: return if command == "stop": node.emergency.set() elif command == "resume": node.emergency.clear() elif command in {"abort", "quit"}: node.abort.set() return def resolve_target(value): names = {"1": "hammer", "2": "wrench", "3": "pliers"} value = (value or "").strip().lower() return names.get(value, value) def run(args): target = resolve_target(args.target) if not target and not args.dry_run: target = resolve_target(input("Цель (1 hammer, 2 wrench, 3 pliers): ")) if not target: target = "hammer" if target not in {"hammer", "wrench", "pliers"}: raise ValueError(f"Неизвестная цель: {target}") scenario = make_scenario(args) routes = scenario.routes() print_plan(scenario, routes, target) if args.dry_run: return 0 if not args.auto: input("Проверьте точки и маршруты. Enter — начать: ") event_log = EventLog() event_log.write("MISSION_START", target=target, log=str(event_log.path)) mission_started = time.monotonic() node, rclpy, MultiThreadedExecutor = build_ros_node(args, event_log) executor = MultiThreadedExecutor(num_threads=3) executor.add_node(node) spin_thread = threading.Thread(target=executor.spin, daemon=True) spin_thread.start() exit_code = 0 arm_workflow = None try: initial_motion = Motion(node, args, event_log, {}) initial_motion.wait_ready() anchors = {} for robot, start_marker, start_yaw in ( ("RMC1", scenario.rmc1_start, args.rmc1_start_yaw), ("RMC2", scenario.rmc2_start, args.rmc2_start_yaw), ): pose, _ = node.pose(robot) world_x, world_y = marker_xy(start_marker) anchors[robot] = Anchor(world_x, world_y, start_yaw, *pose) event_log.write("LOCALIZED", robot=robot, marker=start_marker, odom=pose) motion = Motion(node, args, event_log, anchors) threading.Thread(target=terminal_commands, args=(node,), daemon=True).start() parking = routes["rmc2_clear_assembly"][-1] assembly_entry_yaw = face_marker(parking, scenario.assembly) node.set_lift(False, allow_already=True) motion.drive_route( "RMC2", "to_rack", routes["rmc2_to_rack"], args.rack_yaw, rack_entry=True ) node.set_lift(True) motion.drive_route( "RMC2", "rack_to_assembly", routes["rmc2_to_assembly"], assembly_entry_yaw, carrying=True, rack_exit=True, ) node.set_lift(False) motion.drive_route( "RMC2", "clear_assembly", routes["rmc2_clear_assembly"], face_marker(parking, scenario.assembly), rack_exit=True, ) assembly_yaw = face_marker(scenario.assembly_approach, scenario.assembly) motion.drive_route( "RMC1", "to_assembly", routes["rmc1_to_assembly"], assembly_yaw ) if args.skip_arm: event_log.write("ARM_SKIPPED", stage="pick", reason="--skip-arm") else: arm_workflow = ArmWorkflow(args, event_log, executor, target) arm_workflow.pick() delivery_yaw = face_marker(scenario.delivery_approach, scenario.delivery) motion.drive_route( "RMC1", "to_delivery", routes["rmc1_to_delivery"], delivery_yaw, carrying=not args.skip_arm, ) if args.skip_arm: event_log.write("ARM_SKIPPED", stage="place", reason="--skip-arm") else: arm_workflow.place() motion.drive_route( "RMC2", "back_to_assembly", routes["rmc2_back_to_assembly"], assembly_entry_yaw, rack_entry=True, ) node.set_lift(True) motion.drive_route( "RMC2", "assembly_to_rack", routes["rmc2_return_rack"], args.rack_yaw, carrying=True, rack_entry=True, rack_exit=True, ) node.set_lift(False) motion.drive_route( "RMC1", "return_start", routes["rmc1_return"], args.rmc1_start_yaw ) motion.drive_route( "RMC2", "return_start", routes["rmc2_return"], args.rmc2_start_yaw, rack_exit=True, ) event_log.write( "MISSION_FINISHED", target=target, elapsed_seconds=round(time.monotonic() - mission_started, 2), ) print("\nMISSION FINISHED: оба РМК вернулись на старт") except KeyboardInterrupt: event_log.write("MISSION_ABORTED", reason="Ctrl+C") exit_code = 130 except Exception as error: event_log.write("MISSION_FAILED", reason=str(error)) exit_code = 1 finally: node.abort.set() node.stop_all() if arm_workflow is not None: arm_workflow.close() if not executor.shutdown(timeout_sec=2.0): event_log.write("SHUTDOWN_TIMEOUT", component="ROS executor") # rclpy/MoveItPy Jazzy иногда зависают или падают в C++-деструкторах. # Нулевые скорости и логи уже отправлены; ОС безопасно освободит ресурсы. sys.stdout.flush() sys.stderr.flush() os._exit(exit_code) return exit_code def main(): try: raise SystemExit(run(arguments())) except (ValueError, ImportError) as error: raise SystemExit(str(error)) from error if __name__ == "__main__": main()