前言：

    从2025.7.20。开始做这个项目，一直做到9月10号。期间遇到的报错实在太多太多，一个人做一个项目终究还是太难太耗费时间了。当然，网上也有很多关于这方面的教程，但是我觉得都不是特别全、特别详细，不是很适合新手。我对这个项目做了全套的总结，对涉及到的步骤都罗列了出来，难点也做了详细解释，对于我所有的源码都贴上来了，做了详细的讲解。如果你是新手，也正需要做yolo识别，moveit机械臂夹取的，详细看完不说100%能做出，但肯定有非常大的帮助。

         注：我下面所说的面积判断羽毛球球头方向的方法效果非常差，因为每次夹取的位置可能会有偏差，大家如果还需要进一步判断目标的方向，可以使用yolo-obb加关键点的方法，这个方法我感觉是最好的，但我没有测试过。方法的链接贴出来了，大家可以自行测试。如果效果好跟我说一声哈哈

https://blog.csdn.net/qq_39128381/article/details/141326644

     环境：

         ubuntu：20.04

         ros：noetic

     （1）概述：

            ①项目内容：

             这个项目是通过yolov11s-obb模型识别羽毛球的位置和朝向，然后通过moveit机械臂抓取羽毛球，最后放置在球筒内。

            ②涉及主要知识点：

             这里主要的点分为两个部分，第一部分是识别部分，第二是抓取部分。对于前者，主要是数据集的标注需要全部标注成长方形、像素坐标系转换成相机坐标系、像素图像中的旋转转换成实际的旋转；对于后者主要是需要配置IKFast求解器，其次要明确机械臂抓取的角度范围，最后就是两者的代码。下面就对这几个部分进行详细讲解

            ③整体流程：

            ·yolo的使用，包括模型下载、数据集标注、训练等（这个的教程就很成熟了，大家可以去网上找）

           ·给机器人多配置一个机械臂组（如果有6轴的机械臂组就不需要了）

           ·确保可以moveit控制机械臂

           ·配置IKFast

           ·明确机械臂夹取的角度范围。

           · 将yolo通过ultralytics_ros集成到ros中

          ·自定义msg类型消息，装每次识别的多个位姿

          ·将像素坐标系xyz和角度转换到相机坐标系

          ·通过话题将位姿发布给抓取节点

          ·抓取节点拿到位姿，变换到bast_footprint下

          ·通过笛卡尔坐标系驱动抓取

     （2）数据集标注：

            ①前置知识：

              yolo模型会将不规则的四边边型通过外接矩形的方法选取能外接的最小面积的矩形。

            ②说明：

              数据集的标注要确保每一个框在模型处理之后依旧完完全全是长方形，不然的话模型会找不到长边（正方形），这时候就没有宽大于高或者宽小于高这么一说，输出的r就直接是最后的r了。这样会不准。

     （3）像素坐标系转相机坐标系：

            ①说明：

              像素坐标系转相机坐标系包括两个部分，一个是xyz，一个是角度。对于前者，需要使用到像素坐标系坐标（u, v）、深度信息Zc、相机内参K。对于后者拿到模型的输出的r后处理即可。这里还需要注意内参对齐问题，具体见下面

            ②xyz：

              对于相机坐标系来说，Z朝前，X朝右，Y朝下；对于像素坐标系来说，X朝右，Y朝下。由于针孔成像原理，他们的X Y的平面是平行的，因此可以通过换算（相机内参）直接得出（上面有公式），但是相机到目标的水平距离，即Z轴方向的距离不能直接得出，需要测量出来才可以，这里的Z就是Zc，也就是深度摄像头中测量出来的水平距离了。下面就是根据公式写的具体代码实现

             注：深度图像中测的水平是目标点在Z轴上面的投影，只有相机朝向完全和Z轴平行，才是真正的水平距离。

            ③角度：

