docs: add physical rover setup reference
This commit is contained in:
@@ -8,7 +8,7 @@
|
|||||||
|
|
||||||
- [`materials/presentations`](materials/presentations) — пять учебных презентаций (введение, документация, Python/ROS, симулятор, развитие компетенции).
|
- [`materials/presentations`](materials/presentations) — пять учебных презентаций (введение, документация, Python/ROS, симулятор, развитие компетенции).
|
||||||
- [`materials/pdf`](materials/pdf) — шестая презентация про датасет и модели, расписание.
|
- [`materials/pdf`](materials/pdf) — шестая презентация про датасет и модели, расписание.
|
||||||
- [`materials/final`](materials/final) — полный очный финал: инфраструктурный лист, КЗ модулей А–Д, критерии и приложения.
|
- [`materials/final`](materials/final) — полный очный финал: инфраструктурный лист, КЗ модулей А–Д, критерии, приложения и выданная документация физических РМК.
|
||||||
- [`materials/tasks`](materials/tasks) — отдельный ранний/отборочный комплект по навигации; не путать с полным финалом.
|
- [`materials/tasks`](materials/tasks) — отдельный ранний/отборочный комплект по навигации; не путать с полным финалом.
|
||||||
- [`basic`](basic) — примеры для реального ROS 2: движение к ArUco с odometry/TF/lidar, ручной `cmd_vel`, остановка, лифт, мониторинг и BFS-маршрут.
|
- [`basic`](basic) — примеры для реального ROS 2: движение к ArUco с odometry/TF/lidar, ручной `cmd_vel`, остановка, лифт, мониторинг и BFS-маршрут.
|
||||||
- [`setup/FROM_ZERO.md`](setup/FROM_ZERO.md) — подготовка полностью чистого Windows/Ubuntu и день настройки оборудования.
|
- [`setup/FROM_ZERO.md`](setup/FROM_ZERO.md) — подготовка полностью чистого Windows/Ubuntu и день настройки оборудования.
|
||||||
@@ -22,12 +22,13 @@
|
|||||||
## Порядок чтения
|
## Порядок чтения
|
||||||
|
|
||||||
1. `materials/final/04-final-task.docx` и `materials/final/05-final-criteria.xlsx` — сначала понять конечный результат и баллы очного финала.
|
1. `materials/final/04-final-task.docx` и `materials/final/05-final-criteria.xlsx` — сначала понять конечный результат и баллы очного финала.
|
||||||
2. `1. Введение...` и `2. Документация...` — термины, РМК и структура компетенции.
|
2. `materials/final/09-robot-documentation.pdf` — реальные размеры, топики, камеры, лифт и сеть РМК.
|
||||||
3. `3. Разбор...` — Python, ROS 2, топики и сервисы.
|
3. `1. Введение...` и `2. Документация...` — термины, РМК и структура компетенции.
|
||||||
4. `4. Практика... (симулятор)` — установка и работа с Webots.
|
4. `3. Разбор...` — Python, ROS 2, топики и сервисы.
|
||||||
5. `5. Практика... (датасет + модели)` — разметка и обучение детектора.
|
5. `4. Практика... (симулятор)` — установка и работа с Webots.
|
||||||
6. `setup/FROM_ZERO.md` — подготовка ПК и оборудования.
|
6. `5. Практика... (датасет + модели)` — разметка и обучение детектора.
|
||||||
7. README каждого модуля в `mvch` — точные команды текущего решения.
|
7. `setup/FROM_ZERO.md` — подготовка ПК и оборудования.
|
||||||
|
8. README каждого модуля в `mvch` — точные команды текущего решения.
|
||||||
|
|
||||||
## Быстрый старт на подготовленной VM
|
## Быстрый старт на подготовленной VM
|
||||||
|
|
||||||
@@ -177,6 +178,29 @@ ros2 topic echo /RMC2/scan_front --once
|
|||||||
|
|
||||||
Точный список топиков зависит от активного launch-файла. Если `scan_front` отсутствует, сначала посмотрите `ros2 topic list | sort`, а не меняйте код вслепую.
|
Точный список топиков зависит от активного launch-файла. Если `scan_front` отсутствует, сначала посмотрите `ros2 topic list | sort`, а не меняйте код вслепую.
|
||||||
|
|
||||||
|
## ROS API физических роверов из новой документации
|
||||||
|
|
||||||
|
Физическое поле на схеме имеет сетку 5 × 5 и ID 0–24. Система координат та же,
|
||||||
|
что использует код: `0=(0,0)`, `1=(0,+1 м)`, `5=(-1 м,0)`. Webots остаётся
|
||||||
|
отдельным профилем 6 × 6 с ID 0–35.
|
||||||
|
|
||||||
|
| Назначение | RMC1 | RMC2 |
|
||||||
|
| --- | --- | --- |
|
||||||
|
| Скорость | `/RMC1/cmd_vel` (`Twist`) | `/RMC2/cmd_vel` (`Twist`) |
|
||||||
|
| Одометрия | `/RMC1/odometry` | `/RMC2/odometry` |
|
||||||
|
| Лидары | `/RMC1/scan_front`, `/scan_back`, `/scan` | `/RMC2/scan_front`, `/scan_back`, `/scan` |
|
||||||
|
| TF | `/RMC1/tf`, `/RMC1/tf_static` | `/RMC2/tf`, `/RMC2/tf_static` |
|
||||||
|
| Камера | `/RMC1/arm95/svcam/right/image/compressed` | `/RMC2/camera_bottom/image` |
|
||||||
|
| ArUco ID | в PDF отдельный топик не указан | `/RMC2/camera_bottom/aruco_id` (`String`) |
|
||||||
|
| Лифт | — | `/RMC2/lift` (`Float64`), `/RMC2/lift_status` (`String`, Transient Local) |
|
||||||
|
|
||||||
|
Лифт принимает `0.0` для нижнего положения и `0.05` для верхнего; статусы:
|
||||||
|
`lowered`, `moving`, `raised`. В PDF у RMC1 камера с суффиксом `/compressed`
|
||||||
|
помечена типом `sensor_msgs/Image`, а не `CompressedImage`, и топик `CameraInfo`
|
||||||
|
не перечислен. Это нужно обязательно проверить командой `ros2 topic info -v` на
|
||||||
|
оборудовании: код умеет работать с обоими типами, а калибровку при отсутствии
|
||||||
|
`CameraInfo` можно передать аргументами.
|
||||||
|
|
||||||
## Если на реальном ровере другие датчики или ROS API
|
## Если на реальном ровере другие датчики или ROS API
|
||||||
|
|
||||||
Успешный запуск Python-процесса ещё не означает, что ровер готов ехать. Текущие
|
Успешный запуск Python-процесса ещё не означает, что ровер готов ехать. Текущие
|
||||||
@@ -208,12 +232,14 @@ ros2 topic list -t | tee ~/equipment-audit/topics.txt
|
|||||||
ros2 topic info /RMC2/cmd_vel -v | tee ~/equipment-audit/cmd_vel.txt
|
ros2 topic info /RMC2/cmd_vel -v | tee ~/equipment-audit/cmd_vel.txt
|
||||||
ros2 topic info /RMC2/odometry -v | tee ~/equipment-audit/odometry.txt
|
ros2 topic info /RMC2/odometry -v | tee ~/equipment-audit/odometry.txt
|
||||||
ros2 topic info /RMC2/scan_front -v | tee ~/equipment-audit/scan_front.txt
|
ros2 topic info /RMC2/scan_front -v | tee ~/equipment-audit/scan_front.txt
|
||||||
ros2 topic info /RMC2/aruco_id -v | tee ~/equipment-audit/aruco_id.txt
|
ros2 topic info /RMC2/camera_bottom/aruco_id -v | tee ~/equipment-audit/aruco_id.txt
|
||||||
|
ros2 topic info /RMC2/lift_status -v | tee ~/equipment-audit/lift_status.txt
|
||||||
|
ros2 topic info /RMC1/arm95/svcam/right/image/compressed -v | tee ~/equipment-audit/rmc1_camera.txt
|
||||||
|
|
||||||
timeout 5 ros2 topic hz /RMC2/odometry
|
timeout 5 ros2 topic hz /RMC2/odometry
|
||||||
timeout 5 ros2 topic hz /RMC2/scan_front
|
timeout 5 ros2 topic hz /RMC2/scan_front
|
||||||
ros2 topic echo /RMC2/odometry --once
|
ros2 topic echo /RMC2/odometry --once
|
||||||
ros2 topic echo /RMC2/aruco_id --once
|
ros2 topic echo /RMC2/camera_bottom/aruco_id --once
|
||||||
|
|
||||||
ros2 run tf2_ros tf2_echo RMC2/odom RMC2/base_link
|
ros2 run tf2_ros tf2_echo RMC2/odom RMC2/base_link
|
||||||
ros2 run tf2_ros tf2_echo RMC2/odom aruco_0
|
ros2 run tf2_ros tf2_echo RMC2/odom aruco_0
|
||||||
@@ -257,8 +283,8 @@ python3 basic/watch_robot.py --robot RMC2
|
|||||||
|
|
||||||
| Модуль | Решение | Основной запуск |
|
| Модуль | Решение | Основной запуск |
|
||||||
| --- | --- | --- |
|
| --- | --- | --- |
|
||||||
| Б | навигация RMC2 по ArUco | `python3 module2/module_b.py --target 14 --sim` |
|
| Б | навигация RMC2 по ArUco | реальный: `python3 module2/module_b.py --target 14`; Webots: добавить `--sim` |
|
||||||
| В | YOLO + ARM95 | `module3/.venv/bin/python module3/module3.py --target hammer` |
|
| В | YOLO + ARM95 | реальный: `module3/.venv/bin/python module3/module3.py --target hammer`; Webots: добавить `--sim` |
|
||||||
| Г | веб-FMS двух роверов | `cd module4 && bun run start` |
|
| Г | веб-FMS двух роверов | `cd module4 && bun run start` |
|
||||||
| Д | общий сценарий RMC1 + RMC2 | `module3/.venv/bin/python module5/module5.py --target hammer` |
|
| Д | общий сценарий RMC1 + RMC2 | `module3/.venv/bin/python module5/module5.py --target hammer` |
|
||||||
|
|
||||||
|
|||||||
+13
-4
@@ -1,7 +1,8 @@
|
|||||||
# Базовые примеры для реального ROS 2
|
# Базовые примеры для реального ROS 2
|
||||||
|
|
||||||
Это короткие учебные скрипты, которые работают через штатные ROS 2 интерфейсы
|
Это короткие учебные скрипты, которые работают через штатные ROS 2 интерфейсы
|
||||||
роверов: `cmd_vel`, `odometry`, `scan_front`, `aruco_id`, TF и `lift`. В них нет
|
роверов: `cmd_vel`, `odometry`, `scan_front`, `aruco_id`, TF и `lift`. Имена
|
||||||
|
сверены с выданной документацией физических РМК от 08.09.2026. В них нет
|
||||||
прямого управления моторами и нет подмены симулятора. Один и тот же ROS API можно
|
прямого управления моторами и нет подмены симулятора. Один и тот же ROS API можно
|
||||||
проверять в Webots, а затем перенести на физический РМК после сверки имён топиков,
|
проверять в Webots, а затем перенести на физический РМК после сверки имён топиков,
|
||||||
TF, размеров поля и безопасных скоростей.
|
TF, размеров поля и безопасных скоростей.
|
||||||
@@ -23,7 +24,8 @@ ros2 topic list | sort
|
|||||||
ros2 topic info /RMC2/cmd_vel -v
|
ros2 topic info /RMC2/cmd_vel -v
|
||||||
ros2 topic info /RMC2/odometry -v
|
ros2 topic info /RMC2/odometry -v
|
||||||
ros2 topic info /RMC2/scan_front -v
|
ros2 topic info /RMC2/scan_front -v
|
||||||
ros2 topic echo /RMC2/aruco_id
|
ros2 topic info /RMC2/camera_bottom/aruco_id -v
|
||||||
|
ros2 topic echo /RMC2/camera_bottom/aruco_id --once
|
||||||
ros2 run tf2_ros tf2_echo RMC2/odom aruco_0
|
ros2 run tf2_ros tf2_echo RMC2/odom aruco_0
|
||||||
```
|
```
|
||||||
|
|
||||||
@@ -56,8 +58,9 @@ python3 basic/lift_rmc2.py down
|
|||||||
python3 basic/stop_all.py
|
python3 basic/stop_all.py
|
||||||
```
|
```
|
||||||
|
|
||||||
`drive_to_aruco.py` рассчитан на регулярную сетку 6×6: `0=(0,0)`,
|
`drive_to_aruco.py` по умолчанию использует реальную сетку 5×5: `0=(0,0)`,
|
||||||
`1=(0,+1 м)`, `6=(-1 м,0)`. Ровер должен стартовать центром над видимой меткой.
|
`1=(0,+1 м)`, `5=(-1 м,0)`. С `--sim` выбирается сетка Webots 6×6, где
|
||||||
|
`6=(-1 м,0)`. Ровер должен стартовать центром над видимой меткой.
|
||||||
Скрипт сам определяет старт, строит BFS-маршрут, печатает его до движения, затем
|
Скрипт сам определяет старт, строит BFS-маршрут, печатает его до движения, затем
|
||||||
едет по odometry и уточняет положение через TF каждой ArUco. Без свежих odometry,
|
едет по odometry и уточняет положение через TF каждой ArUco. Без свежих odometry,
|
||||||
лидара, метки на финише или при препятствии ближе `--stop-distance` он отправляет
|
лидара, метки на финише или при препятствии ближе `--stop-distance` он отправляет
|
||||||
@@ -67,8 +70,14 @@ python3 basic/stop_all.py
|
|||||||
`--spacing`. Если организаторы используют другой топик лидара, передайте
|
`--spacing`. Если организаторы используют другой топик лидара, передайте
|
||||||
`--scan-topic /нужный/топик`. Не используйте `--yes` в первом физическом заезде.
|
`--scan-topic /нужный/топик`. Не используйте `--yes` в первом физическом заезде.
|
||||||
|
|
||||||
|
По выданной документации нижняя камера и `aruco_id` есть у RMC2. Поэтому реальный
|
||||||
|
запуск `drive_to_aruco.py` разрешён только для RMC2. Подъём лифта публикует `0.05`,
|
||||||
|
опускание — `0.0`, а статус читается с обязательным QoS `Transient Local`.
|
||||||
|
|
||||||
У RMC1 доступно боковое движение (`linear.y`), у RMC2 оно принудительно запрещено
|
У RMC1 доступно боковое движение (`linear.y`), у RMC2 оно принудительно запрещено
|
||||||
в `move_rmc.py`. Режим `--no-lidar` предназначен только для вывешенного стенда,
|
в `move_rmc.py`. Режим `--no-lidar` предназначен только для вывешенного стенда,
|
||||||
где колёса не могут переместить робот.
|
где колёса не могут переместить робот.
|
||||||
|
|
||||||
`grid_route.py` оставлен как офлайн-проверка графа. Он ничего не публикует в ROS.
|
`grid_route.py` оставлен как офлайн-проверка графа. Он ничего не публикует в ROS.
|
||||||
|
Для реального поля используется его дефолт `--size 5`, для Webots добавьте
|
||||||
|
`--size 6`.
|
||||||
|
|||||||
+55
-7
@@ -16,10 +16,16 @@ from geometry_msgs.msg import Twist
|
|||||||
from nav_msgs.msg import Odometry
|
from nav_msgs.msg import Odometry
|
||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
from rclpy.parameter import Parameter
|
from rclpy.parameter import Parameter
|
||||||
from rclpy.qos import qos_profile_sensor_data
|
from rclpy.qos import (
|
||||||
|
DurabilityPolicy,
|
||||||
|
QoSProfile,
|
||||||
|
ReliabilityPolicy,
|
||||||
|
qos_profile_sensor_data,
|
||||||
|
)
|
||||||
from rclpy.time import Time
|
from rclpy.time import Time
|
||||||
from sensor_msgs.msg import LaserScan
|
from sensor_msgs.msg import LaserScan
|
||||||
from std_msgs.msg import String
|
from std_msgs.msg import String
|
||||||
|
from tf2_msgs.msg import TFMessage
|
||||||
from tf2_ros import Buffer, TransformException, TransformListener
|
from tf2_ros import Buffer, TransformException, TransformListener
|
||||||
|
|
||||||
|
|
||||||
@@ -117,6 +123,26 @@ class ArucoDrive(Node):
|
|||||||
self.odom_frame = f"{self.robot}/odom"
|
self.odom_frame = f"{self.robot}/odom"
|
||||||
self.tf_buffer = Buffer()
|
self.tf_buffer = Buffer()
|
||||||
self.tf_listener = TransformListener(self.tf_buffer, self)
|
self.tf_listener = TransformListener(self.tf_buffer, self)
|
||||||
|
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, f"/{self.robot}/tf", self.on_tf, tf_qos
|
||||||
|
)
|
||||||
|
self.create_subscription(
|
||||||
|
TFMessage,
|
||||||
|
f"/{self.robot}/tf_static",
|
||||||
|
self.on_tf_static,
|
||||||
|
static_tf_qos,
|
||||||
|
)
|
||||||
self.publisher = self.create_publisher(
|
self.publisher = self.create_publisher(
|
||||||
Twist, f"/{self.robot}/cmd_vel", 10
|
Twist, f"/{self.robot}/cmd_vel", 10
|
||||||
)
|
)
|
||||||
@@ -132,8 +158,21 @@ class ArucoDrive(Node):
|
|||||||
self.on_scan,
|
self.on_scan,
|
||||||
qos_profile_sensor_data,
|
qos_profile_sensor_data,
|
||||||
)
|
)
|
||||||
self.create_subscription(
|
self.aruco_topic = (
|
||||||
String, f"/{self.robot}/aruco_id", self.on_marker, 10
|
f"/{self.robot}/aruco_id"
|
||||||
|
if args.sim
|
||||||
|
else f"/{self.robot}/camera_bottom/aruco_id"
|
||||||
|
)
|
||||||
|
self.create_subscription(String, self.aruco_topic, self.on_marker, 10)
|
||||||
|
|
||||||
|
def on_tf(self, message):
|
||||||
|
for transform in message.transforms:
|
||||||
|
self.tf_buffer.set_transform(transform, f"{self.robot} namespaced tf")
|
||||||
|
|
||||||
|
def on_tf_static(self, message):
|
||||||
|
for transform in message.transforms:
|
||||||
|
self.tf_buffer.set_transform_static(
|
||||||
|
transform, f"{self.robot} namespaced tf_static"
|
||||||
)
|
)
|
||||||
|
|
||||||
def on_odometry(self, message):
|
def on_odometry(self, message):
|
||||||
@@ -280,9 +319,9 @@ class ArucoDrive(Node):
|
|||||||
def arguments():
|
def arguments():
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
parser = argparse.ArgumentParser(description=__doc__)
|
||||||
parser.add_argument("target", type=int, help="ID целевой ArUco")
|
parser.add_argument("target", type=int, help="ID целевой ArUco")
|
||||||
parser.add_argument("--robot", default="RMC2", help="RMC1 или RMC2")
|
parser.add_argument("--robot", choices=("RMC1", "RMC2"), default="RMC2")
|
||||||
parser.add_argument("--rows", type=int, default=6)
|
parser.add_argument("--rows", type=int)
|
||||||
parser.add_argument("--columns", type=int, default=6)
|
parser.add_argument("--columns", type=int)
|
||||||
parser.add_argument("--spacing", type=float, default=1.0, help="Шаг сетки, м")
|
parser.add_argument("--spacing", type=float, default=1.0, help="Шаг сетки, м")
|
||||||
parser.add_argument("--blocked", type=int, nargs="*", default=[])
|
parser.add_argument("--blocked", type=int, nargs="*", default=[])
|
||||||
parser.add_argument("--max-speed", type=float, default=0.12, help="м/с")
|
parser.add_argument("--max-speed", type=float, default=0.12, help="м/с")
|
||||||
@@ -295,6 +334,15 @@ def arguments():
|
|||||||
parser.add_argument("--yes", action="store_true", help="Не ждать Enter")
|
parser.add_argument("--yes", action="store_true", help="Не ждать Enter")
|
||||||
parser.add_argument("--sim", action="store_true", help="Использовать /clock Webots")
|
parser.add_argument("--sim", action="store_true", help="Использовать /clock Webots")
|
||||||
args = parser.parse_args()
|
args = parser.parse_args()
|
||||||
|
if args.rows is None:
|
||||||
|
args.rows = 6 if args.sim else 5
|
||||||
|
if args.columns is None:
|
||||||
|
args.columns = 6 if args.sim else 5
|
||||||
|
if not args.sim and args.robot != "RMC2":
|
||||||
|
parser.error(
|
||||||
|
"Документация физического RMC1 не содержит нижнюю ArUco-камеру; "
|
||||||
|
"drive_to_aruco поддерживает реальный RMC2"
|
||||||
|
)
|
||||||
count = args.rows * args.columns
|
count = args.rows * args.columns
|
||||||
if args.rows <= 0 or args.columns <= 0 or not 0 <= args.target < count:
|
if args.rows <= 0 or args.columns <= 0 or not 0 <= args.target < count:
|
||||||
parser.error(f"target должен быть в диапазоне 0..{count - 1}")
|
parser.error(f"target должен быть в диапазоне 0..{count - 1}")
|
||||||
@@ -319,7 +367,7 @@ def main():
|
|||||||
print("Маршрут:", " -> ".join(map(str, route)))
|
print("Маршрут:", " -> ".join(map(str, route)))
|
||||||
print(
|
print(
|
||||||
f"Топики: /{node.robot}/cmd_vel, /{node.robot}/odometry, "
|
f"Топики: /{node.robot}/cmd_vel, /{node.robot}/odometry, "
|
||||||
f"/{node.robot}/aruco_id, {args.scan_topic or f'/{node.robot}/scan_front'}"
|
f"{node.aruco_topic}, {args.scan_topic or f'/{node.robot}/scan_front'}"
|
||||||
)
|
)
|
||||||
if not args.yes:
|
if not args.yes:
|
||||||
input("Проверьте поле и держите аппаратный STOP. Enter — начать: ")
|
input("Проверьте поле и держите аппаратный STOP. Enter — начать: ")
|
||||||
|
|||||||
+15
-16
@@ -1,22 +1,19 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
"""Строит кратчайший маршрут по сетке маркеров 6x6 без ROS."""
|
"""Строит кратчайший маршрут по регулярной сетке ArUco без ROS."""
|
||||||
|
|
||||||
import argparse
|
import argparse
|
||||||
from collections import deque
|
from collections import deque
|
||||||
|
|
||||||
|
|
||||||
SIZE = 6
|
def neighbors(marker: int, size: int):
|
||||||
|
row, column = divmod(marker, size)
|
||||||
|
|
||||||
def neighbors(marker: int):
|
|
||||||
row, column = divmod(marker, SIZE)
|
|
||||||
for dr, dc in ((-1, 0), (1, 0), (0, -1), (0, 1)):
|
for dr, dc in ((-1, 0), (1, 0), (0, -1), (0, 1)):
|
||||||
next_row, next_column = row + dr, column + dc
|
next_row, next_column = row + dr, column + dc
|
||||||
if 0 <= next_row < SIZE and 0 <= next_column < SIZE:
|
if 0 <= next_row < size and 0 <= next_column < size:
|
||||||
yield next_row * SIZE + next_column
|
yield next_row * size + next_column
|
||||||
|
|
||||||
|
|
||||||
def build_route(start: int, target: int, blocked: set[int]):
|
def build_route(start: int, target: int, blocked: set[int], size: int):
|
||||||
queue = deque([start])
|
queue = deque([start])
|
||||||
previous = {start: None}
|
previous = {start: None}
|
||||||
|
|
||||||
@@ -24,7 +21,7 @@ def build_route(start: int, target: int, blocked: set[int]):
|
|||||||
current = queue.popleft()
|
current = queue.popleft()
|
||||||
if current == target:
|
if current == target:
|
||||||
break
|
break
|
||||||
for candidate in neighbors(current):
|
for candidate in neighbors(current, size):
|
||||||
if candidate not in blocked and candidate not in previous:
|
if candidate not in blocked and candidate not in previous:
|
||||||
previous[candidate] = current
|
previous[candidate] = current
|
||||||
queue.append(candidate)
|
queue.append(candidate)
|
||||||
@@ -42,8 +39,9 @@ def build_route(start: int, target: int, blocked: set[int]):
|
|||||||
|
|
||||||
def main():
|
def main():
|
||||||
parser = argparse.ArgumentParser(description=__doc__)
|
parser = argparse.ArgumentParser(description=__doc__)
|
||||||
parser.add_argument("start", type=int, help="Стартовый маркер 0..35")
|
parser.add_argument("start", type=int, help="ID стартового маркера")
|
||||||
parser.add_argument("target", type=int, help="Целевой маркер 0..35")
|
parser.add_argument("target", type=int, help="ID целевого маркера")
|
||||||
|
parser.add_argument("--size", type=int, default=5, help="5 для реального поля, 6 для Webots")
|
||||||
parser.add_argument(
|
parser.add_argument(
|
||||||
"--blocked",
|
"--blocked",
|
||||||
type=int,
|
type=int,
|
||||||
@@ -53,13 +51,15 @@ def main():
|
|||||||
)
|
)
|
||||||
args = parser.parse_args()
|
args = parser.parse_args()
|
||||||
|
|
||||||
if not 0 <= args.start < SIZE * SIZE or not 0 <= args.target < SIZE * SIZE:
|
if args.size < 2:
|
||||||
parser.error("start и target должны быть от 0 до 35")
|
parser.error("--size должен быть не меньше 2")
|
||||||
|
if not 0 <= args.start < args.size**2 or not 0 <= args.target < args.size**2:
|
||||||
|
parser.error(f"start и target должны быть от 0 до {args.size**2 - 1}")
|
||||||
|
|
||||||
blocked = set(args.blocked)
|
blocked = set(args.blocked)
|
||||||
blocked.discard(args.start)
|
blocked.discard(args.start)
|
||||||
blocked.discard(args.target)
|
blocked.discard(args.target)
|
||||||
route = build_route(args.start, args.target, blocked)
|
route = build_route(args.start, args.target, blocked, args.size)
|
||||||
if route is None:
|
if route is None:
|
||||||
raise SystemExit("Маршрут не найден")
|
raise SystemExit("Маршрут не найден")
|
||||||
print(" -> ".join(map(str, route)))
|
print(" -> ".join(map(str, route)))
|
||||||
@@ -67,4 +67,3 @@ def main():
|
|||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
main()
|
main()
|
||||||
|
|
||||||
|
|||||||
+27
-7
@@ -6,6 +6,7 @@ import time
|
|||||||
|
|
||||||
import rclpy
|
import rclpy
|
||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
|
from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
|
||||||
from std_msgs.msg import Float64, String
|
from std_msgs.msg import Float64, String
|
||||||
|
|
||||||
|
|
||||||
@@ -17,29 +18,48 @@ def main():
|
|||||||
|
|
||||||
rclpy.init()
|
rclpy.init()
|
||||||
node = Node("mvch_basic_lift")
|
node = Node("mvch_basic_lift")
|
||||||
|
exit_message = None
|
||||||
|
try:
|
||||||
publisher = node.create_publisher(Float64, "/RMC2/lift", 10)
|
publisher = node.create_publisher(Float64, "/RMC2/lift", 10)
|
||||||
statuses = []
|
statuses = []
|
||||||
node.create_subscription(String, "/RMC2/lift_status", lambda msg: statuses.append(msg.data), 10)
|
status_qos = QoSProfile(
|
||||||
|
depth=1,
|
||||||
|
reliability=ReliabilityPolicy.RELIABLE,
|
||||||
|
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||||
|
)
|
||||||
|
node.create_subscription(
|
||||||
|
String,
|
||||||
|
"/RMC2/lift_status",
|
||||||
|
lambda msg: statuses.append(msg.data),
|
||||||
|
status_qos,
|
||||||
|
)
|
||||||
|
|
||||||
height = 0.1 if args.state == "up" else 0.0
|
height = 0.05 if args.state == "up" else 0.0
|
||||||
|
expected = "raised" if args.state == "up" else "lowered"
|
||||||
print(f"RMC2 lift: {args.state}, команда {height}")
|
print(f"RMC2 lift: {args.state}, команда {height}")
|
||||||
for _ in range(5):
|
for _ in range(5):
|
||||||
publisher.publish(Float64(data=height))
|
publisher.publish(Float64(data=height))
|
||||||
rclpy.spin_once(node, timeout_sec=0.1)
|
rclpy.spin_once(node, timeout_sec=0.1)
|
||||||
|
|
||||||
deadline = time.monotonic() + args.wait
|
deadline = time.monotonic() + args.wait
|
||||||
while time.monotonic() < deadline and not statuses:
|
while time.monotonic() < deadline:
|
||||||
rclpy.spin_once(node, timeout_sec=0.1)
|
rclpy.spin_once(node, timeout_sec=0.1)
|
||||||
|
if statuses and statuses[-1].strip().lower() == expected:
|
||||||
|
break
|
||||||
|
|
||||||
if statuses:
|
if statuses and statuses[-1].strip().lower() == expected:
|
||||||
print(f"Статус: {statuses[-1]}")
|
print(f"Статус: {statuses[-1]}")
|
||||||
else:
|
else:
|
||||||
print("Нового lift_status нет. Это возможно, если лифт уже был в нужном положении.")
|
current = statuses[-1] if statuses else "нет данных"
|
||||||
|
exit_message = (
|
||||||
|
f"Лифт не подтвердил {expected}; текущий статус: {current}"
|
||||||
|
)
|
||||||
|
finally:
|
||||||
node.destroy_node()
|
node.destroy_node()
|
||||||
rclpy.shutdown()
|
rclpy.shutdown()
|
||||||
|
if exit_message:
|
||||||
|
raise SystemExit(exit_message)
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
main()
|
main()
|
||||||
|
|
||||||
|
|||||||
Binary file not shown.
@@ -202,6 +202,10 @@ ros2 topic list | sort
|
|||||||
ros2 topic echo /RMC1/odometry --once
|
ros2 topic echo /RMC1/odometry --once
|
||||||
ros2 topic echo /RMC2/odometry --once
|
ros2 topic echo /RMC2/odometry --once
|
||||||
ros2 topic echo /RMC2/scan_front --once
|
ros2 topic echo /RMC2/scan_front --once
|
||||||
|
ros2 topic info /RMC2/camera_bottom/aruco_id -v
|
||||||
|
ros2 topic echo /RMC2/camera_bottom/aruco_id --once
|
||||||
|
ros2 topic info /RMC2/lift_status -v
|
||||||
|
ros2 topic info /RMC1/arm95/svcam/right/image/compressed -v
|
||||||
```
|
```
|
||||||
|
|
||||||
## Git bundle на флешку
|
## Git bundle на флешку
|
||||||
|
|||||||
Reference in New Issue
Block a user