【从零开始的ROS四轴机械臂控制】（四）- ros、gazebo与opencv，图像处理节点

七、图像处理节点

1.节点功能与实现方法

我们的仿真环境已经搭建好了，接下来就是完成相应的控制和服务节点了。 在编写相应节点之前，一个很重要的事情就是理清楚各个节点的功能和各节点之间的逻辑关系。我们要达成一个什么样的目标。为了达成这个目标我们都需要什么样的节点。每个节点具体的实现方式。 以下是图像处理节点的流程图，我们可以从rgb_camera中获取image_raw的输入，然后经过image_process 节点，最后将目标的中心点和目标范围输出。  
 

2.iamge_process 相关程序

image_process节点  

#!/usr/bin/env python

import math
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image, JointState
from cv_bridge import CvBridge, CvBridgeError

# 建立ImgProcess类
class ImgProcess(object):
    def __init__(self):
        self.node_name = 'img_process'

        # 初始化ROS节点
        rospy.init_node(self.node_name)

        # 创建cv_bridge对象
        self.bridge = CvBridge()

        # 订阅rgb相机数据和发布处理过的图像信息
        self.sub1 = rospy.Subscriber("rgb_camera/image_raw", Image, self.img_process_callback)
		self.pub1 = rospy.Publisher("rgb_camera/image_processed", Image, queue_size=10)  # 测试使用
        rospy.loginfo("Waiting for image topics...")

    def image_process(self, image):
    	# 将图像转化为hsv色彩空间
        image_hsv = cv2.cvtColor(image, cv2.COLOR_RGB2HSV)
		
		# 对目标颜色进行提取
        orange_min = np.array([100, 128, 46])
        orange_max = np.array([124, 255, 255])
        mask = cv2.inRange(image_hsv, orange_min, orange_max)
        ret, binary = cv2.threshold(mask, 1, 256, cv2.THRESH_BINARY)

		# 初始化数据
        non0_point = 0
        non0_rows = 0
        non0_cols = 0
        length_max = 0
        width_max = 0
        length_min = 0
        width_min = 0
        point_exist = False
		
		# 遍历图像
        rows, cols = np.shape(binary)
        for m in range(rows):
            for n in range(cols):
                if binary[m, n] != 0:
                	# 获取中心点数据
                    non0_rows += m
                    non0_cols += n
                    non0_point += 1
					
					# 获取目标范围数据
                    if non0_point == 1:
                        width_min = n
                        length_min = m
                    if n < width_min:
                        width_min = n
                    if n > width_max:
                        width_max = n
                    if m < length_min:
                        length_min = m
                    if m > length_max:
                        length_max = m
                        
        if non0_point != 0:
            point_exist = True
            midpoint = [int(non0_rows / non0_point), int(non0_cols / non0_point)]
            min_point = [length_min, width_min]
            max_point = [length_max, width_max]
        else:
            midpoint = 0
            min_point = 0
            max_point = 0

        res = cv2.bitwise_and(image, image, mask=mask)
        cv2.circle(res, (midpoint[1], midpoint[0]), 5, (0, 0, 255), 2)
        cv2.rectangle(res, (min_point[1], min_point[0]), (max_point[1], max_point[0]), (0, 0, 255))

        return res, midpoint, min_point, max_point, point_exist
        
	# 定义图像回调函数
    def img_process_callback(self, ros_image):
        try:
        	# 将图像转化到opencv可以使用的bgr8格式
            frame = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")
        except CvBridgeError:
            rospy.loginfo('CvBrigeError!')

        # 获取numppy矩阵
        frame = np.array(frame,dtype=np.uint8)

        # 使用image_process来处理图片
        processed_image, midpoint, min_point, max_point, point_exist = self.image_process(frame)

        if point_exist:
            rospy.loginfo('midpoint: %s, image_range: %s, %s', midpoint, min_point, max_point)
            ## 本部分为测试部分
            # 将图像转换成ros信息发布到rab_camera/下，以便rqt可以直接使用
	    	processed_Image = self.bridge.cv2_to_imgmsg(processed_image,"bgr8")
	    	self.pub1.publish(processed_Image)
        else:
            rospy.loginfo('No targets were detected')


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