yolov8旋转目标检测输出的角度转化为适合机械爪抓取的角度（包含代码实现）_yolo 目标识别带角度-CSDN博客

            ·说明：

              模型会输出一个r，这个r并不是简单地绕某个轴旋转。它是以X轴顺时针旋转，打到框的哪个角（具体大家可以看上面的博客），那个角右边的边定义为宽（另一条是高），宽与X轴的夹角就是r。这里要分两种情况，看指定的宽和高哪条长，一般来说：当框大于高时，适合机械臂抓取的角度是-θ；当宽小于高，适合机械臂抓取的角度是90-θ。 但是，不同的机器人机械臂抓取角度范围不同，识别的输出也可以不同。因此这里需要基于一般理论自己去测试。下面是自己的处理过程。在拿到处理后的r后，就可以直接拿x、y旋转角度为0，z旋转角度r这个欧拉角转成四元数去使用了。

           ·自己的机械臂：

             当框朝向右边时（包括180），宽小于高，这时候直接输出的r就直接θ

             当框朝向左边时（包括180），宽大于高，这时候是r是（π/2 – θ）再取负。

            ④深度图与RGB对齐问题：

            ·说明：

              Yolo模型使用的是RGB图像，因此像素坐标系也是相对于RGB图像的，两不同的图像对应着不同的内参话题，不同的内参参数。 因为Zc是相对于深度图像的，在很多深度摄像头中，深度图像和RGB图像是不对齐的，即目标在RGB中可能在左上角，但是在深度图像中却在中间。这样取到的Zc就不是真正RGB中测量出来的距离，因此在求Zc之前，需要先确保两图像像素对齐

            ·注：

              有一些摄像头除了这两图像话题外，还会有一个发布好的对齐RGB的深度图，一般为‘aligned_depth_to_color’

   

            ·判断是否对齐：

              查看两图像对应的内参发布话题的参考坐标系，如果不一致，那就是不对齐，如果一致，那就是对齐了。比如：发布的对齐好之后的RGB深度图的参考坐标系会和RGB参考坐标系一致。

              另外，也可以查看一下两者的内参，如果对齐的话，一般是一样的

            ⑤补充 - 验证内参正确性：

            ·说明：

             这里的内参是相机内参，其获取有两种，一种是手动棋盘标定，另外一种是相机内置话题。后者一般都是不会错误，前者的话可以通过下面两种方法来验证标定是否正确，这两种方法都是围绕像素坐标系到图像坐标系来展开的， 下面是具体讲解。

            ·中点法：

             ·方法描述：

                 取像素坐标系的中点，映射到图像坐标系中，看是否接近零。如果是查几cm，一般对应的就是十几像素，都是正常的（具体像素可以问GPT，用获取到的数值问）

             ·原理：

                 因为像素坐标系原点在左上角，图像坐标系原点在中间，将像素坐标中点映射到图像的中点，那个点会很接近图像坐标系的0

             ·公式：

                  使用的公式就是上面像素坐标系到图像坐标系

            

             ·示例：

                  cx、cy就是像素坐标系取的点，self.cx 、self.cy就是相机内参，对应u0、v0

            ·四点法：

             ·方法描述：

                 在像素坐标系上取图像的四个脚点（0，0）、（0，H）、（W，0）、（W，H）。然后分别映射（映射公式跟上面一样）到图像坐标系中，然后查看左上角的（0,0）点是否和右上角的（W，0）点X轴对称，是否和左下角Y轴对称。除了对称性外， 还可以检查数量级，这个让GPT看

             ·原理：

                 因为像素坐标系原点在左上角，图像坐标系原点在中间，像素坐标系中四个点映射过去之后，左边的是负的，上方的也是负的，借助这个来看对称性。

     （4）IKFast配置：

             IKFast的配置网上有很多教程，需要根据自己的实际情况来。如果实在配置不成功，也可以在评论区或者私信问我。

     （5）识别代码讲解：

            ①前置知识：****

              识别代码是基于ultralytics_ros包的tracker_node.py文件改写的，这个包可以将yolo模型集成到ros中，只需要一个yolo模型的权重文件即可。集成进去之后可以实时地进行目标检测。安装路径如下：GitHub - Alpaca-zip/ultralytics_ros：用于Ultralytics YOLOv8 实时对象检测和分割的 ROS/ROS 2 软件包。https://github.com/ultralytics/ultralytics

            ②代码概述：

              首先传入模型识别必要参数，订阅深度、rgb图像等话题信息，接着对rgb图像进行识别，通过深度、相机内参、识别目标返回等信息求目标的位姿，再将位姿通过话题发布出去，其中，在识别到目标后，对目标将框信息标注图像上，也是通过话题发布出去，方便可视化。下面是分开讲解。

            ③自定义msg消息：

            ·说明：

              对于框的输出位姿信息，自己定义一个简单的msg消息去接受会更简便。因为图像中会有多个目标，会框出多个目标，因此可以定义一个位姿列表，给每个目标一个id即可。

            ·定义基础位姿：

            

            ·定义位姿列表：

            

            ④接收参数、初始化、订阅等：

              首先依旧是导入对应包、类创建、节点初始化。接着在类初始化函数中接收launch文件传来的必要模型识别等参数，

            ⑤回调函数进行深度处理：

              订阅深度图像和相机的内参话题的原因分别是根据深度图像格式拿到深度值单位，并且拿到深度图像；通过内参话题，拿到内参矩阵K。

            ⑥回调函数内对rgb图像进行识别：

              这里主要使用ultralytics_ros包中的模型track函数进行识别，该函数是实时识别并返回对应信息的。在拿到这个目标信息后，分别调用位姿处理函数和图像可视化函数进行处理，最后返回对应两个结果，通过话题通信进行发布。

            ⑦位姿处理函数返回位姿：

            ·整个函数概述：

              这里就是拿到输出的框信息，主要在obb中。对信息进行抽取，拿到每个框的cx、cy、w、h、r。通过角度判断拿到真正相对于相机坐标系z轴的旋转角度。根据cx、cy拿到这个点/附近的深度值，这样就有了cx、cy、Zc。然后通过像素坐标系到相机坐标系的反投影就拿到相机坐标系的值了。最后将这些值组织一下消息内容，最后返回一个位姿列表，每个元素就是一个目标的位姿。

            ·获取深度函数：

              这里主要是根据cx、cy这个像素坐标系的坐标进行深度值获取。分两种情况，一种是指定win（用矩阵），一种是直接获取cy、cy点坐标的（为什么用矩阵见下面的7-②）。用矩阵的话，就以cx、cy为基准，扩展出矩阵，然后选出矩阵中深度值的中位数。

            ·反投影函数

            ⑧可视化检测结果返回处理后图像：

              也是拿到图像的输出obb框信息，根据框的cx、cy、w、h、r信息推算框四个点具体位置，然后画在图像上。还有置信度、id号等也可以选择性画上去。但一般不用

     （6）抓取代码讲解：

            ①代码概述：

            ·概述：

              首先通过关节空间运动规划驱动机械臂去到指定识别位置进行目标识别，识别后通过话题接收目标位姿列表。接着，将列表每个元素拿出来，做坐标变换、补偿、旋转处理，最后返回的依旧是位姿，拿着这个位姿给到笛卡尔运动规划驱动机械臂抓取目标。然后通过面积判断，是球头朝上还是球毛朝上，最后将羽毛球放到球筒中。

            ·机械臂运动流程：

              初始化 ---- 指定识别位置 ---- 抓取位置 ---- 抓取（关闭夹抓） ---- 判断面积位置（上移） ---- 球筒上方 ---- 球筒下方（按压） ---- 放置（打开夹抓） ---- 判断面积位置 ---- 继续抓取、放置

            ②导包、初始化、订阅：

              这里的主要调用代码是在 if __name__ == “__main__”下。但是这里的初始化比较多，因为涉及到机械臂的一些移动时间睡眠、补偿、关节位置指定等。这里的类初始化函数中就仅仅是做节点初始化、订阅位姿话题而已。

            ③回调函数对位姿进行格式转换：

              因为发布过来的是二维列表，怕下面调用解包时顺序错乱，这里接收后，通过一维列表嵌套字典的格式存储。

         ④各函数实现：

            ·说明：

              在总调用中，来来回回调用的都是那几个函数，主要是位置函数，这里就将函数全部贴出来了

            ·笛卡尔实现：

              使用笛卡尔驱动机械臂调用的函数。直接传入位姿即可。大家需要根据自己封装的笛卡尔函数来

            ·保底笛卡尔实现：

              这个是保底是也是笛卡尔运动规划，但是没有姿态，只有xyz。这样如果姿态驱动不过去，也能通过xyz驱动过去。大家需要根据自己封装的笛卡尔函数来

            ·坐标变换：

              这个就是将摄像头的位姿变换到机器人的基坐标系中，这样才能使用笛卡尔坐标系驱动机械臂。这里的变换函数也需要按照自己的坐标系名称来填写。

            ·夹抓控制；

              这个是夹抓的控制，直接调用，加个延时即可。同样，根据自己函数来

            ·机械臂关节直接控制：

              这个同样也是下面关节控制所需要的。同样，根据自己函数来

            ·获取机械臂当前关节位置：

              这个主要是下面关节控制函数所需要的。同样，根据自己函数来

            ·封装的关节控制：

              因为很多移动都是关节驱动，所需要的时间也差不多，有时候运动的起始位置和目标位置还是一样的，因此这里就做了一个简单的封装，先进行位置判断，然后驱动。这样直接传位置、时间进来进行。同样，根据自己函数来

            ④面积处理两函数：

            ·说明：

              这两函数没有跟上面函数写在一个类里面而是单独出来的函数。这两个函数主要是拿到rgb图像后，通过边缘检测处理，计算白点（边缘）与图像底边面积来判断球头朝上还是球毛朝上的。鲁棒性非常差。

            ·面积计算：

              传入的是经过边缘检测后的图像，就直接获取所有白色像素的位置，然后计算该位置和图像底边的距离，并返回面积等信息。

            ·图像边缘处理及判断：

              这个是主要的回调函数，拿到rgb图像后，先对图像进行裁剪，裁剪出羽毛球会出现的区域（整幅图像的右下角），接着，转灰度图、做边缘检测、计算面积，最后通过设置阈值判断球头方向。

            ⑤主要调用：

            ·说明：

              在将所有函数都定义好之后，就可以在主程序中调用了。下面分几个部分进行讲解。

            ·运动到识别位置并处理：

              首先进行类、位置的初始化，接着就运动到识别位置等待识别节点识别并接收返回结果（回调函数持续接收）。确保有位姿后，拿着这个结果做坐标变换、补偿、旋转处理

            ·循环 – 抓取：

              因为有多个目标，因此在拿到位姿后，需要循环一个个抓取。对每一个目标通过笛卡尔坐标系运动到指定位置，关闭夹抓、再运动到判断面积位置。

            ·循环 – 判断球头方向并放置：

              通过面积计算得到球头方向，然后也是通过关节控制将机械臂移动到球筒位置并放置

            ·二次抓取准备：

              因为放置之后考球筒比较近，直接二次抓取会打到球筒，因此再次移动到判断面积位置（球筒左上方），又放置时需要将夹抓张开到最大，但是如果夹抓张开到最大可能会碰到目标旁边的球，因此需要先调会到开始抓取大小。

     （7）经验（注意点）：

            ①笛卡尔坐标系抓取注意点：

            ·说明：

              在识别完目标之后，不要运动到最初始位置，或者说不要距离目标太远，机械臂和目标抓取姿态差别太大。应该选择距离目标近、姿态与抓取姿态相似、任何关节都没有接近限位的姿态作为准备抓取姿态。

            ·举例：

              在机械臂往下运动到识别位置，识别完之后，可以保持机械臂末端朝向不变，将他往上移动一点作为抓取初始姿态即可。

            ②区域取深度目标：

              对于yolo识别出来的目标，特别是有向框，如果取框中间像素的深度，经常会有很大的误差，因为直接取那一点，有可能识别不准，有可能是插值什么的。有误差的话，转换到base_footprint之后高的可以达到16cm的误差。所以，在取深度时，可以以那个中点为中心，延伸一个像素矩阵一般是3 * 3、5 * 5等（理论上不超过框的大小都可以，但一般来说3 * 3是最好的，既能选到合适的点，也能降低误差）。然后取这个矩形的中位数，这样是最稳的。因为这个矩阵是在框里面的，怎么取都不会偏差很多，但可以很有效避免识别不准，插值等问题。

            ③抓取姿态旋转问题：

              对于机械臂的抓取，只要识别的目标xyz正确，然后朝向是朝向前（目标没有坐标系不用管），都可以正确通过xyz + 四元数的方式去驱动机械臂达到目标位置。就是经常会出现夹抓竖着夹的情况，这时候，如果是目标有坐标系的就根据ros笔记中的ar码抓取原则对齐，没有坐标系的话需要自己尝试，一般都是绕z轴转个90°就行。具体可以做的时候调，比如-90度、绕x、y轴等等。

