#!/usr/bin/env python3 import argparse import json import math import re import time from collections import deque from datetime import datetime from pathlib import Path ROWS = 5 COLUMNS = 5 SPACING = 1.0 MAX_SPEED = 0.35 MAX_ANGULAR = 0.60 STOP_DISTANCE = 0.65 TOLERANCE = 0.07 def clamp(value, low, high): return max(low, min(high, value)) def wrap(angle): return math.atan2(math.sin(angle), math.cos(angle)) def yaw(quaternion): return math.atan2( 2 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y), 1 - 2 * (quaternion.y**2 + quaternion.z**2), ) def compose(a, b): cosine, sine = math.cos(a[2]), math.sin(a[2]) return ( a[0] + cosine * b[0] - sine * b[1], a[1] + sine * b[0] + cosine * b[1], wrap(a[2] + b[2]), ) def inverse(pose): cosine, sine = math.cos(pose[2]), math.sin(pose[2]) return ( -cosine * pose[0] - sine * pose[1], sine * pose[0] - cosine * pose[1], -pose[2], ) def marker_pose(marker, columns, spacing): row, column = divmod(marker, columns) return -row * spacing, column * spacing, 0.0 def shortest_path(start, goal, rows, columns, blocked, forbidden=()): if start in blocked or goal in blocked: raise RuntimeError("Старт или цель находятся в закрытой ячейке") forbidden = {frozenset(edge) for edge in forbidden} search = deque([start]) parent = {start: None} while search: current = search.popleft() if current == goal: break row, column = divmod(current, columns) for next_row, next_column in ( (row - 1, column), (row + 1, column), (row, column - 1), (row, column + 1), ): if not (0 <= next_row < rows and 0 <= next_column < columns): continue candidate = next_row * columns + next_column edge = frozenset((current, candidate)) if candidate in blocked or candidate in parent or edge in forbidden: continue parent[candidate] = current search.append(candidate) if goal not in parent: raise RuntimeError(f"Нет маршрута {start} -> {goal}") route = [] current = goal while current is not None: route.append(current) current = parent[current] return list(reversed(route)) def arguments(): parser = argparse.ArgumentParser() parser.add_argument("--target", type=int, required=True) parser.add_argument("--rows", type=int, default=ROWS) parser.add_argument("--columns", type=int, default=COLUMNS) parser.add_argument("--spacing", type=float, default=SPACING) parser.add_argument("--blocked", type=int, nargs="*", default=[]) parser.add_argument("--speed", type=float, default=MAX_SPEED) parser.add_argument("--angular", type=float, default=MAX_ANGULAR) parser.add_argument("--stop-distance", type=float, default=STOP_DISTANCE) parser.add_argument("--tolerance", type=float, default=TOLERANCE) parser.add_argument("--scan-topic", default="/RMC2/scan_front") parser.add_argument("--sim", action="store_true") parser.add_argument("--dry-run", action="store_true") parser.add_argument("--start", type=int, default=0) args = parser.parse_args() count = args.rows * args.columns if args.rows < 1 or args.columns < 1: parser.error("rows и columns должны быть больше нуля") if not 0 <= args.target < count: parser.error(f"target должен быть в диапазоне 0..{count - 1}") if args.spacing <= 0 or args.stop_distance <= 0 or args.tolerance <= 0: parser.error("расстояния должны быть больше нуля") if not 0 < args.speed <= 0.5: parser.error("скорость RMC2 должна быть в диапазоне 0..0.5 м/с") if not 0 < args.angular <= 1.0: parser.error("angular должна быть в диапазоне 0..1.0 рад/с") if args.dry_run and not 0 <= args.start < count: parser.error(f"start должен быть в диапазоне 0..{count - 1}") return args def main(): args = arguments() blocked = set(args.blocked) if args.dry_run: there = shortest_path(args.start, args.target, args.rows, args.columns, blocked) back = shortest_path(args.target, args.start, args.rows, args.columns, blocked) print("К ЦЕЛИ:", " -> ".join(map(str, there))) print("НА СТАРТ:", " -> ".join(map(str, back))) return import rclpy from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry from rclpy.node import Node 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 String from tf2_msgs.msg import TFMessage from tf2_ros import Buffer, TransformException, TransformListener class Rover(Node): def __init__(self): super().__init__("mvch_module_b") self.cmd = self.create_publisher(Twist, "/RMC2/cmd_vel", 10) self.create_subscription( Odometry, "/RMC2/odometry", self.on_odom, qos_profile_sensor_data ) self.create_subscription( LaserScan, args.scan_topic, self.on_scan, qos_profile_sensor_data ) self.create_subscription( String, "/RMC2/camera_bottom/aruco_id", self.on_marker, 10 ) self.create_subscription(String, "/RMC2/aruco_id", self.on_marker, 10) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) tf_qos = QoSProfile( depth=100, reliability=ReliabilityPolicy.BEST_EFFORT, durability=DurabilityPolicy.VOLATILE, ) static_qos = QoSProfile( depth=100, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL, ) self.create_subscription(TFMessage, "/RMC2/tf", self.on_tf, tf_qos) self.create_subscription( TFMessage, "/RMC2/tf_static", self.on_tf_static, static_qos ) self.odom = None self.odom_frame = "RMC2/odom" self.map_from_odom = None self.pose = None self.marker = None self.marker_seen = 0.0 self.marker_error = math.inf self.front = math.inf self.current = None self.leg = "outbound" reports = Path(__file__).parent / "reports" reports.mkdir(exist_ok=True) self.log_path = reports / f"module_b_{datetime.now():%Y%m%d_%H%M%S}.jsonl" self.log_file = self.log_path.open("a", encoding="utf-8", buffering=1) def log(self, event, **data): item = { "time": datetime.now().isoformat(timespec="milliseconds"), "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_tf(self, message): for transform in message.transforms: self.tf_buffer.set_transform(transform, "RMC2 tf") def on_tf_static(self, message): for transform in message.transforms: self.tf_buffer.set_transform_static(transform, "RMC2 tf_static") def on_odom(self, message): frame = message.header.frame_id.strip("/") if frame: self.odom_frame = frame pose = message.pose.pose self.odom = pose.position.x, pose.position.y, yaw(pose.orientation) def on_marker(self, message): match = re.search(r"\d+", message.data) if not match: return marker = int(match.group()) if not 0 <= marker < args.rows * args.columns: return if marker != self.marker: self.log("MARKER", marker=marker) self.marker = marker self.marker_seen = time.monotonic() def on_scan(self, message): front = [] for index, value in enumerate(message.ranges): angle = message.angle_min + index * message.angle_increment if abs(angle) <= math.radians(25): if math.isfinite(value) and message.range_min <= value <= message.range_max: front.append(value) self.front = min(front, default=math.inf) def localize(self): if self.odom is None or self.marker is None: return if time.monotonic() - self.marker_seen > 0.8: return try: transform = self.tf_buffer.lookup_transform( self.odom_frame, f"aruco_{self.marker}", Time() ).transform except TransformException: return odom_marker = ( transform.translation.x, transform.translation.y, yaw(transform.rotation), ) self.map_from_odom = compose( marker_pose(self.marker, args.columns, args.spacing), inverse(odom_marker) ) self.pose = compose(self.map_from_odom, self.odom) self.marker_error = math.hypot( odom_marker[0] - self.odom[0], odom_marker[1] - self.odom[1] ) def update(self): rclpy.spin_once(self, timeout_sec=0.05) self.localize() if self.map_from_odom is not None and self.odom is not None: self.pose = compose(self.map_from_odom, self.odom) def stop(self): self.cmd.publish(Twist()) def send(self, linear, angular): command = Twist() command.linear.x = float(linear) command.angular.z = float(angular) self.cmd.publish(command) def wait_start(self): print("Жду ArUco под ровером. Старт определится автоматически.") while rclpy.ok(): self.update() self.stop() if self.pose is not None and self.marker_error <= args.tolerance: self.current = self.marker self.log("START_FOUND", marker=self.current) return self.current def reached(self, marker): return ( self.marker == marker and time.monotonic() - self.marker_seen < 0.8 and self.marker_error <= args.tolerance ) def drive_to(self, marker, check_obstacle=True): target = marker_pose(marker, args.columns, args.spacing) while rclpy.ok(): self.update() if self.pose is None: self.stop() continue if self.reached(marker): self.stop() return True dx = target[0] - self.pose[0] dy = target[1] - self.pose[1] distance = math.hypot(dx, dy) error = wrap(math.atan2(dy, dx) - self.pose[2]) linear = 0.0 if abs(error) > 0.16 else min(args.speed, max(0.04, 0.7 * distance)) angular = clamp(1.4 * error, -args.angular, args.angular) if check_obstacle and linear > 0 and self.front < args.stop_distance: self.stop() return False self.send(linear, angular) def follow(self, goal, forbidden): route = shortest_path( self.current, goal, args.rows, args.columns, blocked, forbidden ) index = 1 while index < len(route): next_marker = route[index] if self.drive_to(next_marker): self.current = next_marker self.log("WAYPOINT", marker=self.current) index += 1 continue edge = frozenset((self.current, next_marker)) forbidden.add(edge) self.log( "OBSTACLE", edge=sorted(edge), distance=round(self.front, 3), ) print(f"Препятствие перед {next_marker}. Возвращаюсь к {self.current}.") self.drive_to(self.current, check_obstacle=False) route = shortest_path( self.current, goal, args.rows, args.columns, blocked, forbidden ) index = 1 show_route(self, "НОВЫЙ МАРШРУТ", route, "ROUTE_REPLANNED") return route def show_route(rover, title, route, event): print(f"{title}: {' -> '.join(map(str, route))}") rover.log(event, markers=route) rclpy.init(signal_handler_options=SignalHandlerOptions.NO) rover = Rover() exit_code = 0 try: start = rover.wait_start() route_there = shortest_path(start, args.target, args.rows, args.columns, blocked) route_back = shortest_path(args.target, start, args.rows, args.columns, blocked) print() show_route(rover, "МАРШРУТ К ЦЕЛИ", route_there, "ROUTE_TO_TARGET") show_route(rover, "МАРШРУТ НА СТАРТ", route_back, "ROUTE_TO_START") input("\nEnter — начать движение: ") started = time.monotonic() rover.log("MOVEMENT_START", markers=route_there) rover.follow(args.target, set()) first_time = time.monotonic() - started rover.log("MOVEMENT_FINISHED", marker=args.target, seconds=round(first_time, 2)) rover.log("TARGET_REACHED", marker=args.target, seconds=round(first_time, 2)) input("\nЦель достигнута. Поставьте препятствие и нажмите Enter: ") rover.leg = "return" show_route(rover, "МАРШРУТ НА СТАРТ", route_back, "ROUTE_TO_START") input("Enter — начать возврат: ") started = time.monotonic() rover.log("MOVEMENT_START", markers=route_back) rover.follow(start, set()) second_time = time.monotonic() - started rover.stop() rover.log("MOVEMENT_FINISHED", marker=start, seconds=round(second_time, 2)) rover.log("START_REACHED", marker=start, seconds=round(second_time, 2)) rover.log( "MISSION_FINISHED", marker=start, movement_seconds=round(first_time + second_time, 2), log=str(rover.log_path), ) except (KeyboardInterrupt, RuntimeError) as error: exit_code = 1 rover.log("FAILED", reason=str(error) or "Ctrl+C") finally: for _ in range(5): rover.stop() rclpy.spin_once(rover, timeout_sec=0.02) rover.log_file.close() rover.destroy_node() rclpy.shutdown() raise SystemExit(exit_code) if __name__ == "__main__": main()