12.头部相机获取物体的位姿态——多目标检测——左臂用家抓右臂用吸盘
·
1.代码如下：
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CameraInfo, PointCloud2, PointField
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
from cv_bridge import CvBridge
from std_msgs.msg import Header
import cv2
import numpy as np
import struct
import time
from ultralytics import YOLO
import open3d as o3d
from scipy.spatial import ConvexHull
from scipy.spatial.transform import Rotation
from vision_detection_action.action import VisionDetection
from rclpy.action import ActionServer, GoalResponse, CancelResponse
from rclpy.action.server import ServerGoalHandle
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.qos import QoSProfile
import threading
import copy
import os
import itertools
import math
from typing import Tuple
# 屏蔽无关警告
os.environ["QT_LOGGING_RULES"] = "qt.fonts.warning=false"
os.environ["OPENCV_LOG_LEVEL"] = "FATAL"
os.environ["CV_LOG_LEVEL"] = "FATAL"
# ============================================================================
# 📋 可调参数配置区域 - 请根据实际需求修改
# ============================================================================
# -------------------- YOLO模型配置 --------------------
YOLO_MODEL_PATH = "/home/wyq/ros2_ws/weights/best_much.pt" # YOLO模型权重文件路径
YOLO_CONFIDENCE_THRESHOLD = 0.3 # YOLO检测置信度阈值 (0.0-1.0)，越高误检越少
# -------------------- 目标物体尺寸配置 (支持多目标) --------------------
# 每个目标物体定义: {类名: (长度, 宽度, 高度)}
# 注意: 类名必须与YOLO模型中的类别名称一致
TARGET_DIMENSIONS = {
"lefta": (0.091, 0.06, 0.06), # (长度, 宽度, 高度) 单位: 米
"leftb": (0.115, 0.06, 0.06),
"leftc": (0.18, 0.06, 0.06),
"leftd": (0.20, 0.10, 0.15), ####大小不确定
"righta": (0.092, 0.071, 0.02),
"rightb": (0.13, 0.12, 0.05),
"rightc": (0.15, 0.10, 0.056),
"rightd": (0.15, 0.15, 0.052),
}
# ==========================
# 🎯 位姿计算模式配置
# ==========================
# 为每个目标指定位置计算模式:
# "center": 计算物体中心点位置 (默认) - 顶面中心 + 高度/2
# "top_center": 计算顶面中心点位置 - 直接使用检测到的顶面中心
# "bottom_center": 计算底面中心点位置 - 顶面中心 - 高度/2
# "custom": 自定义位置 (需要配合自定义偏移量)
#
# 格式: {类名: 模式}
POSE_MODE_CONFIG = {
"lefta": "center", # 使用物体中心 (默认)
"leftb": "center",
"leftc": "center",
"leftd": "center",
"righta": "top_center", # 使用顶面中心
"rightb": "top_center",
"rightc": "top_center",
"rightd": "top_center",
# 可以添加更多配置
# "target4": "bottom_center",
}
# -------------------- 目标过滤配置 --------------------
# 只发布这些类型的目标（空列表表示发布所有目标）
FILTER_TARGETS = ["righta", "rightb", "rightc", "rightd"] # 只发布 righta 和 rightb
# FILTER_TARGETS = [] # 发布所有目标
# 自定义偏移量配置 (仅当模式为 "custom" 时生效)
# 格式: {类名: (offset_x, offset_y, offset_z)} 单位: 米
CUSTOM_OFFSET_CONFIG = {
# "target1": (0.0, 0.0, 0.05), # 示例: 在Z轴方向偏移5cm
}
# 默认位姿计算模式 (当目标未在POSE_MODE_CONFIG中配置时使用)
DEFAULT_POSE_MODE = "center" # 可选: "center", "top_center", "bottom_center", "custom"
# 默认尺寸 (当检测到的类别未在TARGET_DIMENSIONS中定义时使用)
DEFAULT_TARGET_DIMENSIONS = (0.15, 0.056, 0.10) # (长度, 宽度, 高度)
# -------------------- 相机话题配置 --------------------
COLOR_TOPIC = "/camera/color/image_raw" # 彩色图像话题名称
DEPTH_TOPIC = "/camera/depth/image_raw" # 深度图像话题名称
CAMERA_INFO_TOPIC = "/camera/color/camera_info" # 相机内参话题名称
END_EFFECTOR_POSE_TOPIC = "/end_effector_pose" # 机械臂末端位姿话题名称
# -------------------- 发布话题配置 --------------------
POINTCLOUD_TOPIC = "/detection/pointcloud" # 点云发布话题（基坐标系）
RESULT_IMAGE_TOPIC = "/detection/result_image" # 检测结果图像发布话题
FINAL_POSE_TOPIC = "/detection/final_pose" # 最终位姿发布话题（基坐标系）
ACTION_NAME = "/vision/detection_pose_cloud" # Action Server名称
# -------------------- 处理间隔配置 --------------------
PROCESS_INTERVAL = 1.0 # 处理间隔(秒)，控制CPU负载，0.5表示每秒处理2次
# -------------------- 深度图配置 --------------------
DEPTH_MIN = 0.3 # 深度最小值(米)，小于此值将被忽略
DEPTH_MAX = 3.0 # 深度最大值(米)，大于此值将被忽略
DEPTH_SCALE = 1000.0 # 深度图缩放因子，将mm转换为m (16UC1格式通常为1000)
# -------------------- 点云配置 --------------------
VOXEL_SIZE = 0.005 # 体素滤波大小(米)，用于降采样点云，增大可减少点数
MIN_MASK_PIXELS = 10 # 掩码最小像素数，小于此值认为检测无效
# -------------------- 平面分割配置 (RANSAC) --------------------
RANSAC_DISTANCE_THRESHOLD = 0.002 # RANSAC平面分割距离阈值(米)，越小要求越严格
RANSAC_N = 3 # RANSAC每次采样点数 (3点确定一个平面)
RANSAC_ITERATIONS = 500 # RANSAC迭代次数，越大精度越高但速度越慢
# -------------------- 可视化配置 --------------------
ENABLE_DISPLAY_WINDOW = False # 是否显示OpenCV可视化窗口
ENABLE_3D_VISUALIZATION = False # 🔥 改为True启用3D可视化
WINDOW_WIDTH = 1200 # 增大窗口宽度
WINDOW_HEIGHT = 800 # 增大窗口高度
# -------------------- 点云保存配置 --------------------
ENABLE_SAVE_PLY = True # 是否保存PLY点云文件
SAVE_PLY_DIR = "./pointclouds" # PLY文件保存目录 (当前目录为".")
# -------------------- 点云发布配置 --------------------
PUBLISH_POINTCLOUD = False # 是否发布点云到ROS话题
# -------------------- 形态学操作配置 --------------------
MORPH_KERNEL_SIZE = 5 # 形态学操作核大小 (奇数)
MORPH_DILATE_ITERATIONS = 2 # 膨胀迭代次数，越大掩码扩展越多
# -------------------- 降采样配置 --------------------
SAMPLE_MAX_POINTS = 50000 # 最大点云点数，超过此值将随机降采样
# ============================================================================
# 🔑 手眼标定外参配置 (从标定程序获取)
# ============================================================================
# 这是通过手眼标定程序得到的结果：相机→机械臂末端 (T_cam2gripper)
# 旋转矩阵 R_cam2gripper (3x3)
# HAND_EYE_ROTATION = np.array([
# [0.03790313, -0.05364665, 0.99784036],
# [-0.99896454, -0.02717939, 0.03648460],
# [0.02516342, -0.99819002, -0.05462128]
# ])
HAND_EYE_ROTATION = np.array([
[0.00271188, -0.01911421, 0.99981363,],
[-0.99982551, -0.01853091, 0.00235764],
[0.0184824, -0.99964556, -0.01916113]
])
# 平移向量 t_cam2gripper
# HAND_EYE_TRANSLATION = np.array([0.03406303, 0.02799409, 0.05059718]).reshape(3, 1)
HAND_EYE_TRANSLATION = np.array([0.00730634, 0.00320815, 0.00048779]).reshape(3, 1)
# 自动构建4x4齐次变换矩阵 T_cam2gripper
HAND_EYE_MATRIX = np.eye(4)
HAND_EYE_MATRIX[:3, :3] = HAND_EYE_ROTATION
HAND_EYE_MATRIX[:3, 3:4] = HAND_EYE_TRANSLATION
# 是否启用位姿转换（如果为False，直接输出相机坐标系下的位姿）
ENABLE_HAND_EYE_TRANSFORM = True
# 将位姿由m转换为mm
M_TO_MM = 1000.0
# ==========================
# 全局QoS配置
# ==========================
QOS_PROFILE = QoSProfile(depth=10)
# ==========================
# 目标形状分类配置
# ==========================
# 圆柱体类别（饮料瓶/易拉罐）- 可能是直立或放倒
CYLINDER_CLASSES = ["lefta", "leftb", "leftc", "leftd"]
# 长方体类别
BOX_CLASSES = ["righta", "rightb", "rightc", "rightd"]
# -------------------- 孤立点过滤配置 --------------------
ENABLE_OUTLIER_FILTER = True # 是否启用孤立点过滤
OUTLIER_NB_NEIGHBORS = 20 # 最近邻数量
OUTLIER_STD_RATIO = 2.0 # 标准差倍数（越大保留越多点）
MIN_POINTS_AFTER_FILTER = 50 # 滤波后最少点数，少于该值则使用原始点云
# ============================================================================
# 代码开始
# ============================================================================
def euler_zyx_to_quaternion(
roll: float, pitch: float, yaw: float, degrees: bool = False
) -> np.ndarray:
"""
将欧拉角 (ZYX 顺序) 转换为四元数。
Parameters
----------
roll : float
滚转角 (绕 X 轴), 弧度或角度
pitch : float
俯仰角 (绕 Y 轴), 弧度或角度
yaw : float
偏航角 (绕 Z 轴), 弧度或角度
degrees : bool, optional
如果为 True, 则 roll/pitch/yaw 的单位为度; 默认为 False (弧度)
Returns
-------
np.ndarray
四元数 [w, x, y, z]
Examples
--------
>>> # 绕 Z 轴旋转 90° (纯偏航)
>>> q = euler_zyx_to_quaternion(0, 0, 90, degrees=True)
>>> print(q) # [0.7071, 0, 0, 0.7071]
>>> # 绕 X 轴旋转 180° (纯滚转)
>>> q = euler_zyx_to_quaternion(180, 0, 0, degrees=True)
>>> print(q) # [0, 1, 0, 0]
"""
if degrees:
roll = math.radians(roll)
pitch = math.radians(pitch)
yaw = math.radians(yaw)
# 半角
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
# ZYX 顺序: q = q_z(yaw) * q_y(pitch) * q_x(roll)
w = cr * cp * cy + sr * sp * sy
x = sr * cp * cy - cr * sp * sy
y = cr * sp * cy + sr * cp * sy
z = cr * cp * sy - sr * sp * cy
return np.array([x, y, z, w])
def quaternion_to_euler_zyx(
q: np.ndarray, degrees: bool = False
) -> Tuple[float, float, float]:
"""
将四元数转换回欧拉角 (ZYX 顺序)。
Parameters
----------
q : np.ndarray
四元数 [w, x, y, z]
degrees : bool, optional
如果为 True, 返回角度制; 默认为 False (弧度)
Returns
-------
Tuple[float, float, float]
(roll, pitch, yaw)
Notes
-----
当 pitch 接近 ±90° 时存在万向锁, roll 和 yaw 可能不唯一。
"""
w, x, y, z = q
# 计算 pitch
sin_pitch = 2.0 * (w * y - z * x)
sin_pitch = np.clip(sin_pitch, -1.0, 1.0)
pitch = math.asin(sin_pitch)
# 计算 roll
sin_roll = 2.0 * (w * x + y * z)
cos_roll = 1.0 - 2.0 * (x * x + y * y)
roll = math.atan2(sin_roll, cos_roll)
# 计算 yaw
sin_yaw = 2.0 * (w * z + x * y)
cos_yaw = 1.0 - 2.0 * (y * y + z * z)
yaw = math.atan2(sin_yaw, cos_yaw)
if degrees:
roll = math.degrees(roll)
pitch = math.degrees(pitch)
yaw = math.degrees(yaw)
return roll, pitch, yaw
def filter_outlier_points(points_3d, colors=None,
nb_neighbors=20, std_ratio=2.0,
min_points=50):
"""
对点云进行统计滤波，去除孤立点
参数:
points_3d: numpy.ndarray, 形状为 (N, 3), 3D点云坐标
colors: numpy.ndarray, 形状为 (N, 3), 点云颜色 (可选)
nb_neighbors: int, 统计滤波的最近邻数量
std_ratio: float, 标准差倍数阈值
min_points: int, 滤波后最少点数，少于该值则返回原始点云
返回:
filtered_points: numpy.ndarray, 滤波后的点云坐标
filtered_colors: numpy.ndarray, 滤波后的点云颜色 (如果输入了colors)
removed_count: int, 被移除的点数
success: bool, 是否成功滤波
"""
if len(points_3d) < min_points:
return points_3d, colors, 0, False
try:
# 转换为Open3D点云
o3d_pcd = o3d.geometry.PointCloud()
o3d_pcd.points = o3d.utility.Vector3dVector(points_3d)
if colors is not None and len(colors) > 0:
o3d_pcd.colors = o3d.utility.Vector3dVector(colors / 255.0)
original_count = len(points_3d)
# 统计滤波 - 去除孤立点
o3d_pcd_filtered, ind = o3d_pcd.remove_statistical_outlier(
nb_neighbors=nb_neighbors,
std_ratio=std_ratio
)
filtered_count = len(o3d_pcd_filtered.points)
removed_count = original_count - filtered_count
# 如果滤波后点云太少，返回原始点云
if filtered_count < min_points:
print(f"⚠️ 滤波后点云过少 ({filtered_count} < {min_points})，使用原始点云")
return points_3d, colors, 0, False
# 使用滤波后的点云
filtered_points = np.asarray(o3d_pcd_filtered.points)
filtered_colors = None
if len(o3d_pcd_filtered.colors) > 0:
filtered_colors = (np.asarray(o3d_pcd_filtered.colors) * 255).astype(np.uint8)
print(f"📊 孤立点过滤: {original_count} → {filtered_count} 个点 "
f"(移除 {removed_count} 个, {removed_count / original_count * 100:.1f}%)")
return filtered_points, filtered_colors, removed_count, True
except Exception as e:
print(f"⚠️ 孤立点过滤失败: {e}，使用原始点云")
return points_3d, colors, 0, False
def filter_outlier_points_radius(points_3d, colors=None,
nb_points=16, radius=0.05,
min_points=50):
"""
对点云进行半径滤波，去除周围点太少的点
参数:
points_3d: numpy.ndarray, 形状为 (N, 3), 3D点云坐标
colors: numpy.ndarray, 形状为 (N, 3), 点云颜色 (可选)
nb_points: int, 半径内最少点数
radius: float, 半径大小 (米)
min_points: int, 滤波后最少点数
返回:
filtered_points, filtered_colors, removed_count, success
"""
if len(points_3d) < min_points:
return points_3d, colors, 0, False
try:
o3d_pcd = o3d.geometry.PointCloud()
o3d_pcd.points = o3d.utility.Vector3dVector(points_3d)
if colors is not None and len(colors) > 0:
o3d_pcd.colors = o3d.utility.Vector3dVector(colors / 255.0)
original_count = len(points_3d)
# 半径滤波
o3d_pcd_filtered, ind = o3d_pcd.remove_radius_outlier(
nb_points=nb_points,
radius=radius
)
filtered_count = len(o3d_pcd_filtered.points)
removed_count = original_count - filtered_count
if filtered_count < min_points:
print(f"⚠️ 半径滤波后点云过少 ({filtered_count} < {min_points})，使用原始点云")
return points_3d, colors, 0, False
filtered_points = np.asarray(o3d_pcd_filtered.points)
filtered_colors = None
if len(o3d_pcd_filtered.colors) > 0:
filtered_colors = (np.asarray(o3d_pcd_filtered.colors) * 255).astype(np.uint8)
print(f"📊 半径滤波: {original_count} → {filtered_count} 个点 "
f"(移除 {removed_count} 个, {removed_count / original_count * 100:.1f}%)")
return filtered_points, filtered_colors, removed_count, True
except Exception as e:
print(f"⚠️ 半径滤波失败: {e}，使用原始点云")
return points_3d, colors, 0, False
def get_target_info(class_name):
"""
根据类别名称获取目标物体的尺寸和形状类型
参数:
class_name: 目标类别名称
返回:
(length, width, height, is_cylinder): 尺寸和是否为圆柱体
"""
if class_name in TARGET_DIMENSIONS:
length, width, height = TARGET_DIMENSIONS[class_name]
else:
print(f"⚠️ 警告: 未找到类别 '{class_name}' 的尺寸配置，使用默认尺寸 {DEFAULT_TARGET_DIMENSIONS}")
length, width, height = DEFAULT_TARGET_DIMENSIONS
is_cylinder = class_name in CYLINDER_CLASSES
return length, width, height, is_cylinder
def detect_cylinder_orientation(points_2d):
"""
检测圆柱体的姿态：直立（看到顶面圆形）还是放倒（看到侧面矩形）
返回:
'upright': 直立（顶面朝上，看到圆形）
'lying': 放倒（侧面朝上，看到矩形）
'unknown': 未知
"""
if len(points_2d) < 10:
return 'unknown'
try:
# 计算2D点云的形状特征
hull = ConvexHull(points_2d)
area = hull.volume
perimeter = 0
for i in range(len(hull.vertices)):
j = (i + 1) % len(hull.vertices)
p1 = points_2d[hull.vertices[i]]
p2 = points_2d[hull.vertices[j]]
perimeter += np.linalg.norm(p2 - p1)
if perimeter < 1e-6:
return 'unknown'
# 圆形度 = 4π * 面积 / 周长^2
circularity = 4 * np.pi * area / (perimeter * perimeter)
# 计算长宽比
cov = np.cov(points_2d.T)
eigenvalues, _ = np.linalg.eigh(cov)
aspect_ratio = np.sqrt(eigenvalues.max() / (eigenvalues.min() + 1e-6))
print(f"📊 形状特征: 圆形度={circularity:.3f}, 长宽比={aspect_ratio:.2f}")
# 判断逻辑
if circularity > 0.7 and aspect_ratio < 1.5:
return 'upright'
elif circularity < 0.5 and aspect_ratio > 2.0:
return 'lying'
else:
return 'unknown'
except Exception as e:
print(f"⚠️ 圆柱体姿态检测失败: {e}")
return 'unknown'
# ==========================
# 辅助函数：角度规范化
# ==========================
def normalize_angle_deg(angle_deg):
"""
将角度规范化到 [0, 90] 范围
输入: angle_deg - 角度值(度)
输出: 规范化的角度
功能: 如果角度在[90,180]之间，取补角；如果>45度，取90-角度
"""
angle_deg = abs(angle_deg) % 180
if angle_deg > 90:
angle_deg = 180 - angle_deg
if angle_deg > 45:
angle_deg = 90 - angle_deg
return angle_deg
def normalize_angle_rad(angle_rad):
"""
将角度规范化到 [0, pi/2] 范围
如果角度在 [pi/2, pi] 之间，取补角
"""
angle_rad = angle_rad % np.pi
if angle_rad > np.pi / 2:
angle_rad = np.pi - angle_rad
return angle_rad
def get_target_dimensions(class_name):
"""
根据类别名称获取目标物体的尺寸
参数:
class_name: 目标类别名称 (如 "target1", "target2")
返回:
(length, width, height): 元组，单位: 米
"""
if class_name in TARGET_DIMENSIONS:
return TARGET_DIMENSIONS[class_name]
else:
print(f"⚠️ 警告: 未找到类别 '{class_name}' 的尺寸配置，使用默认尺寸 {DEFAULT_TARGET_DIMENSIONS}")
return DEFAULT_TARGET_DIMENSIONS
def get_pose_mode(class_name):
"""
根据类别名称获取位姿计算模式
参数:
class_name: 目标类别名称
返回:
mode: 字符串，位姿计算模式
offset: 元组，自定义偏移量 (仅当模式为 "custom" 时使用)
"""
mode = POSE_MODE_CONFIG.get(class_name, DEFAULT_POSE_MODE)
offset = CUSTOM_OFFSET_CONFIG.get(class_name, (0.0, 0.0, 0.0))
return mode, offset
# ==========================
# 分割图片的显示和发布 (支持多目标)
# ==========================
def segment_display_and_publish(rgb, detections, bridge, publisher):
"""
显示分割结果并发布到ROS话题 (支持多目标)
"""
overlay = None
try:
if rgb is None or len(rgb.shape) != 3:
return None
if rgb.dtype != np.uint8:
rgb = np.clip(rgb, 0, 255).astype(np.uint8)
# 创建基础图像
overlay = rgb.copy()
# 创建一个单独的掩码叠加层
mask_overlay = np.zeros_like(rgb, dtype=np.float32)
# 为不同目标分配不同颜色
colors = [
(0, 255, 0), # 绿色
(255, 0, 0), # 蓝色
(0, 0, 255), # 红色
(255, 255, 0), # 青色
(255, 0, 255), # 品红
(0, 255, 255), # 黄色
(128, 128, 0), # 橄榄
(128, 0, 128), # 紫色
]
# 绘制每个检测目标
for idx, (mask, bbox, class_name, confidence) in enumerate(detections):
color = colors[idx % len(colors)]
# 绘制掩码（累积到 mask_overlay）
if mask is not None and np.sum(mask) > 0:
if mask.dtype != np.uint8:
mask = mask.astype(np.uint8)
if mask.max() <= 1:
mask = mask * 255
# 创建彩色掩码
mask_colored = np.zeros_like(rgb, dtype=np.float32)
mask_colored[:, :, 0] = mask * (color[2] / 255.0) # B通道
mask_colored[:, :, 1] = mask * (color[1] / 255.0) # G通道
mask_colored[:, :, 2] = mask * (color[0] / 255.0) # R通道
# 累积到 mask_overlay（取最大值，避免覆盖）
mask_overlay = np.maximum(mask_overlay, mask_colored)
# 绘制边界框
if bbox is not None:
x1, y1, x2, y2 = bbox
cv2.rectangle(overlay, (x1, y1), (x2, y2), color, 2)
label = f"{class_name} {confidence:.2f}"
cv2.putText(overlay, label, (x1, y1 - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2)
# 添加编号
cv2.putText(overlay, f"#{idx + 1}", (x1, y2 + 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2)
# 将累积的掩码叠加到图像上
if np.max(mask_overlay) > 0:
overlay = cv2.addWeighted(overlay, 0.7, mask_overlay.astype(np.uint8), 0.3, 0)
# 添加信息
h, w = overlay.shape[:2]
cv2.putText(overlay, f"YOLO Detection | {len(detections)} objects | {w}x{h}", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
# 保存到本地
cv2.imwrite("./result.png", overlay)
# 显示窗口
if ENABLE_DISPLAY_WINDOW:
cv2.namedWindow("Detection Result", cv2.WINDOW_NORMAL)
cv2.resizeWindow("Detection Result", WINDOW_WIDTH, WINDOW_HEIGHT)
cv2.imshow("Detection Result", overlay)
cv2.waitKey(0)
if key == ord('q') or key == 27: # q 或 ESC 退出
cv2.destroyAllWindows()
return overlay
# 发布检测结果图像到ROS话题
if publisher is not None and publisher.get_subscription_count() > 0:
try:
result_msg = bridge.cv2_to_imgmsg(overlay, "bgr8")
result_msg.header.stamp = bridge.get_clock().now().to_msg() if hasattr(bridge, 'get_clock') else None
result_msg.header.frame_id = "camera_link"
publisher.publish(result_msg)
print(f"📤 已发布检测结果图像 ({len(detections)} 个目标)")
except Exception as e:
print(f"发布检测结果图像失败: {e}")
except Exception as e:
print(f"segment_display错误: {e}")
return overlay
# ==========================
# 法向量计算
# ==========================
def calculate_plane_normal(plane_model):
[a, b, c, d] = plane_model
normal = np.array([a, b, c])
norm = np.linalg.norm(normal)
unit_normal = normal / norm
return normal, unit_normal
# ==========================
# RANSAC平面分割
# ==========================
def ranscan(pcd):
if isinstance(pcd, np.ndarray):
o3d_pcd = o3d.geometry.PointCloud()
o3d_pcd.points = o3d.utility.Vector3dVector(pcd)
print(f"📊 已将NumPy数组 ({pcd.shape}) 转换为Open3D点云")
elif isinstance(pcd, o3d.geometry.PointCloud):
o3d_pcd = pcd
else:
raise TypeError(f"不支持的类型: {type(pcd)}")
if len(o3d_pcd.points) == 0:
print("❌ 点云为空")
return None, None, None
plane_model, inliers = o3d_pcd.segment_plane(
RANSAC_DISTANCE_THRESHOLD, RANSAC_N, RANSAC_ITERATIONS
)
[a, b, c, d] = plane_model
print(f"📐 Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
print(f"✅ 找到 {len(inliers)} 个内点 (占比 {len(inliers) / len(o3d_pcd.points) * 100:.1f}%)")
inlier_cloud = o3d_pcd.select_by_index(inliers)
inlier_cloud.paint_uniform_color([0, 0, 1.0])
outlier_cloud = o3d_pcd.select_by_index(inliers, invert=True)
outlier_cloud.paint_uniform_color([1.0, 0, 0])
return plane_model, inlier_cloud, outlier_cloud
# ==========================
# 提取边长方向
# ==========================
def extract_box_axes_from_edges(points, z_axis, box_length, box_width, box_height,
is_cylinder=False, cylinder_orientation='unknown'):
"""
从点云边缘提取盒子的X轴和Y轴方向向量
支持圆柱体（直立/放倒）和长方体
对于长方体：根据分割面的实际尺寸与真实尺寸匹配，确定X/Y轴方向
"""
if len(points) < 10:
return None, None
# 去中心化并投影到平面
centroid = np.mean(points, axis=0)
points_centered = points - centroid
points_proj = points_centered - np.outer(np.dot(points_centered, z_axis), z_axis)
# 建立局部2D坐标系
x_temp = np.array([1, 0, 0])
if abs(np.dot(x_temp, z_axis)) > 0.9:
x_temp = np.array([0, 1, 0])
x_temp = x_temp - np.dot(x_temp, z_axis) * z_axis
x_temp = x_temp / (np.linalg.norm(x_temp) + 1e-8)
y_temp = np.cross(z_axis, x_temp)
y_temp = y_temp / (np.linalg.norm(y_temp) + 1e-8)
# 将3D点云投影到2D平面
points_2d = np.zeros((len(points_proj), 2))
for i, p in enumerate(points_proj):
points_2d[i, 0] = np.dot(p, x_temp)
points_2d[i, 1] = np.dot(p, y_temp)
# 计算2D点云的凸包
hull = ConvexHull(points_2d)
hull_points = points_2d[hull.vertices]
# 提取凸包的边方向和长度
edge_dirs = []
edge_lengths = []
for i in range(len(hull_points)):
j = (i + 1) % len(hull_points)
edge_vec = hull_points[j] - hull_points[i]
edge_len = np.linalg.norm(edge_vec)
if edge_len > 0.001:
edge_dirs.append(edge_vec / edge_len)
edge_lengths.append(edge_len)
# ========== 根据形状类型选择不同的处理策略 ==========
if is_cylinder and cylinder_orientation == 'upright':
# 圆柱体直立（看到顶面圆形）- 使用PCA
cov = np.cov(points_2d.T)
eigenvalues, eigenvectors = np.linalg.eigh(cov)
idx = np.argsort(eigenvalues)[::-1]
x_axis_2d = eigenvectors[:, idx[0]]
if x_axis_2d[0] < 0:
x_axis_2d = -x_axis_2d
y_axis_2d = np.array([-x_axis_2d[1], x_axis_2d[0]])
elif is_cylinder and cylinder_orientation == 'lying':
# 圆柱体放倒（看到侧面矩形）
if len(edge_dirs) >= 2:
edge_dirs = np.array(edge_dirs)
edge_lengths = np.array(edge_lengths)
sorted_idx = np.argsort(edge_lengths)[::-1]
sorted_dirs = edge_dirs[sorted_idx]
dir_long = sorted_dirs[0]
dir_short = sorted_dirs[1]
dir_short = dir_short - np.dot(dir_short, dir_long) * dir_long
dir_short = dir_short / (np.linalg.norm(dir_short) + 1e-8)
# 长边方向对应圆柱轴线方向
x_axis_2d = dir_long
y_axis_2d = dir_short
else:
# 降级方案：使用PCA
cov = np.cov(points_2d.T)
eigenvalues, eigenvectors = np.linalg.eigh(cov)
idx = np.argsort(eigenvalues)[::-1]
x_axis_2d = eigenvectors[:, idx[0]]
y_axis_2d = eigenvectors[:, idx[1]]
else:
# ========== 长方体：根据实际尺寸匹配 ==========
if len(edge_dirs) >= 2:
edge_dirs = np.array(edge_dirs)
edge_lengths = np.array(edge_lengths)
sorted_idx = np.argsort(edge_lengths)[::-1]
sorted_dirs = edge_dirs[sorted_idx]
sorted_lengths = edge_lengths[sorted_idx]
# 取前两条边（最长和次长）
edge1_len = sorted_lengths[0]
edge2_len = sorted_lengths[1] if len(sorted_lengths) > 1 else edge1_len * 0.5
dir1 = sorted_dirs[0]
dir2 = sorted_dirs[1] if len(sorted_dirs) > 1 else np.array([-dir1[1], dir1[0]])
# 正交化方向2
dir2 = dir2 - np.dot(dir2, dir1) * dir1
dir2 = dir2 / (np.linalg.norm(dir2) + 1e-8)
# 🔑 关键：根据真实尺寸匹配
# 可能的尺寸组合：[长, 宽], [长, 高], [宽, 高]
dimensions = [
(box_length, box_width, 'length', 'width'),
(box_length, box_height, 'length', 'height'),
(box_width, box_height, 'width', 'height'),
]
# 计算检测到的边长比
detected_ratio = max(edge1_len, edge2_len) / (min(edge1_len, edge2_len) + 1e-8)
best_match = None
best_score = float('inf')
for dim1, dim2, name1, name2 in dimensions:
# 计算真实尺寸比
real_ratio = max(dim1, dim2) / (min(dim1, dim2) + 1e-8)
# 计算匹配分数（比值越接近越好）
score = abs(detected_ratio - real_ratio) / (real_ratio + 1e-8)
if score < best_score:
best_score = score
best_match = (dim1, dim2, name1, name2)
# 使用最佳匹配确定X/Y轴
if best_match is not None:
dim1, dim2, name1, name2 = best_match
# 如果检测到的最长边对应dim1
if edge1_len >= edge2_len:
x_axis_2d = dir1
y_axis_2d = dir2
else:
x_axis_2d = dir2
y_axis_2d = dir1
print(f"📐 尺寸匹配: 检测边长 ({edge1_len:.3f}, {edge2_len:.3f}) → "
f"真实尺寸 ({dim1:.3f}, {dim2:.3f}) [{name1}, {name2}]")
else:
# 降级方案：按原始逻辑
if box_length >= box_width:
x_axis_2d = dir1
y_axis_2d = dir2
else:
x_axis_2d = dir2
y_axis_2d = dir1
else:
# 降级方案：使用PCA
cov = np.cov(points_2d.T)
eigenvalues, eigenvectors = np.linalg.eigh(cov)
idx = np.argsort(eigenvalues)[::-1]
x_axis_2d = eigenvectors[:, idx[0]]
y_axis_2d = eigenvectors[:, idx[1]]
# 将2D方向向量映射回3D空间
x_axis = x_axis_2d[0] * x_temp + x_axis_2d[1] * y_temp
y_axis = y_axis_2d[0] * x_temp + y_axis_2d[1] * y_temp
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
# 确保Y轴垂直于Z轴
y_axis = y_axis - np.dot(y_axis, z_axis) * z_axis
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
x_axis = np.cross(y_axis, z_axis)
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
return x_axis, y_axis
def generate_box_model_from_pose(center, x_axis, y_axis, z_axis, length, width, height,
num_points_per_face=100):
"""
根据位姿和尺寸生成完整的盒子模型点云（绿色实体）
参数:
center: 盒子中心点 (3D坐标)
x_axis: X轴方向向量 (归一化)
y_axis: Y轴方向向量 (归一化)
z_axis: Z轴方向向量 (归一化)
length: 盒子长度 (沿X轴)
width: 盒子宽度 (沿Y轴)
height: 盒子高度 (沿Z轴)
num_points_per_face: 每个面的采样点数
返回:
box_points: 盒子表面点云 (N, 3)
box_colors: 盒子颜色 (N, 3) - 绿色
"""
# 定义盒子的8个顶点 (中心在原点)
l2, w2, h2 = length / 2, width / 2, height / 2
vertices_local = np.array([
[-l2, -w2, -h2],
[l2, -w2, -h2],
[l2, w2, -h2],
[-l2, w2, -h2],
[-l2, -w2, h2],
[l2, -w2, h2],
[l2, w2, h2],
[-l2, w2, h2]
])
# 将顶点转换到世界坐标系
rotation_matrix = np.column_stack([x_axis, y_axis, z_axis])
vertices_world = (rotation_matrix @ vertices_local.T).T + center
# 定义6个面的顶点索引
faces = [
[0, 1, 2, 3], # 底面
[4, 5, 6, 7], # 顶面
[0, 1, 5, 4], # 后面
[2, 3, 7, 6], # 前面
[0, 3, 7, 4], # 左面
[1, 2, 6, 5] # 右面
]
points = []
# 对每个面进行网格采样
for face_indices in faces:
v0 = vertices_world[face_indices[0]]
v1 = vertices_world[face_indices[1]]
v2 = vertices_world[face_indices[2]]
v3 = vertices_world[face_indices[3]]
n = int(np.sqrt(num_points_per_face))
for i in np.linspace(0, 1, n):
for j in np.linspace(0, 1, n):
# 双线性插值
p = (1 - i) * (1 - j) * v0 + i * (1 - j) * v1 + i * j * v2 + (1 - i) * j * v3
points.append(p)
points = np.array(points)
# 绿色
colors = np.full((len(points), 3), [0, 255, 0], dtype=np.uint8)
return points, colors
def generate_cylinder_model_from_pose(center, x_axis, y_axis, z_axis,
radius, height, num_points=1000):
"""
根据位姿和尺寸生成完整的圆柱体模型点云（绿色实体）
参数:
center: 圆柱体中心点 (3D坐标)
x_axis: X轴方向向量 (归一化)
y_axis: Y轴方向向量 (归一化)
z_axis: Z轴方向向量 (归一化，圆柱轴线方向)
radius: 圆柱体半径
height: 圆柱体高度 (沿Z轴)
num_points: 采样点数
返回:
cylinder_points: 圆柱体表面点云 (N, 3)
cylinder_colors: 圆柱体颜色 (N, 3) - 绿色
"""
# 构建旋转矩阵
rotation_matrix = np.column_stack([x_axis, y_axis, z_axis])
h2 = height / 2
points = []
n_theta = int(np.sqrt(num_points))
n_z = int(np.sqrt(num_points))
# 圆柱体侧面
for i in range(n_theta):
theta = 2 * np.pi * i / n_theta
for j in range(n_z):
z = -h2 + height * j / n_z
# 局部坐标 (圆柱坐标)
local_point = np.array([radius * np.cos(theta), radius * np.sin(theta), z])
world_point = rotation_matrix @ local_point + center
points.append(world_point)
# 顶面 (圆形)
for i in range(n_theta):
r = radius * i / n_theta
for j in range(n_theta):
theta = 2 * np.pi * j / n_theta
local_point = np.array([r * np.cos(theta), r * np.sin(theta), h2])
world_point = rotation_matrix @ local_point + center
points.append(world_point)
# 底面 (圆形)
for i in range(n_theta):
r = radius * i / n_theta
for j in range(n_theta):
theta = 2 * np.pi * j / n_theta
local_point = np.array([r * np.cos(theta), r * np.sin(theta), -h2])
world_point = rotation_matrix @ local_point + center
points.append(world_point)
points = np.array(points)
colors = np.full((len(points), 3), [0, 255, 0], dtype=np.uint8) # 绿色
return points, colors
# ==========================
# 6D位姿估计器 (支持多种位置计算模式)
# ==========================
class Box6DPoseEstimator:
"""
盒子6D位姿估计器
"""
def __init__(self, box_length, box_width, box_height, is_cylinder=False):
self.box_length = box_length
self.box_width = box_width
self.box_height = box_height
self.is_cylinder = is_cylinder # 是否为圆柱体
self.extrinsic_matrix = None
self.end_effector_pose = None
self.pose_mode = "center"
self.custom_offset = (0.0, 0.0, 0.0)
def set_extrinsic_matrix(self, extrinsic):
self.extrinsic_matrix = np.array(extrinsic)
def set_end_effector_pose(self, pose):
self.end_effector_pose = np.array(pose)
def set_pose_mode(self, mode, offset=(0.0, 0.0, 0.0)):
self.pose_mode = mode
self.custom_offset = np.array(offset)
def calculate_position(self, top_center, z_axis):
if self.pose_mode == "top_center":
return top_center.copy()
elif self.pose_mode == "bottom_center":
return top_center - z_axis * (self.box_height / 2.0)
elif self.pose_mode == "custom":
return top_center + np.array(self.custom_offset)
else:
return top_center + z_axis * (self.box_height / 2.0)
def estimate_6d_pose(self, plane_model, inlier_cloud, box_center_camera=None):
points = np.asarray(inlier_cloud.points)
[a, b, c, d] = plane_model
z_axis = np.array([a, b, c])
z_axis = z_axis / (np.linalg.norm(z_axis) + 1e-8)
if z_axis[2] < 0:
z_axis = -z_axis
print(f"📐 法向量: ({z_axis[0]:.4f}, {z_axis[1]:.4f}, {z_axis[2]:.4f})")
# 检测圆柱体姿态
cylinder_orientation = 'unknown'
if self.is_cylinder:
# 投影到2D平面进行形状分析
centroid = np.mean(points, axis=0)
points_centered = points - centroid
points_proj = points_centered - np.outer(np.dot(points_centered, z_axis), z_axis)
# 建立局部2D坐标系
x_temp = np.array([1, 0, 0])
if abs(np.dot(x_temp, z_axis)) > 0.9:
x_temp = np.array([0, 1, 0])
x_temp = x_temp - np.dot(x_temp, z_axis) * z_axis
x_temp = x_temp / (np.linalg.norm(x_temp) + 1e-8)
y_temp = np.cross(z_axis, x_temp)
y_temp = y_temp / (np.linalg.norm(y_temp) + 1e-8)
points_2d = np.zeros((len(points_proj), 2))
for i, p in enumerate(points_proj):
points_2d[i, 0] = np.dot(p, x_temp)
points_2d[i, 1] = np.dot(p, y_temp)
cylinder_orientation = detect_cylinder_orientation(points_2d)
print(f"📐 圆柱体姿态检测: {cylinder_orientation}")
# 传递姿态信息到提取函数（增加 box_height 参数）
x_axis, y_axis = extract_box_axes_from_edges(
points, z_axis, self.box_length, self.box_width, self.box_height,
self.is_cylinder, cylinder_orientation
)
if x_axis is None or y_axis is None:
print("⚠️ 边长提取失败，使用PCA后备方案")
x_axis, y_axis = self._extract_edges_pca(points, z_axis)
print(f"📐 X轴: ({x_axis[0]:.4f}, {x_axis[1]:.4f}, {x_axis[2]:.4f})")
print(f"📐 Y轴: ({y_axis[0]:.4f}, {y_axis[1]:.4f}, {y_axis[2]:.4f})")
top_center = np.mean(points, axis=0)
print(f"📍 顶面中心: ({top_center[0]:.4f}, {top_center[1]:.4f}, {top_center[2]:.4f})")
if box_center_camera is not None:
position = box_center_camera
print(f"📍 使用外部提供的位置: ({position[0]:.4f}, {position[1]:.4f}, {position[2]:.4f})")
else:
position = self.calculate_position(top_center, z_axis)
mode_names = {
"center": "物体中心",
"top_center": "顶面中心",
"bottom_center": "底面中心",
"custom": "自定义位置"
}
mode_name = mode_names.get(self.pose_mode, self.pose_mode)
print(f"📍 {mode_name}: ({position[0]:.4f}, {position[1]:.4f}, {position[2]:.4f})")
rotation_matrix = np.column_stack([x_axis, y_axis, z_axis])
U, _, Vt = np.linalg.svd(rotation_matrix)
rotation_matrix = U @ Vt
if np.linalg.det(rotation_matrix) < 0:
rotation_matrix = -rotation_matrix
pose_camera = np.eye(4)
pose_camera[:3, :3] = rotation_matrix
pose_camera[:3, 3] = position
roll, pitch, yaw = self._rotation_matrix_to_euler(rotation_matrix)
pose_6d_camera = [
float(position[0]),
float(position[1]),
float(position[2]),
float(roll),
float(pitch),
float(yaw)
]
roll_deg = np.degrees(roll)
pitch_deg = np.degrees(pitch)
yaw_deg = np.degrees(yaw)
mode_names = {
"center": "物体中心",
"top_center": "顶面中心",
"bottom_center": "底面中心",
"custom": "自定义位置"
}
mode_name = mode_names.get(self.pose_mode, self.pose_mode)
print(f"🎯 6D位姿估计结果 (相机坐标系, 模式: {mode_name}, 位置单位: m):")
print(f" 位置 (x, y, z): ({pose_6d_camera[0]:.4f}, {pose_6d_camera[1]:.4f}, {pose_6d_camera[2]:.4f}) m")
print(
f" 位置 (x, y, z): ({pose_6d_camera[0] * 1000:.1f}, {pose_6d_camera[1] * 1000:.1f}, {pose_6d_camera[2] * 1000:.1f}) mm")
print(f" 姿态: ({roll_deg:.2f}°, {pitch_deg:.2f}°, {yaw_deg:.2f}°)")
return pose_6d_camera, pose_camera
def _extract_edges_pca(self, points, z_axis):
centroid = np.mean(points, axis=0)
points_centered = points - centroid
points_proj = points_centered - np.outer(np.dot(points_centered, z_axis), z_axis)
cov = np.cov(points_proj.T)
eigenvalues, eigenvectors = np.linalg.eigh(cov)
idx = np.argsort(eigenvalues)[::-1]
eigenvectors = eigenvectors[:, idx]
x_axis = eigenvectors[:, 0]
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
y_axis = np.cross(z_axis, x_axis)
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
x_axis = np.cross(y_axis, z_axis)
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
return x_axis, y_axis
def _rotation_matrix_to_euler(self, R):
try:
from scipy.spatial.transform import Rotation
r = Rotation.from_matrix(R)
roll, pitch, yaw = r.as_euler('zyx', degrees=False)
return roll, pitch, yaw
except:
sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
singular = sy < 1e-6
if not singular:
roll = np.arctan2(R[2, 1], R[2, 2])
pitch = np.arctan2(-R[2, 0], sy)
yaw = np.arctan2(R[1, 0], R[0, 0])
else:
roll = np.arctan2(-R[1, 2], R[1, 1])
pitch = np.arctan2(-R[2, 0], sy)
yaw = 0
return roll, pitch, yaw
def generate_box_model(self, pose=None):
l, w, h = self.box_length, self.box_width, self.box_height
vertices = np.array([
[-l / 2, -w / 2, -h / 2],
[l / 2, -w / 2, -h / 2],
[l / 2, w / 2, -h / 2],
[-l / 2, w / 2, -h / 2],
[-l / 2, -w / 2, h / 2],
[l / 2, -w / 2, h / 2],
[l / 2, w / 2, h / 2],
[-l / 2, w / 2, h / 2]
])
points = []
faces = [
([0, 1, 2, 3], [0, 0, -1]),
([4, 5, 6, 7], [0, 0, 1]),
([0, 1, 5, 4], [0, -1, 0]),
([2, 3, 7, 6], [0, 1, 0]),
([0, 3, 7, 4], [-1, 0, 0]),
([1, 2, 6, 5], [1, 0, 0])
]
for face_indices, normal in faces:
for i in np.linspace(0, 1, 10):
for j in np.linspace(0, 1, 10):
idx = face_indices
p = (1 - i) * (1 - j) * vertices[idx[0]] + i * (1 - j) * vertices[idx[1]] + \
i * j * vertices[idx[2]] + (1 - i) * j * vertices[idx[3]]
points.append(p)
points = np.array(points)
box_cloud = o3d.geometry.PointCloud()
box_cloud.points = o3d.utility.Vector3dVector(points)
if pose is not None:
if isinstance(pose, list) and len(pose) == 6:
matrix = self.pose_to_matrix(pose)
box_cloud.transform(matrix)
elif isinstance(pose, np.ndarray) and pose.shape == (4, 4):
box_cloud.transform(pose)
elif isinstance(pose, np.ndarray) and pose.shape == (6,):
matrix = self.pose_to_matrix(pose.tolist())
box_cloud.transform(matrix)
return box_cloud
def pose_to_matrix(self, pose_6d):
x, y, z, roll, pitch, yaw = pose_6d
R = self._euler_to_rotation_matrix(roll, pitch, yaw)
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = [x, y, z]
return T
def _euler_to_rotation_matrix(self, roll, pitch, yaw):
try:
from scipy.spatial.transform import Rotation
r = Rotation.from_euler('zyx', [roll, pitch, yaw])
return r.as_matrix()
except:
R_x = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]])
R_y = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]])
R_z = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]])
return R_z @ R_y @ R_x
# ==========================
# 创建PointCloud2消息
# ==========================
def create_point_cloud(points_3d, colors, frame_id, clock):
if len(points_3d) == 0:
return PointCloud2()
cloud_msg = PointCloud2()
cloud_msg.header = Header()
cloud_msg.header.stamp = clock.now().to_msg()
cloud_msg.header.frame_id = frame_id
cloud_msg.height = 1
cloud_msg.width = len(points_3d)
cloud_msg.is_bigendian = False
cloud_msg.is_dense = True
cloud_msg.fields = [
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
PointField(name='rgb', offset=12, datatype=PointField.UINT32, count=1),
]
cloud_msg.point_step = 16
cloud_msg.row_step = cloud_msg.point_step * len(points_3d)
data = []
for pt, col in zip(points_3d, colors):
rgb = (int(col[2]) << 16) | (int(col[1]) << 8) | int(col[0])
data.append(struct.pack('ffff', pt[0], pt[1], pt[2], float(rgb)))
cloud_msg.data = b''.join(data)
return cloud_msg
# ==========================
# 检测结果数据结构
# ==========================
class DetectionResult:
"""单个检测目标的结果"""
def __init__(self, class_name, confidence, bbox, mask,
points_3d, colors, plane_model, inlier_cloud,
pose_camera, pose_gripper, pose_base, box_model,
pose_mode="center"):
self.class_name = class_name
self.confidence = confidence
self.bbox = bbox
self.mask = mask
self.points_3d = points_3d
self.colors = colors
self.plane_model = plane_model
self.inlier_cloud = inlier_cloud
self.pose_camera = pose_camera
self.pose_gripper = pose_gripper
self.pose_base = pose_base
self.box_model = box_model
self.pose_mode = pose_mode
self.timestamp = time.time()
# ==========================
# 主节点 - 带Action Server (支持多目标)
# ==========================
class CameraDetectionNode(Node):
def __init__(self):
# ============================================================
# 步骤1: 初始化ROS2节点
# ============================================================
super().__init__('ros2_camera_detection')
# ============================================================
# 步骤2: 创建图像转换桥接器
# ============================================================
self.bridge = CvBridge()
# ============================================================
# 步骤3: 初始化数据缓存变量
# ============================================================
self.rgb_img = None
self.depth_img = None
self.K = None
self.last_process_time = 0
self.process_interval = PROCESS_INTERVAL
self.end_effector_pose = None
self.end_effector_pose_matrix = None
self.end_effector_received = False
# 缓存最新结果(支持多目标)
self.latest_detections = []
self.latest_detection_img = None
# 线程锁
self.pipeline_lock = threading.Lock()
# 3D可视化相关
self.vis_thread = None
self.vis_geometries = []
self.vis_running = False
# 加载手眼标定参数
self.load_hand_eye_calibration()
# 创建点云保存目录
if ENABLE_SAVE_PLY:
os.makedirs(SAVE_PLY_DIR, exist_ok=True)
self.get_logger().info(f"📁 点云保存目录: {SAVE_PLY_DIR}")
# ============================================================
# 步骤5: 加载YOLO模型
# ============================================================
self.yolo = YOLO(YOLO_MODEL_PATH)
self.yolo.to("cpu")
self.get_logger().info("✅ YOLO模型加载成功")
# ============================================================
# 步骤6: 创建话题订阅器
# ============================================================
self.create_subscription(Image, COLOR_TOPIC, self.rgb_cb, 10)
self.create_subscription(Image, DEPTH_TOPIC, self.depth_cb, 10)
self.create_subscription(CameraInfo, CAMERA_INFO_TOPIC, self.caminfo_cb, 10)
self.create_subscription(PoseStamped, END_EFFECTOR_POSE_TOPIC, self.end_effector_pose_cb, 10)
# ============================================================
# 步骤7: 创建话题发布器
# ============================================================
self.pointcloud_pub = self.create_publisher(
PointCloud2,
POINTCLOUD_TOPIC,
10
)
self.get_logger().info(f"📡 发布点云: {POINTCLOUD_TOPIC}")
self.result_image_pub = self.create_publisher(
Image,
RESULT_IMAGE_TOPIC,
10
)
self.get_logger().info(f"📡 发布结果图像: {RESULT_IMAGE_TOPIC}")
self.final_pose_pub = self.create_publisher(
PoseStamped,
FINAL_POSE_TOPIC,
10
)
self.get_logger().info(f"📡 发布最终位姿: {FINAL_POSE_TOPIC}")
# ============================================================
# 步骤8: 创建Action Server
# ============================================================
self.action_server = ActionServer(
node=self,
action_name=ACTION_NAME,
action_type=VisionDetection,
execute_callback=self.execute_callback,
goal_callback=self.goal_callback,
cancel_callback=self.cancel_callback,
)
# ============================================================
# 步骤9: 打印初始化完成日志
# ============================================================
self.get_logger().info("=" * 60)
self.get_logger().info("✅ 节点初始化完成 (支持多目标检测 + 多种位姿模式)")
self.get_logger().info(f"📡 Action Server: {ACTION_NAME}")
self.get_logger().info(f"📡 点云发布: {POINTCLOUD_TOPIC}")
self.get_logger().info(f"📡 结果图片: {RESULT_IMAGE_TOPIC}")
self.get_logger().info(f"📡 最终位姿: {FINAL_POSE_TOPIC}")
self.get_logger().info(f"📡 订阅末端位姿: {END_EFFECTOR_POSE_TOPIC}")
self.get_logger().info(f"📦 支持的目标类型: {list(TARGET_DIMENSIONS.keys())}")
self.get_logger().info(f"🎯 位姿模式配置: {POSE_MODE_CONFIG}")
self.get_logger().info(f"🎯 默认位姿模式: {DEFAULT_POSE_MODE}")
self.get_logger().info(f"🎨 3D可视化: {'启用' if ENABLE_3D_VISUALIZATION else '禁用'}")
self.get_logger().info("=" * 60)
# 启动3D可视化
if ENABLE_3D_VISUALIZATION:
self.start_visualization()
# ============================================================================
# 3D可视化相关函数 (增强版)
# ============================================================================
def start_visualization(self):
"""启动3D可视化线程"""
if self.vis_thread is not None and self.vis_thread.is_alive():
return
self.vis_running = True
self.vis_thread = threading.Thread(target=self.visualization_loop, daemon=True)
self.vis_thread.start()
self.get_logger().info("✅ 3D可视化线程已启动")
def visualization_loop(self):
"""3D可视化主循环 - 显示所有目标的点云和盒子"""
# 创建可视化窗口
vis = o3d.visualization.Visualizer()
vis.create_window(window_name="3D Detection Result - All 8 Targets",
width=WINDOW_WIDTH,
height=WINDOW_HEIGHT)
# 添加坐标系
coord_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.2)
vis.add_geometry(coord_frame)
# 添加地面网格
try:
grid = o3d.geometry.TriangleMesh.create_grid()
grid.transform([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -0.5], [0, 0, 0, 1]])
vis.add_geometry(grid)
except:
pass
# 不同目标的颜色
target_colors = {
"lefta": [1.0, 0.2, 0.2], # 红色
"leftb": [0.2, 1.0, 0.2], # 绿色
"leftc": [0.2, 0.2, 1.0], # 蓝色
"leftd": [1.0, 1.0, 0.2], # 黄色
"righta": [1.0, 0.2, 1.0], # 品红
"rightb": [0.2, 1.0, 1.0], # 青色
"rightc": [1.0, 0.5, 0.0], # 橙色
"rightd": [0.5, 0.5, 0.5], # 灰色
}
last_update_time = 0
update_interval = 0.3 # 每0.3秒更新一次
while self.vis_running:
try:
current_time = time.time()
if current_time - last_update_time > update_interval:
with self.pipeline_lock:
if len(self.latest_detections) > 0:
# 清除旧几何体（保留坐标系和网格）
for geom in self.vis_geometries:
if geom is not None:
try:
vis.remove_geometry(geom, reset_bounding_box=False)
except:
pass
self.vis_geometries.clear()
# 添加每个目标的几何体
for idx, result in enumerate(self.latest_detections):
color = target_colors.get(result.class_name, [0.5, 0.5, 0.5])
# 1. 添加原始点云（转换到基坐标系）
if result.points_3d is not None and len(result.points_3d) > 0:
pcd = o3d.geometry.PointCloud()
# 降采样显示，每5个点取1个
step = max(1, len(result.points_3d) // 5000)
sample_points = result.points_3d[::step]
# 转换到基坐标系
if result.pose_base is not None:
T_base = self.pose_6d_to_matrix(result.pose_base)
points_homo = np.hstack([sample_points, np.ones((len(sample_points), 1))])
points_base = (T_base @ points_homo.T).T[:, :3]
else:
points_base = sample_points
pcd.points = o3d.utility.Vector3dVector(points_base)
if result.colors is not None:
colors_sample = result.colors[::step] / 255.0
pcd.colors = o3d.utility.Vector3dVector(colors_sample)
else:
pcd.paint_uniform_color(color)
vis.add_geometry(pcd)
self.vis_geometries.append(pcd)
# 2. 添加盒子模型（转换到基坐标系）
if result.box_model is not None:
box = copy.deepcopy(result.box_model)
box.paint_uniform_color(color)
if result.pose_base is not None:
T_base = self.pose_6d_to_matrix(result.pose_base)
box.transform(T_base)
vis.add_geometry(box)
self.vis_geometries.append(box)
# 3. 添加位姿坐标系
if result.pose_base is not None:
try:
pose_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(
size=0.04
)
T_base = self.pose_6d_to_matrix(result.pose_base)
pose_frame.transform(T_base)
vis.add_geometry(pose_frame)
self.vis_geometries.append(pose_frame)
except Exception as e:
pass
# 打印所有目标的位置汇总
self.print_all_positions()
last_update_time = current_time
# 渲染
vis.poll_events()
vis.update_renderer()
# 控制更新频率
time.sleep(0.05)
except Exception as e:
self.get_logger().error(f"3D可视化错误: {e}")
time.sleep(0.1)
vis.destroy_window()
self.get_logger().info("3D可视化窗口已关闭")
def print_all_positions(self):
"""打印所有目标的位置汇总（包含6D姿态）"""
if len(self.latest_detections) == 0:
return
self.get_logger().info("=" * 90)
self.get_logger().info("📊 所有目标6D位姿汇总 (基坐标系):")
self.get_logger().info(" 目标名称 模式 位置 (mm) 姿态 (度)")
self.get_logger().info(" " + "-" * 85)
for result in self.latest_detections:
if result.pose_base is not None:
# 位置 (mm)
x, y, z = result.pose_base[0] * 1000, result.pose_base[1] * 1000, result.pose_base[2] * 1000
# 姿态 (度)
roll = np.degrees(result.pose_base[3])
pitch = np.degrees(result.pose_base[4])
yaw = np.degrees(result.pose_base[5])
mode_names = {
"center": "物体中心",
"top_center": "顶面中心",
"bottom_center": "底面中心",
"custom": "自定义位置"
}
mode_name = mode_names.get(result.pose_mode, result.pose_mode)
self.get_logger().info(
f" {result.class_name:8s} {mode_name:10s}: "
f"({x:7.1f}, {y:7.1f}, {z:7.1f}) "
f"({roll:7.1f}°, {pitch:7.1f}°, {yaw:7.1f}°)"
)
self.get_logger().info("=" * 90)
def stop_visualization(self):
"""停止3D可视化"""
self.vis_running = False
if self.vis_thread is not None:
self.vis_thread.join(timeout=2.0)
# ============================================================================
# 加载手眼标定结果
# ============================================================================
def load_hand_eye_calibration(self):
"""加载手眼标定结果 - 从公共常量读取"""
self.R_cam2gripper = HAND_EYE_ROTATION.copy()
self.t_cam2gripper = HAND_EYE_TRANSLATION.copy()
self.T_cam2gripper = HAND_EYE_MATRIX.copy()
self.enable_transform = ENABLE_HAND_EYE_TRANSFORM
det = np.linalg.det(self.R_cam2gripper)
self.get_logger().info("=" * 60)
self.get_logger().info("✅ 手眼标定参数加载完成")
self.get_logger().info(f"📐 旋转矩阵行列式: {det:.6f} {'✅' if abs(det - 1.0) < 1e-6 else '⚠️'}")
self.get_logger().info(f"🔀 位姿转换状态: {'启用' if self.enable_transform else '禁用'}")
self.get_logger().info("=" * 60)
# ============================================================================
# 机械臂末端位姿回调
# ============================================================================
def end_effector_pose_cb(self, msg: PoseStamped):
"""接收机械臂末端位姿（四元数）"""
try:
position = np.array([
msg.pose.position.x,
msg.pose.position.y,
msg.pose.position.z
])
quat = np.array([
msg.pose.orientation.x,
msg.pose.orientation.y,
msg.pose.orientation.z,
msg.pose.orientation.w
])
self.end_effector_pose = {
'position': position,
'quaternion': quat,
'timestamp': msg.header.stamp
}
rotation = Rotation.from_quat(quat)
self.end_effector_pose_matrix = np.eye(4)
self.end_effector_pose_matrix[:3, :3] = rotation.as_matrix()
self.end_effector_pose_matrix[:3, 3] = position
self.end_effector_received = True
except Exception as e:
self.get_logger().error(f"末端位姿处理错误: {e}")
# ============================================================================
# 坐标转换函数
# ============================================================================
def transform_camera_to_gripper(self, pose_camera):
"""将相机坐标系下的位姿转换到机械臂末端坐标系"""
if not self.enable_transform:
return pose_camera
# 将6D位姿转换为齐次矩阵
if isinstance(pose_camera, (list, np.ndarray)) and len(pose_camera) == 6:
T_camera = self.pose_6d_to_matrix(pose_camera)
elif isinstance(pose_camera, np.ndarray) and pose_camera.shape == (4, 4):
T_camera = pose_camera
else:
raise ValueError("输入格式错误，需要6D位姿或4x4齐次矩阵")
# 🔑 关键：T_gripper = T_cam2gripper @ T_camera
T_gripper = self.T_cam2gripper @ T_camera
# 提取位置和姿态
position = T_gripper[:3, 3]
rotation = T_gripper[:3, :3]
roll, pitch, yaw = self.matrix_to_euler(rotation)
return [
float(position[0]),
float(position[1]),
float(position[2]),
float(roll),
float(pitch),
float(yaw)
]
def transform_gripper_to_base(self, pose_gripper):
"""将机械臂末端坐标系下的位姿转换到基坐标系"""
if self.end_effector_pose_matrix is None:
self.get_logger().warn("⚠️ 未收到末端位姿，无法转换到基坐标系")
return None
if isinstance(pose_gripper, (list, np.ndarray)) and len(pose_gripper) == 6:
T_gripper = self.pose_6d_to_matrix(pose_gripper)
elif isinstance(pose_gripper, np.ndarray) and pose_gripper.shape == (4, 4):
T_gripper = pose_gripper
else:
raise ValueError("输入格式错误，需要6D位姿或4x4齐次矩阵")
# 🔑 关键：T_base = T_end_effector @ T_gripper
T_base = self.end_effector_pose_matrix @ T_gripper
position = T_base[:3, 3]
rotation = T_base[:3, :3]
roll, pitch, yaw = self.matrix_to_euler(rotation)
return [
float(position[0]),
float(position[1]),
float(position[2]),
float(roll),
float(pitch),
float(yaw)
]
def pose_6d_to_matrix(self, pose_6d):
"""将6D位姿转换为4x4齐次矩阵"""
if pose_6d is None:
return np.eye(4)
x, y, z, roll, pitch, yaw = pose_6d
try:
from scipy.spatial.transform import Rotation
r = Rotation.from_euler('zyx', [roll, pitch, yaw])
T = np.eye(4)
T[:3, :3] = r.as_matrix()
T[:3, 3] = [x, y, z]
return T
except:
return self.pose_6d_to_matrix_legacy(pose_6d)
def pose_6d_to_matrix_legacy(self, pose_6d):
"""将6D位姿转换为4x4齐次矩阵 (备用)"""
x, y, z, roll, pitch, yaw = pose_6d
R = self.euler_to_matrix(roll, pitch, yaw)
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = [x, y, z]
return T
def euler_to_matrix(self, roll, pitch, yaw):
"""欧拉角转旋转矩阵"""
try:
from scipy.spatial.transform import Rotation
r = Rotation.from_euler('zyx', [roll, pitch, yaw])
return r.as_matrix()
except ImportError:
Rx = np.array([
[1, 0, 0],
[0, np.cos(roll), -np.sin(roll)],
[0, np.sin(roll), np.cos(roll)]
])
Ry = np.array([
[np.cos(pitch), 0, np.sin(pitch)],
[0, 1, 0],
[-np.sin(pitch), 0, np.cos(pitch)]
])
Rz = np.array([
[np.cos(yaw), -np.sin(yaw), 0],
[np.sin(yaw), np.cos(yaw), 0],
[0, 0, 1]
])
return Rz @ Ry @ Rx
def matrix_to_euler(self, R):
"""旋转矩阵转欧拉角"""
try:
from scipy.spatial.transform import Rotation
r = Rotation.from_matrix(R)
roll, pitch, yaw = r.as_euler('zyx')
return roll, pitch, yaw
except ImportError:
sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
singular = sy < 1e-6
if not singular:
roll = np.arctan2(R[2, 1], R[2, 2])
pitch = np.arctan2(-R[2, 0], sy)
yaw = np.arctan2(R[1, 0], R[0, 0])
else:
roll = np.arctan2(-R[1, 2], R[1, 1])
pitch = np.arctan2(-R[2, 0], sy)
yaw = 0
return roll, pitch, yaw
def transform_points_to_base(self, points_camera):
"""将点云从相机坐标系转换到基坐标系"""
if len(points_camera) == 0:
return points_camera
if self.end_effector_pose_matrix is None:
return None
try:
ones = np.ones((len(points_camera), 1))
points_homogeneous = np.hstack([points_camera, ones])
points_gripper = (self.T_cam2gripper @ points_homogeneous.T).T
points_base = (self.end_effector_pose_matrix @ points_gripper.T).T
return points_base[:, :3]
except Exception as e:
self.get_logger().error(f"点云转换失败: {e}")
return None
# ============================================================================
# 发布最终位姿
# ============================================================================
def publish_final_pose(self, pose_base, class_name="target", pose_mode="center"):
"""发布基坐标系下的最终位姿（单位：毫米）"""
if pose_base is None:
return
pose_msg = PoseStamped()
pose_msg.header.stamp = self.get_clock().now().to_msg()
pose_msg.header.frame_id = "base_link"
pose_msg.pose.position.x = float(pose_base[0]) * M_TO_MM
pose_msg.pose.position.y = float(pose_base[1]) * M_TO_MM
pose_msg.pose.position.z = float(pose_base[2]) * M_TO_MM
# r = Rotation.from_euler('zyx', [pose_base[3], pose_base[4], pose_base[5]])
# quat = r.as_quat()
quat = euler_zyx_to_quaternion(pose_base[5], pose_base[4], pose_base[3])
pose_msg.pose.orientation.x = quat[0]
pose_msg.pose.orientation.y = quat[1]
pose_msg.pose.orientation.z = quat[2]
pose_msg.pose.orientation.w = quat[3]
self.final_pose_pub.publish(pose_msg)
mode_names = {
"center": "物体中心",
"top_center": "顶面中心",
"bottom_center": "底面中心",
"custom": "自定义位置"
}
mode_name = mode_names.get(pose_mode, pose_mode)
self.get_logger().info(
f"📤 发布最终位姿 ({class_name}, {mode_name}): "
f"({pose_base[0] * M_TO_MM:.1f}, {pose_base[1] * M_TO_MM:.1f}, {pose_base[2] * M_TO_MM:.1f}) mm"
)
def goal_callback(self, goal_request: VisionDetection.Goal) -> GoalResponse:
self.get_logger().info(f"📥 收到目标请求: {goal_request.target_obj_name}")
return GoalResponse.ACCEPT
def cancel_callback(self, goal_handle: ServerGoalHandle) -> CancelResponse:
self.get_logger().info("❌ 任务取消")
return CancelResponse.ACCEPT
def caminfo_cb(self, msg):
self.K = np.array(msg.k).reshape(3, 3)
def rgb_cb(self, msg):
self.rgb_img = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
def depth_cb(self, msg):
try:
if msg.encoding == "16UC1":
depth_data = np.ndarray(
shape=(msg.height, msg.width),
dtype=np.uint16,
buffer=msg.data
)
else:
self.get_logger().warning(f"未知深度编码: {msg.encoding}")
return
self.depth_img = depth_data
self.process()
except Exception as e:
self.get_logger().error(f"深度图像处理错误: {e}")
def process(self):
"""主处理函数 - 目标检测、点云生成和6D位姿估计"""
if self.rgb_img is None or self.depth_img is None or self.K is None:
return
current_time = time.time()
if current_time - self.last_process_time < self.process_interval:
return
self.last_process_time = current_time
with self.pipeline_lock:
start = time.time()
rgb = self.rgb_img.copy()
h, w = rgb.shape[:2]
depth_aligned = self.depth_img
depth = depth_aligned.astype(np.float32) / DEPTH_SCALE
depth = np.clip(depth, DEPTH_MIN, DEPTH_MAX)
depth[np.isnan(depth)] = DEPTH_MIN
depth[np.isinf(depth)] = DEPTH_MIN
# =================================> YOLO目标检测 <=================================
res = self.yolo.predict(rgb, conf=YOLO_CONFIDENCE_THRESHOLD, verbose=False)
if len(res) < 1 or res[0].boxes is None or len(res[0].boxes) == 0:
return
# 打印检测到的目标
self.get_logger().info(f"🔍 YOLO检测到 {len(res[0].boxes)} 个目标:")
for i in range(len(res[0].boxes)):
class_id = int(res[0].boxes[i].cls.cpu().numpy()[0])
class_name = res[0].names[class_id]
confidence = float(res[0].boxes[i].conf.cpu().numpy()[0])
self.get_logger().info(f" 目标 {i + 1}: {class_name} (置信度: {confidence:.3f})")
# 收集所有检测到的目标
detections_info = []
for i in range(len(res[0].boxes)):
box = res[0].boxes[i].xyxy.cpu().numpy()[0]
x1, y1, x2, y2 = map(int, box)
bbox = (x1, y1, x2, y2)
class_id = int(res[0].boxes[i].cls.cpu().numpy()[0])
class_name = res[0].names[class_id]
confidence = float(res[0].boxes[i].conf.cpu().numpy()[0])
# 🔑 如果配置了过滤列表，只保留指定的目标
if len(FILTER_TARGETS) > 0 and class_name not in FILTER_TARGETS:
self.get_logger().info(f"⏭️ 跳过目标 {class_name} (不在发布列表中)")
continue
if res[0].masks is not None and len(res[0].masks) > i:
m = res[0].masks.data[i].cpu().numpy()
m = cv2.resize(m, (w, h))
mask = (m > 0.5).astype(np.uint8)
else:
mask = None
pose_mode, custom_offset = get_pose_mode(class_name)
detections_info.append({
'class_name': class_name,
'confidence': confidence,
'bbox': bbox,
'mask': mask,
'index': i,
'pose_mode': pose_mode,
'custom_offset': custom_offset
})
mode_names = {
"center": "物体中心",
"top_center": "顶面中心",
"bottom_center": "底面中心",
"custom": "自定义位置"
}
mode_name = mode_names.get(pose_mode, pose_mode)
self.get_logger().info(
f"🎯 检测到目标 {i + 1}: {class_name} "
f"置信度: {confidence:.2f} "
f"模式: {mode_name} "
f"位置: ({x1},{y1})-({x2},{y2})"
)
if len(detections_info) == 0:
return
# =================================> 形态学操作 <=================================
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (MORPH_KERNEL_SIZE, MORPH_KERNEL_SIZE))
for det in detections_info:
if det['mask'] is not None:
det['mask'] = cv2.dilate(det['mask'], kernel, iterations=MORPH_DILATE_ITERATIONS)
# =================================> 显示分割结果并发布图像 <=================================
detections_for_display = [
(det['mask'], det['bbox'], det['class_name'], det['confidence'])
for det in detections_info
]
overlay = segment_display_and_publish(
rgb, detections_for_display, self.bridge, self.result_image_pub
)
self.latest_detection_img = overlay
# =================================> 为每个目标生成点云和6D位姿 <=================================
detection_results = []
fx = self.K[0, 0]
fy = self.K[1, 1]
cx = self.K[0, 2]
cy = self.K[1, 2]
for det in detections_info:
class_name = det['class_name']
mask = det['mask']
bbox = det['bbox']
confidence = det['confidence']
pose_mode = det['pose_mode']
custom_offset = det['custom_offset']
if mask is None or np.sum(mask) < MIN_MASK_PIXELS:
self.get_logger().warning(f"⚠️ 目标 {class_name} 掩码像素不足")
continue
# 生成点云
ys, xs = np.where(mask > 0)
points_3d = []
colors = []
for v, u in zip(ys, xs):
z = depth[v, u]
if z <= 0 or z > DEPTH_MAX:
continue
x = (u - cx) * z / fx
y = (v - cy) * z / fy
b, g, r = rgb[v, u]
points_3d.append([x, y, z])
colors.append([r, g, b])
if len(points_3d) == 0:
self.get_logger().warning(f"⚠️ 目标 {class_name} 无有效点云数据")
continue
points_3d = np.array(points_3d, dtype=np.float32)
colors = np.array(colors, dtype=np.uint8)
# =================================> 🔑 孤立点过滤 (在平面分割前调用) <=================================
if ENABLE_OUTLIER_FILTER and len(points_3d) > MIN_POINTS_AFTER_FILTER:
# 统计滤波 - 去除孤立点
points_3d, colors, removed_count, success = filter_outlier_points(
points_3d,
colors,
nb_neighbors=OUTLIER_NB_NEIGHBORS,
std_ratio=OUTLIER_STD_RATIO,
min_points=MIN_POINTS_AFTER_FILTER
)
if len(points_3d) > SAMPLE_MAX_POINTS:
indices = np.random.choice(len(points_3d), SAMPLE_MAX_POINTS, replace=False)
points_3d = points_3d[indices]
colors = colors[indices]
# 如果还想用半径滤波，可以继续调用
# if success and len(points_3d) > MIN_POINTS_AFTER_FILTER:
# points_3d, colors, removed, success = filter_outlier_points_radius(
# points_3d, colors,
# nb_points=16, radius=0.05,
# min_points=MIN_POINTS_AFTER_FILTER
# )
# =================================> 平面分割 <=================================
plane_model, inlier_cloud, outlier_cloud = ranscan(points_3d)
if plane_model is None:
self.get_logger().warning(f"⚠️ 目标 {class_name} 平面分割失败")
continue
# =================================> 保存点云（分割面蓝色 + 实体红色） <=================================
if ENABLE_SAVE_PLY and inlier_cloud is not None and outlier_cloud is not None:
try:
# 获取平面内点（分割面）- 蓝色
inlier_points = np.asarray(inlier_cloud.points)
if len(inlier_points) > 0:
inlier_colors = np.full((len(inlier_points), 3), [0, 0, 255], dtype=np.uint8)
self.save_ply(inlier_points, inlier_colors,
os.path.join(SAVE_PLY_DIR, f"plane_surface_{class_name}.ply"))
self.get_logger().info(
f"💾 分割面点云已保存: plane_surface_{class_name}.ply ({len(inlier_points)} 个点)")
# 获取平面外点（实体）- 红色
outlier_points = np.asarray(outlier_cloud.points)
if len(outlier_points) > 0:
outlier_colors = np.full((len(outlier_points), 3), [255, 0, 0], dtype=np.uint8)
self.save_ply(outlier_points, outlier_colors,
os.path.join(SAVE_PLY_DIR, f"object_body_{class_name}.ply"))
self.get_logger().info(
f"💾 实体点云已保存: object_body_{class_name}.ply ({len(outlier_points)} 个点)")
# 保存合并点云（分割面蓝色 + 实体红色）
if len(inlier_points) > 0 and len(outlier_points) > 0:
all_points = np.vstack([inlier_points, outlier_points])
all_colors = np.vstack([inlier_colors, outlier_colors])
self.save_ply(all_points, all_colors,
os.path.join(SAVE_PLY_DIR, f"combined_{class_name}.ply"))
self.get_logger().info(
f"💾 合并点云已保存: combined_{class_name}.ply ({len(all_points)} 个点)")
except Exception as e:
self.get_logger().error(f"❌ 保存点云失败: {e}")
# =================================> 获取目标尺寸 <=================================
box_length, box_width, box_height, is_cylinder = get_target_info(class_name)
# =================================> 计算6D位姿 <=================================
box_estimator = Box6DPoseEstimator(
box_length=box_length,
box_width=box_width,
box_height=box_height,
is_cylinder=is_cylinder
)
box_estimator.set_pose_mode(pose_mode, custom_offset)
pose_camera_6d, pose_camera_matrix = box_estimator.estimate_6d_pose(plane_model, inlier_cloud)
# 转换到末端坐标系
try:
pose_gripper_6d = self.transform_camera_to_gripper(pose_camera_6d)
except Exception as e:
self.get_logger().error(f"❌ 转换到末端坐标系失败 ({class_name}): {e}")
pose_gripper_6d = pose_camera_6d
# 转换到基坐标系
pose_base_6d = None
try:
if self.end_effector_pose_matrix is not None:
pose_base_6d = self.transform_gripper_to_base(pose_gripper_6d)
else:
self.get_logger().warn(f"⚠️ 未收到末端位姿，无法转换到基坐标系 ({class_name})")
pose_base_6d = pose_gripper_6d
except Exception as e:
self.get_logger().error(f"❌ 转换到基坐标系失败 ({class_name}): {e}")
pose_base_6d = pose_gripper_6d
# 生成盒子模型
box_model = box_estimator.generate_box_model(pose_camera_matrix)
box_model.paint_uniform_color([0, 1, 0])
# =================================> 构造完整实体模型（绿色）并合并保存 <=================================
if pose_base_6d is not None and pose_camera_matrix is not None:
try:
# 获取分割面的中心点
inlier_points = np.asarray(inlier_cloud.points)
surface_center = np.mean(inlier_points, axis=0)
# 🔑 获取法向量（Z轴）- 直接从平面模型提取，不经过任何修改
plane_normal = np.array(plane_model[:3])
plane_normal = plane_normal / (np.linalg.norm(plane_normal) + 1e-8)
# 🔑 对于圆柱体，法向量就是轴线方向
z_axis = plane_normal.copy()
# 获取X轴和Y轴（从位姿矩阵中提取）
x_axis = pose_camera_matrix[:3, 0]
y_axis = pose_camera_matrix[:3, 1]
# 获取尺寸
box_length, box_width, box_height, is_cylinder = get_target_info(class_name)
# 🔑 计算实体中心：分割面中心 + 法向量 * 高度/2
entity_center = surface_center + z_axis * (box_height / 2.0)
self.get_logger().info(
f"📍 分割面中心: ({surface_center[0]:.3f}, {surface_center[1]:.3f}, {surface_center[2]:.3f})")
self.get_logger().info(
f"📍 实体中心: ({entity_center[0]:.3f}, {entity_center[1]:.3f}, {entity_center[2]:.3f})")
self.get_logger().info(f"📐 法向量方向: ({z_axis[0]:.3f}, {z_axis[1]:.3f}, {z_axis[2]:.3f})")
# 构建实体模型的旋转矩阵
rotation_matrix = np.column_stack([x_axis, y_axis, z_axis])
# 根据形状生成实体模型
if is_cylinder:
radius = box_width / 2
entity_points, entity_colors = generate_cylinder_model_from_pose(
entity_center, x_axis, y_axis, z_axis,
radius, box_height, num_points=800
)
self.get_logger().info(f"🟢 生成圆柱体实体模型: {len(entity_points)} 个点")
else:
entity_points, entity_colors = generate_box_model_from_pose(
entity_center, x_axis, y_axis, z_axis,
box_length, box_width, box_height, num_points_per_face=100
)
self.get_logger().info(f"🟢 生成长方体实体模型: {len(entity_points)} 个点")
# =================================> 合并三种点云到同一个PLY文件 <=================================
if ENABLE_SAVE_PLY:
all_points_list = []
all_colors_list = []
# 1. 分割面点云 (蓝色)
if inlier_cloud is not None:
inlier_points = np.asarray(inlier_cloud.points)
if len(inlier_points) > 0:
inlier_colors = np.full((len(inlier_points), 3), [0, 0, 255], dtype=np.uint8)
all_points_list.append(inlier_points)
all_colors_list.append(inlier_colors)
self.get_logger().info(f"🔵 分割面: {len(inlier_points)} 个点 (蓝色)")
# 2. 构造实体点云 (绿色)
if len(entity_points) > 0:
all_points_list.append(entity_points)
all_colors_list.append(entity_colors)
self.get_logger().info(f"🟢 构造实体: {len(entity_points)} 个点 (绿色)")
# 3. 外点点云 (红色)
if outlier_cloud is not None:
outlier_points = np.asarray(outlier_cloud.points)
if len(outlier_points) > 0:
outlier_colors = np.full((len(outlier_points), 3), [255, 0, 0], dtype=np.uint8)
all_points_list.append(outlier_points)
all_colors_list.append(outlier_colors)
self.get_logger().info(f"🔴 外点: {len(outlier_points)} 个点 (红色)")
# 合并所有点云
if len(all_points_list) > 0:
all_points = np.vstack(all_points_list)
all_colors = np.vstack(all_colors_list)
self.save_ply(all_points, all_colors,
os.path.join(SAVE_PLY_DIR, f"combined_all_{class_name}.ply"))
self.get_logger().info(
f"💾 合并点云已保存: combined_all_{class_name}.ply "
f"(共 {len(all_points)} 个点: 蓝色分割面 + 绿色实体 + 红色外点)"
)
except Exception as e:
self.get_logger().error(f"❌ 构造实体模型失败: {e}")
import traceback
traceback.print_exc()
############################################# 构造实体结束 #######################################################
# 保存结果
result = DetectionResult(
class_name=class_name,
confidence=confidence,
bbox=bbox,
mask=mask,
points_3d=points_3d,
colors=colors,
plane_model=plane_model,
inlier_cloud=inlier_cloud,
pose_camera=pose_camera_6d,
pose_gripper=pose_gripper_6d,
pose_base=pose_base_6d,
box_model=box_model,
pose_mode=pose_mode
)
detection_results.append(result)
# =================================> 打印位姿信息 <=================================
mode_names = {
"center": "物体中心",
"top_center": "顶面中心",
"bottom_center": "底面中心",
"custom": "自定义位置"
}
mode_name = mode_names.get(pose_mode, pose_mode)
self.get_logger().info("=" * 70)
self.get_logger().info(f"🎯 {class_name} 6D位姿结果 (模式: {mode_name}):")
# 相机坐标系
if pose_camera_6d is not None:
roll_cam = np.degrees(pose_camera_6d[3])
pitch_cam = np.degrees(pose_camera_6d[4])
yaw_cam = np.degrees(pose_camera_6d[5])
self.get_logger().info(
f" 📷 相机坐标系: "
f"({pose_camera_6d[0] * 1000:7.1f}, {pose_camera_6d[1] * 1000:7.1f}, {pose_camera_6d[2] * 1000:7.1f}) mm, "
f"姿态: ({roll_cam:6.1f}°, {pitch_cam:6.1f}°, {yaw_cam:6.1f}°)"
)
# 末端坐标系
if pose_gripper_6d is not None:
roll_gripper = np.degrees(pose_gripper_6d[3])
pitch_gripper = np.degrees(pose_gripper_6d[4])
yaw_gripper = np.degrees(pose_gripper_6d[5])
self.get_logger().info(
f" 🔧 末端坐标系: "
f"({pose_gripper_6d[0] * 1000:7.1f}, {pose_gripper_6d[1] * 1000:7.1f}, {pose_gripper_6d[2] * 1000:7.1f}) mm, "
f"姿态: ({roll_gripper:6.1f}°, {pitch_gripper:6.1f}°, {yaw_gripper:6.1f}°)"
)
# 基坐标系
if pose_base_6d is not None:
roll_base = np.degrees(pose_base_6d[3])
pitch_base = np.degrees(pose_base_6d[4])
yaw_base = np.degrees(pose_base_6d[5])
self.get_logger().info(
f" 🔵 基坐标系: "
f"({pose_base_6d[0] * 1000:7.1f}, {pose_base_6d[1] * 1000:7.1f}, {pose_base_6d[2] * 1000:7.1f}) mm, "
f"姿态: ({roll_base:6.1f}°, {pitch_base:6.1f}°, {yaw_base:6.1f}°)"
)
self.get_logger().info("=" * 70)
# =================================> 发布最终位姿 <=================================
if pose_base_6d is not None:
self.publish_final_pose(pose_base_6d, class_name, pose_mode)
# =================================> 保存PLY文件 <=================================
if ENABLE_SAVE_PLY and len(detection_results) > 0:
try:
for i, result in enumerate(detection_results):
filename = f"detection_{result.class_name}_{i}.ply"
self.save_ply(result.points_3d, result.colors,
os.path.join(SAVE_PLY_DIR, filename))
if result.inlier_cloud is not None:
inlier_points = np.asarray(result.inlier_cloud.points)
if len(inlier_points) > 0:
inlier_colors = np.full((len(inlier_points), 3), [0, 0, 255], dtype=np.uint8)
self.save_ply(inlier_points, inlier_colors,
os.path.join(SAVE_PLY_DIR, f"plane_inlier_{result.class_name}_{i}.ply"))
except Exception as e:
self.get_logger().error(f"❌ 保存PLY文件失败: {e}")
# 更新缓存
self.latest_detections = detection_results
# =================================> 发布合并点云 <=================================
if PUBLISH_POINTCLOUD and len(detection_results) > 0:
all_points = []
all_colors = []
for result in detection_results:
all_points.extend(result.points_3d)
all_colors.extend(result.colors)
if len(all_points) > 0:
all_points = np.array(all_points)
all_colors = np.array(all_colors)
points_base = self.transform_points_to_base(all_points)
if points_base is not None:
self.publish_pointcloud(points_base, all_colors)
else:
self.publish_pointcloud(all_points, all_colors)
end = time.time()
self.get_logger().info(
f"⏱️ 处理时间: {(end - start) * 1000:.1f} ms, 检测到 {len(detection_results)} 个目标"
)
def generate_pointcloud(self, rgb, depth, mask):
"""生成点云"""
h, w = rgb.shape[:2]
fx = self.K[0, 0]
fy = self.K[1, 1]
cx = self.K[0, 2]
cy = self.K[1, 2]
ys, xs = np.where(mask > 0)
if len(ys) < MIN_MASK_PIXELS:
return None, None
points_3d = []
colors = []
for v, u in zip(ys, xs):
z = depth[v, u]
if z <= 0 or z > DEPTH_MAX:
continue
x = (u - cx) * z / fx
y = (v - cy) * z / fy
b, g, r = rgb[v, u]
points_3d.append([x, y, z])
colors.append([r, g, b])
if len(points_3d) == 0:
return None, None
points_3d = np.array(points_3d, dtype=np.float32)
colors = np.array(colors, dtype=np.uint8)
if len(points_3d) > SAMPLE_MAX_POINTS:
indices = np.random.choice(len(points_3d), SAMPLE_MAX_POINTS, replace=False)
points_3d = points_3d[indices]
colors = colors[indices]
return points_3d, colors
def save_ply(self, points, colors, filename="detection.ply"):
"""保存PLY点云文件"""
if len(points) == 0:
return
try:
os.makedirs(os.path.dirname(os.path.abspath(filename)), exist_ok=True)
points = points.astype(np.float32)
colors = colors.astype(np.uint8)
with open(filename, 'w') as f:
f.write("ply\nformat ascii 1.0\n")
f.write(f"element vertex {len(points)}\n")
f.write("property float x\nproperty float y\nproperty float z\n")
f.write("property uchar red\nproperty uchar green\nproperty uchar blue\n")
f.write("end_header\n")
for i in range(len(points)):
x, y, z = points[i]
r, g, b = colors[i] if colors is not None else (255, 255, 255)
f.write(f"{x:.6f} {y:.6f} {z:.6f} {int(r)} {int(g)} {int(b)}\n")
self.get_logger().info(f"💾 点云已保存: {filename}")
except Exception as e:
self.get_logger().error(f"❌ 保存PLY失败: {filename} - {e}")
def publish_pointcloud(self, points, colors):
"""发布点云"""
if len(points) == 0:
return
try:
cloud_msg = PointCloud2()
cloud_msg.header = Header()
cloud_msg.header.stamp = self.get_clock().now().to_msg()
cloud_msg.header.frame_id = "base_link"
cloud_msg.height = 1
cloud_msg.width = len(points)
cloud_msg.is_bigendian = False
cloud_msg.is_dense = True
cloud_msg.fields = [
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
PointField(name='rgb', offset=12, datatype=PointField.UINT32, count=1),
]
cloud_msg.point_step = 16
cloud_msg.row_step = cloud_msg.point_step * len(points)
data = []
for pt, col in zip(points, colors):
rgb = (int(col[2]) << 16) | (int(col[1]) << 8) | int(col[0])
data.append(struct.pack('ffff', pt[0], pt[1], pt[2], float(rgb)))
cloud_msg.data = b''.join(data)
self.pointcloud_pub.publish(cloud_msg)
self.get_logger().info(f"☁️ 点云已发布: {len(points)} 个点")
except Exception as e:
self.get_logger().error(f"发布点云失败: {e}")
# ============================================================
# Action Server 执行回调
# ============================================================
def execute_callback(self, goal_handle: ServerGoalHandle):
"""执行回调 - 支持多目标返回"""
self.get_logger().info("=" * 60)
self.get_logger().info("🔴 EXECUTE_CALLBACK 被调用")
self.get_logger().info("=" * 60)
goal = goal_handle.request
result = VisionDetection.Result()
try:
feedback = VisionDetection.Feedback()
feedback.task_status = "Processing..."
feedback.progress_rate = 0.0
feedback.current_step_info = "Initializing..."
detections = self.latest_detections
self.get_logger().info(f"📊 缓存状态: {len(detections)} 个目标")
if len(detections) == 0:
self.get_logger().error("❌ 没有可用数据")
result.success = False
result.status_code = 1
result.error_message = "没有可用的检测结果"
goal_handle.abort()
return result
feedback.progress_rate = 0.3
feedback.current_step_info = f"Processing {len(detections)} objects..."
goal_handle.publish_feedback(feedback)
result.success = True
result.status_code = 0
result.error_message = ""
result.execution_time = time.time() - self.last_process_time
result.header = Header()
result.header.stamp = self.get_clock().now().to_msg()
result.header.frame_id = "base_link"
# =================================> 选择目标 <=================================
target_detection = None
if goal.target_obj_name != "":
for det in detections:
if det.class_name == goal.target_obj_name:
target_detection = det
break
if target_detection is None:
self.get_logger().warn(f"⚠️ 未找到目标 '{goal.target_obj_name}'，使用第一个检测结果")
target_detection = detections[0]
else:
target_detection = detections[0]
if target_detection is None:
result.success = False
result.status_code = 2
result.error_message = "没有可用的目标检测结果"
goal_handle.abort()
return result
# =================================> 填充6D位姿 <=================================
if goal.need_6d_pose:
feedback.progress_rate = 0.6
feedback.current_step_info = "Creating pose message..."
goal_handle.publish_feedback(feedback)
result.target_6d_pose = PoseStamped()
result.target_6d_pose.header = Header()
result.target_6d_pose.header.stamp = self.get_clock().now().to_msg()
result.target_6d_pose.header.frame_id = "base_link"
if target_detection.pose_base is not None:
pose_base = target_detection.pose_base
result.target_6d_pose.pose.position.x = float(pose_base[0]) * M_TO_MM
result.target_6d_pose.pose.position.y = float(pose_base[1]) * M_TO_MM
result.target_6d_pose.pose.position.z = float(pose_base[2]) * M_TO_MM
# r = Rotation.from_euler('zyx', [pose_base[3], pose_base[4], pose_base[5]])
# quat = r.as_quat()
quat = euler_zyx_to_quaternion(pose_base[5], pose_base[4], pose_base[3])
result.target_6d_pose.pose.orientation.x = quat[0]
result.target_6d_pose.pose.orientation.y = quat[1]
result.target_6d_pose.pose.orientation.z = quat[2]
result.target_6d_pose.pose.orientation.w = quat[3]
result.pose_confidence = target_detection.confidence
mode_names = {
"center": "物体中心",
"top_center": "顶面中心",
"bottom_center": "底面中心",
"custom": "自定义位置"
}
mode_name = mode_names.get(target_detection.pose_mode, target_detection.pose_mode)
self.get_logger().info(
f" ✅ 使用基坐标系位姿 ({target_detection.class_name}, {mode_name}): "
f"({pose_base[0] * M_TO_MM:.1f}, {pose_base[1] * M_TO_MM:.1f}, {pose_base[2] * M_TO_MM:.1f}) mm"
)
else:
centroid = np.mean(target_detection.points_3d, axis=0)
result.target_6d_pose.pose.position.x = float(centroid[0]) * M_TO_MM
result.target_6d_pose.pose.position.y = float(centroid[1]) * M_TO_MM
result.target_6d_pose.pose.position.z = float(centroid[2]) * M_TO_MM
result.target_6d_pose.pose.orientation.w = 1.0
result.pose_confidence = 0.5
self.get_logger().warn("⚠️ 使用降级方案 (相机坐标系位姿)")
# =================================> 填充点云 <=================================
if goal.need_env_point_cloud:
feedback.progress_rate = 0.8
feedback.current_step_info = "Creating point cloud..."
goal_handle.publish_feedback(feedback)
points_base = self.transform_points_to_base(target_detection.points_3d)
if points_base is not None:
cloud_frame = "base_link"
cloud_points = points_base
else:
cloud_frame = "camera_link"
cloud_points = target_detection.points_3d
self.get_logger().warn("⚠️ 使用相机坐标系点云")
result.env_point_cloud = create_point_cloud(
cloud_points,
target_detection.colors if target_detection.colors is not None
else np.ones((len(cloud_points), 3), dtype=np.uint8) * 255,
cloud_frame,
self.get_clock()
)
result.point_cloud_frame_id = cloud_frame
self.get_logger().info(f"☁️ 点云已创建，点数: {len(cloud_points)}, 坐标系: {cloud_frame}")
# =================================> 附加信息 <=================================
result.detected_objects = [det.class_name for det in detections]
result.num_objects = len(detections)
self.get_logger().info(f"📦 检测到的目标: {result.detected_objects}")
feedback.progress_rate = 1.0
feedback.task_status = "Completed"
feedback.current_step_info = "Done"
goal_handle.publish_feedback(feedback)
self.get_logger().info("✅ 返回结果")
goal_handle.succeed()
return result
except Exception as e:
self.get_logger().error(f"❌ execute_callback 异常: {e}")
import traceback
traceback.print_exc()
result.success = False
result.status_code = 3
result.error_message = str(e)
goal_handle.abort()
return result
def __del__(self):
self.stop_visualization()
cv2.destroyAllWindows()
# ==========================
# 主函数
# ==========================
def main(args=None):
rclpy.init(args=args)
node = CameraDetectionNode()
executor = rclpy.executors.SingleThreadedExecutor()
executor.add_node(node)
try:
executor.spin()
except KeyboardInterrupt:
node.get_logger().info("接收到退出信号")
finally:
node.stop_visualization()
cv2.destroyAllWindows()
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()
2.代码2如下：
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CameraInfo, PointCloud2, PointField
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
from cv_bridge import CvBridge
from std_msgs.msg import Header
import cv2
import numpy as np
import struct
import time
from ultralytics import YOLO
import open3d as o3d
from scipy.spatial import ConvexHull
from scipy.spatial.transform import Rotation
#from vision_detection_action.action import VisionDetection
from com_interfaces.action import VisionDetection
from rclpy.action import ActionServer, GoalResponse, CancelResponse
from rclpy.action.server import ServerGoalHandle
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.qos import QoSProfile
import threading
import copy
import os
import itertools
import math
from typing import Tuple
# 屏蔽无关警告
os.environ["QT_LOGGING_RULES"] = "qt.fonts.warning=false"
os.environ["OPENCV_LOG_LEVEL"] = "FATAL"
os.environ["CV_LOG_LEVEL"] = "FATAL"
# ============================================================================
# 📋 可调参数配置区域 - 请根据实际需求修改
# ============================================================================
# -------------------- YOLO模型配置 --------------------
YOLO_MODEL_PATH = "/home/wyq/ros2_ws/weights/best_much.pt" # YOLO模型权重文件路径
YOLO_CONFIDENCE_THRESHOLD = 0.3 # YOLO检测置信度阈值 (0.0-1.0)，越高误检越少
# -------------------- 目标物体尺寸配置 (支持多目标) --------------------
# 每个目标物体定义: {类名: (长度, 宽度, 高度)}
# 注意: 类名必须与YOLO模型中的类别名称一致
TARGET_DIMENSIONS = {
"lefta": (0.091, 0.06, 0.06), # (长度, 宽度, 高度) 单位: 米
"leftb": (0.115, 0.06, 0.06),
"leftc": (0.18, 0.06, 0.06),
"leftd": (0.10, 0.10, 0.15), ####大小不确定
"righta": (0.092, 0.071, 0.02),
"rightb": (0.13, 0.12, 0.05),
"rightc": (0.15, 0.10, 0.056),
"rightd": (0.15, 0.15, 0.052),
}
# ==========================
# 🎯 位姿计算模式配置
# ==========================
# 为每个目标指定位置计算模式:
# "center": 计算物体中心点位置 (默认) - 顶面中心 + 高度/2
# "top_center": 计算顶面中心点位置 - 直接使用检测到的顶面中心
# "bottom_center": 计算底面中心点位置 - 顶面中心 - 高度/2
# "custom": 自定义位置 (需要配合自定义偏移量)
#
# 格式: {类名: 模式}
# POSE_MODE_CONFIG = {
# "lefta": "center", # 使用物体中心 (默认)
# "leftb": "center",
# "leftc": "center",
# "leftd": "center",
# "righta": "top_center", # 使用顶面中心
# "rightb": "top_center",
# "rightc": "top_center",
# "rightd": "top_center",
# # 可以添加更多配置
# # "target4": "bottom_center",
# }
POSE_MODE_CONFIG = {
"lefta": "top_center", # 使用物体中心 (默认)
"leftb": "top_center",
"leftc": "top_center",
"leftd": "top_center",
"righta": "top_center", # 使用顶面中心
"rightb": "top_center",
"rightc": "top_center",
"rightd": "top_center",
# 可以添加更多配置
# "target4": "bottom_center",
}
# -------------------- 目标过滤配置 --------------------
# 只发布这些类型的目标（空列表表示发布所有目标）
FILTER_TARGETS = ["lefta","leftb","leftc","leftd","righta", "rightb", "rightc", "rightd"] # 只发布 righta 和 rightb
# FILTER_TARGETS = [] # 发布所有目标
# 自定义偏移量配置 (仅当模式为 "custom" 时生效)
# 格式: {类名: (offset_x, offset_y, offset_z)} 单位: 米
CUSTOM_OFFSET_CONFIG = {
# "target1": (0.0, 0.0, 0.05), # 示例: 在Z轴方向偏移5cm
}
# 默认位姿计算模式 (当目标未在POSE_MODE_CONFIG中配置时使用)
DEFAULT_POSE_MODE = "center" # 可选: "center", "top_center", "bottom_center", "custom"
# 默认尺寸 (当检测到的类别未在TARGET_DIMENSIONS中定义时使用)
DEFAULT_TARGET_DIMENSIONS = (0.15, 0.056, 0.10) # (长度, 宽度, 高度)
# -------------------- 相机话题配置 --------------------
COLOR_TOPIC = "/camera/color/image_raw" # 彩色图像话题名称
DEPTH_TOPIC = "/camera/depth/image_raw" # 深度图像话题名称
CAMERA_INFO_TOPIC = "/camera/color/camera_info" # 相机内参话题名称
END_EFFECTOR_POSE_TOPIC = "/end_effector_pose" # 机械臂末端位姿话题名称
# -------------------- 发布话题配置 --------------------
POINTCLOUD_TOPIC = "/detection/pointcloud" # 点云发布话题（基坐标系）
RESULT_IMAGE_TOPIC = "/detection/result_image" # 检测结果图像发布话题
FINAL_POSE_TOPIC = "/detection/final_pose" # 最终位姿发布话题（基坐标系）
ACTION_NAME = "/vision/detection_pose_cloud" # Action Server名称
# -------------------- 处理间隔配置 --------------------
PROCESS_INTERVAL = 1.0 # 处理间隔(秒)，控制CPU负载，0.5表示每秒处理2次
# -------------------- 深度图配置 --------------------
DEPTH_MIN = 0.6 # 深度最小值(米)，小于此值将被忽略
DEPTH_MAX = 3.0 # 深度最大值(米)，大于此值将被忽略
DEPTH_SCALE = 1000.0 # 深度图缩放因子，将mm转换为m (16UC1格式通常为1000)
# -------------------- 点云配置 --------------------
VOXEL_SIZE = 0.005 # 体素滤波大小(米)，用于降采样点云，增大可减少点数
MIN_MASK_PIXELS = 100 # 掩码最小像素数，小于此值认为检测无效
# -------------------- 平面分割配置 (RANSAC) --------------------
RANSAC_DISTANCE_THRESHOLD = 0.0035 # RANSAC平面分割距离阈值(米)，越小要求越严格
RANSAC_N = 3 # RANSAC每次采样点数 (3点确定一个平面)
RANSAC_ITERATIONS = 500 # RANSAC迭代次数，越大精度越高但速度越慢
# -------------------- 可视化配置 --------------------
ENABLE_DISPLAY_WINDOW = False # 是否显示OpenCV可视化窗口
ENABLE_3D_VISUALIZATION = False # 🔥 改为True启用3D可视化
WINDOW_WIDTH = 1200 # 增大窗口宽度
WINDOW_HEIGHT = 800 # 增大窗口高度
# -------------------- 点云保存配置 --------------------
ENABLE_SAVE_PLY = True # 是否保存PLY点云文件
SAVE_PLY_DIR = "./pointclouds" # PLY文件保存目录 (当前目录为".")
# -------------------- 点云发布配置 --------------------
PUBLISH_POINTCLOUD = False # 是否发布点云到ROS话题
# -------------------- 形态学操作配置 --------------------
MORPH_KERNEL_SIZE = 5 # 形态学操作核大小 (奇数)
MORPH_DILATE_ITERATIONS = 2 # 膨胀迭代次数，越大掩码扩展越多
# -------------------- 降采样配置 --------------------
SAMPLE_MAX_POINTS = 50000 # 最大点云点数，超过此值将随机降采样
# ============================================================================
# 🔑 手眼标定外参配置 (从标定程序获取)
# ============================================================================
# 这是通过手眼标定程序得到的结果：相机→机械臂末端 (T_cam2gripper)
# 旋转矩阵 R_cam2gripper (3x3)
# HAND_EYE_ROTATION = np.array([
# [0.03790313, -0.05364665, 0.99784036],
# [-0.99896454, -0.02717939, 0.03648460],
# [0.02516342, -0.99819002, -0.05462128]
# ])
HAND_EYE_ROTATION = np.array([
[0.00271188, -0.01911421, 0.99981363,],
[-0.99982551, -0.01853091, 0.00235764],
[0.0184824, -0.99964556, -0.01916113]
])
# 平移向量 t_cam2gripper
# HAND_EYE_TRANSLATION = np.array([0.03406303, 0.02799409, 0.05059718]).reshape(3, 1)
HAND_EYE_TRANSLATION = np.array([0.00730634, 0.00320815, 0.00048779]).reshape(3, 1)
# 自动构建4x4齐次变换矩阵 T_cam2gripper
HAND_EYE_MATRIX = np.eye(4)
HAND_EYE_MATRIX[:3, :3] = HAND_EYE_ROTATION
HAND_EYE_MATRIX[:3, 3:4] = HAND_EYE_TRANSLATION
# 是否启用位姿转换（如果为False，直接输出相机坐标系下的位姿）
ENABLE_HAND_EYE_TRANSFORM = True
# 将位姿由m转换为mm
M_TO_MM = 1.0
# ==========================
# 全局QoS配置
# ==========================
QOS_PROFILE = QoSProfile(depth=10)
# ==========================
# 目标形状分类配置
# ==========================
# 圆柱体类别（饮料瓶/易拉罐）- 可能是直立或放倒
#CYLINDER_CLASSES = ["lefta", "leftb", "leftc", "leftd"]
CYLINDER_CLASSES = []
# 长方体类别
BOX_CLASSES = ["lefta", "leftb", "leftc", "leftd","righta", "rightb", "rightc", "rightd"]
# -------------------- 孤立点过滤配置 --------------------
ENABLE_OUTLIER_FILTER = True # 是否启用孤立点过滤
OUTLIER_NB_NEIGHBORS = 20 # 最近邻数量
OUTLIER_STD_RATIO = 2.0 # 标准差倍数（越大保留越多点）
MIN_POINTS_AFTER_FILTER = 50 # 滤波后最少点数，少于该值则使用原始点云
PI = 3.1415926
# ============================================================================
# 代码开始
# ============================================================================
def euler_zyx_to_quaternion(
roll: float, pitch: float, yaw: float, degrees: bool = False
) -> np.ndarray:
"""
将欧拉角 (ZYX 顺序) 转换为四元数。
Parameters
----------
roll : float
滚转角 (绕 X 轴), 弧度或角度
pitch : float
俯仰角 (绕 Y 轴), 弧度或角度
yaw : float
偏航角 (绕 Z 轴), 弧度或角度
degrees : bool, optional
如果为 True, 则 roll/pitch/yaw 的单位为度; 默认为 False (弧度)
Returns
-------
np.ndarray
四元数 [w, x, y, z]
Examples
--------
>>> # 绕 Z 轴旋转 90° (纯偏航)
>>> q = euler_zyx_to_quaternion(0, 0, 90, degrees=True)
>>> print(q) # [0.7071, 0, 0, 0.7071]
>>> # 绕 X 轴旋转 180° (纯滚转)
>>> q = euler_zyx_to_quaternion(180, 0, 0, degrees=True)
>>> print(q) # [0, 1, 0, 0]
"""
if degrees:
roll = math.radians(roll)
pitch = math.radians(pitch)
yaw = math.radians(yaw)
if yaw > -PI/2:
yaw = PI/2 - yaw
else:
yaw = PI/2 - yaw - PI
# 半角
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
# ZYX 顺序: q = q_z(yaw) * q_y(pitch) * q_x(roll)
w = cr * cp * cy + sr * sp * sy
x = sr * cp * cy - cr * sp * sy
y = cr * sp * cy + sr * cp * sy
z = cr * cp * sy - sr * sp * cy
return np.array([x, y, z, w])
def quaternion_to_euler_zyx(
q: np.ndarray, degrees: bool = False
):
"""
将四元数转换回欧拉角 (ZYX 顺序)。
Parameters
----------
q : np.ndarray
四元数 [w, x, y, z]
degrees : bool, optional
如果为 True, 返回角度制; 默认为 False (弧度)
Returns
-------
Tuple[float, float, float]
(roll, pitch, yaw)
Notes
-----
当 pitch 接近 ±90° 时存在万向锁, roll 和 yaw 可能不唯一。
"""
w, x, y, z = q
# 计算 pitch
sin_pitch = 2.0 * (w * y - z * x)
sin_pitch = np.clip(sin_pitch, -1.0, 1.0)
pitch = math.asin(sin_pitch)
# 计算 roll
sin_roll = 2.0 * (w * x + y * z)
cos_roll = 1.0 - 2.0 * (x * x + y * y)
roll = math.atan2(sin_roll, cos_roll)
# 计算 yaw
sin_yaw = 2.0 * (w * z + x * y)
cos_yaw = 1.0 - 2.0 * (y * y + z * z)
yaw = math.atan2(sin_yaw, cos_yaw)
if degrees:
roll = math.degrees(roll)
pitch = math.degrees(pitch)
yaw = math.degrees(yaw)
return roll, pitch, yaw
def filter_outlier_points(points_3d, colors=None,
nb_neighbors=20, std_ratio=2.0,
min_points=50):
"""
对点云进行统计滤波，去除孤立点
参数:
points_3d: numpy.ndarray, 形状为 (N, 3), 3D点云坐标
colors: numpy.ndarray, 形状为 (N, 3), 点云颜色 (可选)
nb_neighbors: int, 统计滤波的最近邻数量
std_ratio: float, 标准差倍数阈值
min_points: int, 滤波后最少点数，少于该值则返回原始点云
返回:
filtered_points: numpy.ndarray, 滤波后的点云坐标
filtered_colors: numpy.ndarray, 滤波后的点云颜色 (如果输入了colors)
removed_count: int, 被移除的点数
success: bool, 是否成功滤波
"""
if len(points_3d) < min_points:
return points_3d, colors, 0, False
try:
# 转换为Open3D点云
o3d_pcd = o3d.geometry.PointCloud()
o3d_pcd.points = o3d.utility.Vector3dVector(points_3d)
if colors is not None and len(colors) > 0:
o3d_pcd.colors = o3d.utility.Vector3dVector(colors / 255.0)
original_count = len(points_3d)
# 统计滤波 - 去除孤立点
o3d_pcd_filtered, ind = o3d_pcd.remove_statistical_outlier(
nb_neighbors=nb_neighbors,
std_ratio=std_ratio
)
filtered_count = len(o3d_pcd_filtered.points)
removed_count = original_count - filtered_count
# 如果滤波后点云太少，返回原始点云
if filtered_count < min_points:
print(f"⚠️ 滤波后点云过少 ({filtered_count} < {min_points})，使用原始点云")
return points_3d, colors, 0, False
# 使用滤波后的点云
filtered_points = np.asarray(o3d_pcd_filtered.points)
filtered_colors = None
if len(o3d_pcd_filtered.colors) > 0:
filtered_colors = (np.asarray(o3d_pcd_filtered.colors) * 255).astype(np.uint8)
print(f"📊 孤立点过滤: {original_count} → {filtered_count} 个点 "
f"(移除 {removed_count} 个, {removed_count / original_count * 100:.1f}%)")
return filtered_points, filtered_colors, removed_count, True
except Exception as e:
print(f"⚠️ 孤立点过滤失败: {e}，使用原始点云")
return points_3d, colors, 0, False
def filter_outlier_points_radius(points_3d, colors=None,
nb_points=16, radius=0.05,
min_points=50):
"""
对点云进行半径滤波，去除周围点太少的点
参数:
points_3d: numpy.ndarray, 形状为 (N, 3), 3D点云坐标
colors: numpy.ndarray, 形状为 (N, 3), 点云颜色 (可选)
nb_points: int, 半径内最少点数
radius: float, 半径大小 (米)
min_points: int, 滤波后最少点数
返回:
filtered_points, filtered_colors, removed_count, success
"""
if len(points_3d) < min_points:
return points_3d, colors, 0, False
try:
o3d_pcd = o3d.geometry.PointCloud()
o3d_pcd.points = o3d.utility.Vector3dVector(points_3d)
if colors is not None and len(colors) > 0:
o3d_pcd.colors = o3d.utility.Vector3dVector(colors / 255.0)
original_count = len(points_3d)
# 半径滤波
o3d_pcd_filtered, ind = o3d_pcd.remove_radius_outlier(
nb_points=nb_points,
radius=radius
)
filtered_count = len(o3d_pcd_filtered.points)
removed_count = original_count - filtered_count
if filtered_count < min_points:
print(f"⚠️ 半径滤波后点云过少 ({filtered_count} < {min_points})，使用原始点云")
return points_3d, colors, 0, False
filtered_points = np.asarray(o3d_pcd_filtered.points)
filtered_colors = None
if len(o3d_pcd_filtered.colors) > 0:
filtered_colors = (np.asarray(o3d_pcd_filtered.colors) * 255).astype(np.uint8)
print(f"📊 半径滤波: {original_count} → {filtered_count} 个点 "
f"(移除 {removed_count} 个, {removed_count / original_count * 100:.1f}%)")
return filtered_points, filtered_colors, removed_count, True
except Exception as e:
print(f"⚠️ 半径滤波失败: {e}，使用原始点云")
return points_3d, colors, 0, False
def get_target_info(class_name):
"""
根据类别名称获取目标物体的尺寸和形状类型
参数:
class_name: 目标类别名称
返回:
(length, width, height, is_cylinder): 尺寸和是否为圆柱体
"""
if class_name in TARGET_DIMENSIONS:
length, width, height = TARGET_DIMENSIONS[class_name]
else:
print(f"⚠️ 警告: 未找到类别 '{class_name}' 的尺寸配置，使用默认尺寸 {DEFAULT_TARGET_DIMENSIONS}")
length, width, height = DEFAULT_TARGET_DIMENSIONS
is_cylinder = class_name in CYLINDER_CLASSES
return length, width, height, is_cylinder
def detect_cylinder_orientation(points_2d):
"""
检测圆柱体的姿态：直立（看到顶面圆形）还是放倒（看到侧面矩形）
返回:
'upright': 直立（顶面朝上，看到圆形）
'lying': 放倒（侧面朝上，看到矩形）
'unknown': 未知
"""
if len(points_2d) < 10:
return 'unknown'
try:
# 计算2D点云的形状特征
hull = ConvexHull(points_2d)
area = hull.volume
perimeter = 0
for i in range(len(hull.vertices)):
j = (i + 1) % len(hull.vertices)
p1 = points_2d[hull.vertices[i]]
p2 = points_2d[hull.vertices[j]]
perimeter += np.linalg.norm(p2 - p1)
if perimeter < 1e-6:
return 'unknown'
# 圆形度 = 4π * 面积 / 周长^2
circularity = 4 * np.pi * area / (perimeter * perimeter)
# 计算长宽比
cov = np.cov(points_2d.T)
eigenvalues, _ = np.linalg.eigh(cov)
aspect_ratio = np.sqrt(eigenvalues.max() / (eigenvalues.min() + 1e-6))
print(f"📊 形状特征: 圆形度={circularity:.3f}, 长宽比={aspect_ratio:.2f}")
# 判断逻辑
if circularity > 0.7 and aspect_ratio < 1.5:
return 'upright'
elif circularity < 0.5 and aspect_ratio > 2.0:
return 'lying'
else:
return 'unknown'
except Exception as e:
print(f"⚠️ 圆柱体姿态检测失败: {e}")
return 'unknown'
# ==========================
# 辅助函数：角度规范化
# ==========================
def normalize_angle_deg(angle_deg):
"""
将角度规范化到 [0, 90] 范围
输入: angle_deg - 角度值(度)
输出: 规范化的角度
功能: 如果角度在[90,180]之间，取补角；如果>45度，取90-角度
"""
angle_deg = abs(angle_deg) % 180
if angle_deg > 90:
angle_deg = 180 - angle_deg
if angle_deg > 45:
angle_deg = 90 - angle_deg
return angle_deg
def normalize_angle_rad(angle_rad):
"""
将角度规范化到 [0, pi/2] 范围
如果角度在 [pi/2, pi] 之间，取补角
"""
angle_rad = angle_rad % np.pi
if angle_rad > np.pi / 2:
angle_rad = np.pi - angle_rad
return angle_rad
def get_target_dimensions(class_name):
"""
根据类别名称获取目标物体的尺寸
参数:
class_name: 目标类别名称 (如 "target1", "target2")
返回:
(length, width, height): 元组，单位: 米
"""
if class_name in TARGET_DIMENSIONS:
return TARGET_DIMENSIONS[class_name]
else:
print(f"⚠️ 警告: 未找到类别 '{class_name}' 的尺寸配置，使用默认尺寸 {DEFAULT_TARGET_DIMENSIONS}")
return DEFAULT_TARGET_DIMENSIONS
def get_pose_mode(class_name):
"""
根据类别名称获取位姿计算模式
参数:
class_name: 目标类别名称
返回:
mode: 字符串，位姿计算模式
offset: 元组，自定义偏移量 (仅当模式为 "custom" 时使用)
"""
mode = POSE_MODE_CONFIG.get(class_name, DEFAULT_POSE_MODE)
offset = CUSTOM_OFFSET_CONFIG.get(class_name, (0.0, 0.0, 0.0))
return mode, offset
# ==========================
# 分割图片的显示和发布 (支持多目标)
# ==========================
def segment_display_and_publish(rgb, detections, bridge, publisher):
"""
显示分割结果并发布到ROS话题 (支持多目标)
"""
overlay = None
try:
if rgb is None or len(rgb.shape) != 3:
return None
if rgb.dtype != np.uint8:
rgb = np.clip(rgb, 0, 255).astype(np.uint8)
# 创建基础图像
overlay = rgb.copy()
# 创建一个单独的掩码叠加层
mask_overlay = np.zeros_like(rgb, dtype=np.float32)
# 为不同目标分配不同颜色
colors = [
(0, 255, 0), # 绿色
(255, 0, 0), # 蓝色
(0, 0, 255), # 红色
(255, 255, 0), # 青色
(255, 0, 255), # 品红
(0, 255, 255), # 黄色
(128, 128, 0), # 橄榄
(128, 0, 128), # 紫色
]
# 绘制每个检测目标
for idx, (mask, bbox, class_name, confidence) in enumerate(detections):
color = colors[idx % len(colors)]
# 绘制掩码（累积到 mask_overlay）
if mask is not None and np.sum(mask) > 0:
if mask.dtype != np.uint8:
mask = mask.astype(np.uint8)
if mask.max() <= 1:
mask = mask * 255
# 创建彩色掩码
mask_colored = np.zeros_like(rgb, dtype=np.float32)
mask_colored[:, :, 0] = mask * (color[2] / 255.0) # B通道
mask_colored[:, :, 1] = mask * (color[1] / 255.0) # G通道
mask_colored[:, :, 2] = mask * (color[0] / 255.0) # R通道
# 累积到 mask_overlay（取最大值，避免覆盖）
mask_overlay = np.maximum(mask_overlay, mask_colored)
# 绘制边界框
if bbox is not None:
x1, y1, x2, y2 = bbox
cv2.rectangle(overlay, (x1, y1), (x2, y2), color, 2)
label = f"{class_name} {confidence:.2f}"
cv2.putText(overlay, label, (x1, y1 - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2)
# 添加编号
cv2.putText(overlay, f"#{idx + 1}", (x1, y2 + 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 2)
# 将累积的掩码叠加到图像上
if np.max(mask_overlay) > 0:
overlay = cv2.addWeighted(overlay, 0.7, mask_overlay.astype(np.uint8), 0.3, 0)
# 添加信息
h, w = overlay.shape[:2]
cv2.putText(overlay, f"YOLO Detection | {len(detections)} objects | {w}x{h}", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
# 保存到本地
cv2.imwrite("./result.png", overlay)
# 显示窗口
if ENABLE_DISPLAY_WINDOW:
cv2.namedWindow("Detection Result", cv2.WINDOW_NORMAL)
cv2.resizeWindow("Detection Result", WINDOW_WIDTH, WINDOW_HEIGHT)
cv2.imshow("Detection Result", overlay)
cv2.waitKey(0)
if key == ord('q') or key == 27: # q 或 ESC 退出
cv2.destroyAllWindows()
return overlay
# 发布检测结果图像到ROS话题
if publisher is not None and publisher.get_subscription_count() > 0:
try:
result_msg = bridge.cv2_to_imgmsg(overlay, "bgr8")
result_msg.header.stamp = bridge.get_clock().now().to_msg() if hasattr(bridge, 'get_clock') else None
result_msg.header.frame_id = "camera_link"
publisher.publish(result_msg)
print(f"📤 已发布检测结果图像 ({len(detections)} 个目标)")
except Exception as e:
print(f"发布检测结果图像失败: {e}")
except Exception as e:
print(f"segment_display错误: {e}")
return overlay
# ==========================
# 法向量计算
# ==========================
def calculate_plane_normal(plane_model):
[a, b, c, d] = plane_model
normal = np.array([a, b, c])
norm = np.linalg.norm(normal)
unit_normal = normal / norm
return normal, unit_normal
# ==========================
# RANSAC平面分割
# ==========================
def ranscan(pcd):
if isinstance(pcd, np.ndarray):
o3d_pcd = o3d.geometry.PointCloud()
o3d_pcd.points = o3d.utility.Vector3dVector(pcd)
print(f"📊 已将NumPy数组 ({pcd.shape}) 转换为Open3D点云")
elif isinstance(pcd, o3d.geometry.PointCloud):
o3d_pcd = pcd
else:
raise TypeError(f"不支持的类型: {type(pcd)}")
if len(o3d_pcd.points) == 0:
print("❌ 点云为空")
return None, None, None
plane_model, inliers = o3d_pcd.segment_plane(
RANSAC_DISTANCE_THRESHOLD, RANSAC_N, RANSAC_ITERATIONS
)
[a, b, c, d] = plane_model
print(f"📐 Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")
print(f"✅ 找到 {len(inliers)} 个内点 (占比 {len(inliers) / len(o3d_pcd.points) * 100:.1f}%)")
inlier_cloud = o3d_pcd.select_by_index(inliers)
inlier_cloud.paint_uniform_color([0, 0, 1.0])
outlier_cloud = o3d_pcd.select_by_index(inliers, invert=True)
outlier_cloud.paint_uniform_color([1.0, 0, 0])
return plane_model, inlier_cloud, outlier_cloud
# ==========================
# 提取边长方向
# ==========================
def extract_box_axes_from_edges(points, z_axis, box_length, box_width, box_height,
is_cylinder=False, cylinder_orientation='unknown'):
"""
从点云边缘提取盒子的X轴和Y轴方向向量
支持圆柱体（直立/放倒）和长方体
对于长方体：根据分割面的实际尺寸与真实尺寸匹配，确定X/Y轴方向
"""
if len(points) < 10:
return None, None
# 去中心化并投影到平面
centroid = np.mean(points, axis=0)
points_centered = points - centroid
points_proj = points_centered - np.outer(np.dot(points_centered, z_axis), z_axis)
# 建立局部2D坐标系
x_temp = np.array([1, 0, 0])
if abs(np.dot(x_temp, z_axis)) > 0.9:
x_temp = np.array([0, 1, 0])
x_temp = x_temp - np.dot(x_temp, z_axis) * z_axis
x_temp = x_temp / (np.linalg.norm(x_temp) + 1e-8)
y_temp = np.cross(z_axis, x_temp)
y_temp = y_temp / (np.linalg.norm(y_temp) + 1e-8)
# 将3D点云投影到2D平面
points_2d = np.zeros((len(points_proj), 2))
for i, p in enumerate(points_proj):
points_2d[i, 0] = np.dot(p, x_temp)
points_2d[i, 1] = np.dot(p, y_temp)
# 计算2D点云的凸包
hull = ConvexHull(points_2d)
hull_points = points_2d[hull.vertices]
# 提取凸包的边方向和长度
edge_dirs = []
edge_lengths = []
for i in range(len(hull_points)):
j = (i + 1) % len(hull_points)
edge_vec = hull_points[j] - hull_points[i]
edge_len = np.linalg.norm(edge_vec)
if edge_len > 0.001:
edge_dirs.append(edge_vec / edge_len)
edge_lengths.append(edge_len)
# ========== 根据形状类型选择不同的处理策略 ==========
if is_cylinder and cylinder_orientation == 'upright':
# 圆柱体直立（看到顶面圆形）- 使用PCA
cov = np.cov(points_2d.T)
eigenvalues, eigenvectors = np.linalg.eigh(cov)
idx = np.argsort(eigenvalues)[::-1]
x_axis_2d = eigenvectors[:, idx[0]]
if x_axis_2d[0] < 0:
x_axis_2d = -x_axis_2d
y_axis_2d = np.array([-x_axis_2d[1], x_axis_2d[0]])
elif is_cylinder and cylinder_orientation == 'lying':
# 圆柱体放倒（看到侧面矩形）
if len(edge_dirs) >= 2:
edge_dirs = np.array(edge_dirs)
edge_lengths = np.array(edge_lengths)
sorted_idx = np.argsort(edge_lengths)[::-1]
sorted_dirs = edge_dirs[sorted_idx]
dir_long = sorted_dirs[0]
dir_short = sorted_dirs[1]
dir_short = dir_short - np.dot(dir_short, dir_long) * dir_long
dir_short = dir_short / (np.linalg.norm(dir_short) + 1e-8)
# 长边方向对应圆柱轴线方向
x_axis_2d = dir_long
y_axis_2d = dir_short
else:
# 降级方案：使用PCA
cov = np.cov(points_2d.T)
eigenvalues, eigenvectors = np.linalg.eigh(cov)
idx = np.argsort(eigenvalues)[::-1]
x_axis_2d = eigenvectors[:, idx[0]]
y_axis_2d = eigenvectors[:, idx[1]]
else:
# ========== 长方体：根据实际尺寸匹配 ==========
if len(edge_dirs) >= 2:
edge_dirs = np.array(edge_dirs)
edge_lengths = np.array(edge_lengths)
sorted_idx = np.argsort(edge_lengths)[::-1]
sorted_dirs = edge_dirs[sorted_idx]
sorted_lengths = edge_lengths[sorted_idx]
# 取前两条边（最长和次长）
edge1_len = sorted_lengths[0]
edge2_len = sorted_lengths[1] if len(sorted_lengths) > 1 else edge1_len * 0.5
dir1 = sorted_dirs[0]
dir2 = sorted_dirs[1] if len(sorted_dirs) > 1 else np.array([-dir1[1], dir1[0]])
# 正交化方向2
dir2 = dir2 - np.dot(dir2, dir1) * dir1
dir2 = dir2 / (np.linalg.norm(dir2) + 1e-8)
# 🔑 关键：根据真实尺寸匹配
# 可能的尺寸组合：[长, 宽], [长, 高], [宽, 高]
dimensions = [
(box_length, box_width, 'length', 'width'),
(box_length, box_height, 'length', 'height'),
(box_width, box_height, 'width', 'height'),
]
# 计算检测到的边长比
detected_ratio = max(edge1_len, edge2_len) / (min(edge1_len, edge2_len) + 1e-8)
best_match = None
best_score = float('inf')
for dim1, dim2, name1, name2 in dimensions:
# 计算真实尺寸比
real_ratio = max(dim1, dim2) / (min(dim1, dim2) + 1e-8)
# 计算匹配分数（比值越接近越好）
score = abs(detected_ratio - real_ratio) / (real_ratio + 1e-8)
if score < best_score:
best_score = score
best_match = (dim1, dim2, name1, name2)
# 使用最佳匹配确定X/Y轴
if best_match is not None:
dim1, dim2, name1, name2 = best_match
# 如果检测到的最长边对应dim1
if edge1_len >= edge2_len:
x_axis_2d = dir1
y_axis_2d = dir2
else:
x_axis_2d = dir2
y_axis_2d = dir1
print(f"📐 尺寸匹配: 检测边长 ({edge1_len:.3f}, {edge2_len:.3f}) → "
f"真实尺寸 ({dim1:.3f}, {dim2:.3f}) [{name1}, {name2}]")
else:
# 降级方案：按原始逻辑
if box_length >= box_width:
x_axis_2d = dir1
y_axis_2d = dir2
else:
x_axis_2d = dir2
y_axis_2d = dir1
else:
# 降级方案：使用PCA
cov = np.cov(points_2d.T)
eigenvalues, eigenvectors = np.linalg.eigh(cov)
idx = np.argsort(eigenvalues)[::-1]
x_axis_2d = eigenvectors[:, idx[0]]
y_axis_2d = eigenvectors[:, idx[1]]
# 将2D方向向量映射回3D空间
x_axis = x_axis_2d[0] * x_temp + x_axis_2d[1] * y_temp
y_axis = y_axis_2d[0] * x_temp + y_axis_2d[1] * y_temp
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
# 确保Y轴垂直于Z轴
y_axis = y_axis - np.dot(y_axis, z_axis) * z_axis
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
x_axis = np.cross(y_axis, z_axis)
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
return x_axis, y_axis
def generate_box_model_from_pose(center, x_axis, y_axis, z_axis, length, width, height,
num_points_per_face=100):
"""
根据位姿和尺寸生成完整的盒子模型点云（绿色实体）
参数:
center: 盒子中心点 (3D坐标)
x_axis: X轴方向向量 (归一化)
y_axis: Y轴方向向量 (归一化)
z_axis: Z轴方向向量 (归一化)
length: 盒子长度 (沿X轴)
width: 盒子宽度 (沿Y轴)
height: 盒子高度 (沿Z轴)
num_points_per_face: 每个面的采样点数
返回:
box_points: 盒子表面点云 (N, 3)
box_colors: 盒子颜色 (N, 3) - 绿色
"""
# 定义盒子的8个顶点 (中心在原点)
l2, w2, h2 = length / 2, width / 2, height / 2
vertices_local = np.array([
[-l2, -w2, -h2],
[l2, -w2, -h2],
[l2, w2, -h2],
[-l2, w2, -h2],
[-l2, -w2, h2],
[l2, -w2, h2],
[l2, w2, h2],
[-l2, w2, h2]
])
# 将顶点转换到世界坐标系
rotation_matrix = np.column_stack([x_axis, y_axis, z_axis])
vertices_world = (rotation_matrix @ vertices_local.T).T + center
# 定义6个面的顶点索引
faces = [
[0, 1, 2, 3], # 底面
[4, 5, 6, 7], # 顶面
[0, 1, 5, 4], # 后面
[2, 3, 7, 6], # 前面
[0, 3, 7, 4], # 左面
[1, 2, 6, 5] # 右面
]
points = []
# 对每个面进行网格采样
for face_indices in faces:
v0 = vertices_world[face_indices[0]]
v1 = vertices_world[face_indices[1]]
v2 = vertices_world[face_indices[2]]
v3 = vertices_world[face_indices[3]]
n = int(np.sqrt(num_points_per_face))
for i in np.linspace(0, 1, n):
for j in np.linspace(0, 1, n):
# 双线性插值
p = (1 - i) * (1 - j) * v0 + i * (1 - j) * v1 + i * j * v2 + (1 - i) * j * v3
points.append(p)
points = np.array(points)
# 绿色
colors = np.full((len(points), 3), [0, 255, 0], dtype=np.uint8)
return points, colors
def generate_cylinder_model_from_pose(center, x_axis, y_axis, z_axis,
radius, height, num_points=1000):
"""
根据位姿和尺寸生成完整的圆柱体模型点云（绿色实体）
参数:
center: 圆柱体中心点 (3D坐标)
x_axis: X轴方向向量 (归一化)
y_axis: Y轴方向向量 (归一化)
z_axis: Z轴方向向量 (归一化，圆柱轴线方向)
radius: 圆柱体半径
height: 圆柱体高度 (沿Z轴)
num_points: 采样点数
返回:
cylinder_points: 圆柱体表面点云 (N, 3)
cylinder_colors: 圆柱体颜色 (N, 3) - 绿色
"""
# 构建旋转矩阵
rotation_matrix = np.column_stack([x_axis, y_axis, z_axis])
h2 = height / 2
points = []
n_theta = int(np.sqrt(num_points))
n_z = int(np.sqrt(num_points))
# 圆柱体侧面
for i in range(n_theta):
theta = 2 * np.pi * i / n_theta
for j in range(n_z):
z = -h2 + height * j / n_z
# 局部坐标 (圆柱坐标)
local_point = np.array([radius * np.cos(theta), radius * np.sin(theta), z])
world_point = rotation_matrix @ local_point + center
points.append(world_point)
# 顶面 (圆形)
for i in range(n_theta):
r = radius * i / n_theta
for j in range(n_theta):
theta = 2 * np.pi * j / n_theta
local_point = np.array([r * np.cos(theta), r * np.sin(theta), h2])
world_point = rotation_matrix @ local_point + center
points.append(world_point)
# 底面 (圆形)
for i in range(n_theta):
r = radius * i / n_theta
for j in range(n_theta):
theta = 2 * np.pi * j / n_theta
local_point = np.array([r * np.cos(theta), r * np.sin(theta), -h2])
world_point = rotation_matrix @ local_point + center
points.append(world_point)
points = np.array(points)
colors = np.full((len(points), 3), [0, 255, 0], dtype=np.uint8) # 绿色
return points, colors
# ==========================
# 6D位姿估计器 (支持多种位置计算模式)
# ==========================
class Box6DPoseEstimator:
"""
盒子6D位姿估计器
"""
def __init__(self, box_length, box_width, box_height, is_cylinder=False):
self.box_length = box_length
self.box_width = box_width
self.box_height = box_height
self.is_cylinder = is_cylinder # 是否为圆柱体
self.extrinsic_matrix = None
self.end_effector_pose = None
self.pose_mode = "center"
self.custom_offset = (0.0, 0.0, 0.0)
def set_extrinsic_matrix(self, extrinsic):
self.extrinsic_matrix = np.array(extrinsic)
def set_end_effector_pose(self, pose):
self.end_effector_pose = np.array(pose)
def set_pose_mode(self, mode, offset=(0.0, 0.0, 0.0)):
self.pose_mode = mode
self.custom_offset = np.array(offset)
def calculate_position(self, top_center, z_axis):
if self.pose_mode == "top_center":
return top_center.copy()
elif self.pose_mode == "bottom_center":
return top_center - z_axis * (self.box_height / 2.0)
elif self.pose_mode == "custom":
return top_center + np.array(self.custom_offset)
else:
return top_center + z_axis * (self.box_height / 2.0)
def estimate_6d_pose(self, plane_model, inlier_cloud, box_center_camera=None):
points = np.asarray(inlier_cloud.points)
[a, b, c, d] = plane_model
z_axis = np.array([a, b, c])
z_axis = z_axis / (np.linalg.norm(z_axis) + 1e-8)
if z_axis[2] < 0:
z_axis = -z_axis
print(f"📐 法向量: ({z_axis[0]:.4f}, {z_axis[1]:.4f}, {z_axis[2]:.4f})")
# 检测圆柱体姿态
cylinder_orientation = 'unknown'
if self.is_cylinder:
# 投影到2D平面进行形状分析
centroid = np.mean(points, axis=0)
points_centered = points - centroid
points_proj = points_centered - np.outer(np.dot(points_centered, z_axis), z_axis)
# 建立局部2D坐标系
x_temp = np.array([1, 0, 0])
if abs(np.dot(x_temp, z_axis)) > 0.9:
x_temp = np.array([0, 1, 0])
x_temp = x_temp - np.dot(x_temp, z_axis) * z_axis
x_temp = x_temp / (np.linalg.norm(x_temp) + 1e-8)
y_temp = np.cross(z_axis, x_temp)
y_temp = y_temp / (np.linalg.norm(y_temp) + 1e-8)
points_2d = np.zeros((len(points_proj), 2))
for i, p in enumerate(points_proj):
points_2d[i, 0] = np.dot(p, x_temp)
points_2d[i, 1] = np.dot(p, y_temp)
cylinder_orientation = detect_cylinder_orientation(points_2d)
print(f"📐 圆柱体姿态检测: {cylinder_orientation}")
# 传递姿态信息到提取函数（增加 box_height 参数）
x_axis, y_axis = extract_box_axes_from_edges(
points, z_axis, self.box_length, self.box_width, self.box_height,
self.is_cylinder, cylinder_orientation
)
if x_axis is None or y_axis is None:
print("⚠️ 边长提取失败，使用PCA后备方案")
x_axis, y_axis = self._extract_edges_pca(points, z_axis)
print(f"📐 X轴: ({x_axis[0]:.4f}, {x_axis[1]:.4f}, {x_axis[2]:.4f})")
print(f"📐 Y轴: ({y_axis[0]:.4f}, {y_axis[1]:.4f}, {y_axis[2]:.4f})")
top_center = np.mean(points, axis=0)
print(f"📍 顶面中心: ({top_center[0]:.4f}, {top_center[1]:.4f}, {top_center[2]:.4f})")
if box_center_camera is not None:
position = box_center_camera
print(f"📍 使用外部提供的位置: ({position[0]:.4f}, {position[1]:.4f}, {position[2]:.4f})")
else:
position = self.calculate_position(top_center, z_axis)
mode_names = {
"center": "物体中心",
"top_center": "顶面中心",
"bottom_center": "底面中心",
"custom": "自定义位置"
}
mode_name = mode_names.get(self.pose_mode, self.pose_mode)
print(f"📍 {mode_name}: ({position[0]:.4f}, {position[1]:.4f}, {position[2]:.4f})")
rotation_matrix = np.column_stack([x_axis, y_axis, z_axis])
U, _, Vt = np.linalg.svd(rotation_matrix)
rotation_matrix = U @ Vt
if np.linalg.det(rotation_matrix) < 0:
rotation_matrix = -rotation_matrix
pose_camera = np.eye(4)
pose_camera[:3, :3] = rotation_matrix
pose_camera[:3, 3] = position
roll, pitch, yaw = self._rotation_matrix_to_euler(rotation_matrix)
pose_6d_camera = [
float(position[0]),
float(position[1]),
float(position[2]),
float(roll),
float(pitch),
float(yaw)
]
roll_deg = np.degrees(roll)
pitch_deg = np.degrees(pitch)
yaw_deg = np.degrees(yaw)
mode_names = {
"center": "物体中心",
"top_center": "顶面中心",
"bottom_center": "底面中心",
"custom": "自定义位置"
}
mode_name = mode_names.get(self.pose_mode, self.pose_mode)
print(f"🎯 6D位姿估计结果 (相机坐标系, 模式: {mode_name}, 位置单位: m):")
print(f" 位置 (x, y, z): ({pose_6d_camera[0]:.4f}, {pose_6d_camera[1]:.4f}, {pose_6d_camera[2]:.4f}) m")
print(
f" 位置 (x, y, z): ({pose_6d_camera[0] * 1000:.1f}, {pose_6d_camera[1] * 1000:.1f}, {pose_6d_camera[2] * 1000:.1f}) mm")
print(f" 姿态: ({roll_deg:.2f}°, {pitch_deg:.2f}°, {yaw_deg:.2f}°)")
return pose_6d_camera, pose_camera
def _extract_edges_pca(self, points, z_axis):
centroid = np.mean(points, axis=0)
points_centered = points - centroid
points_proj = points_centered - np.outer(np.dot(points_centered, z_axis), z_axis)
cov = np.cov(points_proj.T)
eigenvalues, eigenvectors = np.linalg.eigh(cov)
idx = np.argsort(eigenvalues)[::-1]
eigenvectors = eigenvectors[:, idx]
x_axis = eigenvectors[:, 0]
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
y_axis = np.cross(z_axis, x_axis)
y_axis = y_axis / (np.linalg.norm(y_axis) + 1e-8)
x_axis = np.cross(y_axis, z_axis)
x_axis = x_axis / (np.linalg.norm(x_axis) + 1e-8)
return x_axis, y_axis
def _rotation_matrix_to_euler(self, R):
try:
from scipy.spatial.transform import Rotation
r = Rotation.from_matrix(R)
roll, pitch, yaw = r.as_euler('zyx', degrees=False)
return roll, pitch, yaw
except:
sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
singular = sy < 1e-6
if not singular:
roll = np.arctan2(R[2, 1], R[2, 2])
pitch = np.arctan2(-R[2, 0], sy)
yaw = np.arctan2(R[1, 0], R[0, 0])
else:
roll = np.arctan2(-R[1, 2], R[1, 1])
pitch = np.arctan2(-R[2, 0], sy)
yaw = 0
return roll, pitch, yaw
def generate_box_model(self, pose=None):
l, w, h = self.box_length, self.box_width, self.box_height
vertices = np.array([
[-l / 2, -w / 2, -h / 2],
[l / 2, -w / 2, -h / 2],
[l / 2, w / 2, -h / 2],
[-l / 2, w / 2, -h / 2],
[-l / 2, -w / 2, h / 2],
[l / 2, -w / 2, h / 2],
[l / 2, w / 2, h / 2],
[-l / 2, w / 2, h / 2]
])
points = []
faces = [
([0, 1, 2, 3], [0, 0, -1]),
([4, 5, 6, 7], [0, 0, 1]),
([0, 1, 5, 4], [0, -1, 0]),
([2, 3, 7, 6], [0, 1, 0]),
([0, 3, 7, 4], [-1, 0, 0]),
([1, 2, 6, 5], [1, 0, 0])
]
for face_indices, normal in faces:
for i in np.linspace(0, 1, 10):
for j in np.linspace(0, 1, 10):
idx = face_indices
p = (1 - i) * (1 - j) * vertices[idx[0]] + i * (1 - j) * vertices[idx[1]] + \
i * j * vertices[idx[2]] + (1 - i) * j * vertices[idx[3]]
points.append(p)
points = np.array(points)
box_cloud = o3d.geometry.PointCloud()
box_cloud.points = o3d.utility.Vector3dVector(points)
if pose is not None:
if isinstance(pose, list) and len(pose) == 6:
matrix = self.pose_to_matrix(pose)
box_cloud.transform(matrix)
elif isinstance(pose, np.ndarray) and pose.shape == (4, 4):
box_cloud.transform(pose)
elif isinstance(pose, np.ndarray) and pose.shape == (6,):
matrix = self.pose_to_matrix(pose.tolist())
box_cloud.transform(matrix)
return box_cloud
def pose_to_matrix(self, pose_6d):
x, y, z, roll, pitch, yaw = pose_6d
R = self._euler_to_rotation_matrix(roll, pitch, yaw)
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = [x, y, z]
return T
def _euler_to_rotation_matrix(self, roll, pitch, yaw):
try:
from scipy.spatial.transform import Rotation
r = Rotation.from_euler('zyx', [roll, pitch, yaw])
return r.as_matrix()
except:
R_x = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]])
R_y = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]])
R_z = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]])
return R_z @ R_y @ R_x
# ==========================
# 创建PointCloud2消息
# ==========================
def create_point_cloud(points_3d, colors, frame_id, clock):
if len(points_3d) == 0:
return PointCloud2()
cloud_msg = PointCloud2()
cloud_msg.header = Header()
cloud_msg.header.stamp = clock.now().to_msg()
cloud_msg.header.frame_id = frame_id
cloud_msg.height = 1
cloud_msg.width = len(points_3d)
cloud_msg.is_bigendian = False
cloud_msg.is_dense = True
cloud_msg.fields = [
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
PointField(name='rgb', offset=12, datatype=PointField.UINT32, count=1),
]
cloud_msg.point_step = 16
cloud_msg.row_step = cloud_msg.point_step * len(points_3d)
data = []
for pt, col in zip(points_3d, colors):
rgb = (int(col[2]) << 16) | (int(col[1]) << 8) | int(col[0])
data.append(struct.pack('ffff', pt[0], pt[1], pt[2], float(rgb)))
cloud_msg.data = b''.join(data)
return cloud_msg
# ==========================
# 检测结果数据结构
# ==========================
class DetectionResult:
"""单个检测目标的结果"""
def __init__(self, class_name, confidence, bbox, mask,
points_3d, colors, plane_model, inlier_cloud,
pose_camera, pose_gripper, pose_base, box_model,
pose_mode="center"):
self.class_name = class_name
self.confidence = confidence
self.bbox = bbox
self.mask = mask
self.points_3d = points_3d
self.colors = colors
self.plane_model = plane_model
self.inlier_cloud = inlier_cloud
self.pose_camera = pose_camera
self.pose_gripper = pose_gripper
self.pose_base = pose_base
self.box_model = box_model
self.pose_mode = pose_mode
self.timestamp = time.time()
# ==========================
# 主节点 - 带Action Server (支持多目标)
# ==========================
class CameraDetectionNode(Node):
def __init__(self):
# ============================================================
# 步骤1: 初始化ROS2节点
# ============================================================
super().__init__('ros2_camera_detection')
# ============================================================
# 步骤2: 创建图像转换桥接器
# ============================================================
self.bridge = CvBridge()
# ============================================================
# 步骤3: 初始化数据缓存变量
# ============================================================
self.rgb_img = None
self.depth_img = None
self.K = None
self.last_process_time = 0
self.process_interval = PROCESS_INTERVAL
self.end_effector_pose = None
self.end_effector_pose_matrix = None
self.end_effector_received = False
# 缓存最新结果(支持多目标)
self.latest_detections = []
self.latest_detection_img = None
# 线程锁
self.pipeline_lock = threading.Lock()
# 3D可视化相关
self.vis_thread = None
self.vis_geometries = []
self.vis_running = False
# 加载手眼标定参数
self.load_hand_eye_calibration()
# 创建点云保存目录
if ENABLE_SAVE_PLY:
os.makedirs(SAVE_PLY_DIR, exist_ok=True)
self.get_logger().info(f"📁 点云保存目录: {SAVE_PLY_DIR}")
# ============================================================
# 步骤5: 加载YOLO模型
# ============================================================
self.yolo = YOLO(YOLO_MODEL_PATH)
self.yolo.to("cpu")
self.get_logger().info("✅ YOLO模型加载成功")
# ============================================================
# 步骤6: 创建话题订阅器
# ============================================================
self.create_subscription(Image, COLOR_TOPIC, self.rgb_cb, 10)
self.create_subscription(Image, DEPTH_TOPIC, self.depth_cb, 10)
self.create_subscription(CameraInfo, CAMERA_INFO_TOPIC, self.caminfo_cb, 10)
self.create_subscription(PoseStamped, END_EFFECTOR_POSE_TOPIC, self.end_effector_pose_cb, 10)
# ============================================================
# 步骤7: 创建话题发布器
# ============================================================
self.pointcloud_pub = self.create_publisher(
PointCloud2,
POINTCLOUD_TOPIC,
10
)
self.get_logger().info(f"📡 发布点云: {POINTCLOUD_TOPIC}")
self.result_image_pub = self.create_publisher(
Image,
RESULT_IMAGE_TOPIC,
10
)
self.get_logger().info(f"📡 发布结果图像: {RESULT_IMAGE_TOPIC}")
self.final_pose_pub = self.create_publisher(
PoseStamped,
FINAL_POSE_TOPIC,
10
)
self.get_logger().info(f"📡 发布最终位姿: {FINAL_POSE_TOPIC}")
# ============================================================
# 步骤8: 创建Action Server
# ============================================================
self.action_server = ActionServer(
node=self,
action_name=ACTION_NAME,
action_type=VisionDetection,
execute_callback=self.execute_callback,
goal_callback=self.goal_callback,
cancel_callback=self.cancel_callback,
)
# ============================================================
# 步骤9: 打印初始化完成日志
# ============================================================
self.get_logger().info("=" * 60)
self.get_logger().info("✅ 节点初始化完成 (支持多目标检测 + 多种位姿模式)")
self.get_logger().info(f"📡 Action Server: {ACTION_NAME}")
self.get_logger().info(f"📡 点云发布: {POINTCLOUD_TOPIC}")
self.get_logger().info(f"📡 结果图片: {RESULT_IMAGE_TOPIC}")
self.get_logger().info(f"📡 最终位姿: {FINAL_POSE_TOPIC}")
self.get_logger().info(f"📡 订阅末端位姿: {END_EFFECTOR_POSE_TOPIC}")
self.get_logger().info(f"📦 支持的目标类型: {list(TARGET_DIMENSIONS.keys())}")
self.get_logger().info(f"🎯 位姿模式配置: {POSE_MODE_CONFIG}")
self.get_logger().info(f"🎯 默认位姿模式: {DEFAULT_POSE_MODE}")
self.get_logger().info(f"🎨 3D可视化: {'启用' if ENABLE_3D_VISUALIZATION else '禁用'}")
self.get_logger().info("=" * 60)
# 启动3D可视化
if ENABLE_3D_VISUALIZATION:
self.start_visualization()
# ============================================================================
# 3D可视化相关函数 (增强版)
# ============================================================================
def start_visualization(self):
"""启动3D可视化线程"""
if self.vis_thread is not None and self.vis_thread.is_alive():
return
self.vis_running = True
self.vis_thread = threading.Thread(target=self.visualization_loop, daemon=True)
self.vis_thread.start()
self.get_logger().info("✅ 3D可视化线程已启动")
def visualization_loop(self):
"""3D可视化主循环 - 显示所有目标的点云和盒子"""
# 创建可视化窗口
vis = o3d.visualization.Visualizer()
vis.create_window(window_name="3D Detection Result - All 8 Targets",
width=WINDOW_WIDTH,
height=WINDOW_HEIGHT)
# 添加坐标系
coord_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.2)
vis.add_geometry(coord_frame)
# 添加地面网格
try:
grid = o3d.geometry.TriangleMesh.create_grid()
grid.transform([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -0.5], [0, 0, 0, 1]])
vis.add_geometry(grid)
except:
pass
# 不同目标的颜色
target_colors = {
"lefta": [1.0, 0.2, 0.2], # 红色
"leftb": [0.2, 1.0, 0.2], # 绿色
"leftc": [0.2, 0.2, 1.0], # 蓝色
"leftd": [1.0, 1.0, 0.2], # 黄色
"righta": [1.0, 0.2, 1.0], # 品红
"rightb": [0.2, 1.0, 1.0], # 青色
"rightc": [1.0, 0.5, 0.0], # 橙色
"rightd": [0.5, 0.5, 0.5], # 灰色
}
last_update_time = 0
update_interval = 0.3 # 每0.3秒更新一次
while self.vis_running:
try:
current_time = time.time()
if current_time - last_update_time > update_interval:
with self.pipeline_lock:
if len(self.latest_detections) > 0:
# 清除旧几何体（保留坐标系和网格）
for geom in self.vis_geometries:
if geom is not None:
try:
vis.remove_geometry(geom, reset_bounding_box=False)
except:
pass
self.vis_geometries.clear()
# 添加每个目标的几何体
for idx, result in enumerate(self.latest_detections):
color = target_colors.get(result.class_name, [0.5, 0.5, 0.5])
# 1. 添加原始点云（转换到基坐标系）
if result.points_3d is not None and len(result.points_3d) > 0:
pcd = o3d.geometry.PointCloud()
# 降采样显示，每5个点取1个
step = max(1, len(result.points_3d) // 5000)
sample_points = result.points_3d[::step]
# 转换到基坐标系
if result.pose_base is not None:
T_base = self.pose_6d_to_matrix(result.pose_base)
points_homo = np.hstack([sample_points, np.ones((len(sample_points), 1))])
points_base = (T_base @ points_homo.T).T[:, :3]
else:
points_base = sample_points
pcd.points = o3d.utility.Vector3dVector(points_base)
if result.colors is not None:
colors_sample = result.colors[::step] / 255.0
pcd.colors = o3d.utility.Vector3dVector(colors_sample)
else:
pcd.paint_uniform_color(color)
vis.add_geometry(pcd)
self.vis_geometries.append(pcd)
# 2. 添加盒子模型（转换到基坐标系）
if result.box_model is not None:
box = copy.deepcopy(result.box_model)
box.paint_uniform_color(color)
if result.pose_base is not None:
T_base = self.pose_6d_to_matrix(result.pose_base)
box.transform(T_base)
vis.add_geometry(box)
self.vis_geometries.append(box)
# 3. 添加位姿坐标系
if result.pose_base is not None:
try:
pose_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(
size=0.04
)
T_base = self.pose_6d_to_matrix(result.pose_base)
pose_frame.transform(T_base)
vis.add_geometry(pose_frame)
self.vis_geometries.append(pose_frame)
except Exception as e:
pass
# 打印所有目标的位置汇总
self.print_all_positions()
last_update_time = current_time
# 渲染
vis.poll_events()
vis.update_renderer()
# 控制更新频率
time.sleep(0.05)
except Exception as e:
self.get_logger().error(f"3D可视化错误: {e}")
time.sleep(0.1)
vis.destroy_window()
self.get_logger().info("3D可视化窗口已关闭")
def print_all_positions(self):
"""打印所有目标的位置汇总（包含6D姿态）"""
if len(self.latest_detections) == 0:
return
self.get_logger().info("=" * 90)
self.get_logger().info("📊 所有目标6D位姿汇总 (基坐标系):")
self.get_logger().info(" 目标名称 模式 位置 (mm) 姿态 (度)")
self.get_logger().info(" " + "-" * 85)
for result in self.latest_detections:
if result.pose_base is not None:
# 位置 (mm)
x, y, z = result.pose_base[0] * 1000, result.pose_base[1] * 1000, result.pose_base[2] * 1000
# 姿态 (度)
roll = np.degrees(result.pose_base[3])
pitch = np.degrees(result.pose_base[4])
yaw = np.degrees(result.pose_base[5])
mode_names = {
"center": "物体中心",
"top_center": "顶面中心",
"bottom_center": "底面中心",
"custom": "自定义位置"
}
mode_name = mode_names.get(result.pose_mode, result.pose_mode)
self.get_logger().info(
f" {result.class_name:8s} {mode_name:10s}: "
f"({x:7.1f}, {y:7.1f}, {z:7.1f}) "
f"({roll:7.1f}°, {pitch:7.1f}°, {yaw:7.1f}°)"
)
self.get_logger().info("=" * 90)
def stop_visualization(self):
"""停止3D可视化"""
self.vis_running = False
if self.vis_thread is not None:
self.vis_thread.join(timeout=2.0)
# ============================================================================
# 加载手眼标定结果
# ============================================================================
def load_hand_eye_calibration(self):
"""加载手眼标定结果 - 从公共常量读取"""
self.R_cam2gripper = HAND_EYE_ROTATION.copy()
self.t_cam2gripper = HAND_EYE_TRANSLATION.copy()
self.T_cam2gripper = HAND_EYE_MATRIX.copy()
self.enable_transform = ENABLE_HAND_EYE_TRANSFORM
det = np.linalg.det(self.R_cam2gripper)
self.get_logger().info("=" * 60)
self.get_logger().info("✅ 手眼标定参数加载完成")
self.get_logger().info(f"📐 旋转矩阵行列式: {det:.6f} {'✅' if abs(det - 1.0) < 1e-6 else '⚠️'}")
self.get_logger().info(f"🔀 位姿转换状态: {'启用' if self.enable_transform else '禁用'}")
self.get_logger().info("=" * 60)
# ============================================================================
# 机械臂末端位姿回调
# ============================================================================
def end_effector_pose_cb(self, msg: PoseStamped):
"""接收机械臂末端位姿（四元数）"""
try:
position = np.array([
msg.pose.position.x,
msg.pose.position.y,
msg.pose.position.z
])
quat = np.array([
msg.pose.orientation.x,
msg.pose.orientation.y,
msg.pose.orientation.z,
msg.pose.orientation.w
])
self.end_effector_pose = {
'position': position,
'quaternion': quat,
'timestamp': msg.header.stamp
}
rotation = Rotation.from_quat(quat)
self.end_effector_pose_matrix = np.eye(4)
self.end_effector_pose_matrix[:3, :3] = rotation.as_matrix()
self.end_effector_pose_matrix[:3, 3] = position
self.end_effector_received = True
except Exception as e:
self.get_logger().error(f"末端位姿处理错误: {e}")
# ============================================================================
# 坐标转换函数
# ============================================================================
def transform_camera_to_gripper(self, pose_camera):
"""将相机坐标系下的位姿转换到机械臂末端坐标系"""
if not self.enable_transform:
return pose_camera
# 将6D位姿转换为齐次矩阵
if isinstance(pose_camera, (list, np.ndarray)) and len(pose_camera) == 6:
T_camera = self.pose_6d_to_matrix(pose_camera)
elif isinstance(pose_camera, np.ndarray) and pose_camera.shape == (4, 4):
T_camera = pose_camera
else:
raise ValueError("输入格式错误，需要6D位姿或4x4齐次矩阵")
# 🔑 关键：T_gripper = T_cam2gripper @ T_camera
T_gripper = self.T_cam2gripper @ T_camera
# 提取位置和姿态
position = T_gripper[:3, 3]
rotation = T_gripper[:3, :3]
roll, pitch, yaw = self.matrix_to_euler(rotation)
return [
float(position[0]),
float(position[1]),
float(position[2]),
float(roll),
float(pitch),
float(yaw)
]
def transform_gripper_to_base(self, pose_gripper):
"""将机械臂末端坐标系下的位姿转换到基坐标系"""
if self.end_effector_pose_matrix is None:
self.get_logger().warn("⚠️ 未收到末端位姿，无法转换到基坐标系")
return None
if isinstance(pose_gripper, (list, np.ndarray)) and len(pose_gripper) == 6:
T_gripper = self.pose_6d_to_matrix(pose_gripper)
elif isinstance(pose_gripper, np.ndarray) and pose_gripper.shape == (4, 4):
T_gripper = pose_gripper
else:
raise ValueError("输入格式错误，需要6D位姿或4x4齐次矩阵")
# 🔑 关键：T_base = T_end_effector @ T_gripper
T_base = self.end_effector_pose_matrix @ T_gripper
position = T_base[:3, 3]
rotation = T_base[:3, :3]
roll, pitch, yaw = self.matrix_to_euler(rotation)
return [
float(position[0]),
float(position[1]),
float(position[2]),
float(roll),
float(pitch),
float(yaw)
]
def pose_6d_to_matrix(self, pose_6d):
"""将6D位姿转换为4x4齐次矩阵"""
if pose_6d is None:
return np.eye(4)
x, y, z, roll, pitch, yaw = pose_6d
try:
from scipy.spatial.transform import Rotation
r = Rotation.from_euler('zyx', [roll, pitch, yaw])
T = np.eye(4)
T[:3, :3] = r.as_matrix()
T[:3, 3] = [x, y, z]
return T
except:
return self.pose_6d_to_matrix_legacy(pose_6d)
def pose_6d_to_matrix_legacy(self, pose_6d):
"""将6D位姿转换为4x4齐次矩阵 (备用)"""
x, y, z, roll, pitch, yaw = pose_6d
R = self.euler_to_matrix(roll, pitch, yaw)
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = [x, y, z]
return T
def euler_to_matrix(self, roll, pitch, yaw):
"""欧拉角转旋转矩阵"""
try:
from scipy.spatial.transform import Rotation
r = Rotation.from_euler('zyx', [roll, pitch, yaw])
return r.as_matrix()
except ImportError:
Rx = np.array([
[1, 0, 0],
[0, np.cos(roll), -np.sin(roll)],
[0, np.sin(roll), np.cos(roll)]
])
Ry = np.array([
[np.cos(pitch), 0, np.sin(pitch)],
[0, 1, 0],
[-np.sin(pitch), 0, np.cos(pitch)]
])
Rz = np.array([
[np.cos(yaw), -np.sin(yaw), 0],
[np.sin(yaw), np.cos(yaw), 0],
[0, 0, 1]
])
return Rz @ Ry @ Rx
def matrix_to_euler(self, R):
"""旋转矩阵转欧拉角"""
try:
from scipy.spatial.transform import Rotation
r = Rotation.from_matrix(R)
roll, pitch, yaw = r.as_euler('zyx')
return roll, pitch, yaw
except ImportError:
sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
singular = sy < 1e-6
if not singular:
roll = np.arctan2(R[2, 1], R[2, 2])
pitch = np.arctan2(-R[2, 0], sy)
yaw = np.arctan2(R[1, 0], R[0, 0])
else:
roll = np.arctan2(-R[1, 2], R[1, 1])
pitch = np.arctan2(-R[2, 0], sy)
yaw = 0
return roll, pitch, yaw
def transform_points_to_base(self, points_camera):
"""将点云从相机坐标系转换到基坐标系"""
if len(points_camera) == 0:
return points_camera
if self.end_effector_pose_matrix is None:
return None
try:
ones = np.ones((len(points_camera), 1))
points_homogeneous = np.hstack([points_camera, ones])
points_gripper = (self.T_cam2gripper @ points_homogeneous.T).T
points_base = (self.end_effector_pose_matrix @ points_gripper.T).T
return points_base[:, :3]
except Exception as e:
self.get_logger().error(f"点云转换失败: {e}")
return None
# ============================================================================
# 发布最终位姿
# ============================================================================
def publish_final_pose(self, pose_base, class_name="target", pose_mode="center"):
"""发布基坐标系下的最终位姿（单位：毫米）"""
if pose_base is None:
return
pose_msg = PoseStamped()
pose_msg.header.stamp = self.get_clock().now().to_msg()
pose_msg.header.frame_id = "base_link"
pose_msg.pose.position.x = float(pose_base[0]) * M_TO_MM
pose_msg.pose.position.y = float(pose_base[1]) * M_TO_MM
pose_msg.pose.position.z = float(pose_base[2]) * M_TO_MM
# r = Rotation.from_euler('zyx', [pose_base[3], pose_base[4], pose_base[5]])
# quat = r.as_quat()
quat = euler_zyx_to_quaternion(pose_base[5], pose_base[4], pose_base[3])
pose_msg.pose.orientation.x = quat[0]
pose_msg.pose.orientation.y = quat[1]
pose_msg.pose.orientation.z = quat[2]
pose_msg.pose.orientation.w = quat[3]
self.final_pose_pub.publish(pose_msg)
mode_names = {
"center": "物体中心",
"top_center": "顶面中心",
"bottom_center": "底面中心",
"custom": "自定义位置"
}
mode_name = mode_names.get(pose_mode, pose_mode)
self.get_logger().info(
f"📤 发布最终位姿 ({class_name}, {mode_name}): "
f"({pose_base[0] * M_TO_MM:.4f}, {pose_base[1] * M_TO_MM:.4f}, {pose_base[2] * M_TO_MM:.4f}) mm"
f"📤 4元数 ({quat[0]:.4f}, {quat[1]:.4f}，{quat[2]:.4f}，{quat[3]:.4f}): "
)
def goal_callback(self, goal_request: VisionDetection.Goal) -> GoalResponse:
self.get_logger().info(f"📥 收到目标请求: {goal_request.target_obj_name}")
return GoalResponse.ACCEPT
def cancel_callback(self, goal_handle: ServerGoalHandle) -> CancelResponse:
self.get_logger().info("❌ 任务取消")
return CancelResponse.ACCEPT
def caminfo_cb(self, msg):
self.K = np.array(msg.k).reshape(3, 3)
def rgb_cb(self, msg):
self.rgb_img = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
def depth_cb(self, msg):
try:
if msg.encoding == "16UC1":
depth_data = np.ndarray(
shape=(msg.height, msg.width),
dtype=np.uint16,
buffer=msg.data
)
else:
self.get_logger().warning(f"未知深度编码: {msg.encoding}")
return
self.depth_img = depth_data
self.process()
except Exception as e:
self.get_logger().error(f"深度图像处理错误: {e}")
def process(self):
"""主处理函数 - 目标检测、点云生成和6D位姿估计"""
if self.rgb_img is None or self.depth_img is None or self.K is None:
return
current_time = time.time()
if current_time - self.last_process_time < self.process_interval:
return
self.last_process_time = current_time
with self.pipeline_lock:
start = time.time()
rgb = self.rgb_img.copy()
h, w = rgb.shape[:2]
depth_aligned = self.depth_img
depth = depth_aligned.astype(np.float32) / DEPTH_SCALE
depth = np.clip(depth, DEPTH_MIN, DEPTH_MAX)
depth[np.isnan(depth)] = DEPTH_MIN
depth[np.isinf(depth)] = DEPTH_MIN
# =================================> YOLO目标检测 <=================================
res = self.yolo.predict(rgb, conf=YOLO_CONFIDENCE_THRESHOLD, verbose=False)
if len(res) < 1 or res[0].boxes is None or len(res[0].boxes) == 0:
return
# 打印检测到的目标
self.get_logger().info(f"🔍 YOLO检测到 {len(res[0].boxes)} 个目标:")
for i in range(len(res[0].boxes)):
class_id = int(res[0].boxes[i].cls.cpu().numpy()[0])
class_name = res[0].names[class_id]
confidence = float(res[0].boxes[i].conf.cpu().numpy()[0])
self.get_logger().info(f" 目标 {i + 1}: {class_name} (置信度: {confidence:.3f})")
# 收集所有检测到的目标
detections_info = []
for i in range(len(res[0].boxes)):
box = res[0].boxes[i].xyxy.cpu().numpy()[0]
x1, y1, x2, y2 = map(int, box)
bbox = (x1, y1, x2, y2)
class_id = int(res[0].boxes[i].cls.cpu().numpy()[0])
class_name = res[0].names[class_id]
confidence = float(res[0].boxes[i].conf.cpu().numpy()[0])
# 🔑 如果配置了过滤列表，只保留指定的目标
if len(FILTER_TARGETS) > 0 and class_name not in FILTER_TARGETS:
self.get_logger().info(f"⏭️ 跳过目标 {class_name} (不在发布列表中)")
continue
if res[0].masks is not None and len(res[0].masks) > i:
m = res[0].masks.data[i].cpu().numpy()
m = cv2.resize(m, (w, h))
mask = (m > 0.5).astype(np.uint8)
else:
mask = None
pose_mode, custom_offset = get_pose_mode(class_name)
detections_info.append({
'class_name': class_name,
'confidence': confidence,
'bbox': bbox,
'mask': mask,
'index': i,
'pose_mode': pose_mode,
'custom_offset': custom_offset
})
mode_names = {
"center": "物体中心",
"top_center": "顶面中心",
"bottom_center": "底面中心",
"custom": "自定义位置"
}
mode_name = mode_names.get(pose_mode, pose_mode)
self.get_logger().info(
f"🎯 检测到目标 {i + 1}: {class_name} "
f"置信度: {confidence:.2f} "
f"模式: {mode_name} "
f"位置: ({x1},{y1})-({x2},{y2})"
)
if len(detections_info) == 0:
return
# =================================> 形态学操作 <=================================
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (MORPH_KERNEL_SIZE, MORPH_KERNEL_SIZE))
for det in detections_info:
if det['mask'] is not None:
det['mask'] = cv2.dilate(det['mask'], kernel, iterations=MORPH_DILATE_ITERATIONS)
# =================================> 显示分割结果并发布图像 <=================================
detections_for_display = [
(det['mask'], det['bbox'], det['class_name'], det['confidence'])
for det in detections_info
]
overlay = segment_display_and_publish(
rgb, detections_for_display, self.bridge, self.result_image_pub
)
self.latest_detection_img = overlay
# =================================> 为每个目标生成点云和6D位姿 <=================================
detection_results = []
fx = self.K[0, 0]
fy = self.K[1, 1]
cx = self.K[0, 2]
cy = self.K[1, 2]
for det in detections_info:
class_name = det['class_name']
mask = det['mask']
bbox = det['bbox']
confidence = det['confidence']
pose_mode = det['pose_mode']
custom_offset = det['custom_offset']
if mask is None or np.sum(mask) < MIN_MASK_PIXELS:
self.get_logger().warning(f"⚠️ 目标 {class_name} 掩码像素不足")
continue
# 生成点云
ys, xs = np.where(mask > 0)
points_3d = []
colors = []
for v, u in zip(ys, xs):
z = depth[v, u]
if z <= 0 or z > DEPTH_MAX:
continue
x = (u - cx) * z / fx
y = (v - cy) * z / fy
b, g, r = rgb[v, u]
points_3d.append([x, y, z])
colors.append([r, g, b])
if len(points_3d) == 0:
self.get_logger().warning(f"⚠️ 目标 {class_name} 无有效点云数据")
continue
points_3d = np.array(points_3d, dtype=np.float32)
colors = np.array(colors, dtype=np.uint8)
# =================================> 🔑 孤立点过滤 (在平面分割前调用) <=================================
if ENABLE_OUTLIER_FILTER and len(points_3d) > MIN_POINTS_AFTER_FILTER:
# 统计滤波 - 去除孤立点
points_3d, colors, removed_count, success = filter_outlier_points(
points_3d,
colors,
nb_neighbors=OUTLIER_NB_NEIGHBORS,
std_ratio=OUTLIER_STD_RATIO,
min_points=MIN_POINTS_AFTER_FILTER
)
if len(points_3d) > SAMPLE_MAX_POINTS:
indices = np.random.choice(len(points_3d), SAMPLE_MAX_POINTS, replace=False)
points_3d = points_3d[indices]
colors = colors[indices]
# 如果还想用半径滤波，可以继续调用
# if success and len(points_3d) > MIN_POINTS_AFTER_FILTER:
# points_3d, colors, removed, success = filter_outlier_points_radius(
# points_3d, colors,
# nb_points=16, radius=0.05,
# min_points=MIN_POINTS_AFTER_FILTER
# )
# =================================> 平面分割 <=================================
plane_model, inlier_cloud, outlier_cloud = ranscan(points_3d)
if plane_model is None:
self.get_logger().warning(f"⚠️ 目标 {class_name} 平面分割失败")
continue
# =================================> 保存点云（分割面蓝色 + 实体红色） <=================================
if ENABLE_SAVE_PLY and inlier_cloud is not None and outlier_cloud is not None:
try:
# 获取平面内点（分割面）- 蓝色
inlier_points = np.asarray(inlier_cloud.points)
if len(inlier_points) > 0:
inlier_colors = np.full((len(inlier_points), 3), [0, 0, 255], dtype=np.uint8)
self.save_ply(inlier_points, inlier_colors,
os.path.join(SAVE_PLY_DIR, f"plane_surface_{class_name}.ply"))
self.get_logger().info(
f"💾 分割面点云已保存: plane_surface_{class_name}.ply ({len(inlier_points)} 个点)")
# 获取平面外点（实体）- 红色
outlier_points = np.asarray(outlier_cloud.points)
if len(outlier_points) > 0:
outlier_colors = np.full((len(outlier_points), 3), [255, 0, 0], dtype=np.uint8)
self.save_ply(outlier_points, outlier_colors,
os.path.join(SAVE_PLY_DIR, f"object_body_{class_name}.ply"))
self.get_logger().info(
f"💾 实体点云已保存: object_body_{class_name}.ply ({len(outlier_points)} 个点)")
# 保存合并点云（分割面蓝色 + 实体红色）
if len(inlier_points) > 0 and len(outlier_points) > 0:
all_points = np.vstack([inlier_points, outlier_points])
all_colors = np.vstack([inlier_colors, outlier_colors])
self.save_ply(all_points, all_colors,
os.path.join(SAVE_PLY_DIR, f"combined_{class_name}.ply"))
self.get_logger().info(
f"💾 合并点云已保存: combined_{class_name}.ply ({len(all_points)} 个点)")
except Exception as e:
self.get_logger().error(f"❌ 保存点云失败: {e}")
# =================================> 获取目标尺寸 <=================================
box_length, box_width, box_height, is_cylinder = get_target_info(class_name)
# =================================> 计算6D位姿 <=================================
box_estimator = Box6DPoseEstimator(
box_length=box_length,
box_width=box_width,
box_height=box_height,
is_cylinder=is_cylinder
)
box_estimator.set_pose_mode(pose_mode, custom_offset)
pose_camera_6d, pose_camera_matrix = box_estimator.estimate_6d_pose(plane_model, inlier_cloud)
# 转换到末端坐标系
try:
pose_gripper_6d = self.transform_camera_to_gripper(pose_camera_6d)
except Exception as e:
self.get_logger().error(f"❌ 转换到末端坐标系失败 ({class_name}): {e}")
pose_gripper_6d = pose_camera_6d
# 转换到基坐标系
pose_base_6d = None
try:
if self.end_effector_pose_matrix is not None:
pose_base_6d = self.transform_gripper_to_base(pose_gripper_6d)
else:
self.get_logger().warn(f"⚠️ 未收到末端位姿，无法转换到基坐标系 ({class_name})")
pose_base_6d = pose_gripper_6d
except Exception as e:
self.get_logger().error(f"❌ 转换到基坐标系失败 ({class_name}): {e}")
pose_base_6d = pose_gripper_6d
# 生成盒子模型
box_model = box_estimator.generate_box_model(pose_camera_matrix)
box_model.paint_uniform_color([0, 1, 0])
# =================================> 构造完整实体模型（绿色）并合并保存 <=================================
if pose_base_6d is not None and pose_camera_matrix is not None:
try:
# 获取分割面的中心点
inlier_points = np.asarray(inlier_cloud.points)
surface_center = np.mean(inlier_points, axis=0)
# 🔑 获取法向量（Z轴）- 直接从平面模型提取，不经过任何修改
plane_normal = np.array(plane_model[:3])
plane_normal = plane_normal / (np.linalg.norm(plane_normal) + 1e-8)
# 🔑 对于圆柱体，法向量就是轴线方向
z_axis = plane_normal.copy()
# 获取X轴和Y轴（从位姿矩阵中提取）
x_axis = pose_camera_matrix[:3, 0]
y_axis = pose_camera_matrix[:3, 1]
# 获取尺寸
box_length, box_width, box_height, is_cylinder = get_target_info(class_name)
# 🔑 计算实体中心：分割面中心 + 法向量 * 高度/2
entity_center = surface_center + z_axis * (box_height / 2.0)
self.get_logger().info(
f"📍 分割面中心: ({surface_center[0]:.3f}, {surface_center[1]:.3f}, {surface_center[2]:.3f})")
self.get_logger().info(
f"📍 实体中心: ({entity_center[0]:.3f}, {entity_center[1]:.3f}, {entity_center[2]:.3f})")
self.get_logger().info(f"📐 法向量方向: ({z_axis[0]:.3f}, {z_axis[1]:.3f}, {z_axis[2]:.3f})")
# 构建实体模型的旋转矩阵
rotation_matrix = np.column_stack([x_axis, y_axis, z_axis])
# 根据形状生成实体模型
if is_cylinder:
radius = box_width / 2
entity_points, entity_colors = generate_cylinder_model_from_pose(
entity_center, x_axis, y_axis, z_axis,
radius, box_height, num_points=800
)
self.get_logger().info(f"🟢 生成圆柱体实体模型: {len(entity_points)} 个点")
else:
entity_points, entity_colors = generate_box_model_from_pose(
entity_center, x_axis, y_axis, z_axis,
box_length, box_width, box_height, num_points_per_face=100
)
self.get_logger().info(f"🟢 生成长方体实体模型: {len(entity_points)} 个点")
# =================================> 合并三种点云到同一个PLY文件 <=================================
if ENABLE_SAVE_PLY:
all_points_list = []
all_colors_list = []
# 1. 分割面点云 (蓝色)
if inlier_cloud is not None:
inlier_points = np.asarray(inlier_cloud.points)
if len(inlier_points) > 0:
inlier_colors = np.full((len(inlier_points), 3), [0, 0, 255], dtype=np.uint8)
all_points_list.append(inlier_points)
all_colors_list.append(inlier_colors)
self.get_logger().info(f"🔵 分割面: {len(inlier_points)} 个点 (蓝色)")
# 2. 构造实体点云 (绿色)
if len(entity_points) > 0:
all_points_list.append(entity_points)
all_colors_list.append(entity_colors)
self.get_logger().info(f"🟢 构造实体: {len(entity_points)} 个点 (绿色)")
# 3. 外点点云 (红色)
if outlier_cloud is not None:
outlier_points = np.asarray(outlier_cloud.points)
if len(outlier_points) > 0:
outlier_colors = np.full((len(outlier_points), 3), [255, 0, 0], dtype=np.uint8)
all_points_list.append(outlier_points)
all_colors_list.append(outlier_colors)
self.get_logger().info(f"🔴 外点: {len(outlier_points)} 个点 (红色)")
# 合并所有点云
if len(all_points_list) > 0:
all_points = np.vstack(all_points_list)
all_colors = np.vstack(all_colors_list)
self.save_ply(all_points, all_colors,
os.path.join(SAVE_PLY_DIR, f"combined_all_{class_name}.ply"))
self.get_logger().info(
f"💾 合并点云已保存: combined_all_{class_name}.ply "
f"(共 {len(all_points)} 个点: 蓝色分割面 + 绿色实体 + 红色外点)"
)
except Exception as e:
self.get_logger().error(f"❌ 构造实体模型失败: {e}")
import traceback
traceback.print_exc()
############################################# 构造实体结束 #######################################################
# 保存结果
result = DetectionResult(
class_name=class_name,
confidence=confidence,
bbox=bbox,
mask=mask,
points_3d=points_3d,
colors=colors,
plane_model=plane_model,
inlier_cloud=inlier_cloud,
pose_camera=pose_camera_6d,
pose_gripper=pose_gripper_6d,
pose_base=pose_base_6d,
box_model=box_model,
pose_mode=pose_mode
)
detection_results.append(result)
# =================================> 打印位姿信息 <=================================
mode_names = {
"center": "物体中心",
"top_center": "顶面中心",
"bottom_center": "底面中心",
"custom": "自定义位置"
}
mode_name = mode_names.get(pose_mode, pose_mode)
self.get_logger().info("=" * 70)
self.get_logger().info(f"🎯 {class_name} 6D位姿结果 (模式: {mode_name}):")
# 相机坐标系
if pose_camera_6d is not None:
roll_cam = np.degrees(pose_camera_6d[3])
pitch_cam = np.degrees(pose_camera_6d[4])
yaw_cam = np.degrees(pose_camera_6d[5])
self.get_logger().info(
f" 📷 相机坐标系: "
f"({pose_camera_6d[0] * 1000:7.1f}, {pose_camera_6d[1] * 1000:7.1f}, {pose_camera_6d[2] * 1000:7.1f}) mm, "
f"姿态: ({roll_cam:6.1f}°, {pitch_cam:6.1f}°, {yaw_cam:6.1f}°)"
)
# 末端坐标系
if pose_gripper_6d is not None:
roll_gripper = np.degrees(pose_gripper_6d[3])
pitch_gripper = np.degrees(pose_gripper_6d[4])
yaw_gripper = np.degrees(pose_gripper_6d[5])
self.get_logger().info(
f" 🔧 末端坐标系: "
f"({pose_gripper_6d[0] * 1000:7.1f}, {pose_gripper_6d[1] * 1000:7.1f}, {pose_gripper_6d[2] * 1000:7.1f}) mm, "
f"姿态: ({roll_gripper:6.1f}°, {pitch_gripper:6.1f}°, {yaw_gripper:6.1f}°)"
)
# 基坐标系
if pose_base_6d is not None:
roll_base = np.degrees(pose_base_6d[3])
pitch_base = np.degrees(pose_base_6d[4])
yaw_base = np.degrees(pose_base_6d[5])
self.get_logger().info(
f" 🔵 基坐标系: "
f"({pose_base_6d[0] * 1000:7.1f}, {pose_base_6d[1] * 1000:7.1f}, {pose_base_6d[2] * 1000:7.1f}) mm, "
f"姿态: ({roll_base:6.1f}°, {pitch_base:6.1f}°, {yaw_base:6.1f}°)"
)
self.get_logger().info("=" * 70)
# =================================> 发布最终位姿 <=================================
if pose_base_6d is not None:
self.publish_final_pose(pose_base_6d, class_name, pose_mode)
# =================================> 保存PLY文件 <=================================
if ENABLE_SAVE_PLY and len(detection_results) > 0:
try:
for i, result in enumerate(detection_results):
filename = f"detection_{result.class_name}_{i}.ply"
self.save_ply(result.points_3d, result.colors,
os.path.join(SAVE_PLY_DIR, filename))
if result.inlier_cloud is not None:
inlier_points = np.asarray(result.inlier_cloud.points)
if len(inlier_points) > 0:
inlier_colors = np.full((len(inlier_points), 3), [0, 0, 255], dtype=np.uint8)
self.save_ply(inlier_points, inlier_colors,
os.path.join(SAVE_PLY_DIR, f"plane_inlier_{result.class_name}_{i}.ply"))
except Exception as e:
self.get_logger().error(f"❌ 保存PLY文件失败: {e}")
# 更新缓存
self.latest_detections = detection_results
# =================================> 发布合并点云 <=================================
if PUBLISH_POINTCLOUD and len(detection_results) > 0:
all_points = []
all_colors = []
for result in detection_results:
all_points.extend(result.points_3d)
all_colors.extend(result.colors)
if len(all_points) > 0:
all_points = np.array(all_points)
all_colors = np.array(all_colors)
points_base = self.transform_points_to_base(all_points)
if points_base is not None:
self.publish_pointcloud(points_base, all_colors)
else:
self.publish_pointcloud(all_points, all_colors)
end = time.time()
self.get_logger().info(
f"⏱️ 处理时间: {(end - start) * 1000:.1f} ms, 检测到 {len(detection_results)} 个目标"
)
def generate_pointcloud(self, rgb, depth, mask):
"""生成点云"""
h, w = rgb.shape[:2]
fx = self.K[0, 0]
fy = self.K[1, 1]
cx = self.K[0, 2]
cy = self.K[1, 2]
ys, xs = np.where(mask > 0)
if len(ys) < MIN_MASK_PIXELS:
return None, None
points_3d = []
colors = []
for v, u in zip(ys, xs):
z = depth[v, u]
if z <= 0 or z > DEPTH_MAX:
continue
x = (u - cx) * z / fx
y = (v - cy) * z / fy
b, g, r = rgb[v, u]
points_3d.append([x, y, z])
colors.append([r, g, b])
if len(points_3d) == 0:
return None, None
points_3d = np.array(points_3d, dtype=np.float32)
colors = np.array(colors, dtype=np.uint8)
if len(points_3d) > SAMPLE_MAX_POINTS:
indices = np.random.choice(len(points_3d), SAMPLE_MAX_POINTS, replace=False)
points_3d = points_3d[indices]
colors = colors[indices]
return points_3d, colors
def save_ply(self, points, colors, filename="detection.ply"):
"""保存PLY点云文件"""
if len(points) == 0:
return
try:
os.makedirs(os.path.dirname(os.path.abspath(filename)), exist_ok=True)
points = points.astype(np.float32)
colors = colors.astype(np.uint8)
with open(filename, 'w') as f:
f.write("ply\nformat ascii 1.0\n")
f.write(f"element vertex {len(points)}\n")
f.write("property float x\nproperty float y\nproperty float z\n")
f.write("property uchar red\nproperty uchar green\nproperty uchar blue\n")
f.write("end_header\n")
for i in range(len(points)):
x, y, z = points[i]
r, g, b = colors[i] if colors is not None else (255, 255, 255)
f.write(f"{x:.6f} {y:.6f} {z:.6f} {int(r)} {int(g)} {int(b)}\n")
self.get_logger().info(f"💾 点云已保存: {filename}")
except Exception as e:
self.get_logger().error(f"❌ 保存PLY失败: {filename} - {e}")
def publish_pointcloud(self, points, colors):
"""发布点云"""
if len(points) == 0:
return
try:
cloud_msg = PointCloud2()
cloud_msg.header = Header()
cloud_msg.header.stamp = self.get_clock().now().to_msg()
cloud_msg.header.frame_id = "base_link"
cloud_msg.height = 1
cloud_msg.width = len(points)
cloud_msg.is_bigendian = False
cloud_msg.is_dense = True
cloud_msg.fields = [
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
PointField(name='rgb', offset=12, datatype=PointField.UINT32, count=1),
]
cloud_msg.point_step = 16
cloud_msg.row_step = cloud_msg.point_step * len(points)
data = []
for pt, col in zip(points, colors):
rgb = (int(col[2]) << 16) | (int(col[1]) << 8) | int(col[0])
data.append(struct.pack('ffff', pt[0], pt[1], pt[2], float(rgb)))
cloud_msg.data = b''.join(data)
self.pointcloud_pub.publish(cloud_msg)
self.get_logger().info(f"☁️ 点云已发布: {len(points)} 个点")
except Exception as e:
self.get_logger().error(f"发布点云失败: {e}")
# ============================================================
# Action Server 执行回调
# ============================================================
def execute_callback(self, goal_handle: ServerGoalHandle):
"""执行回调 - 根据目标名称返回对应的位姿"""
self.get_logger().info("=" * 60)
self.get_logger().info("🔴 EXECUTE_CALLBACK 被调用")
self.get_logger().info("=" * 60)
goal = goal_handle.request
result = VisionDetection.Result()
try:
feedback = VisionDetection.Feedback()
feedback.task_status = "Processing..."
feedback.progress_rate = 0.0
feedback.current_step_info = "Initializing..."
detections = self.latest_detections
self.get_logger().info(f"📊 缓存状态: {len(detections)} 个目标")
if len(detections) == 0:
self.get_logger().error("❌ 没有可用数据")
result.success = False
result.status_code = 1
result.error_message = "没有可用的检测结果"
goal_handle.abort()
return result
feedback.progress_rate = 0.3
feedback.current_step_info = f"Processing {len(detections)} objects..."
goal_handle.publish_feedback(feedback)
result.success = True
result.status_code = 0
result.error_message = ""
result.execution_time = time.time() - self.last_process_time
result.header = Header()
result.header.stamp = self.get_clock().now().to_msg()
result.header.frame_id = "base_link"
# =================================> 🔑 根据目标名称查找对应的检测结果 <=================================
target_detection = None
target_name = goal.target_obj_name
if target_name != "":
# 精确匹配目标名称
for det in detections:
if det.class_name == target_name:
target_detection = det
self.get_logger().info(f"✅ 找到目标: {target_name}")
break
if target_detection is None:
self.get_logger().warn(
f"⚠️ 未找到目标 '{target_name}'，可用目标: {[d.class_name for d in detections]}")
# 返回错误信息，告诉客户端没有找到该目标
result.success = False
result.status_code = 2
result.error_message = f"未找到目标 '{target_name}'，可用目标: {[d.class_name for d in detections]}"
# result.detected_objects = [d.class_name for d in detections]
# result.num_objects = len(detections)
goal_handle.abort()
return result
else:
# 如果没有指定目标名称，使用第一个检测结果
target_detection = detections[0]
self.get_logger().info(f"📌 未指定目标，使用第一个: {target_detection.class_name}")
# =================================> 填充6D位姿 <=================================
if goal.need_6d_pose:
feedback.progress_rate = 0.6
feedback.current_step_info = "Creating pose message..."
goal_handle.publish_feedback(feedback)
result.target_6d_pose = PoseStamped()
result.target_6d_pose.header = Header()
result.target_6d_pose.header.stamp = self.get_clock().now().to_msg()
result.target_6d_pose.header.frame_id = "base_link"
if target_detection.pose_base is not None:
pose_base = target_detection.pose_base
result.target_6d_pose.pose.position.x = float(pose_base[0]) * M_TO_MM
result.target_6d_pose.pose.position.y = float(pose_base[1]) * M_TO_MM
result.target_6d_pose.pose.position.z = float(pose_base[2]) * M_TO_MM
quat = euler_zyx_to_quaternion(pose_base[5], pose_base[4], pose_base[3])
result.target_6d_pose.pose.orientation.x = quat[0]
result.target_6d_pose.pose.orientation.y = quat[1]
result.target_6d_pose.pose.orientation.z = quat[2]
result.target_6d_pose.pose.orientation.w = quat[3]
result.pose_confidence = target_detection.confidence
mode_names = {
"center": "物体中心",
"top_center": "顶面中心",
"bottom_center": "底面中心",
"custom": "自定义位置"
}
mode_name = mode_names.get(target_detection.pose_mode, target_detection.pose_mode)
self.get_logger().info(
f" ✅ 使用基坐标系位姿 ({target_detection.class_name}, {mode_name}): "
f"({pose_base[0] * M_TO_MM:.1f}, {pose_base[1] * M_TO_MM:.1f}, {pose_base[2] * M_TO_MM:.1f}) mm"
)
else:
centroid = np.mean(target_detection.points_3d, axis=0)
result.target_6d_pose.pose.position.x = float(centroid[0]) * M_TO_MM
result.target_6d_pose.pose.position.y = float(centroid[1]) * M_TO_MM
result.target_6d_pose.pose.position.z = float(centroid[2]) * M_TO_MM
result.target_6d_pose.pose.orientation.w = 1.0
result.pose_confidence = 0.5
self.get_logger().warn("⚠️ 使用降级方案 (相机坐标系位姿)")
# =================================> 填充点云 <=================================
if goal.need_env_point_cloud:
feedback.progress_rate = 0.8
feedback.current_step_info = "Creating point cloud..."
goal_handle.publish_feedback(feedback)
points_base = self.transform_points_to_base(target_detection.points_3d)
if points_base is not None:
cloud_frame = "base_link"
cloud_points = points_base
else:
cloud_frame = "camera_link"
cloud_points = target_detection.points_3d
self.get_logger().warn("⚠️ 使用相机坐标系点云")
result.env_point_cloud = create_point_cloud(
cloud_points,
target_detection.colors if target_detection.colors is not None
else np.ones((len(cloud_points), 3), dtype=np.uint8) * 255,
cloud_frame,
self.get_clock()
)
result.point_cloud_frame_id = cloud_frame
self.get_logger().info(f"☁️ 点云已创建，点数: {len(cloud_points)}, 坐标系: {cloud_frame}")
# =================================> 附加信息 <=================================
#result.detected_objects = [d.class_name for d in detections]
#result.num_objects = len(detections)
#self.get_logger().info(f"📦 检测到的目标: {result.detected_objects}")
feedback.progress_rate = 1.0
feedback.task_status = "Completed"
feedback.current_step_info = "Done"
goal_handle.publish_feedback(feedback)
self.get_logger().info("✅ 返回结果")
goal_handle.succeed()
return result
except Exception as e:
self.get_logger().error(f"❌ execute_callback 异常: {e}")
import traceback
traceback.print_exc()
result.success = False
result.status_code = 3
result.error_message = str(e)
goal_handle.abort()
return result
def __del__(self):
self.stop_visualization()
cv2.destroyAllWindows()
# ==========================
# 主函数
# ==========================
def main(args=None):
rclpy.init(args=args)
node = CameraDetectionNode()
executor = rclpy.executors.SingleThreadedExecutor()
executor.add_node(node)
try:
executor.spin()
except KeyboardInterrupt:
node.get_logger().info("接收到退出信号")
finally:
node.stop_visualization()
cv2.destroyAllWindows()
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()
该代码根据请求发布目标和位姿。
汇聚全球AI编程工具，助力开发者即刻编程。
更多推荐
5
0- 0
已为社区贡献3条内容
所有评论(0)