（8）识别、抓取完整代码：

      ①识别：

          这里贴的是launch文件对应的python文件

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import cv_bridge  # cv格式与ros格式互转库
import numpy as np
import roslib.packages
import rospy
from sensor_msgs.msg import  Image as ImageMsg, CameraInfo
from ultralytics import YOLO
from ultralytics_ros.msg import glab_pose, glab_pose_array  # 自己创建的偏移量 + 四元数类
import cv2, math
import tf.transformations as tfs


class TrackerNode:
    def __init__(self):
        yolo_model = rospy.get_param("~yolo_model", "yolov8n.pt")
        self.input_topic = rospy.get_param("~input_topic", "image_raw")
        self.result_topic = rospy.get_param("~result_topic", "yolo_result")
        self.result_image_topic = rospy.get_param("~result_image_topic", "yolo_image")
        self.conf_thres = rospy.get_param("~conf_thres", 0.25)
        self.iou_thres = rospy.get_param("~iou_thres", 0.45)
        self.max_det = rospy.get_param("~max_det", 300)
        self.classes = rospy.get_param("~classes", None)
        self.tracker = rospy.get_param("~tracker", "bytetrack.yaml")
        self.device = rospy.get_param("~device", None)
        self.result_conf = rospy.get_param("~result_conf", True)
        self.result_line_width = rospy.get_param("~result_line_width", None)
        self.result_font_size = rospy.get_param("~result_font_size", None)
        self.result_font = rospy.get_param("~result_font", "Arial.ttf")
        self.result_labels = rospy.get_param("~result_labels", True)
        self.result_boxes = rospy.get_param("~result_boxes", True)
        path = roslib.packages.get_pkg_dir("ultralytics_ros")

        # 使用绝对路径加载模型
        self.model = YOLO("/home/vkrobot/workspace/badminton_grab/src/ultralytics_ros/models/best.pt")
        # rospy.loginfo(f"self.model ---- {self.model}")

        # 融合BN 提速
        self.model.fuse()

        # 实例化
        self.bridge = cv_bridge.CvBridge()

        # 相机内参、深度图等变量
        self.fx = self.fy = self.cx = self.cy = None
        self.depth_image = None
        self.depth_scale = 1.0  # 后面做深度单位变换（米）

        # 相机内参订阅
        self.cam_info_sub = rospy.Subscriber(
            "/arm_camera/color/camera_info", CameraInfo, self.cam_info_cb, queue_size=1)
        
        # 深度图像订阅
        self.depth_sub = rospy.Subscriber(
            "/arm_camera/aligned_depth_to_color/image_raw", ImageMsg, self.depth_cb, queue_size=1)

        # 原图像订阅（做检测）
        self.sub = rospy.Subscriber(
            self.input_topic,
            ImageMsg,
            self.image_callback,
            queue_size=1,
            buff_size=2**24,
        )

        # 位姿发布
        self.results_pub = rospy.Publisher(self.result_topic, glab_pose_array, queue_size=1)
        
        # 处理结果图片发布
        self.result_image_pub = rospy.Publisher(
            self.result_image_topic, ImageMsg, queue_size=1
        )
         
    # 获取深度图像及单位
    def depth_cb(self, image):
        # cv格式转ros格式。passthrough：保持原精度
        depth = self.bridge.imgmsg_to_cv2(image, desired_encoding="passthrough")

        # 根据数据类型指定深度单位。uint16表示整型、float32表示浮点型

        if depth.dtype == np.uint16:
            self.depth_scale = 0.001
        else:
            self.depth_scale = 1.0

        self.depth_image = depth

    # 获取相机内参（msg: CameraInfo表示期望类型）
    def cam_info_cb(self, msg: CameraInfo):
        # K 是 3x3 展平： [fx, 0, cx, 0, fy, cy, 0, 0, 1]
        self.fx = float(msg.K[0])
        self.fy = float(msg.K[4])
        self.cx = float(msg.K[2])
        self.cy = float(msg.K[5])
        # self.cam_frame = msg.header.frame_id  # e.g. "arm_camera_color_optical_frame"

    # 查询深度图像深度
    def get_depth_at(self, u, v, win=0):
        """
        返回像素(u,v)附近的深度(米)。win>0 时，取 (2*win+1)^2 窗口的中位数。
        """
        if self.depth_image is None:
            return None

        H, W = self.depth_image.shape[:2]

        # 将像素坐标转成整数
        u = int(round(u)); v = int(round(v))

        # 只取单个点的深度
        if win == 0:
            # 判断是否越界
            if not (0 <= u < W and 0 <= v < H):
                return None
            
            # 获取深度值，因为深度图像转成了cv格式，索引时（行，列），又深度图像中的像素就是深度（距离信息），因此这样获取的就直接是深度信息了
            z_raw = float(self.depth_image[v, u])
            
            # 过滤掉NaN、小于0的
            if not np.isfinite(z_raw) or z_raw <= 0.0:
                return None
            # 返回深度，单位为m（实际的物理单位）
            return z_raw * self.depth_scale

        # 取窗口的中点深度
        u0, u1 = max(0, u - win), min(W, u + win + 1)
        v0, v1 = max(0, v - win), min(H, v + win + 1)

        # 获取一个区域的像素矩阵，并转成float32(为了后面的计算能容得下多个小数，不转也可以)
        patch = self.depth_image[v0:v1, u0:u1].astype(np.float32)
        
        # 过滤Nan、小于0的
        patch = patch[np.isfinite(patch) & (patch > 0)]

        # 检查长度
        if patch.size == 0:
            return None

        # 返回中位数。self.depth_scale：缩放单位
        return float(np.median(patch)) * self.depth_scale

    # 像素反投影到相机坐标系
    def deproject_pixel_to_3d(self, u, v, z):
        """
            針孔模型：给定像素(u,v)与深度z(米)，返回相机坐标系下的 (X,Y,Z)[米]。
        """
        # any表达式
        if any(x is None for x in (self.fx, self.fy, self.cx, self.cy)):
            raise RuntimeError("Camera intrinsics not set. Wait for /camera_info.")
        X = (u - self.cx) * z / self.fx
        Y = (v - self.cy) * z / self.fy
        Z = z
        return float(X), float(Y), float(Z)


    # 处理图像主函数
    def image_callback(self, msg):
        cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")

        # # 缩放
        # h, w = cv_image.shape[:2]
        # center = (w / 2.0, h / 2.0)
        # # 仿射：旋转+缩放
        # M = cv2.getRotationMatrix2D(center, 0, 0.8)

        # # 逆变换（用于把检测结果映回原图）
        # # M_inv = cv2.invertAffineTransform(M)

        # # 变换后的图像
        # transformed = cv2.warpAffine(
        #     cv_image, M, (w, h),
        #     flags=cv2.INTER_LINEAR, borderMode=cv2.BORDER_REPLICATE
        # )


        # 做单帧跟踪/检测。这里的track（也有仅预测模式）表示做跟踪，这样预测出来的目标会持续使用给定编号（唯一编号），还可以借助这个计算开始后到后面移动的距离等
        results = self.model.track(  
            source=cv_image,
            conf=self.conf_thres,  # 置信度阈值（过滤低分框）
            iou=self.iou_thres,  # NMS 的 IoU 阈值（框合并）
            max_det=self.max_det,  # 每帧最大保留框数
            classes=self.classes,  # 类别过滤（None = 不过滤；或列表如 [0,1]）
            tracker=self.tracker,  # 跟踪器配置文件（如 bytetrack.yaml）
            device=self.device,   # 推理设备（'cpu'、'0'、'0,1'；None=自动）
            verbose=False,  # 关闭冗余日志
            retina_masks=True,   # 分割模型时使用高分辨率掩码（-seg.pt 时有用）
        )

        # 确保有结果
        if results is not None:
            result_image = ImageMsg()  # 可视化结果图片
            result_image.header = msg.header

            # 返回位姿
            result_pose = self.create_detections_array(results)

            # 返回图像
            result_image = self.create_result_image(results)
            
            if result_pose is not None:
                # 发布位姿
                self.results_pub.publish(result_pose)
            else:
                rospy.loginfo("此刻消息没有检测到目标")
            
            if result_image is not None:
                # 发布图像
                self.result_image_pub.publish(result_image)
            else:
                rospy.loginfo(f"此刻图像没有检测到目标")

    # 返回pose
    def create_detections_array(self, results):

        # 判断类型
        res0 = results[0] if isinstance(results, (list, tuple)) else results

        # print("res0", res0)
        
        # 防止res0为空或者没有obb对象
        if res0 is None or getattr(res0, "obb", None) is None or len(res0.obb) == 0:
            return None

        obb = res0.obb  
        xywhr = obb.xywhr.detach().cpu().numpy()  # 提取xywh r
        confs = obb.conf.detach().cpu().numpy().astype(float)  # 提取置信度
        ids   = obb.id.detach().cpu().numpy().astype(int) if getattr(obb, "id", None) is not None else None

        msg = glab_pose_array()

        for i, det in enumerate(xywhr):
 
            # 拿到像素坐标系的框中心坐标、宽、高、旋转角度r
            cx, cy, w, h, r = det 

            # print('--------------', xywhr, '------------------')

            if w > h:
                r_new = -(math.pi/2 - r) 
                # rospy.loginfo(f'高度大于宽度 - 互补后取反 - r:{r} - r_new:{r_new}')

            elif w < h:
                r_new = r
                # rospy.loginfo(f'宽度大于高度 - 直接取 - r:{r} - r_new:{r_new}')

            # 传入像素坐标系坐标，取深度
            # 注：这里一定要用窗口取，单点的话他可能会因为光线、插值等原因不准。窗口为3、5都可以只要不必目标大就行，一般就这两个
            z = self.get_depth_at(cx, cy, win=3)
            
            if z is None or not np.isfinite(z) or z <= 0:
                # 没有有效深度就回退像素系
                X, Y, Z = float(cx), float(cy), 0.0
            else:
                X, Y, Z = self.deproject_pixel_to_3d(cx, cy, z)
            

            # r在相机坐标系中的值和像素坐标系中一样，直接欧拉角转四元素
            qx, qy, qz, qw = tfs.quaternion_from_euler(0.0, 0.0, r_new)

            pose = glab_pose()
            pose.id = int(ids[i] if ids is not None else -1)
            pose.x = float(X)
            pose.y = float(Y)
            pose.z = float(Z)

            pose.q_x = qx 
            pose.q_y = qy 
            pose.q_z = qz 
            pose.q_w = qw

            rospy.loginfo(f"pose ----- {pose}")
            
            msg.poses.append(pose)

            # rospy.loginfo(f"识别到的位姿：{pose} ----- r为 ----- {r} ----- r_new为：{r_new}")
        return msg


    # 可视化检测结果
    def create_result_image(self, results):
        
        # 取当前这一帧的结果对象（兼容 list / 单个）
        res = results[0] if isinstance(results, (list, tuple)) else results
        if res is None:
            # 没有结果，返回一张空图由上层处理（也可以抛异常）
            raise RuntimeError("No results to plot.")

        # 以原图为底图进行绘制((res.orig_img是推理时输入的图)
        img = res.orig_img.copy()
        
        # 获取图像 H W信息，后面画十字架用
        H, W = img.shape[:2]

        obb = getattr(res, "obb", None)
        if obb is not None and len(obb) != 0:
            # 取四点坐标、多余信息
            # .detach().cpu().numpy()表示从GPU上分离，再拷贝到CPU，转numpy，最后将数据类型转成int
            polys = obb.xyxyxyxy.detach().cpu().numpy().astype(int)   # 框定点坐标矩阵
            confs = obb.conf.detach().cpu().numpy()                   # 置信度
            clses = obb.cls.detach().cpu().numpy().astype(int)        # 类别
            xywhr = obb.xywhr.detach().cpu().numpy()  # 框坐标
            ids = None

            # 检查是否是有跟踪，有跟踪就有ID
            if getattr(obb, "id", None) is not None:
                ids = obb.id.detach().cpu().numpy().astype(int)       # (N,)

            # 类别号和类别名的映射字典
            names = getattr(res, "names", {}) or {}

            # 从参数中读取画图样式
            lw = int(self.result_line_width) if self.result_line_width else 2  # 线宽
            fs = float(self.result_font_size) if self.result_font_size else 0.5  # 字体大小
            draw_boxes = True if self.result_boxes in (True, "true", "True") else False  # 是否绘制边框
            draw_labels = True if self.result_labels in (True, "true", "True") else False  # 是否绘制类别标签
            draw_conf = True if self.result_conf in (True, "true", "True") else False  # 是否绘制置信度


            # 1.在置信度最高的框中心绘制一个十字架
            i = int(np.argmax(confs))  
            cx, cy, w, h, r = xywhr[i]
            cx_i, cy_i = int(round(cx)), int(round(cy))
            #   十字长度：按目标框尺寸自适应（你也可以改为固定像素长度）
            arm = int(max(6, min(w, h) * 0.25))  # 下限 6 像素，避免太小看不见

            #   端点（裁剪到图像边界，防止越界）
            x1 = max(0, cx_i - arm); x2 = min(W - 1, cx_i + arm)
            y1 = max(0, cy_i - arm); y2 = min(H - 1, cy_i + arm)

            #   画横线与竖线（颜色/线宽复用现有参数）
            cv2.line(img, (x1, cy_i), (x2, cy_i), (0, 255, 0), lw, cv2.LINE_AA)
            cv2.line(img, (cx_i, y1), (cx_i, y2), (0, 255, 0), lw, cv2.LINE_AA)

            # 2.遍历所有坐标矩阵，画框、置信度等
            for i, poly in enumerate(polys):
                # 绘制框
                if draw_boxes:
                    cv2.polylines(img, [poly], isClosed=True, color=(0, 255, 0), thickness=lw)

                # # 根据布尔值获取对应信息
                # label_parts = []
                
                # if draw_labels:  # 标签
                #     cname = names.get(int(clses[i]), str(int(clses[i])))
                #     label_parts.append(str(cname))
                # if draw_conf:  # 置信度
                #     label_parts.append(f"{confs[i]:.2f}")
                # if ids is not None:  # 类别号
                #     label_parts.append(f"ID:{ids[i]}")

                # # 如果有就在框上面标注
                # if label_parts:
                #     x, y = int(poly[0][0]), int(poly[0][1]) - 5
                #     cv2.putText(
                #         img,
                #         " ".join(label_parts),
                #         (x, max(y, 0)),
                #         cv2.FONT_HERSHEY_SIMPLEX,
                #         fs,
                #         (0, 255, 0),
                #         max(1, lw),
                #         cv2.LINE_AA,
                #     )

            result_image_msg = self.bridge.cv2_to_imgmsg(img, encoding="bgr8")
            return result_image_msg


