fix: align modules with physical rover API
This commit is contained in:
@@ -61,9 +61,21 @@ python3 module2/module_b.py --target 14 --sim --auto --return-delay 10
|
||||
python3 module2/module_b.py --xy -2 2 --target-yaw 1.57 --sim
|
||||
```
|
||||
|
||||
На физическом роботе `--sim` не указывать. Перед переносом обязательно проверить
|
||||
размеры корпуса, тормозной путь, TF, шаг и ориентации графа, датчики и аппаратную
|
||||
аварийную остановку. Испытания в Webots не подтверждают готовность к реальному полю.
|
||||
На физическом роботе `--sim` не указывать:
|
||||
|
||||
```bash
|
||||
python3 module2/module_b.py --target 14
|
||||
```
|
||||
|
||||
Тогда используется подтверждённый документацией профиль: поле 5×5 (ID 0–24),
|
||||
ArUco `/RMC2/camera_bottom/aruco_id`, TF `/RMC2/tf` и `/RMC2/tf_static`, габарит
|
||||
RMC2 до 0.870 м. С `--sim` остаётся прежний профиль Webots 6×6 (ID 0–35),
|
||||
`/RMC2/aruco_id` и глобальные `/tf`/`/tf_static`.
|
||||
|
||||
Перед физическим заездом всё равно проверить частоту топиков, имена frame_id, шаг
|
||||
сетки, тормозной путь и аппаратную аварийную остановку. Документ не сообщает шаг
|
||||
между маркерами и точные имена TF-фреймов. Испытания в Webots не подтверждают
|
||||
готовность к реальному полю.
|
||||
У штатного Webots-драйвера нет таймаута последней команды: при SIGKILL/обрыве питания
|
||||
процесса программная остановка не гарантируется; остановить симулятор вручную.
|
||||
|
||||
@@ -74,7 +86,8 @@ python3 module2/module_b.py --xy -2 2 --target-yaw 1.57 --sim
|
||||
|
||||
Читайте в таком порядке: `Field.route` → `localize` → `scan_points` → `plan` → `drive` → `tick`.
|
||||
|
||||
Координаты штатного поля: `0=(0,0)`, `6=(-1,0)`, `1=(0,1)`.
|
||||
Координаты реального поля: `0=(0,0)`, `5=(-1,0)`, `1=(0,1)`; в Webots 6×6:
|
||||
`0=(0,0)`, `6=(-1,0)`, `1=(0,1)`.
|
||||
Поворот `yaw` каждой метки — ориентация её TF-системы относительно поля.
|
||||
Измеренное положение метки в odom сравнивается с её известным положением на поле;
|
||||
так получаем преобразование `map_from_odom`. Между метками движение идёт по одометрии.
|
||||
@@ -88,13 +101,14 @@ python3 module2/module_b.py --xy -2 2 --target-yaw 1.57 --sim
|
||||
к предыдущему маркеру по своему ребру и публикует новый маршрут `ROUTE_REPLANNED`.
|
||||
Если отступить безопасно нельзя, он стоит и завершает попытку по таймауту.
|
||||
|
||||
Размеры сетки, шаг и известные занятые клетки задаются константами `ROWS`, `COLS`,
|
||||
`MARKER_SPACING`, `BLOCKED_MARKERS` в начале скрипта. Все длины — метры.
|
||||
Профили сетки находятся в `REAL_FIELD` и `SIM_FIELD`, известные занятые клетки —
|
||||
в `BLOCKED_MARKERS`. Все длины — метры.
|
||||
|
||||
## Проверки и логи
|
||||
|
||||
```bash
|
||||
python3 module2/module_b.py --target 14 --dry-run
|
||||
python3 module2/module_b.py --target 14 --dry-run # реальное поле 5x5
|
||||
python3 module2/module_b.py --target 35 --sim --dry-run # Webots 6x6
|
||||
```
|
||||
|
||||
`reports/module_b_*.jsonl` содержат маршрут, движение, ошибки, препятствия,
|
||||
|
||||
+63
-22
@@ -19,20 +19,17 @@ from datetime import datetime
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
# Все параметры модуля Б находятся здесь. Перед реальным полем меняются в одном месте.
|
||||
ROWS = 6
|
||||
COLS = 6
|
||||
MARKER_SPACING = 1.0
|
||||
# Профили поля из симулятора и документации физических РМК от 08.09.2026.
|
||||
SIM_FIELD = (6, 6, 1.0)
|
||||
REAL_FIELD = (5, 5, 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
|
||||
# В Webots край корпуса около 0.375 м. Реальный RMC2 имеет габарит до 0.870 м.
|
||||
SIM_HALF_SIZE = 0.375
|
||||
REAL_HALF_SIZE = 0.435
|
||||
SAFETY_MARGIN = 0.04
|
||||
ROUTE_CLEARANCE = 0.44
|
||||
|
||||
MAX_LINEAR = 0.25
|
||||
MAX_ANGULAR = 0.45
|
||||
@@ -84,17 +81,21 @@ def segment_distance(point, a, b):
|
||||
class Field:
|
||||
"""Регулярная сетка ArUco и кратчайший путь по ней."""
|
||||
|
||||
def __init__(self):
|
||||
def __init__(self, rows, columns, spacing, clearance):
|
||||
self.rows = rows
|
||||
self.columns = columns
|
||||
self.spacing = spacing
|
||||
self.clearance = clearance
|
||||
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)
|
||||
for row in range(rows):
|
||||
for col in range(columns):
|
||||
marker = row*columns+col
|
||||
self.poses[marker] = (-row*spacing, col*spacing, 0.0)
|
||||
self.graph[marker] = [
|
||||
r*COLS+c for r, c in
|
||||
r*columns+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
|
||||
if 0 <= r < rows and 0 <= c < columns
|
||||
]
|
||||
|
||||
def nearest(self, x, y):
|
||||
@@ -126,7 +127,7 @@ class Field:
|
||||
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):
|
||||
if any(segment_distance(p, a, b) < self.clearance for p in obstacle_points):
|
||||
continue
|
||||
new_cost = cost+distance(a, b)
|
||||
if new_cost < costs.get(neighbor, math.inf):
|
||||
@@ -161,7 +162,9 @@ def arguments():
|
||||
|
||||
def main():
|
||||
args = arguments()
|
||||
field = Field()
|
||||
rows, columns, spacing = SIM_FIELD if args.sim else REAL_FIELD
|
||||
half_size = SIM_HALF_SIZE if args.sim else REAL_HALF_SIZE
|
||||
field = Field(rows, columns, spacing, half_size+SAFETY_MARGIN)
|
||||
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("Целевая метка отсутствует в графе или заблокирована")
|
||||
@@ -174,11 +177,17 @@ def main():
|
||||
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.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 Bool, String
|
||||
from tf2_msgs.msg import TFMessage
|
||||
from tf2_ros import Buffer, TransformListener, TransformException
|
||||
|
||||
def yaw(q):
|
||||
@@ -203,18 +212,41 @@ def main():
|
||||
}
|
||||
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,
|
||||
"clearance": half_size+SAFETY_MARGIN,
|
||||
"half_length": half_size,
|
||||
"half_width": half_size,
|
||||
"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)
|
||||
if not args.sim:
|
||||
# На физических РМК TF опубликован в namespaced-топиках.
|
||||
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"/{ns}/tf", self.on_tf, tf_qos
|
||||
)
|
||||
self.create_subscription(
|
||||
TFMessage, f"/{ns}/tf_static", self.on_tf_static, static_tf_qos
|
||||
)
|
||||
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)
|
||||
aruco_topic = (
|
||||
f"/{ns}/aruco_id" if args.sim else f"/{ns}/camera_bottom/aruco_id"
|
||||
)
|
||||
self.create_subscription(String, aruco_topic, 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()
|
||||
@@ -250,8 +282,17 @@ def main():
|
||||
# Таймер на монотонных часах: остановка сработает даже при паузе /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,
|
||||
field=f"{rows}x{columns}", aruco_topic=aruco_topic,
|
||||
instructions="Enter/go: старт; return: построить возврат; stop/resume; quit")
|
||||
|
||||
def on_tf(self, message):
|
||||
for transform in message.transforms:
|
||||
self.buffer.set_transform(transform, "RMC2 namespaced tf")
|
||||
|
||||
def on_tf_static(self, message):
|
||||
for transform in message.transforms:
|
||||
self.buffer.set_transform_static(transform, "RMC2 namespaced tf_static")
|
||||
|
||||
def now(self):
|
||||
return self.get_clock().now().nanoseconds / 1e9
|
||||
|
||||
|
||||
+41
-5
@@ -1,6 +1,8 @@
|
||||
# Модуль В: датасет и YOLO
|
||||
|
||||
Камера: `/RMC1/arm95/camera_gripper/image_color` (`1600x1200`).
|
||||
Камера Webots: `/RMC1/arm95/camera_gripper/image_color`. На физическом RMC1 новая
|
||||
документация указывает `/RMC1/arm95/svcam/right/image/compressed` с типом
|
||||
`sensor_msgs/msg/Image` — несмотря на суффикс `compressed`.
|
||||
|
||||
Основной зачетный скрипт: `module3.py`. Он показывает обработанный видеопоток,
|
||||
определяет координаты выбранной детали относительно `Base_link`, захватывает ее,
|
||||
@@ -47,9 +49,40 @@ source .venv/bin/activate
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
source ~/ros2_ws/install/setup.bash
|
||||
cd ~/scripts/mvch
|
||||
module3/.venv/bin/python module3/module3.py --target 1
|
||||
module3/.venv/bin/python module3/module3.py --target 1 --sim
|
||||
```
|
||||
|
||||
Для Webots `--sim` обязателен: он выбирает симуляторную камеру, глобальные TF и
|
||||
`use_sim_time`.
|
||||
|
||||
На физическом RMC1 `--sim` не нужен. До запуска проверьте фактический тип камеры:
|
||||
|
||||
```bash
|
||||
ros2 topic info /RMC1/arm95/svcam/right/image/compressed -v
|
||||
ros2 topic echo /RMC1/arm95/svcam/right/camera_info --once
|
||||
ros2 run tf2_ros tf2_echo Base_link ИМЯ_FRAME_КАМЕРЫ
|
||||
```
|
||||
|
||||
Если `ros2 topic info` вопреки PDF показывает
|
||||
`sensor_msgs/msg/CompressedImage`, добавьте `--compressed`. Если `CameraInfo`
|
||||
публикуется под другим именем, задайте `--camera-info-topic`. При полном отсутствии
|
||||
`CameraInfo` можно передать калибровку камеры вручную:
|
||||
|
||||
```bash
|
||||
module3/.venv/bin/python module3/module3.py --target hammer \
|
||||
--fx FX --fy FY --cx CX --cy CY
|
||||
```
|
||||
|
||||
Без CameraInfo либо четырёх параметров калибровки скрипт распознает класс, но
|
||||
намеренно не двигает ARM95: по одному bounding box нельзя получить метрические
|
||||
координаты детали.
|
||||
|
||||
MoveIt-конфигурация в скрипте повторяет выданный
|
||||
`moveit_api_example.launch.py`: `arm95_webots.urdf`, группы `arm95_group` и
|
||||
`gripper`, namespace `/RMC1/arm95`. Для физического запуска меняется только
|
||||
`use_sim_time=false`. Если на площадке организаторы выдадут обновлённый launch или
|
||||
URDF для железа, он имеет приоритет — сначала сравните его с этим примером.
|
||||
|
||||
Цели: `1/hammer`, `2/wrench`, `3/pliers`. После того как эксперт зафиксировал
|
||||
видеопоток, нажмите `G` или `Space` в окне либо введите `g` и
|
||||
нажмите Enter в терминале. `Esc` останавливает скрипт до начала движения.
|
||||
@@ -57,7 +90,7 @@ module3/.venv/bin/python module3/module3.py --target 1
|
||||
Сначала обязательно проверьте только координаты:
|
||||
|
||||
```bash
|
||||
module3/.venv/bin/python module3/module3.py --target hammer --dry-run
|
||||
module3/.venv/bin/python module3/module3.py --target hammer --sim --dry-run
|
||||
```
|
||||
|
||||
В режиме `--dry-run` подтверждать запуск клавишей не нужно: скрипт завершится
|
||||
@@ -66,7 +99,7 @@ module3/.venv/bin/python module3/module3.py --target hammer --dry-run
|
||||
Для проверки через SSH без окна:
|
||||
|
||||
```bash
|
||||
module3/.venv/bin/python module3/module3.py --target hammer --dry-run --no-window
|
||||
module3/.venv/bin/python module3/module3.py --target hammer --sim --dry-run --no-window
|
||||
```
|
||||
|
||||
Геометрию реального стенда нельзя брать из симулятора вслепую. Перед зачетом
|
||||
@@ -110,9 +143,12 @@ wrench
|
||||
```bash
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
source ~/ros2_ws/install/setup.bash
|
||||
python3 module3/capture_dataset.py --count 30
|
||||
python3 module3/capture_dataset.py --sim --count 30
|
||||
```
|
||||
|
||||
На реальном RMC1 `capture_dataset.py` по умолчанию использует новую камеру. Если
|
||||
её реальный тип окажется `CompressedImage`, добавьте `--compressed`.
|
||||
|
||||
Во время сбора плавно меняйте положение RMC1/манипулятора. Скрипт пропускает почти
|
||||
одинаковые кадры. Для одиночного снимка используйте `--count 1 --min-change 0`.
|
||||
|
||||
|
||||
@@ -12,11 +12,12 @@ HERE = Path(__file__).resolve().parent
|
||||
|
||||
def arguments():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--sim", action="store_true", help="Использовать камеру Webots")
|
||||
parser.add_argument(
|
||||
"--topic",
|
||||
default="/RMC1/arm95/camera_gripper/image_color",
|
||||
help="ROS-топик sensor_msgs/Image",
|
||||
help="ROS-топик камеры; по умолчанию выбирается по --sim",
|
||||
)
|
||||
parser.add_argument("--compressed", action="store_true", help="Тип CompressedImage")
|
||||
parser.add_argument("--output", default="dataset/raw", help="Каталог для JPG")
|
||||
parser.add_argument("--count", type=int, default=30, help="Сколько кадров сохранить")
|
||||
parser.add_argument("--interval", type=float, default=0.5, help="Минимум секунд между JPG")
|
||||
@@ -29,6 +30,12 @@ def arguments():
|
||||
parser.add_argument("--timeout", type=float, default=180.0, help="Общий таймаут")
|
||||
parser.add_argument("--jpeg-quality", type=int, default=95)
|
||||
args = parser.parse_args()
|
||||
if args.topic is None:
|
||||
args.topic = (
|
||||
"/RMC1/arm95/camera_gripper/image_color"
|
||||
if args.sim
|
||||
else "/RMC1/arm95/svcam/right/image/compressed"
|
||||
)
|
||||
if args.count < 1 or args.interval < 0 or args.timeout <= 0:
|
||||
parser.error("count >= 1, interval >= 0, timeout > 0")
|
||||
if not 1 <= args.jpeg_quality <= 100 or args.min_change < 0:
|
||||
@@ -45,7 +52,7 @@ def main():
|
||||
from cv_bridge import CvBridge
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import Image
|
||||
from sensor_msgs.msg import CompressedImage, Image
|
||||
except ImportError as error:
|
||||
raise SystemExit(
|
||||
f"Нет ROS/OpenCV зависимости: {error}. Запускайте после source ROS 2 в VM."
|
||||
@@ -63,14 +70,22 @@ def main():
|
||||
self.saved = 0
|
||||
self.previous = None
|
||||
self.last_save = 0.0
|
||||
self.create_subscription(Image, args.topic, self.on_image, qos_profile_sensor_data)
|
||||
image_type = CompressedImage if args.compressed else Image
|
||||
self.create_subscription(image_type, args.topic, self.on_image, qos_profile_sensor_data)
|
||||
|
||||
def on_image(self, message):
|
||||
now = time.monotonic()
|
||||
if now - self.last_save < args.interval:
|
||||
return
|
||||
try:
|
||||
frame = self.bridge.imgmsg_to_cv2(message, desired_encoding="bgr8")
|
||||
if args.compressed:
|
||||
frame = self.bridge.compressed_imgmsg_to_cv2(
|
||||
message, desired_encoding="bgr8"
|
||||
)
|
||||
else:
|
||||
frame = self.bridge.imgmsg_to_cv2(
|
||||
message, desired_encoding="bgr8"
|
||||
)
|
||||
except Exception as error:
|
||||
self.get_logger().error(f"Не удалось преобразовать кадр: {error}")
|
||||
return
|
||||
|
||||
+128
-14
@@ -49,7 +49,19 @@ def arguments():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--target", help="1/hammer, 2/wrench или 3/pliers")
|
||||
parser.add_argument("--weights", default="models/latest.pt")
|
||||
parser.add_argument("--topic", default="/RMC1/arm95/camera_gripper/image_color")
|
||||
parser.add_argument("--sim", action="store_true", help="Камера/TF и часы Webots")
|
||||
parser.add_argument("--topic", help="ROS-топик камеры; по умолчанию выбирается по --sim")
|
||||
parser.add_argument("--camera-info-topic")
|
||||
parser.add_argument(
|
||||
"--compressed",
|
||||
action="store_true",
|
||||
help="Топик имеет тип sensor_msgs/CompressedImage, а не Image",
|
||||
)
|
||||
parser.add_argument("--base-frame", default="Base_link")
|
||||
parser.add_argument("--fx", type=float, help="Фокус X, если CameraInfo отсутствует")
|
||||
parser.add_argument("--fy", type=float, help="Фокус Y, если CameraInfo отсутствует")
|
||||
parser.add_argument("--cx", type=float, help="Главная точка X")
|
||||
parser.add_argument("--cy", type=float, help="Главная точка Y")
|
||||
parser.add_argument("--conf", type=float, default=0.20)
|
||||
parser.add_argument("--imgsz", type=int, default=512)
|
||||
parser.add_argument("--device", default="cpu")
|
||||
@@ -70,12 +82,26 @@ def arguments():
|
||||
parser.add_argument("--plan-only", action="store_true", help="Спланировать подход, не двигать руку")
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.topic is None:
|
||||
args.topic = (
|
||||
"/RMC1/arm95/camera_gripper/image_color"
|
||||
if args.sim
|
||||
else "/RMC1/arm95/svcam/right/image/compressed"
|
||||
)
|
||||
|
||||
if not 0.0 <= args.conf <= 1.0:
|
||||
parser.error("--conf должен быть от 0 до 1")
|
||||
if args.timeout <= 0 or args.approach_z <= args.pick_z:
|
||||
parser.error("timeout > 0, approach-z должен быть выше pick-z")
|
||||
if (args.drop_x is None) != (args.drop_y is None):
|
||||
parser.error("--drop-x и --drop-y задаются вместе")
|
||||
intrinsics = (args.fx, args.fy, args.cx, args.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("--fx, --fy, --cx и --cy задаются только вместе")
|
||||
if args.fx is not None and (args.fx <= 0 or args.fy <= 0):
|
||||
parser.error("--fx и --fy должны быть больше нуля")
|
||||
return args
|
||||
|
||||
|
||||
@@ -213,18 +239,24 @@ def card_near_center(frame, requested, cv2, np):
|
||||
|
||||
|
||||
def estimate_pose(node, detection, plane_z):
|
||||
if node.camera_info is None:
|
||||
raise RuntimeError("Нет CameraInfo")
|
||||
frame = node.camera_frame or node.camera_info.header.frame_id
|
||||
if node.camera_info is None and node.manual_k is None:
|
||||
raise RuntimeError(
|
||||
f"Нет CameraInfo с {node.info_topic}; задайте калибровку --fx/--fy/--cx/--cy"
|
||||
)
|
||||
frame = node.camera_frame or (
|
||||
node.camera_info.header.frame_id if node.camera_info is not None else ""
|
||||
)
|
||||
if not frame:
|
||||
raise RuntimeError("В сообщении камеры не указан frame_id")
|
||||
transform = node.tf_buffer.lookup_transform(
|
||||
"Base_link", frame, node.Time(), timeout=node.Duration(seconds=1.0)
|
||||
node.base_frame, frame, node.Time(), timeout=node.Duration(seconds=1.0)
|
||||
)
|
||||
translation = transform.transform.translation
|
||||
rotation = transform.transform.rotation
|
||||
quaternion = (rotation.x, rotation.y, rotation.z, rotation.w)
|
||||
|
||||
u, v = detection.center
|
||||
matrix = node.camera_info.k
|
||||
matrix = node.camera_info.k if node.camera_info is not None else node.manual_k
|
||||
fx, fy, cx, cy = matrix[0], matrix[4], matrix[2], matrix[5]
|
||||
ray_camera = ((u - cx) / fx, (v - cy) / fy, 1.0)
|
||||
ray_base = rotate_vector(ray_camera, quaternion)
|
||||
@@ -249,27 +281,93 @@ def estimate_pose(node, detection, plane_z):
|
||||
|
||||
|
||||
def build_vision_node(args, imports):
|
||||
Node, CvBridge, Image, CameraInfo, qos, Buffer, TransformListener, Duration, Time = imports
|
||||
(
|
||||
Node,
|
||||
CvBridge,
|
||||
Image,
|
||||
CompressedImage,
|
||||
CameraInfo,
|
||||
qos,
|
||||
Buffer,
|
||||
TransformListener,
|
||||
Duration,
|
||||
Time,
|
||||
TFMessage,
|
||||
Parameter,
|
||||
QoSProfile,
|
||||
ReliabilityPolicy,
|
||||
DurabilityPolicy,
|
||||
) = imports
|
||||
|
||||
class VisionNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__("mvch_module3_vision")
|
||||
self.set_parameters(
|
||||
[Parameter("use_sim_time", value=args.sim)]
|
||||
)
|
||||
self.Duration = Duration
|
||||
self.Time = Time
|
||||
self.base_frame = args.base_frame
|
||||
self.bridge = CvBridge()
|
||||
self.frame = None
|
||||
self.camera_info = None
|
||||
self.manual_k = (
|
||||
(args.fx, 0.0, args.cx, 0.0, args.fy, args.cy, 0.0, 0.0, 1.0)
|
||||
if args.fx is not None
|
||||
else None
|
||||
)
|
||||
self.camera_frame = None
|
||||
self.lock = threading.Lock()
|
||||
self.tf_buffer = Buffer()
|
||||
self.tf_listener = TransformListener(self.tf_buffer, self)
|
||||
self.create_subscription(Image, args.topic, self.on_image, qos)
|
||||
info_topic = args.topic.rsplit("/", 1)[0] + "/camera_info"
|
||||
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, "/RMC1/tf", self.on_tf, tf_qos
|
||||
)
|
||||
self.create_subscription(
|
||||
TFMessage, "/RMC1/tf_static", self.on_tf_static, static_tf_qos
|
||||
)
|
||||
image_type = CompressedImage if args.compressed else Image
|
||||
self.create_subscription(image_type, args.topic, self.on_image, qos)
|
||||
info_topic = args.camera_info_topic
|
||||
if info_topic is None:
|
||||
if args.topic.endswith("/image/compressed"):
|
||||
info_topic = args.topic.removesuffix("/image/compressed") + "/camera_info"
|
||||
else:
|
||||
info_topic = args.topic.rsplit("/", 1)[0] + "/camera_info"
|
||||
self.create_subscription(CameraInfo, info_topic, self.on_info, qos)
|
||||
self.info_topic = info_topic
|
||||
|
||||
def on_tf(self, message):
|
||||
for transform in message.transforms:
|
||||
self.tf_buffer.set_transform(transform, "RMC1 namespaced tf")
|
||||
|
||||
def on_tf_static(self, message):
|
||||
for transform in message.transforms:
|
||||
self.tf_buffer.set_transform_static(
|
||||
transform, "RMC1 namespaced tf_static"
|
||||
)
|
||||
|
||||
def on_image(self, message):
|
||||
try:
|
||||
frame = self.bridge.imgmsg_to_cv2(message, desired_encoding="bgr8")
|
||||
if args.compressed:
|
||||
frame = self.bridge.compressed_imgmsg_to_cv2(
|
||||
message, desired_encoding="bgr8"
|
||||
)
|
||||
else:
|
||||
frame = self.bridge.imgmsg_to_cv2(
|
||||
message, desired_encoding="bgr8"
|
||||
)
|
||||
except Exception as error:
|
||||
self.get_logger().error(f"Ошибка кадра: {error}")
|
||||
return
|
||||
@@ -296,6 +394,7 @@ class Arm:
|
||||
quaternion_from_euler,
|
||||
event_log,
|
||||
plan_only=False,
|
||||
base_frame="Base_link",
|
||||
):
|
||||
self.robot = moveit(
|
||||
node_name="mvch_module3_moveit",
|
||||
@@ -309,6 +408,7 @@ class Arm:
|
||||
self.quaternion_from_euler = quaternion_from_euler
|
||||
self.log = event_log
|
||||
self.plan_only = plan_only
|
||||
self.base_frame = base_frame
|
||||
|
||||
def execute(self, component, stage):
|
||||
plan = component.plan()
|
||||
@@ -327,7 +427,7 @@ class Arm:
|
||||
|
||||
def pose(self, x, y, z, yaw, stage):
|
||||
goal = self.PoseStamped()
|
||||
goal.header.frame_id = "Base_link"
|
||||
goal.header.frame_id = self.base_frame
|
||||
qx, qy, qz, qw = self.quaternion_from_euler(math.pi, 0.0, yaw)
|
||||
goal.pose.orientation.x = qx
|
||||
goal.pose.orientation.y = qy
|
||||
@@ -529,9 +629,16 @@ def main():
|
||||
from rclpy.duration import Duration
|
||||
from rclpy.executors import SingleThreadedExecutor
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
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, Image
|
||||
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
|
||||
@@ -554,12 +661,18 @@ def main():
|
||||
Node,
|
||||
CvBridge,
|
||||
Image,
|
||||
CompressedImage,
|
||||
CameraInfo,
|
||||
qos_profile_sensor_data,
|
||||
Buffer,
|
||||
TransformListener,
|
||||
Duration,
|
||||
Time,
|
||||
TFMessage,
|
||||
Parameter,
|
||||
QoSProfile,
|
||||
ReliabilityPolicy,
|
||||
DurabilityPolicy,
|
||||
)
|
||||
node = build_vision_node(args, imports)
|
||||
executor = SingleThreadedExecutor()
|
||||
@@ -594,7 +707,7 @@ def main():
|
||||
.to_moveit_configs()
|
||||
.to_dict()
|
||||
)
|
||||
moveit_config["use_sim_time"] = True
|
||||
moveit_config["use_sim_time"] = args.sim
|
||||
# Wildcard нужен из-за namespace /RMC1/arm95: обычный config_dict
|
||||
# записывает параметры только для имени узла без namespace.
|
||||
with NamedTemporaryFile("w", suffix=".yaml", delete=False) as stream:
|
||||
@@ -608,6 +721,7 @@ def main():
|
||||
quaternion_from_euler,
|
||||
event_log,
|
||||
args.plan_only,
|
||||
args.base_frame,
|
||||
)
|
||||
moveit_active = True
|
||||
finally:
|
||||
|
||||
+13
-3
@@ -7,7 +7,7 @@ Tailwind CSS 4, Bun и небольшой ROS 2-мост на Python.
|
||||
|
||||
- выбор RMC1 или RMC2;
|
||||
- положение, скорость и история движения обоих роверов;
|
||||
- ArUco-граф 6 × 6 с координатами маркеров 0–35;
|
||||
- ArUco-граф 5 × 5 для физического поля или 6 × 6 для Webots;
|
||||
- карта занятости RMC1 из `/map`;
|
||||
- облако точек `/RMC*/scan`, повернутое и перенесённое вместе с ровером;
|
||||
- план Nav2 из `/RMC*/plan` и текущая целевая точка;
|
||||
@@ -54,6 +54,13 @@ ros2 launch ar_nav_ros2 bringup_launch.py namespace:=/RMC1 robot_type:=rmc1
|
||||
использует простой контроллер по одометрии. Он доводит оба ровера до x/y/yaw, но
|
||||
не строит обход сложных препятствий, поэтому Nav2 для RMC1 предпочтительнее.
|
||||
|
||||
Для показанной в PDF физической площадки ничего задавать не нужно: используется
|
||||
сетка 5 × 5, ID 0–24. В Webots перед запуском моста задайте его сетку 6 × 6:
|
||||
|
||||
```bash
|
||||
export MVCH_FIELD_SIZE=6
|
||||
```
|
||||
|
||||
3. В третьем, также ROS-sourced терминале запустить FMS:
|
||||
|
||||
```bash
|
||||
@@ -94,8 +101,11 @@ WebSocket и компоновки интерфейса.
|
||||
| План | `/RMC1/plan` | `/RMC2/plan` |
|
||||
| Рабочий инструмент | `.../gripper_trajectory_controller/joint_trajectory` | `/RMC2/lift` |
|
||||
|
||||
Дополнительно используются `/map`, `/RMC2/aruco_id` и
|
||||
`/RMC2/lift_status`.
|
||||
Дополнительно используются `/map`, реальный
|
||||
`/RMC2/camera_bottom/aruco_id` (и старый `/RMC2/aruco_id` для Webots), а также
|
||||
`/RMC2/lift_status`. По документации реального RMC2 статус лифта имеет QoS
|
||||
`Transient Local` и значения `lowered`, `moving`, `raised`; команды — `0.0` вниз
|
||||
и `0.05` м вверх. Мост настроен именно так.
|
||||
|
||||
Симулятор из открытого пакета не публикует напряжение аккумуляторов. Мост уже
|
||||
подписан на `/RMC*/battery_state` (`sensor_msgs/BatteryState`) и
|
||||
|
||||
@@ -33,7 +33,7 @@ export function isCommand(value: unknown): value is Command {
|
||||
case 'set_goal':
|
||||
return isPose(command.pose)
|
||||
case 'lift':
|
||||
return command.robot === 'RMC2' && (command.height === 0 || command.height === 0.1)
|
||||
return command.robot === 'RMC2' && (command.height === 0 || command.height === 0.05)
|
||||
case 'gripper':
|
||||
return command.robot === 'RMC1' && (command.state === 'open' || command.state === 'closed')
|
||||
default:
|
||||
|
||||
@@ -8,6 +8,7 @@ stderr: diagnostics
|
||||
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import queue
|
||||
import sys
|
||||
import threading
|
||||
@@ -81,6 +82,7 @@ class FmsBridge(Node):
|
||||
"type": "state",
|
||||
"timestamp": int(time.time() * 1000),
|
||||
"mode": "ros",
|
||||
"fieldSize": max(2, min(10, int(os.environ.get("MVCH_FIELD_SIZE", "5")))),
|
||||
"bridgeOnline": True,
|
||||
"bridgeError": None,
|
||||
"map": None,
|
||||
@@ -154,8 +156,20 @@ class FmsBridge(Node):
|
||||
|
||||
self.create_subscription(OccupancyGrid, "/map", self.on_map, map_qos)
|
||||
self.create_subscription(OccupancyGrid, "/RMC1/map", self.on_map, map_qos)
|
||||
# Первый топик физического RMC2, второй оставлен для Webots.
|
||||
self.create_subscription(
|
||||
String, "/RMC2/camera_bottom/aruco_id", self.on_aruco, sensor_qos
|
||||
)
|
||||
self.create_subscription(String, "/RMC2/aruco_id", self.on_aruco, sensor_qos)
|
||||
self.create_subscription(String, "/RMC2/lift_status", self.on_lift_status, 10)
|
||||
lift_qos = QoSProfile(
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
)
|
||||
self.create_subscription(
|
||||
String, "/RMC2/lift_status", self.on_lift_status, lift_qos
|
||||
)
|
||||
self.lift_publisher = self.create_publisher(Float64, "/RMC2/lift", 10)
|
||||
self.gripper_publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
@@ -335,7 +349,7 @@ class FmsBridge(Node):
|
||||
self.state["robots"][robot]["goal"] = None
|
||||
self.state["robots"][robot]["plan"] = []
|
||||
elif kind == "lift" and robot == "RMC2" and not emergency:
|
||||
height = 0.1 if float(command.get("height", 0)) >= 0.05 else 0.0
|
||||
height = 0.05 if float(command.get("height", 0)) >= 0.025 else 0.0
|
||||
self.lift_publisher.publish(Float64(data=height))
|
||||
with self.lock:
|
||||
self.state["robots"][robot]["liftStatus"] = "command: up" if height else "command: down"
|
||||
|
||||
@@ -5,12 +5,12 @@ import { applyMockCommand, initialState, tickMock } from './state'
|
||||
describe('command validation', () => {
|
||||
test('accepts valid commands', () => {
|
||||
expect(isCommand({ type: 'manual', robot: 'RMC1', x: 0.4, y: 0.2, yaw: 0 })).toBe(true)
|
||||
expect(isCommand({ type: 'lift', robot: 'RMC2', height: 0.1 })).toBe(true)
|
||||
expect(isCommand({ type: 'lift', robot: 'RMC2', height: 0.05 })).toBe(true)
|
||||
expect(isCommand({ type: 'gripper', robot: 'RMC1', state: 'closed' })).toBe(true)
|
||||
})
|
||||
|
||||
test('rejects wrong robot tools and non-finite coordinates', () => {
|
||||
expect(isCommand({ type: 'lift', robot: 'RMC1', height: 0.1 })).toBe(false)
|
||||
expect(isCommand({ type: 'lift', robot: 'RMC1', height: 0.05 })).toBe(false)
|
||||
expect(isCommand({ type: 'gripper', robot: 'RMC2', state: 'open' })).toBe(false)
|
||||
expect(isCommand({ type: 'set_goal', robot: 'RMC1', pose: { x: Number.NaN, y: 0, yaw: 0 } })).toBe(false)
|
||||
})
|
||||
@@ -23,7 +23,7 @@ describe('mock state', () => {
|
||||
tickMock(state, 1)
|
||||
expect(state.robots.RMC2.pose.x).toBeCloseTo(0.5)
|
||||
|
||||
applyMockCommand(state, { type: 'lift', robot: 'RMC2', height: 0.1 })
|
||||
applyMockCommand(state, { type: 'lift', robot: 'RMC2', height: 0.05 })
|
||||
applyMockCommand(state, { type: 'emergency', robot: 'RMC2', active: true })
|
||||
expect(state.robots.RMC2.velocity.x).toBe(0)
|
||||
expect(state.robots.RMC2.liftStatus).toBe('up')
|
||||
|
||||
@@ -22,6 +22,7 @@ export function initialState(mode: 'ros' | 'mock'): FmsState {
|
||||
type: 'state',
|
||||
timestamp: Date.now(),
|
||||
mode,
|
||||
fieldSize: mode === 'mock' ? 6 : 5,
|
||||
bridgeOnline: mode === 'mock',
|
||||
bridgeError: null,
|
||||
map: mockMap,
|
||||
|
||||
+10
-8
@@ -24,6 +24,7 @@ const initialState: FmsState = {
|
||||
type: 'state',
|
||||
timestamp: 0,
|
||||
mode: 'ros',
|
||||
fieldSize: 5,
|
||||
bridgeOnline: false,
|
||||
bridgeError: null,
|
||||
map: null,
|
||||
@@ -89,10 +90,11 @@ function FieldMap({ state, selected }: { state: FmsState; selected: RobotId }) {
|
||||
return result.slice(0, 5000)
|
||||
}, [state.map])
|
||||
|
||||
const markers = Array.from({ length: 36 }, (_, id) => ({
|
||||
const fieldSize = state.fieldSize || 5
|
||||
const markers = Array.from({ length: fieldSize ** 2 }, (_, id) => ({
|
||||
id,
|
||||
x: -Math.floor(id / 6),
|
||||
y: id % 6,
|
||||
x: -Math.floor(id / fieldSize),
|
||||
y: id % fieldSize,
|
||||
}))
|
||||
|
||||
return (
|
||||
@@ -115,10 +117,10 @@ function FieldMap({ state, selected }: { state: FmsState; selected: RobotId }) {
|
||||
)
|
||||
})}
|
||||
|
||||
{Array.from({ length: 6 }, (_, row) => Array.from({ length: 6 }, (_, col) => {
|
||||
{Array.from({ length: fieldSize }, (_, row) => Array.from({ length: fieldSize }, (_, col) => {
|
||||
const here = worldToSvg({ x: -row, y: col })
|
||||
const right = col < 5 ? worldToSvg({ x: -row, y: col + 1 }) : null
|
||||
const down = row < 5 ? worldToSvg({ x: -(row + 1), y: col }) : null
|
||||
const right = col < fieldSize - 1 ? worldToSvg({ x: -row, y: col + 1 }) : null
|
||||
const down = row < fieldSize - 1 ? worldToSvg({ x: -(row + 1), y: col }) : null
|
||||
return (
|
||||
<g key={`edges-${row}-${col}`} stroke="#1e3a5f" strokeWidth="2">
|
||||
{right && <line x1={here.x} y1={here.y} x2={right.x} y2={right.y} />}
|
||||
@@ -165,7 +167,7 @@ function FieldMap({ state, selected }: { state: FmsState; selected: RobotId }) {
|
||||
})}
|
||||
</svg>
|
||||
<div className="absolute left-3 top-3 rounded-lg bg-slate-950/80 px-3 py-2 text-xs text-slate-300 backdrop-blur">
|
||||
ArUco: шаг 1 м · X вниз · Y вправо
|
||||
ArUco: поле {fieldSize}×{fieldSize} · шаг 1 м · X вниз · Y вправо
|
||||
</div>
|
||||
<div className="absolute bottom-3 right-3 flex gap-3 rounded-lg bg-slate-950/80 px-3 py-2 text-xs backdrop-blur">
|
||||
<span className="text-cyan-300">● RMC1</span><span className="text-amber-300">● RMC2</span><span className="text-violet-300">-- маршрут</span>
|
||||
@@ -385,7 +387,7 @@ export default function App() {
|
||||
{selected === 'RMC1' ? (
|
||||
<div className="grid grid-cols-2 gap-2"><button disabled={rover.emergency} onClick={() => send({ type: 'gripper', robot: 'RMC1', state: 'open' })} className="rounded-lg bg-cyan-800 px-3 py-2 disabled:opacity-40">Открыть схват</button><button disabled={rover.emergency} onClick={() => send({ type: 'gripper', robot: 'RMC1', state: 'closed' })} className="rounded-lg bg-cyan-800 px-3 py-2 disabled:opacity-40">Закрыть схват</button></div>
|
||||
) : (
|
||||
<div className="grid grid-cols-2 gap-2"><button disabled={rover.emergency} onClick={() => send({ type: 'lift', robot: 'RMC2', height: 0.1 })} className="rounded-lg bg-amber-800 px-3 py-2 disabled:opacity-40">Поднять лифт</button><button disabled={rover.emergency} onClick={() => send({ type: 'lift', robot: 'RMC2', height: 0 })} className="rounded-lg bg-amber-800 px-3 py-2 disabled:opacity-40">Опустить лифт</button></div>
|
||||
<div className="grid grid-cols-2 gap-2"><button disabled={rover.emergency} onClick={() => send({ type: 'lift', robot: 'RMC2', height: 0.05 })} className="rounded-lg bg-amber-800 px-3 py-2 disabled:opacity-40">Поднять лифт</button><button disabled={rover.emergency} onClick={() => send({ type: 'lift', robot: 'RMC2', height: 0 })} className="rounded-lg bg-amber-800 px-3 py-2 disabled:opacity-40">Опустить лифт</button></div>
|
||||
)}
|
||||
</Card>
|
||||
</aside>
|
||||
|
||||
@@ -31,6 +31,7 @@ export type FmsState = {
|
||||
type: 'state'
|
||||
timestamp: number
|
||||
mode: 'ros' | 'mock'
|
||||
fieldSize: number
|
||||
bridgeOnline: boolean
|
||||
bridgeError: string | null
|
||||
map: MapState | null
|
||||
@@ -44,5 +45,5 @@ export type Command =
|
||||
| { type: 'set_pose'; robot: RobotId; pose: Pose }
|
||||
| { type: 'set_goal'; robot: RobotId; pose: Pose }
|
||||
| { type: 'cancel_goal'; robot: RobotId }
|
||||
| { type: 'lift'; robot: 'RMC2'; height: 0 | 0.1 }
|
||||
| { type: 'lift'; robot: 'RMC2'; height: 0 | 0.05 }
|
||||
| { type: 'gripper'; robot: 'RMC1'; state: 'open' | 'closed' }
|
||||
|
||||
+28
-2
@@ -13,7 +13,8 @@
|
||||
|
||||
Решение специально последовательное: это медленнее параллельного управления,
|
||||
зато уменьшает риск столкновения RMC1 и RMC2. Маршруты строятся BFS по графу
|
||||
ArUco 6 × 6, занятые клетки исключаются. Движение замыкается по odometry,
|
||||
ArUco, занятые клетки исключаются. Для Webots используется сетка 6 × 6, для
|
||||
физического поля из документации — 5 × 5. Движение замыкается по odometry,
|
||||
`scan_front` вызывает остановку перед препятствием, команды скорости сглаживаются.
|
||||
Соседняя парковочная точка RMC2 автоматически берётся из построенного пути к
|
||||
комплектации, отдельный параметр для неё не нужен.
|
||||
@@ -63,7 +64,7 @@ python3 module5/module5.py --target 1
|
||||
Перед движением программа показывает все точки и маршруты. После проверки
|
||||
нажмите Enter. Классы цели: `1=hammer`, `2=wrench`, `3=pliers`.
|
||||
|
||||
Пример с точками эксперта:
|
||||
Пример с точками эксперта для Webots:
|
||||
|
||||
```bash
|
||||
python3 module5/module5.py --target wrench \
|
||||
@@ -72,6 +73,26 @@ python3 module5/module5.py --target wrench \
|
||||
--delivery 5 --delivery-approach 4 --blocked 31,35
|
||||
```
|
||||
|
||||
На физическом поле обязательно добавьте `--real` и явно передайте все семь точек
|
||||
эксперта. Без них скрипт откажется запускаться, чтобы случайно не применить
|
||||
симуляторный пресет 0–35 к реальным ID 0–24:
|
||||
|
||||
```bash
|
||||
python3 module5/module5.py --real --target wrench \
|
||||
--rmc1-start "$RMC1_START" --rmc2-start "$RMC2_START" \
|
||||
--rack "$RACK" --assembly "$ASSEMBLY" \
|
||||
--assembly-approach "$ASSEMBLY_APPROACH" \
|
||||
--delivery "$DELIVERY" --delivery-approach "$DELIVERY_APPROACH" \
|
||||
--blocked "$BLOCKED" --dry-run
|
||||
```
|
||||
|
||||
Сначала присвойте переменным фактические ID и проверьте этот `--dry-run`. Затем
|
||||
уберите только `--dry-run`. Реальный профиль также выбирает камеру
|
||||
`/RMC1/arm95/svcam/right/image/compressed`, лифт `0.05` м и системное время.
|
||||
Суффикс камеры противоречит указанному в PDF типу `sensor_msgs/Image`, поэтому
|
||||
сначала выполните `ros2 topic info ... -v`; если фактический тип
|
||||
`CompressedImage`, добавьте `--camera-compressed`.
|
||||
|
||||
Только маршруты, без ROS и движения:
|
||||
|
||||
```bash
|
||||
@@ -118,6 +139,11 @@ Webots не имеет аппаратного watchdog: после SIGKILL до
|
||||
поле: геометрия полки может отличаться. Если ARM95 пока не откалиброван,
|
||||
`--skip-arm` позволяет отдельно проверить всю логистику RMC1/RMC2.
|
||||
|
||||
Для расчёта координат детали нужен `CameraInfo`. Если его нет в списке топиков,
|
||||
передайте калибровку как `--camera-fx`, `--camera-fy`, `--camera-cx`,
|
||||
`--camera-cy`. Имена frame камеры и базы проверьте через TF; базу можно заменить
|
||||
аргументом `--base-frame`.
|
||||
|
||||
## Логи и критерии
|
||||
|
||||
Логи сохраняются в `module5/logs/module5_*.jsonl` и не попадают в Git. В них
|
||||
|
||||
+127
-23
@@ -33,7 +33,7 @@ def wrap_angle(value):
|
||||
|
||||
|
||||
def marker_xy(marker_id):
|
||||
"""Координаты поля: 0=(0,0), 6=(-1,0), 1=(0,1)."""
|
||||
"""Координаты поля: 0=(0,0), COLS=(-1,0), 1=(0,1)."""
|
||||
row, column = divmod(marker_id, COLS)
|
||||
return -row * MARKER_SPACING, column * MARKER_SPACING
|
||||
|
||||
@@ -128,7 +128,9 @@ class Scenario:
|
||||
}
|
||||
for name, marker in points.items():
|
||||
if marker not in range(ROWS * COLS):
|
||||
raise ValueError(f"{name}: ID {marker} вне диапазона 0..35")
|
||||
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):
|
||||
@@ -185,16 +187,19 @@ class Anchor:
|
||||
|
||||
|
||||
def arguments():
|
||||
global ROWS, COLS, MARKER_SPACING
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--target", help="1/hammer, 2/wrench, 3/pliers")
|
||||
parser.add_argument("--rmc1-start", type=int, default=15)
|
||||
parser.add_argument("--rmc2-start", type=int, default=0)
|
||||
parser.add_argument("--rack", type=int, default=34)
|
||||
parser.add_argument("--assembly", type=int, default=16)
|
||||
parser.add_argument("--assembly-approach", type=int, default=10)
|
||||
parser.add_argument("--delivery", type=int, default=5)
|
||||
parser.add_argument("--delivery-approach", type=int, default=4)
|
||||
parser.add_argument("--blocked", type=parse_markers, default={31, 35})
|
||||
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)
|
||||
@@ -202,9 +207,14 @@ def arguments():
|
||||
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", default="/RMC1/arm95/camera_gripper/image_color"
|
||||
)
|
||||
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)
|
||||
@@ -217,10 +227,65 @@ def arguments():
|
||||
parser.add_argument("--sensor-timeout", type=float, default=3.0)
|
||||
parser.add_argument("--waypoint-timeout", type=float, default=35.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
|
||||
@@ -265,7 +330,12 @@ def build_ros_node(args, event_log):
|
||||
from nav_msgs.msg import Odometry
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from rclpy.qos import (
|
||||
DurabilityPolicy,
|
||||
QoSProfile,
|
||||
ReliabilityPolicy,
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
from sensor_msgs.msg import LaserScan
|
||||
from std_msgs.msg import Bool, Float64, String
|
||||
|
||||
@@ -303,7 +373,14 @@ def build_ros_node(args, event_log):
|
||||
lambda message, robot=name: self.on_scan(robot, message),
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
self.create_subscription(String, "/RMC2/lift_status", self.on_lift, 10)
|
||||
lift_qos = QoSProfile(
|
||||
depth=1,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||
)
|
||||
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)
|
||||
|
||||
@@ -388,7 +465,8 @@ def build_ros_node(args, event_log):
|
||||
time.sleep(0.04)
|
||||
|
||||
def set_lift(self, raised, allow_already=False):
|
||||
height = 0.1 if raised else 0.0
|
||||
height = 0.05 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
|
||||
@@ -401,9 +479,10 @@ def build_ros_node(args, event_log):
|
||||
with self.lock:
|
||||
status = self.lift_status
|
||||
received_update = self.lift_updates > updates_before
|
||||
# Драйвер публикует status только при его изменении. Если лифт уже
|
||||
# был внизу до нашей подписки, команда down не создаст сообщения.
|
||||
if time.monotonic() - started >= 1.1 and received_update and "success" in status.lower():
|
||||
# Реальный драйвер публикует 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:
|
||||
@@ -573,9 +652,16 @@ class ArmWorkflow:
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
from rclpy.duration import Duration
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
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, Image
|
||||
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
|
||||
@@ -615,14 +701,31 @@ class ArmWorkflow:
|
||||
Node,
|
||||
CvBridge,
|
||||
Image,
|
||||
CompressedImage,
|
||||
CameraInfo,
|
||||
qos_profile_sensor_data,
|
||||
Buffer,
|
||||
TransformListener,
|
||||
Duration,
|
||||
Time,
|
||||
TFMessage,
|
||||
Parameter,
|
||||
QoSProfile,
|
||||
ReliabilityPolicy,
|
||||
DurabilityPolicy,
|
||||
)
|
||||
self.vision = build_vision_node(SimpleNamespace(topic=args.camera_topic), imports)
|
||||
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))
|
||||
@@ -638,7 +741,7 @@ class ArmWorkflow:
|
||||
.to_moveit_configs()
|
||||
.to_dict()
|
||||
)
|
||||
moveit_config["use_sim_time"] = True
|
||||
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
|
||||
@@ -649,6 +752,7 @@ class ArmWorkflow:
|
||||
PoseStamped,
|
||||
quaternion_from_euler,
|
||||
event_log,
|
||||
base_frame=args.base_frame,
|
||||
)
|
||||
finally:
|
||||
Path(params_file).unlink(missing_ok=True)
|
||||
|
||||
Reference in New Issue
Block a user