功能简介:
摄像头识别红色物体,框选(圆),并把图像信息发布
同时发布控制信息,当红色物体在左侧时,小乌龟左转,以此类推
实现原理:
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(￣┰￣*)ゞ