if __name__ == "__main__":

    rospy.init_node("tracker_node")
    node = TrackerNode()
    rospy.spin()

      ②抓取：

#!/usr/bin/env python3
#coding=utf8
    
#加载需要的模块库
import rospy
import tf2_ros, tf2_geometry_msgs

#加载机械臂需要的模块文件
from e1_moveit_control.srv import set_gripper, use_cartesian_path
# 导入自己定义的类型
from arm_pkg.srv import pose_quaternion
from arm_pkg.srv import set_joint, get_joint
from ultralytics_ros.msg import glab_pose_array
import tf.transformations as tfs
import numpy as np
from sensor_msgs.msg import  Image as ImageMsg
import cv2
from cv_bridge import CvBridge, CvBridgeError


# 两个位姿
original_pose = None # 持续接收的位姿

# 识别误差补偿
W_value=[-0.01, 0.0, 0.07]

# 给机械臂指定一些值
#   夹抓张开值
gripper_open=0.4
#   夹爪闭合值
gripper_close=0.05

# 机械臂运动的时间
#   夹爪张合时间
gripper_control_sleep_time = 1
#   运动到初始位姿的时间
arm_joint_time = 2
#   抓取、放置时间
arm_control_time = 4

#   机械臂 init 姿态
init_pose = [0.0, 0.0, 0.0, 0.0, 1.545, 0.0] # xyz rpy


