ROS+python-opencv(4)

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
# Copyright (c) 2021 PS-Micro, Co. Ltd.
#
# SPDX-License-Identifier: Apache-2.0
#

# 该python文件可以当作节点放在工作空间中启动

import time

import rospy
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import numpy as np
from math import *
from geometry_msgs.msg import Pose



class ImageConverter:
    def __init__(self):
        # 创建图像缓存相关的变量
        self.cv_image = None
        self.get_image = False

        # 创建cv_bridge
        self.bridge = CvBridge()

        # 声明图像的发布者和订阅者
        self.image_pub = rospy.Publisher("land_detect_image",
                                         Image,
                                         queue_size=1)
        self.target_pub = rospy.Publisher("land_detect_pose",
                                          Pose,
                                          queue_size=1)
        self.image_sub = rospy.Subscriber("/iris_0/camera/image_raw",
                                          Image,
                                          self.callback,
                                          queue_size=1)

    def callback(self, data):
        # 判断当前图像是否处理完
        if not self.get_image:
            # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
            try:
                self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
            except CvBridgeError as e:
                print(e)
            # 设置标志，表示收到图像
            self.get_image = True

    def detect_object(self):
        # 高斯滤波，对图像邻域内像素进行平滑
        gray = cv2.cvtColor(self.cv_image, cv2.COLOR_BGR2GRAY)

        # 应用高斯模糊来减少图像噪声
        blurred = cv2.GaussianBlur(gray, (5, 5), 0)

        # 使用Hough变换检测圆圈
        circles = cv2.HoughCircles(blurred, cv2.HOUGH_GRADIENT, 1, 100, param1=50, param2=30, minRadius=50,
                                   maxRadius=400)

        # 遍历找到的所有轮廓线
        if circles is not None:
            # 将检测到的圆圈的坐标转换为整数
            circles = np.uint16(np.around(circles))

            # 遍历每个检测到的圆圈
            for i in circles[0, :]:
                # 获取圆圈的圆心坐标和半径
                x, y, r = i

                # 在原始图像上绘制圆圈
                cv2.circle(self.cv_image, (x, y), r, (0, 255, 0), 2)
                cv2.circle(self.cv_image, (x, y), 2, (0, 0, 255), 3)  # 绘制圆心



            # 将目标位置通过话题发布
            objPose = Pose()
            objPose.position.x = x
            objPose.position.y = y
            objPose.position.z = 3.14*r**2
            self.target_pub.publish(objPose)

        # 再将opencv格式额数据转换成ros image格式的数据发布
        try:
            self.image_pub.publish(self.bridge.cv2_to_imgmsg(self.cv_image, "bgr8"))
        except CvBridgeError as e:
            print(e)

    def loop(self):
        if self.get_image:
            self.detect_object()
            self.get_image = False


if __name__ == '__main__':
    try:
        # 初始化ros节点
        rospy.init_node("land_detect")
        rospy.loginfo("Starting detect land")
        image_converter = ImageConverter()
        rate = rospy.Rate(100)
        while not rospy.is_shutdown():
            image_converter.loop()
            rate.sleep()
    except KeyboardInterrupt:
        print("Shutting down land_detect node.")
        cv2.destroyAllWindows()