部分程序解释

		...
		try:
        	# 将图像转化到opencv可以使用的bgr8格式
            frame = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")
			...
		if point_exist:
			...
			processed_Image = self.bridge.cv2_to_imgmsg(processed_image,"bgr8")
			...

  ROS以其自己的sensor_msgs/Image消息格式发布图像，但是我们需要将图像转换成OpenCV可以使用的格式。 CvBridge是一个ROS库，提供ROS和OpenCV之间的接口。在这里使用了bridge.imgmsg_to_cv2和bridge.cv2_to_imgmsg两个函数，图像格式选择了“bgr8”。其前后格式要保持一致。  

		# 遍历图像
        rows, cols = np.shape(binary)
        for m in range(rows):
            for n in range(cols):
                if binary[m, n] != 0:
                	# 获取中心点数据
                    non0_rows += m
                    non0_cols += n
                    non0_point += 1
					
					# 获取目标范围数据
                    if non0_point == 1:
                        width_min = n
                        length_min = m
                    if n < width_min:
                        width_min = n
                    if n > width_max:
                        width_max = n
                    if m < length_min:
                        length_min = m
                    if m > length_max:
                        length_max = m

  此部分为遍历图像获取二值化图像的非零点的位置。非零点就是我们使用颜色阈值的目标区域。non0_point是非零点的个数、non0_rows和non0_cols是图像非零点的行坐标和纵坐标的累加值，其目的是为了计算目标中心。 width_min和length_min、width_max和length_max是目标矩形框图的两个角的坐标。 此外，我在编写imge_process时候采用了本地测试。究其原因，ros是虚拟机环境，编译器也没有pycharm方便。以下是本地测试时使用的程序：  

import cv2
import numpy as np
def image_process(image):
    # 将图像转换到hsv色彩空间
    image_hsv = cv2.cvtColor(image, cv2.COLOR_RGB2HSV)

    # 颜色提取
    orange_min = np.array([100, 128, 46])
    orange_max = np.array([124, 255, 255])
    mask = cv2.inRange(image_hsv, orange_min, orange_max)
    ret, binary = cv2.threshold(mask, 1, 256, cv2.THRESH_BINARY)

    # 数据初始化
    non0_point = 0
    non0_rows = 0
    non0_cols = 0
    length_max = 0
    width_max = 0
    length_min = 0
    width_min = 0


    # 目标确定
    rows, cols = np.shape(binary)
    print([rows, cols])
    for m in range(rows):
        for n in range(cols):
            if binary[m,n] != 0:
                # 确定中心点
                non0_rows += m
                non0_cols += n
                non0_point += 1

                # 确定object范围
                if non0_point == 1:
                    width_min = n
                    length_min = m
                if n < width_min:
                    width_min = n
                if n > width_max:
                    width_max = n
                if m < length_min:
                    length_min = m
                if m > length_max:
                    length_max = m
    # 中心点坐标
    midpoint=[int(non0_rows/non0_point), int(non0_cols/non0_point)]

    # object范围坐标
    min_point = [length_min, width_min]
    max_point = [length_max, width_max]

    print('midpoint:',midpoint, 'range:', min_point, max_point)
    res = cv2.bitwise_and(image, image, mask=mask)
    return res,midpoint, min_point, max_point


if __name__ == '__main__':
    image = cv2.imread('E:/model/arm_0.1/color.png')
    image_test, midpoint, min_point, max_point = image_process(image)
    print(type(midpoint))
    cv2.circle(image_test, (midpoint[1], midpoint[0]), 5, (0, 0, 255), 2)
    cv2.rectangle(image_test, (min_point[1], min_point[0]), (max_point[1], max_point[0]), (0, 0, 255))
    cv2.imshow('image', image_test)
    cv2.waitKey()

  以下为测试程序的输出。测试程序和img_process节点中使用的函数还是有些不一样的。测试程序只是作为验证逻辑关系，此外在img_process中修复了一些ros运行上的bug。  
 

3.节点运行与测试

将image_process文件保存在~/catkin_ws/src/arm1/script/目录下。 运行命令：  

$  cd ~/catkin_ws/src/arm1/script/
$  sudo chmod 777 image_process

  新打开一个终端，按照之前介绍的操作打开gazebo，然后插入物块，再将之调整到合适的位置上，比如下图。  
  新建一个终端,输入命令来运行img_process节点  

$ source ~/catkin_ws/devel/setup.bash
$ rosrun arm1 image_process

  若一切运行正常，则如下图所示：  
  （ps，如果此时运行simple_mover来使机械臂移动，img_process在显示上出现了一个较大的延迟，需要后期改进img_process算法） 新建一个终端，输入命令  

$ rqt_image_view /rgb_camera/image_raw

  若运行正常就会如下图所示：  
  更换image_view，改为查看image_processed发布的图像，则如下图所示，相关区域已经被标注：  
  此时可以使用命令来查看节点关系图：  

rosrun rqt_graph rqt_graph

 
  现在我们的图像处理节点已经基本编写完成，接下来就是运动控制部分了。