# 单椅子
ckeck_pose = [-0.002099999925121665
            ,0.0421999990940094
            ,1.0083999633789062
            ,0.07169999927282333
            ,2.111599922180176
            ,0.07069999724626541]
# 先靠近一下目标
target_pose = [
            -0.3100000023841858
            ,0.49459999799728394
            ,2.0023999214172363
            ,-0.36079999804496765
            ,-0.8870000243186951
            ,0.310699999332428]

# target_pose = [-0.0027000000700354576
#             ,0.2694000005722046
#             ,1.5870000123977661
#             ,0.06560000032186508
#             ,1.2770999670028687
#             ,-0.10350000113248825]

# 二次夹取位置
pose_pick_up = [
-0.059700001031160355
,0.8715000152587891
,1.1109999418258667
,0.188400000333786
,1.2618000507354736
,-0.15289999544620514
] 

# 放置目标：
place_pose_positive = [
-1.208400011062622
,1.6964999437332153
,-0.21299999952316284
,-1.6323000192642212
,-1.2264000177383423
,1.7143000364303589]

place_pose_against = [
-1.2031999826431274
,1.717900037765503
,-0.29589998722076416
,-1.6576000452041626
,-1.2151999473571777
,-1.3519999980926514
]

# 按压位姿：
press_positive = [
-1.190999984741211
,1.5358999967575073
,0.4027999937534332
,-1.4520000219345093
,-1.186400055885315
,1.2092000246047974
]

