功能简介:

摄像头识别红色物体,框选(圆),并把图像信息发布
同时发布控制信息,当红色物体在左侧时,小乌龟左转,以此类推

实现原理:

openCV : 转HSV 去噪点 用cv_bridge发布图像

控制小乌龟(turtlesim): 发布twist消息

源码:

publisher与图像处理:

#!/usr/bin/env python
# license removed for brevity
import rospy
import numpy as np
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
import cv2
from cv_bridge import CvBridge
redLower = np.array([170, 100, 100])
redUpper = np.array([179, 255, 255])
def talker():
	count = 0
	cmd_pub = rospy.Publisher('/turtle1/cmd_vel',Twist,queue_size=1)
	cmd = Twist()
	cmd.linear.x = 0
	cmd.angular.z = 0
	pub = rospy.Publisher('/tutorial/image', Image, queue_size=10) 
	rospy.init_node('talker', anonymous=True) 
	rate = rospy.Rate(30)
	bridge = CvBridge()
	Video = cv2.VideoCapture(0)
	Video.set(3,640) 
	Video.set(4,480)
	while not rospy.is_shutdown():
		ret, frame = Video.read()
		#cv2.imshow("talker", img)
		#x, y = img.shape[0:2]
		#img_test1 = cv2.resize(img, (int(y / 4), int(x / 4)))
		hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
		mask = cv2.inRange(hsv, redLower, redUpper)
		# mask = cv2.erode(mask, None, iterations=2)
	
		# mask = cv2.dilate(mask, None, iterations=2)
		cv2.imshow('mask',mask)
	
		cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
	
		center = None
	
		if len(cnts) > 10:
		
		    c = max(cnts, key = cv2.contourArea)
	
		    ((x, y), radius) = cv2.minEnclosingCircle(c)
	
		    M = cv2.moments(c)
	
		    center = (int(M["m10"]/M["m00"]), int(M["m01"]/M["m00"]))
		 
		    if radius > 10:
		        print('X:%d,Y:%d'%(x,y))
		        cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 255), 2)
		        cv2.circle(frame, center, 5, (0, 0, 255), -1)
		        if x > 320:
					cmd.linear.x = 0.5
					cmd.angular.z = 0.5
					cmd_pub.publish(cmd)
					print('left')
		        if x < 320:
					cmd.linear.x = 0.5
					cmd.angular.z = -0.5
					cmd_pub.publish(cmd)
					print('right')		
		cv2.imshow('resize0', frame)
		#gray = cv2.cvtColor(img,cv2.COLOR_RGB2GRAY)
		#cv2.imshow('gray',gray)
		cv2.waitKey(10)
		count += 1
		if count ==30:
			print("30")
			count = 0
		pub.publish(bridge.cv2_to_imgmsg(frame, "bgr8"))
		#pub.publish(bridge.cv2_to_imgmsg(gray, "mono8"))
		rate.sleep()

if __name__ == '__main__':
	try:
		talker()
	except rospy.ROSInterruptException:
		pass

subscriber 接受图像(ssh访问就可远端接收,Rviz也可)

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image
import cv2
from cv_bridge import CvBridge

def callback(imgmsg):
    bridge = CvBridge()
    img = bridge.imgmsg_to_cv2(imgmsg, "bgr8")
    cv2.imshow("listener", img)
    cv2.waitKey(10)

def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # node are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("/tutorial/image", Image, callback)
    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()
if __name__ == '__main__':
    listener()

opencv 部分代码解释

from collections import  deque
import numpy as np
#import imutils
import cv2
import time
#设定红色阈值，HSV空间
redLower = np.array([170, 100, 100])
redUpper = np.array([179, 255, 255])
#初始化追踪点的列表
mybuffer = 64
pts = deque(maxlen=mybuffer)
#打开摄像头
camera = cv2.VideoCapture(0)
camera.set(3,640) #设置分辨率
camera.set(4,480)
fps =camera.get(cv2.CAP_PROP_FPS) #获取视频帧数
#等待两秒
print(fps)
#遍历每一帧，检测红色瓶盖
while True:
    #读取帧
    (ret, frame) = camera.read()
    #判断是否成功打开摄像头
    if not ret:
        print('No Camera')
        break
    #frame = imutils.resize(frame, width=600)
    #转到HSV空间
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    #根据阈值构建掩膜
    mask = cv2.inRange(hsv, redLower, redUpper)
    #腐蚀操作
    # mask = cv2.erode(mask, None, iterations=2)
    #膨胀操作，其实先腐蚀再膨胀的效果是开运算，去除噪点
    # mask = cv2.dilate(mask, None, iterations=2)
    cv2.imshow('mask',mask)
    #轮廓检测
    cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
    #初始化瓶盖圆形轮廓质心
    center = None
    #如果存在轮廓
    if len(cnts) > 10:
        #找到面积最大的轮廓
        c = max(cnts, key = cv2.contourArea)
        #确定面积最大的轮廓的外接圆
        ((x, y), radius) = cv2.minEnclosingCircle(c)
        #计算轮廓的矩
        M = cv2.moments(c)
        #计算质心
        center = (int(M["m10"]/M["m00"]), int(M["m01"]/M["m00"]))
        #只有当半径大于10时，才执行画图
        if radius > 10:
            print('X:%d,Y:%d'%(x,y))
            cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 255), 2)
            cv2.circle(frame, center, 5, (0, 0, 255), -1)
            if x > 320:
                print('左')
            if x < 320:
                print('右')
            #把质心添加到pts中，并且是添加到列表左侧
            # pts.appendleft(center)
    # #遍历追踪点，分段画出轨迹
    # for i in xrange(1, len(pts)):
    #     if pts[i - 1] is None or pts[i] is None:
    #         continue
    #     #计算所画小线段的粗细
    #     thickness = int(np.sqrt(mybuffer / float(i + 1)) * 2.5)
    #     #画出小线段
    #     cv2.line(frame, pts[i - 1], pts[i], (0, 0, 255), thickness)
    #res = cv2.bitwise_and(frame, frame, mask=mask)
    cv2.imshow('Frame', frame)
    #键盘检测，检测到esc键退出
    k = cv2.waitKey(5)&0xFF
    if k == 27:
        break
#摄像头释放
camera.release()
#销毁所有窗口
cv2.destroyAllWindows()

觉得好就点赞吧
o(￣┰￣*)ゞ