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