press_against = [
-1.193600058555603
,1.7592999935150146
,-0.1137000024318695
,-1.5674999952316284
,-1.1996999979019165
,-1.6289000511169434
]


class Arm_glab():

    def __init__(self):
        rospy.init_node('target_glab')
        
        # 订阅检测结果
        self.result_sub = rospy.Subscriber("/yolo_result", glab_pose_array, self.pose_callback, queue_size=1)

    # 位姿订阅(拿到所有id和位姿)
    def pose_callback(self, msg):
        global original_pose

        original_pose = []
    
        # 列表里面嵌套字典，每一个目标为一个字典
        for p in msg.poses:
            # 每个 p 就是一个 glab_pose
            pose_item = {
                "id": p.id,
                "x": p.x,
                "y": p.y,
                "z": p.z,
                "q_x": p.q_x,
                "q_y": p.q_y,
                "q_z": p.q_z,
                "q_w": p.q_w,
            }
            original_pose.append(pose_item)
    
    # 笛卡尔实现
    def pose_cartesian(self, goal_pose): # goal: [x, y, z, rr, rp, ry]
        try:
            client = rospy.ServiceProxy("/python_my_descartes", pose_quaternion)
            response = client(*goal_pose)
            return response.result     
        
        except:
            rospy.logwarn("don't to set cartesian control...")
            return False
    
    # 保底笛卡尔坐标实现
    def optimize_cartesian(self, data):
        try:
            control_data = [ data[i] for i in range(3) ]
            client = rospy.ServiceProxy("/python_cartesian_pose_with_rpy", use_cartesian_path)
            response = client(*control_data)
            return response.result
        except rospy.ServiceException as e:
            rospy.logwarn("set_pose service call failed because: {}".format(e))
            return False
        
    # 坐标变换实现（机器人到摄像头）
    def change_tf_pose(self, data):
        # 初始化
        buffer = tf2_ros.Buffer() # 创建缓存对象，这是一个tf2变换库的缓存池，所有tf变换关系都在这
        sub = tf2_ros.TransformListener(buffer) # 创建一个监听器

        # 确保有基坐标系到摄像头的坐标变换
        timeout = rospy.Duration(3.0)
        start = rospy.Time.now()

        while not buffer.can_transform("base_footprint", "arm_camera_color_optical_frame", rospy.Time(0)):
            if rospy.Time.now() - start > timeout:
                print("look up tf fail...")
                break
            rospy.sleep(0.1)  # 每100ms检查一次


        # 组织目标坐标系参考摄像头坐标系（小）的位置
        ps = tf2_geometry_msgs.PoseStamped()
        ps.header.stamp = rospy.Time(0)  # 时间
        ps.header.frame_id = "arm_camera_color_optical_frame"  # 参考的坐标系
        
        #   偏移量
        ps.pose.position.x = data[0]
        ps.pose.position.y = data[1]
        ps.pose.position.z = data[2]
        #   四元数
        ps.pose.orientation.x = data[3]
        ps.pose.orientation.y = data[4]
        ps.pose.orientation.z = data[5]
        ps.pose.orientation.w = data[6]

        try:
            # 获取基坐标系到目标点的距离
            ps_out = buffer.transform(ps, "base_footprint")
            return [ps_out.pose.position.x, ps_out.pose.position.y, ps_out.pose.position.z, 
                    ps_out.pose.orientation.x, ps_out.pose.orientation.y, 
                    ps_out.pose.orientation.z, ps_out.pose.orientation.w]
        
        except Exception as e:  # 捕捉所有标准错误
            rospy.logwarn("tf2 transform error: %s", e)
            return False
        
    # 设置夹爪打开张开大小
    def e1_set_gripper(self, value): # close 0~0.9 open
        try:
            client = rospy.ServiceProxy("/python_set_gripper_pose", set_gripper)
            response = client(value)
            return response.result
        
        except rospy.ServiceException as e:
            rospy.logwarn("set_gripper service call failed because: {}".format(e))
            return False
    
    # 设置机械臂joint关节值
    def e1_set_joint(self, data): # 6 dof
        try:
            client = rospy.ServiceProxy("/python_set_joint", set_joint)
            # response = client(*[i for i in data])
            response = client(*data)  # 对列表参数进行拆解，传参
            return response.result  # 查看一下数据
        
        except rospy.ServiceException as e:
            rospy.logwarn("set_joint service call failed because: {}".format(e))
            return False

    # 获取arm当前joint位置
    def get_current_joint(self):
        try:
            client = rospy.ServiceProxy("/python_get_arm_joint", get_joint)
            response = client()
            current_pose = [response.j1, response.j2, response.j3, 
                            response.j4, response.j5, response.j6]
            return current_pose
        
        except rospy.ServiceException as e:
            rospy.logwarn("the faild reason is : {}".format(e))
            return False
        
    # 设置关节到初始位姿
    def set_joint_pose(self, time=arm_control_time, joint_pose=init_pose):
        global init_pose, arm_joint_time
        
        # 获取当前joint
        current_joint = self.get_current_joint()
        
        if joint_pose == current_joint:
            pass
        else:
            self.e1_set_joint(joint_pose)
            rospy.sleep(time)



# 计算面积函数    
def edge_height_metrics(edges):
    """
    对二值边缘图计算“到下边界的高度”统计
    返回: dict，包括总和/均值/分位数/覆盖率等
    """
    H, W = edges.shape[:2]

    ys, xs = np.where(edges > 0)  # 拿到所有255像素坐标

    if ys.size == 0:
        return {
            "count": 0, "sum": 0.0, "mean": 0.0, "std": 0.0,
            "q90": 0.0, "q75": 0.0, "max": 0.0, "coverage": 0.0
        }

    d = (H - 1) - ys  # 到底边的距离，像素

    # 每一列是否出现过边缘点（做个稳定性参考）
    col_hit = np.zeros(W, dtype=bool)
    col_hit[xs] = True

    return {
        "count": int(d.size),  # 点数量
        "sum": float(d.sum()),  # 距离和
        "mean": float(d.mean()),  # 距离均值
        "std": float(d.std()),  # 标准差
        "q90": float(np.percentile(d, 90)),  # 边缘点距离的第90分位数
        "q75": float(np.percentile(d, 75)),  # 边缘点距离的第75分位数
        "max": float(d.max()),  # 最大值
        "coverage": float(col_hit.mean())  # 边缘在水平方向上的覆盖率
    }

# 处理深度图像返回布尔值
def direction_judgment(msg):
    global direction, sum_check

    direction = True # 方向布尔值

    # 将ros转cv
    bridge = CvBridge()

    try:
        # 宽：640,高：480
        cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")   
        img = cv_image[380:480, 380:580, :]

    except CvBridgeError as e:
        rospy.logerr("格式转换错误:%s", e)
        return
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

    # 边缘检测。图片 低阀值 高阀值（像素值）
    edges = cv2.Canny(gray, 130, 160)

    result = edge_height_metrics(edges)
    
    sum_check = result.get('sum')

    rospy.loginfo(f"面积 ---- {result.get('sum')}")

    """
    没有：25000
    
    球头朝上 - 一点：23000
    球头朝上 - 大的：28000

    球头朝上 - 长的: 79000
    球毛朝上 - 一点 ：46000

    """

    # 球毛朝上
    if result.get('sum') > 40000 or result.get('sum') < 12000:
        direction = True
    
    # 球头朝上
    else:
        direction = False

    # cv2.imshow("image1", img)
    # # cv2.imshow("image2", gray)
    # cv2.imshow("image3", edges)
    # cv2.waitKey(0)
    # cv2.destroyAllWindows()

    return direction


