ROS利用仿真摄像头识别并抓取物体（一）
ROS利用仿真摄像头识别物体
在机械臂URDF文件中添加仿真摄像头、桌子和物体。运行launch文件启动gazebo。
roslaunch prrobot_gazebo probot_anno_with_gripper_gazebo_world.launch
运行结果如下图所示：
接下来运行以下python程序对绿色物体进行识别
#!/usr/bin/env python
# -*- coding: utf-8 -*-
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 Image_converter:
def __init__(self):
self.image_pub = rospy.Publisher('table_detect_test',Image,queue_size = 10)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber('/probot_anno/camera/image_raw',Image,self.callback)
def callback(self,data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data,"bgr8")
except CvBridgeError as e:
print e
size = self.detect_table(cv_image)
detect_image = self.detect_box(size[0], size[1], size[2], size[3], size[4])
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(detect_image, "bgr8"))
except CvBridgeError as e:
print e
def detect_table(self,image):
b, g, r = cv2.split(image)
binary_image = cv2.medianBlur(r, 3)
for i in range (binary_image.shape[0]):
for j in range (binary_image.shape[1]):
editValue = binary_image[i, j]
editValue2 = g[i, j]
if editValue >= 0 and editValue < 20 and editValue2 >= 0 and editValue2 < 20:
binary_image[i, j] = 255
else:
binary_image[i, j] = 0
img, cnts, hierarchy = cv2.findContours(binary_image, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
x, y, w, h = cv2.boundingRect(binary_image)
cv2.rectangle(image, (x, y), (x+w, y+h), (0, 0, 255), 5)
# loop over the contours
for c in cnts:
# compute the center of the contour
M = cv2.moments(c)
if int(M["m00"]) not in range(20000, 250000):
continue
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
cv2.circle(image, (cX, cY), 5, (0, 0, 255), -1)
return image, x, y, w, h
def detect_box(self,image, x, y, w, h):
b, g, r = cv2.split(image)
binary_image = cv2.medianBlur(g, 3)
for i in range (binary_image.shape[0]):
for j in range (binary_image.shape[1]):
editValue = binary_image[i, j]
if i < y or i > y+h or j < x or j > x+w:
binary_image[i, j] = 0
else:
if editValue > 120 and editValue <= 255:
binary_image[i, j] = 255
else:
binary_image[i, j] = 0
img, cnts, hierarchy = cv2.findContours(binary_image, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
cv2.drawContours(image, cnts, -1, (255, 0, 0), 4)
for c in cnts:
# compute the center of the contour
M = cv2.moments(c)
if int(M["m00"]) not in range(20000, 250000):
continue
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
cv2.circle(image, (cX, cY), 5, (255, 0, 0), -1)
return image
if __name__ == "__main__":
rospy.init_node("vision_manager")
rospy.loginfo("start")
Image_converter()
rospy.spin()
识别结果如下图所示：
程序基本思路：
创建一个订阅者，用来订阅仿真摄像头发布的图像信息；
创建一个发布者，用于发布识别后的图像信息。
self.image_pub = rospy.Publisher('table_detect_test',Image,queue_size = 10)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber('/probot_anno/camera/image_raw',Image,self.callback)
订阅器订阅到摄像头信息/probot_anno/camera/image_raw后，就会进入回调函数。在回调函数中，首先将image信息转成Opencv可以识别的cv_image,然后运行两个图像识别函数，最后将识别的opencv图像信息专程ROS识别的image信息发布出去。
def callback(self,data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data,"bgr8")
except CvBridgeError as e:
print e
size = self.detect_table(cv_image)
detect_image = self.detect_box(size[0], size[1], size[2], size[3], size[4])
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(detect_image, "bgr8"))
except CvBridgeError as e:
print e
物体识别过程主要分别两部，即两个函数，第一部识别图像中黑色的桌面。将图像的红色通道分离出来，通过红色通道的颜色差异进行图像二值化，然后通过cv2.findContours进行识别，并通过cv2.boundingRect匡出桌子，函数返回桌子的坐标。
def detect_table(self,image):
b, g, r = cv2.split(image)
binary_image = cv2.medianBlur(r, 3)
for i in range (binary_image.shape[0]):
for j in range (binary_image.shape[1]):
editValue = binary_image[i, j]
editValue2 = g[i, j]
if editValue >= 0 and editValue < 20 and editValue2 >= 0 and editValue2 < 20:
binary_image[i, j] = 255
else:
binary_image[i, j] = 0
img, cnts, hierarchy = cv2.findContours(binary_image, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
x, y, w, h = cv2.boundingRect(binary_image)
cv2.rectangle(image, (x, y), (x+w, y+h), (0, 0, 255), 5)
# loop over the contours
for c in cnts:
# compute the center of the contour
M = cv2.moments(c)
if int(M["m00"]) not in range(20000, 250000):
continue
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
cv2.circle(image, (cX, cY), 5, (0, 0, 255), -1)
return image, x, y, w, h
第二部在桌子范围内对绿色物体进行识别，方法与上一步方法一模一样，只是多了一个桌面的范围限制。
def detect_box(self,image, x, y, w, h):
b, g, r = cv2.split(image)
binary_image = cv2.medianBlur(g, 3)
for i in range (binary_image.shape[0]):
for j in range (binary_image.shape[1]):
editValue = binary_image[i, j]
if i < y or i > y+h or j < x or j > x+w:
binary_image[i, j] = 0
else:
if editValue > 120 and editValue <= 255:
binary_image[i, j] = 255
else:
binary_image[i, j] = 0
img, cnts, hierarchy = cv2.findContours(binary_image, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
cv2.drawContours(image, cnts, -1, (255, 0, 0), 4)
for c in cnts:
# compute the center of the contour
M = cv2.moments(c)
if int(M["m00"]) not in range(20000, 250000):
continue
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
cv2.circle(image, (cX, cY), 5, (255, 0, 0), -1)
return image