#!/usr/bin/env python3 """Модуль Б: один запуск, маршрут туда, пауза эксперта, маршрут обратно. python3 module_b.py --target 14 --sim Команды в терминале: Enter / go, stop, resume, return, quit. """ import argparse import heapq import json import math import queue import re import signal import sys import threading import time from datetime import datetime from pathlib import Path # Все параметры модуля Б находятся здесь. Перед реальным полем меняются в одном месте. ROWS = 6 COLS = 6 MARKER_SPACING = 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 SAFETY_MARGIN = 0.04 ROUTE_CLEARANCE = 0.44 MAX_LINEAR = 0.25 MAX_ANGULAR = 0.45 LINEAR_ACCEL = 0.35 ANGULAR_ACCEL = 0.80 POSITION_TOLERANCE = 0.025 YAW_TOLERANCE = 0.045 MOVE_YAW_TOLERANCE = 0.14 SENSOR_TIMEOUT = 1.5 MARKER_TIMEOUT = 0.8 WAYPOINT_TIMEOUT = 50.0 MISSION_TIMEOUT = 330.0 def wrap(angle): return math.atan2(math.sin(angle), math.cos(angle)) def clamp(value, low, high): return max(low, min(high, value)) def compose(a, b): """Переносит позу b из системы a в её родительскую систему.""" x, y, yaw = a c, s = math.cos(yaw), math.sin(yaw) return x + c*b[0] - s*b[1], y + s*b[0] + c*b[1], wrap(yaw+b[2]) def inverse(pose): x, y, yaw = pose c, s = math.cos(yaw), math.sin(yaw) return -c*x-s*y, s*x-c*y, -yaw def distance(a, b): return math.hypot(a[0]-b[0], a[1]-b[1]) def segment_distance(point, a, b): dx, dy = b[0]-a[0], b[1]-a[1] length2 = dx*dx+dy*dy if length2 == 0: return distance(point, a) t = clamp(((point[0]-a[0])*dx+(point[1]-a[1])*dy)/length2, 0, 1) return distance(point, (a[0]+t*dx, a[1]+t*dy)) class Field: """Регулярная сетка ArUco и кратчайший путь по ней.""" def __init__(self): 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) self.graph[marker] = [ r*COLS+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 ] def nearest(self, x, y): return min(self.poses, key=lambda marker: distance((x, y), self.poses[marker])) def route(self, start, goal, obstacle_points=(), forbidden_edges=()): """Дейкстра: кратчайший по длине свободный путь.""" if start not in self.poses or goal not in self.poses: raise ValueError("ID старта или цели отсутствует в графе") if start in BLOCKED_MARKERS or goal in BLOCKED_MARKERS: raise ValueError("Старт или цель заблокированы") forbidden = {frozenset(edge) for edge in forbidden_edges} costs = {start: 0.0} parents = {start: None} queue_path = [(0.0, start)] while queue_path: cost, current = heapq.heappop(queue_path) if cost > costs[current]: continue if current == goal: path = [] while current is not None: path.append(current) current = parents[current] return path[::-1] for neighbor in self.graph[current]: edge = frozenset((current, neighbor)) 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): continue new_cost = cost+distance(a, b) if new_cost < costs.get(neighbor, math.inf): costs[neighbor] = new_cost parents[neighbor] = current heapq.heappush(queue_path, (new_cost, neighbor)) raise ValueError(f"Нет безопасного маршрута {start} -> {goal}") def arguments(): parser = argparse.ArgumentParser(description=__doc__) 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("--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, секунд симуляции") parser.add_argument("--dry-run", action="store_true", help="Показать путь без ROS и без движения") parser.add_argument("--start", type=int, default=0, help="Старт только для --dry-run") args = parser.parse_args() if args.auto and not args.sim: parser.error("--auto разрешён только с --sim") if args.return_delay < 0 or not math.isfinite(args.return_delay): parser.error("--return-delay должен быть конечным неотрицательным числом") if args.target_yaw is not None and not math.isfinite(args.target_yaw): parser.error("--target-yaw должен быть конечным числом") if args.xy and not all(math.isfinite(v) for v in args.xy): parser.error("--xy должен содержать конечные числа") return args def main(): args = arguments() field = Field() 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("Целевая метка отсутствует в графе или заблокирована") if args.dry_run: print("ROUTE", field.route(args.start, target), "TARGET", field.poses[target]) return import rclpy from geometry_msgs.msg import Twist 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.signals import SignalHandlerOptions from rclpy.time import Time from sensor_msgs.msg import LaserScan from std_msgs.msg import Bool, String from tf2_ros import Buffer, TransformListener, TransformException def yaw(q): return math.atan2(2*(q.w*q.z+q.x*q.y), 1-2*(q.y*q.y+q.z*q.z)) def tf_pose(transform): t = transform.transform return t.translation.x, t.translation.y, yaw(t.rotation) class Mission(Node): def __init__(self): super().__init__("mvch_module_b") self.set_parameters([rclpy.parameter.Parameter("use_sim_time", value=args.sim)]) self.cfg = { "max_linear": MAX_LINEAR, "max_angular": MAX_ANGULAR, "linear_accel": LINEAR_ACCEL, "angular_accel": ANGULAR_ACCEL, "position_tolerance": POSITION_TOLERANCE, "yaw_tolerance": YAW_TOLERANCE, "move_yaw_tolerance": MOVE_YAW_TOLERANCE, "sensor_timeout": SENSOR_TIMEOUT, "marker_timeout": MARKER_TIMEOUT, "waypoint_timeout": WAYPOINT_TIMEOUT, "mission_timeout": MISSION_TIMEOUT, } 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, } 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) 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) 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() self.state, self.leg = "WAIT_SENSORS", "outbound" self.odom = self.map_from_odom = self.pose = None self.odom_wall = self.scan_wall = 0.0 self.scan = None self.points, self.body_points = [], [] self.lidar_tf_wall = 0.0 self.lidar_tf_error = "TF лидара ещё не получен" self.marker = None self.marker_wall = self.marker_stamp = 0.0 self.marker_distance = math.inf self.start = self.current = None self.start_yaw = 0.0 self.goal, self.route, self.index = target, [], 0 self.forbidden = set() self.paused = False self.terminal = False self.failed = False self.v = self.w = 0.0 self.last_sim = self.now() self.last_wall = self.clock_wall = time.monotonic() self.last_clock = self.last_sim self.leg_started = self.waypoint_started = self.last_sim self.total_time = 0.0 self.last_status = 0.0 self.wait_reason = "" log_dir = Path(__file__).parent / "reports" log_dir.mkdir(exist_ok=True) self.log_path = log_dir / f"module_b_{datetime.now():%Y%m%d_%H%M%S_%f}.jsonl" self.log_file = self.log_path.open("a", encoding="utf-8", buffering=1) # Таймер на монотонных часах: остановка сработает даже при паузе /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, instructions="Enter/go: старт; return: построить возврат; stop/resume; quit") def now(self): return self.get_clock().now().nanoseconds / 1e9 def log(self, event, **data): item = dict(time=datetime.now().isoformat(timespec="milliseconds"), sim_time=round(self.now(), 3), event=event, leg=self.leg, **data) line = json.dumps(item, ensure_ascii=False) print(line, flush=True) self.log_file.write(line+"\n") def on_odom(self, msg): p = msg.pose.pose pose = p.position.x, p.position.y, yaw(p.orientation) if all(math.isfinite(v) for v in pose): self.odom, self.odom_wall = pose, time.monotonic() def on_scan(self, msg): # Пустой/повреждённый скан не считается свободным пространством. if not msg.ranges or msg.angle_increment == 0: return if not any(math.isfinite(r) and msg.range_min <= r <= msg.range_max for r in msg.ranges): return self.scan, self.scan_wall = msg, time.monotonic() def on_marker(self, msg): match = re.fullmatch(re.escape(MARKER_PREFIX)+r"(\d+)", msg.data.strip()) if not match and msg.data.strip().isdigit(): marker = int(msg.data.strip()) elif match: marker = int(match[1]) else: return if marker in field.poses: self.marker, self.marker_wall = marker, time.monotonic() def on_stop(self, msg): if msg.data: self.paused = True self.stop() self.log("EMERGENCY_STOP") # false сам по себе не разрешает движение: требуется resume. def localize(self): if self.odom is None or self.marker is None: return frame = MARKER_PREFIX+str(self.marker) try: transform = self.buffer.lookup_transform(self.odom_frame, frame, Time()) stamp = transform.header.stamp.sec+transform.header.stamp.nanosec/1e9 if not 0 <= self.now()-stamp < self.cfg["marker_timeout"]: return if time.monotonic()-self.marker_wall > self.cfg["sensor_timeout"]: return odom_marker = tf_pose(transform) self.marker_distance = distance(odom_marker, self.odom) if stamp <= self.marker_stamp: return correction = compose(field.poses[self.marker], inverse(odom_marker)) if self.map_from_odom is not None: old_pose = compose(self.map_from_odom, self.odom) new_pose = compose(correction, self.odom) if distance(old_pose, new_pose) > 0.4 or abs(wrap(old_pose[2]-new_pose[2])) > 0.6: return # Не принимаем скачок распознавания; таймаут остановит у метки. self.map_from_odom, self.marker_stamp = correction, stamp except TransformException: pass def scan_points(self): scan = self.scan try: # В этом симуляторе laser_merged повёрнут на 120 градусов! mount = tf_pose(self.buffer.lookup_transform(self.base, scan.header.frame_id, Time())) # Последний TF достаточно свежий: scan проверяется отдельно по wall-clock. # Запрос строго по stamp иногда попадает между двумя odom TF и кратко падает. at_scan = tf_pose(self.buffer.lookup_transform(self.odom_frame, self.base, Time())) except TransformException as error: self.lidar_tf_error = str(error) return False scan_in_map = compose(compose(self.map_from_odom, at_scan), mount) body_from_map = inverse(self.pose) points = {} for i, r in enumerate(scan.ranges): if not math.isfinite(r) or not scan.range_min <= r <= scan.range_max: continue angle = scan.angle_min+i*scan.angle_increment p = compose(scan_in_map, (r*math.cos(angle), r*math.sin(angle), 0))[:2] b = compose(body_from_map, (*p, 0)) if abs(b[0]) < self.robot["half_length"] and abs(b[1]) < self.robot["half_width"]: continue # собственный корпус, не внешнее препятствие points[(round(p[0]/0.04), round(p[1]/0.04))] = p self.points = list(points.values()) self.body_points = [compose(body_from_map, (*p, 0)) for p in self.points] self.lidar_tf_wall = time.monotonic() self.lidar_tf_error = "" return True def plan(self): self.stop() try: self.route = field.route( self.current, self.goal, self.points, self.forbidden ) except ValueError as error: self.abort(str(error)) return self.index = 1 self.state = "WAIT_GO" direction = "К ЦЕЛИ" if self.leg == "outbound" else "НА СТАРТ" print(f"\nМАРШРУТ {direction}: {' -> '.join(map(str, self.route))}", flush=True) print("Ровер пока стоит. Эксперт проверяет маршрут; Enter/go разрешает движение.\n", flush=True) self.log("ROUTE", markers=self.route, goal=self.goal) self.log("WAIT_GO", message="Эксперт фиксирует маршрут. Enter/go разрешает движение.") def stop(self): self.v = self.w = 0.0 self.pub.publish(Twist()) def abort(self, reason): self.stop() self.failed = self.terminal = True self.state = "FAILED" self.log("FAILED", reason=reason, current=self.current) def command(self, command): command = command.strip().lower() if command in ("stop", "s"): self.paused = True self.stop() self.log("EMERGENCY_STOP") elif command == "resume": self.paused = False self.log("RESUME") elif command in ("quit", "q"): self.abort("Остановлено оператором") elif self.state == "WAIT_RETURN" and command in ("", "return", "go"): self.leg, self.goal = "return", self.start self.forbidden.clear() self.plan() elif self.state == "WAIT_GO" and command in ("", "go") and not self.paused: # Новая преграда за время паузы: сначала показать новый маршрут. try: updated = field.route( self.current, self.goal, self.points, self.forbidden ) except ValueError as error: self.abort(str(error)) return if updated != self.route: self.plan() return self.state = "DRIVE" self.leg_started = self.waypoint_started = self.now() self.log("movement_start", markers=self.route) def blocked_motion(self, v, w): margin = self.robot["safety_margin"] # Тормозной путь + время реакции; размеры считаются от центра корпуса. braking = v*v/(2*self.cfg["linear_accel"])+abs(v)*0.25 front = self.robot["half_length"]+margin+braking width = self.robot["half_width"]+margin if abs(v) > 0.001: direction = 1 if v > 0 else -1 return any(0 < direction*x < front and abs(y) < width for x, y, _ in self.body_points) if abs(w) > 0.001: return any(math.hypot(x, y) < self.robot["clearance"] for x, y, _ in self.body_points) return False def send(self, v, w, dt): if self.blocked_motion(v, w): self.stop() return False self.v += clamp(v-self.v, -self.cfg["linear_accel"]*dt, self.cfg["linear_accel"]*dt) self.w += clamp(w-self.w, -self.cfg["angular_accel"]*dt, self.cfg["angular_accel"]*dt) # Поворот на месте и аварийная остановка не должны оставлять линейный хвост. if v == 0: self.v = 0.0 if self.blocked_motion(self.v, self.w): self.stop() return False msg = Twist() msg.linear.x, msg.angular.z = float(self.v), float(self.w) self.pub.publish(msg) return True def drive(self, dt): if self.now()-self.waypoint_started > self.cfg["waypoint_timeout"]: self.abort("Таймаут точки: проверить камеру, препятствие и положение робота") return if self.state == "ALIGN": desired = args.target_yaw if self.leg == "outbound" else self.start_yaw error = wrap(desired-self.pose[2]) if abs(error) <= self.cfg["yaw_tolerance"]: self.arrived() else: self.send(0, clamp(1.4*error, -self.cfg["max_angular"], self.cfg["max_angular"]), dt) return if self.index >= len(self.route) and self.state != "BACKTRACK": if self.leg == "return" or args.target_yaw is not None: self.state = "ALIGN" else: self.arrived() return marker = self.current if self.state == "BACKTRACK" else self.route[self.index] target_pose = field.poses[marker] dx, dy = target_pose[0]-self.pose[0], target_pose[1]-self.pose[1] d = math.hypot(dx, dy) fresh = self.marker == marker and 0 <= self.now()-self.marker_stamp < self.cfg["marker_timeout"] if d < self.cfg["position_tolerance"] and fresh and self.marker_distance < 0.04: self.stop() if self.state == "BACKTRACK": self.log("BACKTRACK_DONE", marker=marker) try: self.route = field.route( self.current, self.goal, self.points, self.forbidden ) except ValueError as error: self.abort(str(error)) return self.index, self.state = 1, "DRIVE" self.log("ROUTE_REPLANNED", markers=self.route) else: self.current = marker self.index += 1 self.log("WAYPOINT", marker=marker, pose=list(self.pose), marker_error=self.marker_distance) self.waypoint_started = self.now() return if d < self.cfg["position_tolerance"] and not fresh: self.stop() # Без свежей метки достижение НЕ засчитываем. return error = wrap(math.atan2(dy, dx)-self.pose[2]) # Если следующая метка сзади, едем к ней задним ходом. Это не даёт # роботу разворачиваться корпусом вплотную к стеллажам у marker 33. reverse = self.state == "BACKTRACK" or abs(error) > math.pi/2 if reverse: error = wrap(error-math.pi) v = min(self.cfg["max_linear"], 0.8*d) if reverse: v = -min(v, 0.12) if abs(error) > self.cfg["move_yaw_tolerance"]: v = 0 w = clamp(1.5*error, -self.cfg["max_angular"], self.cfg["max_angular"]) if not self.send(v, w, dt) and self.state == "DRIVE": edge = (self.current, marker) self.forbidden.add(edge) self.log("OBSTACLE", edge=edge, pose=list(self.pose)) # Вернуться по уже пройденному ребру, затем ехать по новому графу. self.state = "BACKTRACK" self.waypoint_started = self.now() self.log("BACKTRACK", marker=self.current) def arrived(self): self.stop() elapsed = self.now()-self.leg_started self.total_time += elapsed self.log("movement_stop", marker=self.goal, seconds=round(elapsed, 3)) if self.leg == "outbound": self.state = "WAIT_RETURN" self.return_wait = self.now() self.log("TARGET_REACHED", marker=self.goal, message="Выставьте препятствие. return/Enter строит обратный маршрут.") else: self.state, self.terminal = "FINISHED", True self.log("MISSION_FINISHED", marker=self.start, movement_seconds=round(self.total_time, 3)) def tick(self): wall, now = time.monotonic(), self.now() dt = clamp(now-self.last_sim, 0, 0.15) if now < self.last_sim-0.01: self.abort("Часы симуляции сброшены; запустите новую попытку") return if now != self.last_clock: self.clock_wall, self.last_clock = wall, now self.last_sim = now 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) reasons = [] if self.pose is None: reasons.append("нет локализации") if self.scan is None: reasons.append("нет scan") if wall-self.odom_wall >= SENSOR_TIMEOUT: reasons.append(f"odometry старше {wall-self.odom_wall:.2f}с") if wall-self.scan_wall >= SENSOR_TIMEOUT: reasons.append(f"scan старше {wall-self.scan_wall:.2f}с") if wall-self.clock_wall >= SENSOR_TIMEOUT: reasons.append(f"clock старше {wall-self.clock_wall:.2f}с") healthy = not reasons if healthy and not self.scan_points(): # Один краткий промах TF не должен ставить миссию на вечную паузу. if wall-self.lidar_tf_wall >= SENSOR_TIMEOUT: reasons.append("TF лидара: "+self.lidar_tf_error) healthy = False if not healthy: self.stop() if self.state != "WAIT_SENSORS" and not self.paused: self.paused = True self.log("SENSOR_STOP", reason="; ".join(reasons), message="После восстановления введите resume") while not self.commands.empty(): command = self.commands.get_nowait() if healthy or command.strip() in ("stop", "quit", "q", "s"): self.command(command) if not healthy or self.paused or self.terminal: self.stop() return if wall-self.last_status > 1: msg = String() msg.data = json.dumps(dict(state=self.state, leg=self.leg, pose=self.pose, current=self.current, route=self.route)) self.status_pub.publish(msg) self.last_status = wall if self.state == "WAIT_SENSORS": if self.marker_distance > 0.04: return self.start = self.current = self.marker self.start_yaw = self.pose[2] self.log("LOCALIZED", marker=self.start, pose=list(self.pose)) self.plan() elif self.state == "WAIT_GO": self.stop() if args.auto: self.command("go") elif self.state == "WAIT_RETURN": self.stop() if args.auto and now-self.return_wait >= args.return_delay: self.command("return") else: if self.total_time+now-self.leg_started > self.cfg["mission_timeout"]: self.abort("Превышено время миссии") else: self.drive(dt) rclpy.init(signal_handler_options=SignalHandlerOptions.NO) node = Mission() quitting = threading.Event() signal.signal(signal.SIGINT, lambda *_: quitting.set()) signal.signal(signal.SIGTERM, lambda *_: quitting.set()) def read_commands(): for line in sys.stdin: node.commands.put(line) threading.Thread(target=read_commands, daemon=True).start() try: while rclpy.ok() and not quitting.is_set() and not node.terminal: rclpy.spin_once(node, timeout_sec=0.05) if quitting.is_set() and not node.terminal: node.abort("SIGINT/SIGTERM") except Exception as error: node.abort(f"{type(error).__name__}: {error}") raise finally: # Перед закрытием соединения несколько раз отправить настоящий нулевой Twist. for _ in range(5): node.stop() time.sleep(0.04) node.log_file.close() node.destroy_node() rclpy.shutdown() if node.failed: raise SystemExit(1) if __name__ == "__main__": main()