From 4ba36d8b18ec42a67b165d8e8ed57083acd56e1d Mon Sep 17 00:00:00 2001 From: mavxa Date: Mon, 14 Sep 2026 21:56:54 +0300 Subject: [PATCH] refactor(module2): keep only target argument --- README.md | 34 +++++--------- module2/module_b.py | 112 ++++++++++++++------------------------------ 2 files changed, 46 insertions(+), 100 deletions(-) diff --git a/README.md b/README.md index b457b0b..0b5df9f 100644 --- a/README.md +++ b/README.md @@ -73,7 +73,7 @@ cd ~/mvch git pull --ff-only source /opt/ros/jazzy/setup.bash source ~/ros2_ws/install/setup.bash -python3 module2/module_b.py --target 14 --sim +python3 module2/module_b.py --target 14 ``` 1. Поставьте РМК-2 центром над любой меткой. Код сам определит стартовый ID и @@ -86,31 +86,25 @@ python3 module2/module_b.py --target 14 --sim исключит ребро и выведет `ROUTE_REPLANNED`. 6. Успешный конец — `MISSION_FINISHED`. `Ctrl+C` публикует нулевую скорость. -На физическом РМК-2 команда такая же, только `--sim` можно не писать: +На физическом РМК-2 команда такая же: ```bash python3 module2/module_b.py --target 14 ``` -Без `--sim` используется площадочное поле 5×5, ID 0–24. Флаг `--sim` -включает старый Webots 6×6, ID 0–35. Закрытые ячейки передаются через -`--blocked`: +Единственный аргумент — целевой ID: ```bash -python3 module2/module_b.py --target 14 --blocked 7 8 13 --sim +python3 module2/module_b.py --target 14 ``` -Основные параметры: +Остальные настройки находятся в начале `module2/module_b.py`: -- `--target` — целевой ArUco ID; -- `--blocked` — закрытые ячейки; -- `--sim` — профиль симулятора 6×6; -- `--rows`, `--columns`, `--spacing` — геометрия сетки; -- `--speed` — линейная скорость, по умолчанию 0.35 м/с, предел RMC2 0.5 м/с; -- `--angular` — скорость поворота, по умолчанию 0.6 рад/с; -- `--stop-distance` — дистанция срабатывания объезда, по умолчанию 0.65 м; -- `--tolerance` — допустимое смещение от центра ArUco, по умолчанию 0.07 м; -- `--scan-topic` — передний лидар, по умолчанию `/RMC2/scan_front`. +- `ROWS`, `COLUMNS` — площадка 5×5; для старого симулятора поставить 6 и 6; +- `SPACING` — шаг между метками; +- `BLOCKED` — закрытые ID, например `{7, 8, 13}`; +- `MAX_SPEED`, `MAX_ANGULAR` — скорости; +- `STOP_DISTANCE`, `TOLERANCE`, `SCAN_TOPIC` — lidar и точность остановки. ## Как устроен код @@ -126,13 +120,7 @@ odometry, а на каждой следующей метке снова корр workspace RMC2 управляется через `/RMC2/cmd_vel`; Nav2 настроен для RMC1. Поэтому модуль Б напрямую использует ArUco, odometry и `/RMC2/scan_front`. -## Проверки и логи - -```bash -python3 module2/module_b.py --target 14 --start 0 --dry-run -python3 module2/module_b.py --target 8 --start 0 --sim --dry-run -python3 module2/module_b.py --target 24 --start 6 --blocked 7 8 --dry-run -``` +## Логи `module2/reports/module_b_*.jsonl` содержит обнаруженные маркеры, оба маршрута, препятствия, достигнутые точки и чистое время движения. Паузы на установку diff --git a/module2/module_b.py b/module2/module_b.py index 5048103..83e6b07 100644 --- a/module2/module_b.py +++ b/module2/module_b.py @@ -16,6 +16,8 @@ MAX_SPEED = 0.35 MAX_ANGULAR = 0.60 STOP_DISTANCE = 0.65 TOLERANCE = 0.07 +SCAN_TOPIC = "/RMC2/scan_front" +BLOCKED = set() def clamp(value, low, high): @@ -51,14 +53,14 @@ def inverse(pose): ) -def marker_pose(marker, columns, spacing): - row, column = divmod(marker, columns) - return -row * spacing, column * spacing, 0.0 +def marker_pose(marker): + 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("Старт или цель находятся в закрытой ячейке") +def shortest_path(start, goal, forbidden=()): + if start in BLOCKED or goal in BLOCKED: + raise RuntimeError("невозможная цель") forbidden = {frozenset(edge) for edge in forbidden} search = deque([start]) @@ -68,24 +70,24 @@ def shortest_path(start, goal, rows, columns, blocked, forbidden=()): current = search.popleft() if current == goal: break - row, column = divmod(current, columns) + 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): + if not (0 <= next_row < ROWS and 0 <= next_column < COLUMNS): continue - candidate = next_row * columns + next_column + candidate = next_row * COLUMNS + next_column edge = frozenset((current, candidate)) - if candidate in blocked or candidate in parent or edge in forbidden: + 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}") + raise RuntimeError(f"нет маршрута {start} к {goal}") route = [] current = goal @@ -98,51 +100,11 @@ def shortest_path(start, goal, rows, columns, blocked, forbidden=()): def arguments(): parser = argparse.ArgumentParser() parser.add_argument("--target", type=int, required=True) - parser.add_argument("--rows", type=int) - parser.add_argument("--columns", type=int) - 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", help="Симулятор с сеткой 6x6") - parser.add_argument("--dry-run", action="store_true") - parser.add_argument("--start", type=int, default=0) - args = parser.parse_args() - - if args.rows is None: - args.rows = 6 if args.sim else ROWS - if args.columns is None: - args.columns = 6 if args.sim else COLUMNS - - 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 + return parser.parse_args().target 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 + target_id = arguments() import rclpy from geometry_msgs.msg import Twist @@ -169,7 +131,7 @@ def main(): Odometry, "/RMC2/odometry", self.on_odom, qos_profile_sensor_data ) self.create_subscription( - LaserScan, args.scan_topic, self.on_scan, qos_profile_sensor_data + LaserScan, SCAN_TOPIC, self.on_scan, qos_profile_sensor_data ) self.create_subscription( String, "/RMC2/camera_bottom/aruco_id", self.on_marker, 10 @@ -240,7 +202,7 @@ def main(): if not match: return marker = int(match.group()) - if not 0 <= marker < args.rows * args.columns: + if not 0 <= marker < ROWS * COLUMNS: return if marker != self.marker: self.log("MARKER", marker=marker) @@ -274,7 +236,7 @@ def main(): yaw(transform.rotation), ) self.map_from_odom = compose( - marker_pose(self.marker, args.columns, args.spacing), inverse(odom_marker) + marker_pose(self.marker), inverse(odom_marker) ) self.pose = compose(self.map_from_odom, self.odom) self.marker_error = math.hypot( @@ -301,7 +263,7 @@ def main(): while rclpy.ok(): self.update() self.stop() - if self.pose is not None and self.marker_error <= args.tolerance: + if self.pose is not None and self.marker_error <= TOLERANCE: self.current = self.marker self.log("START_FOUND", marker=self.current) return self.current @@ -310,11 +272,11 @@ def main(): return ( self.marker == marker and time.monotonic() - self.marker_seen < 0.8 - and self.marker_error <= args.tolerance + and self.marker_error <= TOLERANCE ) def drive_to(self, marker, check_obstacle=True): - target = marker_pose(marker, args.columns, args.spacing) + target = marker_pose(marker) while rclpy.ok(): self.update() if self.pose is None: @@ -328,18 +290,16 @@ def main(): 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) + linear = 0.0 if abs(error) > 0.16 else min(MAX_SPEED, max(0.04, 0.7 * distance)) + angular = clamp(1.4 * error, -MAX_ANGULAR, MAX_ANGULAR) - if check_obstacle and linear > 0 and self.front < args.stop_distance: + if check_obstacle and linear > 0 and self.front < 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 - ) + route = shortest_path(self.current, goal, forbidden) index = 1 while index < len(route): next_marker = route[index] @@ -358,15 +318,13 @@ def main(): ) 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 - ) + route = shortest_path(self.current, goal, 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))}") + print(f"{title}: {' путь: '.join(map(str, route))}") rover.log(event, markers=route) rclpy.init(signal_handler_options=SignalHandlerOptions.NO) @@ -374,25 +332,25 @@ def main(): 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) + route_there = shortest_path(start, target_id) + route_back = shortest_path(target_id, start) print() show_route(rover, "МАРШРУТ К ЦЕЛИ", route_there, "ROUTE_TO_TARGET") show_route(rover, "МАРШРУТ НА СТАРТ", route_back, "ROUTE_TO_START") - input("\nEnter — начать движение: ") + input("\nenter - начать движение:") started = time.monotonic() rover.log("MOVEMENT_START", markers=route_there) - rover.follow(args.target, set()) + rover.follow(target_id, 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)) + rover.log("MOVEMENT_FINISHED", marker=target_id, seconds=round(first_time, 2)) + rover.log("TARGET_REACHED", marker=target_id, seconds=round(first_time, 2)) - input("\nЦель достигнута. Поставьте препятствие и нажмите Enter: ") + input("\nцель достигнута. ожидание препятствие и enter: ") rover.leg = "return" show_route(rover, "МАРШРУТ НА СТАРТ", route_back, "ROUTE_TO_START") - input("Enter — начать возврат: ") + input("Enter - начать возврат: ") started = time.monotonic() rover.log("MOVEMENT_START", markers=route_back)