ROS+Python-opencv(1)

宇星YUXING

已于 2024-03-25 16:14:09 修改

阅读量782

点赞数 23

文章标签： opencv python 人工智能

于 2024-03-10 21:30:05 首次发布

本文链接：https://blog.csdn.net/lixin1752/article/details/136609544

版权

#!/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
from std_msgs.msg import Float32

lower_color = np.array([0, 0, 0])
upper_color = np.array([50, 50, 70])
kernel_size = 5

def find_line_center(binary_image):
    contours, _ = cv2.findContours(binary_image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if not contours:
        return None

    # 找到最大轮廓，假设它是线条
    max_contour = max(contours, key=cv2.contourArea)
    M = cv2.moments(max_contour)

    # 如果矩存在，则计算轮廓的中心
    if M["m00"] != 0:
        cX = int(M["m10"] / M["m00"])
        cY = int(M["m01"] / M["m00"])
        return (cX, cY)

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

# 创建cv_bridge
self.bridge = CvBridge()

        # 声明图像的发布者和订阅者
        self.image_pub = rospy.Publisher("line_detect_image",
                                         Image,
                                         queue_size=1)
        self.target_pub = rospy.Publisher("line_detect_dispose",
                                          Pose,
                                          queue_size=1)
        self.image_shape_pub = rospy.Publisher("image_shape",
                                          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)

        height, width = self.cv_image.shape[:2]
        shape = Pose()
        shape.position.x = width
        shape.position.y = height
        self.image_shape_pub.publish(shape)
        # 应用高斯模糊以减少图像噪声
        blurred = cv2.GaussianBlur(gray, (5, 5), 0)

# 使用Canny边缘检测来检测线条
edges = cv2.Canny(blurred, 50, 150)

# 使用颜色阈值创建一个黑色线条的掩模
mask = cv2.inRange(self.cv_image, lower_color, upper_color)

        # 应用形态学操作来清理和增强线条
        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, np.ones((kernel_size, kernel_size)), iterations=4)
        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, np.ones((kernel_size, kernel_size)), iterations=4)

line_center = find_line_center(mask)

        # 如果找到线条中心，绘制一个点或线条来指示它
        if line_center:
            cv2.circle(self.cv_image, line_center, 10, (0, 255, 0), -1) # 在原图上绘制一个绿色的点
            cv2.line(self.cv_image, (0, line_center[1]), (self.cv_image.shape[1], line_center[1]), (0, 255, 0), 2)
            disPose = Pose()
            disPose.position.x = line_center[0]
            disPose.position.y = line_center[1]
            self.target_pub.publish(disPose)

        # 再将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("line_detect")
        rospy.loginfo("Starting detect object")
        image_converter = ImageConverter()
        rate = rospy.Rate(100)
        while not rospy.is_shutdown():
            image_converter.loop()
            rate.sleep()

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
from std_msgs.msg import Float32


lower_color = np.array([0, 0, 0])
upper_color = np.array([50, 50, 70])
kernel_size = 5

def find_line_center(binary_image):
    contours, _ = cv2.findContours(binary_image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if not contours:
        return None

    # 找到最大轮廓，假设它是线条
    max_contour = max(contours, key=cv2.contourArea)
    M = cv2.moments(max_contour)

    # 如果矩存在，则计算轮廓的中心
    if M["m00"] != 0:
        cX = int(M["m10"] / M["m00"])
        cY = int(M["m01"] / M["m00"])
        return (cX, cY)

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

        # 创建cv_bridge
        self.bridge = CvBridge()

        # 声明图像的发布者和订阅者
        self.image_pub = rospy.Publisher("line_detect_image",
                                         Image,
                                         queue_size=1)
        self.target_pub = rospy.Publisher("line_detect_dispose",
                                          Pose,
                                          queue_size=1)
        self.image_shape_pub = rospy.Publisher("image_shape",
                                          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)

        height, width = self.cv_image.shape[:2]
        shape = Pose()
        shape.position.x = width
        shape.position.y = height
        self.image_shape_pub.publish(shape)
        # 应用高斯模糊以减少图像噪声
        blurred = cv2.GaussianBlur(gray, (5, 5), 0)

        # 使用Canny边缘检测来检测线条
        edges = cv2.Canny(blurred, 50, 150)

        # 使用颜色阈值创建一个黑色线条的掩模
        mask = cv2.inRange(self.cv_image, lower_color, upper_color)

        # 应用形态学操作来清理和增强线条
        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, np.ones((kernel_size, kernel_size)), iterations=4)
        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, np.ones((kernel_size, kernel_size)), iterations=4)

        line_center = find_line_center(mask)

        # 如果找到线条中心，绘制一个点或线条来指示它
        if line_center:
            cv2.circle(self.cv_image, line_center, 10, (0, 255, 0), -1)  # 在原图上绘制一个绿色的点
            cv2.line(self.cv_image, (0, line_center[1]), (self.cv_image.shape[1], line_center[1]), (0, 255, 0), 2)  
            disPose = Pose()
            disPose.position.x = line_center[0]
            disPose.position.y = line_center[1]
            self.target_pub.publish(disPose)
                
        # 再将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("line_detect")
        rospy.loginfo("Starting detect object")
        image_converter = ImageConverter()
        rate = rospy.Rate(100)
        while not rospy.is_shutdown():
            image_converter.loop()
            rate.sleep()
    except KeyboardInterrupt:
        print ("Shutting down object_detect node.")
        cv2.destroyAllWindows()

    except KeyboardInterrupt:
        print ("Shutting down object_detect node.")
        cv2.destroyAllWindows()