【從零開始的ROS四軸機械臂控制（四）】

• • 七、圖像處理節(jié)點
  • • 1.節(jié)點功能與實現(xiàn)方法
    • 2.iamge_process 相關(guān)程序
    • • • 部分程序解釋
    • 3.節(jié)點運行與測試

七、圖像處理節(jié)點

1.節(jié)點功能與實現(xiàn)方法

我們的仿真環(huán)境已經(jīng)搭建好了，接下來就是完成相應的控制和服務(wù)節(jié)點了。

在編寫相應節(jié)點之前，一個很重要的事情就是理清楚各個節(jié)點的功能和各節(jié)點之間的邏輯關(guān)系。我們要達成一個什么樣的目標。為了達成這個目標我們都需要什么樣的節(jié)點。每個節(jié)點具體的實現(xiàn)方式。

以下是圖像處理節(jié)點的流程圖，我們可以從rgb_camera中獲取image_raw的輸入，然后經(jīng)過image_process 節(jié)點，最后將目標的中心點和目標范圍輸出。

2.iamge_process 相關(guān)程序

image_process節(jié)點

#!/usr/bin/env pythonimport 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節(jié)點rospy.init_node(self.node_name)# 創(chuàng)建cv_bridge對象self.bridge = CvBridge()# 訂閱rgb相機數(shù)據(jù)和發(fā)布處理過的圖像信息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):# 將圖像轉(zhuǎn)化為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)# 初始化數(shù)據(jù)non0_point = 0non0_rows = 0non0_cols = 0length_max = 0width_max = 0length_min = 0width_min = 0point_exist = False# 遍歷圖像rows, cols = np.shape(binary)for m in range(rows):for n in range(cols):if binary[m, n] != 0:# 獲取中心點數(shù)據(jù)non0_rows += mnon0_cols += nnon0_point += 1# 獲取目標范圍數(shù)據(jù)if non0_point == 1:width_min = nlength_min = mif n < width_min:width_min = nif n > width_max:width_max = nif m < length_min:length_min = mif m > length_max:length_max = mif non0_point != 0:point_exist = Truemidpoint = [int(non0_rows / non0_point), int(non0_cols / non0_point)]min_point = [length_min, width_min]max_point = [length_max, width_max]else:midpoint = 0min_point = 0max_point = 0res = 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# 定義圖像回調(diào)函數(shù)def img_process_callback(self, ros_image):try:# 將圖像轉(zhuǎn)化到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)## 本部分為測試部分# 將圖像轉(zhuǎn)換成ros信息發(fā)布到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:# 將圖像轉(zhuǎn)化到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消息格式發(fā)布圖像，但是我們需要將圖像轉(zhuǎn)換成OpenCV可以使用的格式。

CvBridge是一個ROS庫，提供ROS和OpenCV之間的接口。在這里使用了bridge.imgmsg_to_cv2和bridge.cv2_to_imgmsg兩個函數(shù)，圖像格式選擇了“bgr8”。其前后格式要保持一致。

---

		# 遍歷圖像rows, cols = np.shape(binary)for m in range(rows):for n in range(cols):if binary[m, n] != 0:# 獲取中心點數(shù)據(jù)non0_rows += mnon0_cols += nnon0_point += 1# 獲取目標范圍數(shù)據(jù)if non0_point == 1:width_min = nlength_min = mif n < width_min:width_min = nif n > width_max:width_max = nif m < length_min:length_min = mif m > length_max:length_max = m

此部分為遍歷圖像獲取二值化圖像的非零點的位置。非零點就是我們使用顏色閾值的目標區(qū)域。non0_point是非零點的個數(shù)、non0_rows和non0_cols是圖像非零點的行坐標和縱坐標的累加值，其目的是為了計算目標中心。

width_min和length_min、width_max和length_max是目標矩形框圖的兩個角的坐標。

此外，我在編寫imge_process時候采用了本地測試。究其原因，ros是虛擬機環(huán)境，編譯器也沒有pycharm方便。以下是本地測試時使用的程序：

import cv2
import numpy as npdef image_process(image):# 將圖像轉(zhuǎn)換到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)# 數(shù)據(jù)初始化non0_point = 0non0_rows = 0non0_cols = 0length_max = 0width_max = 0length_min = 0width_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 += mnon0_cols += nnon0_point += 1# 確定object范圍if non0_point == 1:width_min = nlength_min = mif n < width_min:width_min = nif n > width_max:width_max = nif m < length_min:length_min = mif 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_pointif __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節(jié)點中使用的函數(shù)還是有些不一樣的。測試程序只是作為驗證邏輯關(guān)系，此外在img_process中修復了一些ros運行上的bug。

3.節(jié)點運行與測試

將image_process文件保存在~/catkin_ws/src/arm1/script/目錄下。

運行命令：

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

新打開一個終端，按照之前介紹的操作打開gazebo，然后插入物塊，再將之調(diào)整到合適的位置上，比如下圖。

新建一個終端,輸入命令來運行img_process節(jié)點

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

若一切運行正常，則如下圖所示：

（ps，如果此時運行simple_mover來使機械臂移動，img_process在顯示上出現(xiàn)了一個較大的延遲，需要后期改進img_process算法）

新建一個終端，輸入命令

$ rqt_image_view /rgb_camera/image_raw

若運行正常就會如下圖所示：

更換image_view，改為查看image_processed發(fā)布的圖像，則如下圖所示，相關(guān)區(qū)域已經(jīng)被標注：

此時可以使用命令來查看節(jié)點關(guān)系圖：

rosrun rqt_graph rqt_graph

現(xiàn)在我們的圖像處理節(jié)點已經(jīng)基本編寫完成，接下來就是運動控制部分了。

總結(jié)

以上是生活随笔為你收集整理的【从零开始的ROS四轴机械臂控制】（四）- ros、gazebo与opencv，图像处理节点的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。