（五）焊缝检测之--测量间距并用ROS消息包发送

前言

在上一小节记录了矩形参照物的检测。这一小节将介绍基于识别到的圆形焊缝和矩形参照物的坐标，读出两点之间在x和y方向上的间距，并通过话题通信的方式发送初期

一、测量圆心和焊缝间距

在之前小节中已经分别通过霍夫圆检测拿到了圆心坐标和矩形参照物角点坐标，分别将其记作（point0[0],point0[1]）,（point1[0],point1[1]）。在这里需要读出的是两点之间在x轴方向和y轴方向上的间距，后期发送给机械臂让其沿着这两端间距移动到焊缝的位置。因此，直接将两点在同轴上的坐标作差并取绝对值即可。

distance1 = abs(point0[0] - point1[0])
distance2 = abs(point0[1] - point1[1])   #对距离取绝对值
 print(distance1, distance2)

在这里，矩形角点坐标由order_points(pts)函数计算，从2x4的数组rect中读出；圆心坐标由霍夫圆检测输出的数组circle读出。代码和注释在之前博客里都有详细介绍。最终读取出的效果如下 ：

这里有一个小细节，就是支撑圆形焊缝的底座也是个矩形框，在识别的时候也会被识别进去。因此对实际读取到的距离需要做一个范围上的取值，过滤掉底座矩形的取值，剩下的才是我们需要的值

二、制作ROS消息包并发送

1.下载ROS驱动包

网上有已经提供好的基于海康MVS相机的ROS驱动接口，直接下载放到创建的工作空间下编译即可。这一步参考博客《海康威视工业相机在ROS下驱动》来进行的
https://blog.csdn.net/weixin_60003973/article/details/135824902
ROS包下载链接:
https://github.com/luckyluckydadada/HIKROBOT-MVS-CAMERA-ROS
编译通过后运行指令

 roslaunch hikrobot_camera hikrobot_camera.launch  

最终成功导入包的效果如下：

2.制作ROS消息包并发送距离

制作ROS消息包并发送全过程的包括：
1.创建并初始化ROS节点
2.创建发布者并指明对象格式
3.制作消息包和获取数据
4.整合数据并发送

代码如下：

# 初始化ros节点
    rospy.init_node("cv_bridge_test")  
    rospy.loginfo("Starting cv_bridge_test node")  

# 创建距离发布者，并指明对象格式为string
    distance_pub = rospy.Publisher("distance", String, queue_size=10)

    msg = String()
    msg_front = "两点的距离是"
    msg_middle = "  "   

   # 整合发布的数据并发送
   try:
          msg.data = msg_front + str(distance1) + msg_middle + str(distance2)
          if distance2>500:
               distance_pub.publish(msg)
               ospy.loginfo("发布的数据是%s", msg.data)
           except rospy.is_shutdown():
                print("Shutting down cv_bridge_test node.")
                cv2.destroyAllWindows()

效果如下：

3.自定义数据格式发送坐标和距离

在上一步中发送的是距离信息，但在实际情况下距离以及圆心和矩形框角点坐标都要发送给机械臂，并且是以浮点数而非数组的形式发送。因此，需要自定义数据格式，从数组中提取出各个数据，再一并发送。
创建dispoint.msg文件，自定义数据格式如下：

float32 distance_1      #两点在x轴方向间距
float32 distance_2      #两点在y轴方向间距
float32 point_1            #矩形角点x轴坐标
float32 point_2            #圆心x轴坐标
float32 point_3            #矩形角点y轴坐标
float32 point_4            #圆心y轴坐标

之后的流程和第二步里的一样，在发送数据前将获取到的数据赋给自定义数据格式，再发送即可：

# 整合发布的数据并发送
                try:
                    dispoint_msg.distance_1 = distance1
                    dispoint_msg.distance_2 = distance2   #注意自定义消息格式里的数据命名不能和工程里的变量重名
                    dispoint_msg.point_1 = point0[0]
                    dispoint_msg.point_2 = point1[0]
                    dispoint_msg.point_3 = point0[1]
                    dispoint_msg.point_4 = point1[1]
                    #msg.data = msg_front + str(distance1) + msg_middle + str(distance2)
                    if distance2>500.0:
                        # distance_pub.publish(msg)
                        # rospy.loginfo("发布的数据是%s", msg.data)
                        dispoint_pub.publish(dispoint_msg)
                        rospy.loginfo("发布的数据是[%s, %s, %s, %s, %s, %s]", dispoint_msg.distance_1,dispoint_msg.distance_2,dispoint_msg.point_1,dispoint_msg.point_2,dispoint_msg.point_3,dispoint_msg.point_4)
                except rospy.is_shutdown():
                    print("Shutting down cv_bridge_test node.")
                    cv2.destroyAllWindows()

最终的效果如下：

三.完整代码

这里整合从第三节霍夫圆检测到这一节共三节的完整代码，结合前两届海康SDK的基本框架，便可实现如上功能。

# 获取矩形四点原始点坐标
def order_points(pts):
    # 进行初始化点的位置，左上、右上、左下、右下
    rect = np.zeros((4, 2), dtype="float32")   #rect为二维数组，第一维是不同坐标，第二维是坐标上的x和y，circle同理
    # 采取四个点的x+y的和，以最小的和为左上的点，x+y最大为右下的点
    s = pts.sum(axis=2)  # axis为标，防止溢出
    rect[0] = pts[np.argmin(s)]
    rect[2] = pts[np.argmax(s)]
    # 定义一左下点和右上点的位置，用|x-y|表示，小的为右上，大的为左下
    diff = np.diff(pts, axis=2)
    rect[1] = pts[np.argmin(diff)]
    rect[3] = pts[np.argmax(diff)]

    point0[0] = rect[0,0]
    point0[1] = rect[0,1]
    #print(point0)
    print(rect[0,0],rect[0,1])   #矩形左上角角点坐标
    #print(rect)


# 显示图像
def image_show(image, name):
    # 创建距离发布者，并指明对象格式为string
    dispoint_pub = rospy.Publisher("dispoint", dispoint, queue_size=10)
    msg = String()
    dispoint_msg = dispoint()
    msg_front = "两点的距离是"
    msg_middle = "  "
    image = cv2.resize(image, (1440, 1080), interpolation=cv2.INTER_AREA)
    name = str(name)

    gay_img = cv2.cvtColor(image, cv2.COLOR_BGRA2GRAY)

    # 进行矩形边缘检测
    edges = cv2.Canny(gay_img, 50, 150)
    img_circle = cv2.medianBlur(gay_img, 7)  # 进行中值模糊，去噪点



    # 寻找矩形轮廓
    contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    # 霍夫圆检测
    circles = cv2.HoughCircles(img_circle, cv2.HOUGH_GRADIENT, 1, 100,
                               param1=70, param2=90, minRadius=0,
                               maxRadius=10000)  # 根据识别圆形效果差异，调整参数param2（圆形密集则加大param2）

    # 遍历轮廓，筛选矩形轮廓
    rectangles = []
    if contours is not None and circles is not None:
        circles = np.uint16(np.around(circles))   #确保 circles 数组中的圆形坐标和半径是整数类型
        #print(circles)
        #P = circles[0]
        for i in circles[0, :]:  # 遍历矩阵每一行的数据
            cv2.circle(image, (i[0], i[1]), i[2], (0, 255, 0), 2)
            cv2.circle(image, (i[0], i[1]), 2, (0, 0, 255), 3)   #circle本身为opencv自带的库函数
            point1 = [i[0], i[1]]
            print(point1)   #圆心坐标，不含半径
            # cv2.imshow("gay_img", image)

        for contour in contours:
            # 计算轮廓的面积
            area = cv2.contourArea(contour)
            # 计算轮廓的周长
            perimeter = cv2.arcLength(contour, True)
            # 近似轮廓，用于逼近多边形曲线
            approx = cv2.approxPolyDP(contour, 0.02 * perimeter, True)

            # 如果近似轮廓有四个顶点，则认为它是一个矩形
            if len(approx) == 4 and area > 50000 and area < 1000000:  # 添加面积限制
                # 获取矩形的边界框
                x, y, w, h = cv2.boundingRect(approx)
                aspect_ratio = float(w) / h
                #获得矩形四点坐标
                order_points(approx)
                # 添加长宽比限制
                if aspect_ratio > 0.1 and aspect_ratio < 100.0:
                    # 绘制矩形
                    cv2.drawContours(image, [approx], 0, (0, 255, 0), 2)
                # 使用欧几里得距离公式计算两点之间的距离
                #distance = (pow(point0[0] - point1[0], 2) + pow(point0[1] - point1[1], 2))**0.5
                distance1 = abs(point0[0] - point1[0])
                distance2 = abs(point0[1] - point1[1])   #对距离取绝对值
                print(distance1, distance2)
                # 整合发布的数据并发送
                try:
                    dispoint_msg.distance_1 = distance1
                    dispoint_msg.distance_2 = distance2   #注意自定义消息格式里的数据命名不能和工程里的变量重名
                    dispoint_msg.point_1 = point0[0]
                    dispoint_msg.point_2 = point1[0]
                    dispoint_msg.point_3 = point0[1]
                    dispoint_msg.point_4 = point1[1]
                    #msg.data = msg_front + str(distance1) + msg_middle + str(distance2)
                    if distance2>500.0:
                        # distance_pub.publish(msg)
                        # rospy.loginfo("发布的数据是%s", msg.data)
                        dispoint_pub.publish(dispoint_msg)
                        rospy.loginfo("发布的数据是[%s, %s, %s, %s, %s, %s]", dispoint_msg.distance_1,dispoint_msg.distance_2,dispoint_msg.point_1,dispoint_msg.point_2,dispoint_msg.point_3,dispoint_msg.point_4)
                except rospy.is_shutdown():
                    print("Shutting down cv_bridge_test node.")
                    cv2.destroyAllWindows()
                cv2.imshow("image_rectangle", image)

    else:
        print('x: None y: None')

    # 显示图像
    cv2.imshow("image", image)
    #image_publish(image)
    k = cv2.waitKey(1) & 0xff

四.总结

以上就是本小节的全部内容，结合前几节，焊缝识别的基本功能框架已经实现，准备在下一节再做个颜色识别的博客，以应对不同环境下的干扰。然后就是焊缝检测部分的重难点，标定以及坐标变换，估计到时候更新的速度会变慢，但只要有收获就会继续更新下去。万里长征才刚刚起步，未来任重道远，道阻且长。

五.参考链接

海康威视工业相机在ROS下驱动
https://blog.csdn.net/weixin_60003973/article/details/135824902
海康Camera MVS Linux SDK二次开发封装ROS packge过程记录（c++）
https://blog.csdn.net/weixin_41965898/article/details/116801491
赵虚左ROS课程：话题通信
https://www.bilibili.com/video/BV1Ci4y1L7ZZ?p=47&vd_source=c4f360981aa4bb15debb8ab108d9956d