mavxa/chvt-bak
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: align modules with physical rover API

Browse Source
This commit is contained in:
mavxa
2026-09-10 22:06:08 +03:00
parent 0a8894f3b2
commit cb5e751d15
14 changed files with 474 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
module3/README.md
+41 -5
View File
@@ -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`.
module3/capture_dataset.py
+20 -5
View File
@@ -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
module3/module3.py
+128 -14
View File
@@ -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: