YOLO26 识别串番茄果梗

YOLO26 相比 YOLOv8/v11 最大优势：STAL 小目标感知标签分配 + ProgLoss 渐进损失 + 原生无 NMS 推理 ，天生适合细、像素占比极低的果梗；seg 分割头增强边界感知，pose 关键点头可以直接输出剪切点 + 果梗方向 ，完美匹配手眼标定、夹爪剪切、MoveIt 避障轨迹规划。 推荐路线：YOLO26-seg 优先做原型；真机联调阶段切换 YOLO26-pose。

一、3 种方案选型（按项目阶段）

1. YOLO26-det（检测框）：SDK 版本快速验证 类别：tomato、stem；只输出果梗 bbox。优点：标注快、训练快；缺点：只能拿到框中心，得不到果梗轮廓与角度，切点精度差，仅适合前期 Demo 验证。
2. YOLO26-seg（实例分割，推荐首选） 输出果梗 mask 掩码，后处理骨架提取得到果梗中心线、剪切点、果梗倾斜角度。 优势：YOLO26 分割头增加多尺度原型 + 语义辅助分支，细条状果梗掩码边缘更锐利，遮挡断裂 mask 修复效果优于 YOLOv8-seg；输出 mask 还可以直接喂给环境地图模块，用于 MoveIt 避障地图构建（对应 WBS：接收视觉拟合地图）CSDN博...。
3. YOLO26-pose（关键点，最终真机方案） 标注 2 个关键点：stem_root（果梗与果实连接点）、cut_point（剪切点，预留 3~5mm 远离果实）。模型直接回归 2D 像素坐标 + 可见度，省去骨架提取后处理，输出切点和果梗向量，直接换算 3D 目标位姿给机械臂。 适合：MoveIt 版本手眼避障联调，减少视觉推理延迟，时序更稳定。

二、数据集采集与标注（果梗识别成败核心）

1. 图像采集

相机：D435 / 海康 RGB-D；温室多角度采集：逆光、阴影、枝叶遮挡、果串密集重叠； 数量：1200~2000 张 ，必须保证大量部分遮挡果梗样本；train/val/test=7:2:1。

重点：果梗属于小目标，一张图中 stem 像素远少于 tomato，天然类别不平衡，YOLO26 的 ProgLoss 可以动态平衡大小目标损失，但数据集层面依然要做样本增强。

2. 标注规范

• seg：LabelMe 多边形标注完整果梗轮廓，不要只标一小段；
• pose：标注 2 个关键点：stem_root、cut_point；
• det：矩形框包裹整段果梗。

3. 数据增强（针对细梗）

开启：HSV 扰动、亮度 / 对比度、高斯模糊、随机裁剪、小目标放大增强； 可选：Mosaic、MixUp；不要过度缩放把细梗直接抹掉。

三、YOLO26 训练 yaml 配置（针对果梗小目标优化）

# tomato_stem.yaml
path: ./dataset
train: images/train
val: images/val
test: images/test

nc: 2
names: ['tomato','stem']

# YOLO26自带STAL小目标分配，不需要手动聚类anchor（YOLO26无anchor）
imgsz: 640
batch: 16
epochs: 150
optimizer: musgd  # YOLO26原生MuSGD，小目标收敛更稳
# 开启ProgLoss，动态平衡大果实、细果梗损失
loss: prog

训练命令：

from ultralytics import YOLO
model = YOLO("yolo26s-seg.pt") # 起步yolo26n-seg，精度不够换s/m
results = model.train(data="tomato_stem.yaml", imgsz=640, epochs=150, conf=0.4)

训练重点监控：stem 类 AP，而不是整体 mAP；果梗漏检是最大问题。

四、推理后处理流程（YOLO26-seg）

1. RGB 图像送入 YOLO26-seg，得到 stem 掩码 mask；YOLO26 支持nms=False端到端无 NMS 推理，推理抖动更小，适合工控机实时推理Ultralytic...
2. mask 二值化，形态学开 / 闭运算，滤除枝叶噪点；
3. Zhang-Suen 骨架提取，提取果梗单像素中心线；
4. 在骨架线上，从stem_root沿果梗向外偏移 3~5mm 像素距离 → cut_point 剪切点；
5. 骨架线拟合直线，得到果梗向量（夹爪剪切姿态角）；
6. 读取 D435 深度图，取 cut_point 邻域深度做插值修复；
7. 通过手眼标定齐次矩阵 ：像素 + 深度 → 机械臂基座坐标系(X,Y,Z,R,P,Y)；
8. 输出：
   • SDK 版本：给到动作集合，调用夹爪剪切（WBS SDK：采摘 Demo、夹爪剪切测试）
   • MoveIt 版本：ROS2 话题发布cut_pose、环境 mask 点云地图，用于避障规划（WBS MoveIt：接收视觉拟合地图、避障手眼联调）

YOLO26-seg 极简推理代码

from ultralytics import YOLO
import cv2
import numpy as np

model = YOLO("best_yolo26s-seg.pt")
img = cv2.imread("tomato_truss.jpg")
results = model(img, conf=0.4, nms=False) # YOLO26原生无NMS推理

for res in results:
    if res.masks is not None:
        for mask in res.masks.data:
            mask_np = mask.cpu().numpy()
            mask_np = (mask_np * 255).astype(np.uint8)
            cv2.imshow("stem_mask", mask_np)
            # 此处插入骨架提取、剪切点计算代码
cv2.waitKey(0)

五、YOLO26 识别果梗特有优势（对比 YOLOv8）

1. STAL 小目标感知标签分配：训练时提升细果梗这类极小目标正样本分配权重，不容易被大番茄样本压制，果梗召回率明显提升；
2. ProgLoss 渐进损失：训练前期学习大果实，后期聚焦果梗这类细结构，自动缓解类别不平衡；
3. MuSGD 优化器：收敛更稳定，小目标训练不容易震荡；
4. 无 NMS 端到端推理：推理时延稳定，没有 NMS 带来的随机延迟，机器人闭环控制非常关键；
5. 分割头增加边界感知监督，果梗这种细长结构掩码边缘更连续，减少 mask 断裂；
6. OBB 定向检测头可选：直接预测果梗旋转角度，也可用于果梗姿态获取。

六、工程坑点（采摘机器人实测）

1. 深度相机细梗深度失效：D435 在很细的果梗上会出现深度空洞；对策：剪切点邻域深度插值，或者取果梗中心线多点深度做均值。
2. 枝叶和果梗灰度颜色接近误检：YOLO26 提升特征，但仍会误检；后处理增加几何过滤：果梗是细长条状，计算 mask 轮廓长宽比，过滤块状叶子噪点。
3. 遮挡导致 mask 断裂：数据集大量加入遮挡样本；后处理对断裂骨架做线段拟合补全。
4. 光照漂移：相机固定曝光 + 白平衡；数据集覆盖强光、阴影。

七、对接你的 WBS 两个版本

✅ SDK 版本（原生机器人 SDK，无 MoveIt）

YOLO26（seg/det）部署在工控机，输出剪切点 3D 坐标 → 手眼标定矩阵转换基座坐标 → 调用 SDK 动作集合，依次执行趋近、夹持、剪切、复位。

对应 WBS 任务：手眼标定、动作集合标定、采摘 Demo、夹爪剪切测试、两套加减速、影响因素分析。

✅ MoveIt 版本（ROS2）

YOLO26 推荐pose 关键点版本 ，ROS2 节点发布话题：/cut_pose（剪切点位姿），同时把 mask 转点云发布环境地图；MoveIt 订阅目标位姿与环境地图，做带避障的轨迹规划，手眼联调。

对应 WBS 任务：基础功能包编写、仿真避障、真机联合调试、接收视觉拟合地图、避障手眼联调。

八、推荐开发顺序

1. YOLO26-det 快速标注，验证能不能检出果梗，快速跑通 SDK Demo；
2. 升级 YOLO26-seg，开发 mask + 骨架提取，拿到剪切点和果梗角度；
3. 稳定后，数据集标注关键点，切换 YOLO26-pose，去掉骨架后处理，降低推理延迟；
4. 封装 ROS2 节点，对接 MoveIt，完成手眼 + 避障真机联调。

YOLO26-Seg + 骨架提取 获取番茄果梗剪切点完整代码

功能说明：

1. YOLO26-seg 推理，提取 stem 果梗 mask
2. OpenCV 形态学降噪
3. Zhang-Suen 骨架细化算法提取果梗中心线
4. 骨架点拟合直线，得到果梗方向向量
5. 从靠近番茄一端沿着果梗向外偏移固定像素距离，输出剪切点 cut_point
6. 输出：剪切点像素坐标、果梗角度，可对接 D435 深度图做 3D 反投影、手眼标定

适配串果采摘机器人，可直接封装成 ROS2 节点

from ultralytics import YOLO
import cv2
import numpy as np

# ===================== Zhang-Suen 骨架细化算法 =====================
def zhang_suen_thinning(img: np.ndarray) -> np.ndarray:
    """
    二值图骨架提取，前景白色(255)，背景黑色(0)
    """
    img = img.copy()
    img[img > 0] = 1
    h, w = img.shape
    changed = True
    while changed:
        changed = False
        marker = np.zeros_like(img)
        # Step1
        for y in range(1, h - 1):
            for x in range(1, w - 1):
                p2 = img[y-1, x]
                p3 = img[y-1, x+1]
                p4 = img[y, x+1]
                p5 = img[y+1, x+1]
                p6 = img[y+1, x]
                p7 = img[y+1, x-1]
                p8 = img[y, x-1]
                p9 = img[y-1, x-1]
                neighbors = [p2,p3,p4,p5,p6,p7,p8,p9]
                non_zero = np.sum(neighbors)
                transitions = np.sum(np.roll(neighbors, -1) - neighbors == 1)
                if img[y,x]==1 and 2<=non_zero<=6 and transitions==1 and p2*p4*p6==0 and p4*p6*p8==0:
                    marker[y,x]=1
                    changed=True
        img[marker>0]=0
        marker = np.zeros_like(img)
        # Step2
        for y in range(1, h - 1):
            for x in range(1, w - 1):
                p2 = img[y-1, x]
                p3 = img[y-1, x+1]
                p4 = img[y, x+1]
                p5 = img[y+1, x+1]
                p6 = img[y+1, x]
                p7 = img[y+1, x-1]
                p8 = img[y, x-1]
                p9 = img[y-1, x-1]
                neighbors = [p2,p3,p4,p5,p6,p7,p8,p9]
                non_zero = np.sum(neighbors)
                transitions = np.sum(np.roll(neighbors, -1) - neighbors == 1)
                if img[y,x]==1 and 2<=non_zero<=6 and transitions==1 and p2*p4*p8==0 and p2*p6*p8==0:
                    marker[y,x]=1
                    changed=True
        img[marker>0]=0
    return (img * 255).astype(np.uint8)

# ===================== 果梗后处理：找剪切点 =====================
def get_stem_cut_point(mask: np.ndarray, offset_px: int = 8):
    """
    mask: stem二值掩码 0/255
    offset_px: 从果实连接处向外偏移多少像素作为剪切点
    return: cut_point (x,y), stem_angle(deg), skeleton_points
    """
    # 形态学去噪
    kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2, 2))
    mask_clean = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
    mask_clean = cv2.morphologyEx(mask_clean, cv2.MORPH_OPEN, kernel)

    # 骨架提取
    skeleton = zhang_suen_thinning(mask_clean)
    pts = np.argwhere(skeleton > 0)
    if len(pts) < 10:
        return None, None, skeleton

    # pts -> (x,y)
    pts = np.fliplr(pts)

    # 最小包围矩形，区分果梗两端
    rect = cv2.minAreaRect(pts)
    box = cv2.boxPoints(rect)
    # 直线拟合
    vx, vy, cx, cy = cv2.fitLine(pts, cv2.DIST_L2, 0, 0.01, 0.01)
    stem_vec = np.array([vx[0], vy[0]])
    stem_angle = np.rad2deg(np.arctan2(vy[0], vx[0]))

    # 距离聚类：一端靠近番茄，一端远离番茄
    dists = np.linalg.norm(pts - np.array([cx, cy]), axis=1)
    idx_min = np.argmin(dists)
    center_pt = pts[idx_min]
    # 沿着果梗向量向外偏移
    unit_v = stem_vec / np.linalg.norm(stem_vec)
    cut_point = center_pt + unit_v * offset_px
    cut_point = np.array([int(cut_point[0]), int(cut_point[1])])

    return cut_point, stem_angle, skeleton

# ===================== YOLO26推理主函数 =====================
def detect_tomato_stem(img_bgr: np.ndarray, model, conf_thresh=0.4):
    results = model(img_bgr, conf=conf_thresh, nms=False)
    img_draw = img_bgr.copy()

    for res in results:
        if res.masks is None:
            continue
        # 遍历所有mask
        for idx, mask_data in enumerate(res.masks.data):
            cls_id = int(res.boxes.cls[idx])
            cls_name = res.names[cls_id]
            if cls_name != "stem":
                continue
            # mask还原到原图尺寸
            mask_np = mask_data.cpu().numpy()
            mask_np = (mask_np * 255).astype(np.uint8)
            mask_np = cv2.resize(mask_np, (img_bgr.shape[1], img_bgr.shape[0]))

            cut_pt, angle, skeleton = get_stem_cut_point(mask_np, offset_px=8)
            if cut_pt is None:
                continue

            # ===== 可视化 =====
            cv2.circle(img_draw, cut_pt, 4, (0,0,255), -1) # 红色剪切点
            cv2.putText(img_draw, f"cut({cut_pt[0]},{cut_pt[1]}) ang:{angle:.1f}",
                        (cut_pt[0]+10, cut_pt[1]), cv2.FONT_HERSHEY_SIMPLEX,0.4,(0,0,255),1)
            sk_color = cv2.cvtColor(skeleton, cv2.COLOR_GRAY2BGR)
            img_draw = cv2.addWeighted(img_draw, 1.0, sk_color, 0.5, 0)
            print(f"【果梗】剪切点像素坐标 x={cut_pt[0]}, y={cut_pt[1]}, 果梗角度={angle:.2f} °")
    return img_draw

if __name__ == "__main__":
    # 加载YOLO26-seg模型，替换成你的best权重
    model = YOLO("yolo26s-seg.pt")
    img = cv2.imread("tomato_stem.jpg")

    vis_img = detect_tomato_stem(img, model, conf_thresh=0.4)
    cv2.imshow("stem_detect_result", vis_img)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

下一步对接 D435 深度相机，获取剪切点 3D 坐标（补充代码片段）

# depth_img: D435深度图，单位mm，与RGB图像已配准
def get_3d_cut_point(cut_px, depth_img, intrinsic):
    u, v = cut_px
    depth = depth_img[v, u]
    if depth <= 0:
        # 深度空洞：邻域均值修复
        patch = depth_img[max(0,v-3):v+4, max(0,u-3):u+4]
        depth = np.mean(patch[patch>0])
    fx, fy, cx, cy = intrinsic
    z = depth / 1000.0
    x = (u - cx) * z / fx
    y = (v - cy) * z / fy
    return np.array([x,y,z]) # 相机坐标系下3D点

拿到相机坐标系 3D 点后，代入手眼标定齐次矩阵 T_base_cam：

\(P_{base}=T_{base\cam} \cdot P{cam}\) 得到机械臂基座坐标系目标点位，给到 SDK 动作集合或者 MoveIt。

参数调优说明

1. offset_px = 8：像素偏移量，对应物理距离由相机内参决定；D435 在 0.6~0.8m 采摘距离，8 像素≈3~5mm，刚好预留剪切安全距离，可按需修改
2. conf_thresh=0.4：果梗小目标置信度不要设太高，否则容易漏检；现场可 0.3~0.45 之间调
3. 形态学核(2,2)：如果枝叶噪点多，可放大到 (3,3)，但会丢失细梗细节

工程适配提示（对应你的 WBS）

• SDK 版本：直接运行此代码，输出基座坐标，调用机器人 SDK 动作集合，完成采摘 Demo
• MoveIt ROS2 版本：把代码封装成 ROS2 节点，发布话题 cut_pose（geometry_msgs/PoseStamped），同时 mask 转点云发布环境地图，用于避障规划

可选改进点

1. 多个果梗同时出现时增加 IoU 过滤，只取离番茄最近的果梗
2. 骨架点做 RANSAC 直线拟合，增强抗遮挡能力
3. 增加长宽比过滤，剔除细长叶片误检 mask

YOLO26-Seg ROS2 Humble 节点：番茄果梗识别，输出剪切点 PoseStamped

功能：

1. 订阅 RGB 图像话题（/camera/rgb/image_raw）
2. YOLO26-seg 推理得到果梗 mask → 骨架提取 → 计算剪切点像素、果梗角度
3. 订阅相机内参 camera_info，结合 D435 对齐后的深度图 /camera/aligned_depth_to_color/image_raw
4. 像素点反投影得到相机坐标系 3D 点
5. 发布 geometry_msgs/PoseStamped：/stem/cut_pose（相机坐标系下剪切点位姿，z 轴沿果梗方向，方便夹爪）
6. 可选：发布可视化标记 /stem/marker（RViz 看切点）

适配：MoveIt 版本，MoveIt 订阅 /stem/cut_pose 作为采摘目标； 也可单独提取 3D 坐标给 SDK 版本做 Demo。

package 结构

tomato_stem_detector/
├── tomato_stem_detector/
│   ├── __init__.py
│   ├── stem_detector_node.py
├── config/
│   └── params.yaml
├── launch/
│   └── detector.launch.py
├── package.xml
└── setup.py

1. stem_detector_node.py

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CameraInfo
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
from visualization_msgs.msg import Marker
from cv_bridge import CvBridge, CvBridgeError
import cv2
import numpy as np
from ultralytics import YOLO
from scipy.spatial.transform import Rotation

# ===================== Zhang-Suen 骨架细化 =====================
def zhang_suen_thinning(img: np.ndarray) -> np.ndarray:
    img = img.copy()
    img[img > 0] = 1
    h, w = img.shape
    changed = True
    while changed:
        changed = False
        marker = np.zeros_like(img)
        # Step 1
        for y in range(1, h - 1):
            for x in range(1, w - 1):
                p2,p3,p4,p5,p6,p7,p8,p9 = img[y-1,x],img[y-1,x+1],img[y,x+1],img[y+1,x+1],img[y+1,x],img[y+1,x-1],img[y,x-1],img[y-1,x-1]
                neighbors = [p2,p3,p4,p5,p6,p7,p8,p9]
                non_zero = np.sum(neighbors)
                transitions = np.sum(np.roll(neighbors, -1) - neighbors == 1)
                if img[y,x]==1 and 2<=non_zero<=6 and transitions==1 and p2*p4*p6==0 and p4*p6*p8==0:
                    marker[y,x]=1
                    changed=True
        img[marker>0]=0
        marker = np.zeros_like(img)
        # Step2
        for y in range(1, h - 1):
            for x in range(1, w - 1):
                p2,p3,p4,p5,p6,p7,p8,p9 = img[y-1,x],img[y-1,x+1],img[y,x+1],img[y+1,x+1],img[y+1,x],img[y+1,x-1],img[y,x-1],img[y-1,x-1]
                neighbors = [p2,p3,p4,p5,p6,p7,p8,p9]
                non_zero = np.sum(neighbors)
                transitions = np.sum(np.roll(neighbors, -1) - neighbors == 1)
                if img[y,x]==1 and 2<=non_zero<=6 and transitions==1 and p2*p4*p8==0 and p2*p6*p8==0:
                    marker[y,x]=1
                    changed=True
        img[marker>0]=0
    return (img * 255).astype(np.uint8)

def get_stem_cut_point(mask: np.ndarray, offset_px: int = 8):
    kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2,2))
    mask_clean = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
    mask_clean = cv2.morphologyEx(mask_clean, cv2.MORPH_OPEN, kernel)
    skeleton = zhang_suen_thinning(mask_clean)
    pts = np.argwhere(skeleton>0)
    if len(pts) < 10:
        return None, None, skeleton
    pts = np.fliplr(pts)
    vx, vy, cx, cy = cv2.fitLine(pts, cv2.DIST_L2, 0, 0.01, 0.01)
    stem_vec = np.array([vx[0], vy[0]])
    stem_angle = np.rad2deg(np.arctan2(vy[0], vx[0]))
    dists = np.linalg.norm(pts - np.array([cx, cy]), axis=1)
    idx_min = np.argmin(dists)
    center_pt = pts[idx_min]
    unit_v = stem_vec / np.linalg.norm(stem_vec)
    cut_point = center_pt + unit_v * offset_px
    cut_point = np.array([int(cut_point[0]), int(cut_point[1])])
    return cut_point, stem_angle, skeleton

class StemDetectorNode(Node):
    def __init__(self):
        super().__init__("tomato_stem_detector")
        self.declare_parameter("model_path", "yolo26s-seg.pt")
        self.declare_parameter("conf_thresh", 0.4)
        self.declare_parameter("offset_px", 8)
        self.declare_parameter("frame_id", "camera_color_optical_frame")

        self.model_path = self.get_parameter("model_path").value
        self.conf_thresh = self.get_parameter("conf_thresh").value
        self.offset_px = self.get_parameter("offset_px").value
        self.frame_id = self.get_parameter("frame_id").value

        self.bridge = CvBridge()
        self.model = YOLO(self.model_path)

        self.camera_info = None
        self.fx = self.fy = self.cx = self.cy = None

        self.sub_rgb = self.create_subscription(Image, "/camera/rgb/image_raw", self.rgb_callback, 10)
        self.sub_depth = self.create_subscription(Image, "/camera/aligned_depth_to_color/image_raw", self.depth_callback, 10)
        self.sub_cam_info = self.create_subscription(CameraInfo, "/camera/rgb/camera_info", self.cam_info_callback, 10)

        self.pub_cut_pose = self.create_publisher(PoseStamped, "/stem/cut_pose", 10)
        self.pub_marker = self.create_publisher(Marker, "/stem/marker", 10)

        self.depth_img_cv = None
        self.get_logger().info("YOLO26 Stem Detector Node Started")

    def cam_info_callback(self, msg: CameraInfo):
        if self.camera_info is None:
            self.camera_info = msg
            self.fx = msg.k[0]
            self.fy = msg.k[4]
            self.cx = msg.k[2]
            self.cy = msg.k[5]
            self.get_logger().info(f"Camera intrinsic loaded fx:{self.fx:.2f}")

    def depth_callback(self, msg: Image):
        try:
            self.depth_img_cv = self.bridge.imgmsg_to_cv2(msg, desired_encoding="16UC1")
        except CvBridgeError as e:
            self.get_logger().error(f"Depth bridge error: {e}")

    def rgb_callback(self, msg: Image):
        if self.camera_info is None or self.depth_img_cv is None:
            return
        try:
            img_bgr = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
        except CvBridgeError as e:
            self.get_logger().error(f"RGB bridge error: {e}")
            return

        results = self.model(img_bgr, conf=self.conf_thresh, nms=False)
        for res in results:
            if res.masks is None:
                continue
            for idx, mask_data in enumerate(res.masks.data):
                cls_id = int(res.boxes.cls[idx])
                cls_name = res.names[cls_id]
                if cls_name != "stem":
                    continue
                mask_np = mask_data.cpu().numpy()
                mask_np = (mask_np * 255).astype(np.uint8)
                mask_np = cv2.resize(mask_np, (img_bgr.shape[1], img_bgr.shape[0]))
                cut_pt, angle, skeleton = get_stem_cut_point(mask_np, offset_px=self.offset_px)
                if cut_pt is None:
                    continue
                u, v = cut_pt
                # Read depth with hole repair
                depth_val = self.depth_img_cv[v, u]
                if depth_val <= 0:
                    patch = self.depth_img_cv[max(0, v-3):v+4, max(0, u-3):u+4]
                    valid = patch[patch>0]
                    if len(valid) == 0:
                        continue
                    depth_val = np.mean(valid)
                z = depth_val / 1000.0
                x = (u - self.cx) * z / self.fx
                y = (v - self.cy) * z / self.fy

                # 构建姿态：原点(x,y,z), z轴沿果梗方向
                stem_rad = np.deg2rad(angle)
                # 果梗方向向量
                dir_vec = np.array([np.cos(stem_rad), np.sin(stem_rad), 0])
                rot = Rotation.from_rotvec(dir_vec)
                q = rot.as_quat() # x,y,z,w

                pose_stamped = PoseStamped()
                pose_stamped.header.stamp = self.get_clock().now().to_msg()
                pose_stamped.header.frame_id = self.frame_id
                pose_stamped.pose.position = Point(x=x, y=y, z=z)
                pose_stamped.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
                self.pub_cut_pose.publish(pose_stamped)

                # RViz marker
                marker = Marker()
                marker.header = pose_stamped.header
                marker.ns = "stem_cut_point"
                marker.id = idx
                marker.type = Marker.SPHERE
                marker.action = Marker.ADD
                marker.pose = pose_stamped.pose
                marker.scale.x = marker.scale.y = marker.scale.z = 0.005
                marker.color.r = 1.0
                marker.color.g = 0.0
                marker.color.b = 0.0
                marker.color.a = 1.0
                self.pub_marker.publish(marker)
                self.get_logger().info(f"Cut point cam frame: x={x:.3f}, y={y:.3f}, z={z:.3f}, angle={angle:.2f}")

def main(args=None):
    rclpy.init(args=args)
    node = StemDetectorNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    main()

2. params.yaml

tomato_stem_detector:
  ros__parameters:
    model_path: "yolo26s-seg.pt"
    conf_thresh: 0.4
    offset_px: 8
    frame_id: "camera_color_optical_frame"

3. detector.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    params_file = PathJoinSubstitution([
        FindPackageShare("tomato_stem_detector"),
        "config",
        "params.yaml"
    ])
    node = Node(
        package="tomato_stem_detector",
        executable="stem_detector_node",
        name="tomato_stem_detector",
        parameters=[params_file],
        output="screen"
    )
    return LaunchDescription([node])

4. package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>tomato_stem_detector</name>
  <version>0.0.0</version>
  <description>YOLO26-seg tomato stem detector for picking robot</description>
  <maintainer email="xxx@xxx.com">xxx</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_python</buildtool_depend>
  <build_depend>rclpy</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>visualization_msgs</build_depend>
  <build_depend>cv_bridge</build_depend>
  <build_depend>numpy</build_depend>
  <build_depend>scipy</build_depend>
  <build_depend>ultralytics</build_depend>

  <exec_depend>rclpy</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>visualization_msgs</exec_depend>
  <exec_depend>cv_bridge</exec_depend>
  <exec_depend>numpy</exec_depend>
  <exec_depend>scipy</exec_depend>
  <exec_depend>ultralytics</exec_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
  <export>
    <build_type>ament_python</build_type>
  </export>
</package>

5. setup.py

from setuptools import find_packages, setup
import os
from glob import glob

package_name = "tomato_stem_detector"
setup(
    name=package_name,
    version="0.0.0",
    packages=find_packages(exclude=["test"]),
    data_files=[
        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
        ("share/" + package_name, ["package.xml"]),
        (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")),
        (os.path.join("share", package_name, "config"), glob("config/*.yaml")),
    ],
    install_requires=["setuptools"],
    zip_safe=True,
    maintainer="xxx",
    maintainer_email="xxx@xxx.com",
    description="YOLO26 stem detector",
    license="Apache-2.0",
    tests_require=["pytest"],
    entry_points={
        "console_scripts": [
            "stem_detector_node = tomato_stem_detector.stem_detector_node:main",
        ],
    },
)

编译 & 运行

colcon build --packages-select tomato_stem_detector
source install/setup.bash
ros2 launch tomato_stem_detector detector.launch.py

话题列表

表格

Topic	Type	说明
/stem/cut_pose	PoseStamped	相机坐标系剪切点位姿，MoveIt 直接订阅作为目标
/stem/marker	visualization_msgs/Marker	RViz 可视化红色切点小球

和 MoveIt 对接要点

1. TF 树：需要 camera_color_optical_frame → base_link 的 TF（手眼标定输出的静态 TF）
2. MoveIt 接收 /stem/cut_pose，调用 move_group.set_pose_target()，规划带避障轨迹
3. 多果梗场景：当前代码只处理检测到的每一个 stem，你可以加逻辑只保留距离相机最近的果梗，避免多目标冲突

可选扩展（对应 WBS MoveIt：接收视觉拟合地图）

如果你需要把 mask 转为点云发布作为环境地图给 MoveIt 避障，我可以再加一段代码：把 stem mask + 深度图生成 sensor_msgs/PointCloud2，发布 /stem/obstacle_cloud。