if __name__ == '__main__':
    try:
        #初始化类
        bot = Arm_glab()

        # 运动到初始姿态
        rospy.loginfo("运动到初始姿态\n")
        bot.set_joint_pose(arm_joint_time+1)
       
        # 打开夹爪
        rospy.loginfo("打开夹爪\n")
        bot.e1_set_gripper(gripper_open)
        rospy.sleep(gripper_control_sleep_time)

        # 运动到识别位置
        rospy.loginfo("运动到识别位置并开始识别\n\n")
        bot.set_joint_pose(arm_joint_time, ckeck_pose)
        rospy.sleep(2)
        
        # 将位姿转换后拿出来
        if original_pose is not None:
            
            rospy.loginfo(f"识别到了{len(original_pose)}个目标，正在处理目标\n")

            pose_list = []

            # 迭代位姿列表
            for pose_dict in original_pose:
                
                # 将位姿拿出来
                pose_foru_elements = [
                                    pose_dict["x"], pose_dict["y"], pose_dict["z"],
                                    pose_dict["q_x"], pose_dict["q_y"], pose_dict["q_z"], pose_dict["q_w"]
                                    ]  # 将位姿拿出来

                # 将位姿转换到base_footprint下
                original_pose_new = bot.change_tf_pose(pose_foru_elements)
                # rospy.loginfo(f"转换后的位姿：{original_pose_new}")

                # 对转换后的位姿进行处理
                q_tag = original_pose_new[3:]  # 将位姿中的四元数提取出来
                q_z90  = tfs.quaternion_from_euler(0, 0, np.pi/2)  # 生成一个绕自身z轴旋转90度的四元数

                #   做旋转
                q_corr = tfs.quaternion_multiply(q_tag, q_z90)  # 在原来基础上做90度旋转（z轴）

                #   将旋转好的四元数重新给到位姿
                original_pose_new[3:] = q_corr

                # 补偿
                original_pose_new = [ original_pose_new[i] + W_value[i] for i in range(3) ] + original_pose_new[3:]

                # 添加位姿
                pose_list.append(original_pose_new)
            
            rospy.loginfo('位姿转换完毕\n')

            # 抓取代码
            for idx, pose in enumerate(pose_list):
                
                rospy.loginfo(f"准备夹爪{idx + 1}号目标\n")

                # 移动到目标位姿（自己写笛卡尔）
                if not bot.pose_cartesian(pose):
                    rospy.loginfo(f"夹取 - 自己写的笛卡尔直线规划失败， 改用优化版规划")
                    result = bot.optimize_cartesian(original_pose_new[:3])
                    if result:
                        rospy.loginfo(f"夹取 - 优化版笛卡尔规划成功\n\n")
                    else:
                        rospy.loginfo(f"夹取 - 优化版笛卡尔规划失败\n\n")
                else:
                    rospy.loginfo(f"夹取 - 自己写的夹取笛卡尔规划成功\n\n")
                rospy.sleep(arm_control_time)

                # 夹取（合并夹爪）
                bot.e1_set_gripper(gripper_close)
                rospy.sleep(gripper_control_sleep_time)
                rospy.loginfo("夹取\n")
                
                # 靠近目标点
                bot.set_joint_pose(arm_joint_time, target_pose)
                rospy.loginfo("靠近放置点\n\n")
                
                # 订阅深度图像
                global sum_check
                direction_sub = rospy.Subscriber("/arm_camera/color/image_raw", ImageMsg, direction_judgment, queue_size=1)
                rospy.loginfo("进行球头方向判断")
                rospy.sleep(5)

                rospy.loginfo(f"面积：{sum_check}")

                # 判断球方向    
                if direction:  # 球毛朝上   
                    rospy.loginfo("球毛朝上 - 直接放置\n")
                    # 移动到目标点
                    bot.set_joint_pose(arm_joint_time, place_pose_positive)
                    # 按压
                    bot.set_joint_pose(arm_joint_time, press_positive)
                
                else:  # 球头朝上
                    rospy.loginfo("球头朝上 - 旋转180度后放置\n")
                    # 移动到目标点
                    bot.set_joint_pose(arm_joint_time, place_pose_against)
                    # 按压
                    bot.set_joint_pose(arm_joint_time, press_against)

                # 打开夹爪
                bot.e1_set_gripper(0.8)
                rospy.sleep(gripper_control_sleep_time)
                
                # 运动到二次夹取位置
                bot.set_joint_pose(arm_joint_time,target_pose)

                # 再次回到抓取尺度
                bot.e1_set_gripper(gripper_open)
                rospy.sleep(2)


                rospy.loginfo(f"目标{idx + 1}抓取完毕\n")
                
        else:
            raise RuntimeError("No target detected, skipping further processing.")


    except rospy.ROSInterruptException:
        rospy.loginfo("e1 mission has some question..")