#!/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()
except KeyboardInterrupt:
print ("Shutting down object_detect node.")
cv2.destroyAllWindows()文章来源:https://www.toymoban.com/news/detail-841338.html
文章来源地址https://www.toymoban.com/news/detail-841338.html
到了这里,关于ROS+Python-opencv(1)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!