一、視覺模塊架構(gòu)設(shè)計(jì)

考慮的幾個(gè)方面：

傳感器主要使用話題（topic）通信機(jī)制持續(xù)向外部發(fā)布圖像信息；

圖像接受與處理、以及發(fā)送處理結(jié)果節(jié)點(diǎn)有兩種形式，一種是使用服務(wù)（service）通信機(jī)制，一種是使用話題機(jī)制，兩者均可。本人在網(wǎng)上搜集了一些信息，并且參考了chatgpt的意見，得到了一個(gè)不錯(cuò)的結(jié)果：話題速度更快，并且實(shí)現(xiàn)更簡(jiǎn)單，開發(fā)中一般默認(rèn)使用話題，如果隨著開發(fā)的進(jìn)行，話題不再滿足我們的需求，可以轉(zhuǎn)至service機(jī)制。由于工業(yè)機(jī)械臂中的傳感器（camera）和AI模型（一般一個(gè)機(jī)械臂只會(huì)用到一個(gè)）并不復(fù)雜，所以我選擇topic通信機(jī)制開發(fā)圖像數(shù)據(jù)的接受處理、結(jié)果發(fā)送模塊。

使用訂閱者來接受CV model的處理結(jié)果。

[sensor publisher (camera_pub.py)] --> [subscriber and publisher node (cam_sub_and_detection_pub.py)] --> [subscriber (detection_results_sub.py)]

二、代碼編寫

一、新建工作空間

1. 創(chuàng)建src文件夾以存放源碼；

2. 在src目錄下新建cv_devel_pkg與interfaces_pkg，分別存放視覺開發(fā)模塊的源碼與topic數(shù)據(jù)接口（interface）文件；

2.1 interfaces_pkg編寫

需要注意的是，在新建interface pkg時(shí)，build type暫時(shí)只能選擇c++（信息來源：ros官方文檔），并且我們需要修改cmakelists.txt與package.xml：

cmakelists.txt:

新增：

# find_package( REQUIRED)


find_package(geometry_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)


rosidl_generate_interfaces(${PROJECT_NAME}
"msg/DetectionResults.msg"
s  DEPENDENCIES geometry_msgs
)

package.xml：

新增：

 
geometry_msgs
rosidl_default_generators
rosidl_default_runtime
rosidl_interface_packages
 

其中，DetectionResults.msg中存放的信息是結(jié)果中心坐標(biāo)的msg：

int32 position_x

int32 position_y

至此，interface pkg代碼編寫結(jié)束。

2.2 cv_devel_pkg編寫

(1) camera_pub.py

import rclpy
from rclpy.node import Node
from cv_bridge import CvBridge
from sensor_msgs.msg import Image


import cv2




class CameraPubNode(Node):
def __init__(self, name):
super().__init__(name)
self.pub = self.create_publisher(Image, 'image_raw', 10)
self.timer = self.create_timer(0.5, self.timer_callback)
self.cap = cv2.VideoCapture(0)
self.cv_bridge = CvBridge()




def timer_callback(self):
        ret = self.cap.grab()
if ret:
            flag, frame = self.cap.retrieve()
if flag:
self.pub.publish(self.cv_bridge.cv2_to_imgmsg(frame, 'bgr8'))
self.get_logger().info('Publish image successfully!')
else:
self.get_logger().info('Did not get image info!')




def main(args=None):
    rclpy.init(args=args)
    node = CameraPubNode('CameraPubNode')
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

編寫完成后，在pkg目錄下的setup.py中注冊(cè)節(jié)點(diǎn)，并分別執(zhí)行colcon build、source install/local_setup.sh、ros2 run cv_devel_pkg camera_pub。

如圖，正常運(yùn)行：

(2) cam_sub_and_detection_pub.py

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from interfaces_pkg.msg import DetectionResults
from cv_bridge import CvBridge


import cv2
import numpy as np




class CamSubAndDetectionPubNode(Node):
def __init__(self, name):
        super().__init__(name)
        self.sub = self.create_subscription(Image, 'image_raw', self.listen_callback, 10)
        self.pub = self.create_publisher(DetectionResults, 'detection_results', 10)
        self.cv_bridge = CvBridge()
        self.position_x = 0
        self.position_y = 0


def listen_callback(self, data):
        self.get_logger().info('Get image! I will process it!')
        image = self.cv_bridge.imgmsg_to_cv2(data, 'bgr8')
        self.detect(image)
        position = DetectionResults()
        position.position_x = self.position_x
        position.position_y = self.position_y
        self.get_logger().info('Position is: ({}, {})'.format(self.position_x, self.position_y))
        self.pub.publish(position)


def detect(self, image):
pass#這里可以嵌入自己的機(jī)器視覺或者AI視覺代碼


def main(args=None):
    rclpy.init(args=args)
    node = CamSubAndDetectionPubNode('CamSubAndDetectionPubNode')
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

（3）detection_results_sub.py

import rclpy
from rclpy.node import Node
from interfaces_pkg.msg import DetectionResults




class DetectionResultsSubNode(Node):
def __init__(self, name):
super().__init__(name)
self.sub = self.create_subscription(DetectionResults, 'detection_results', self.listen_callback, 10)


def listen_callback(self, data):
self.get_logger().info('I get the position: ({},{})'.format(data.position_x, data.position_y))




def main(args=None):
    rclpy.init(args=args)
    node = DetectionResultsSubNode('detection_results_sub_node')
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

三、完工

cv_devel_pkg中的節(jié)點(diǎn)代碼全部編寫完成后，在setup.py中注冊(cè)，然后build & run。

檢測(cè)結(jié)果展示：

三個(gè)節(jié)點(diǎn)可正常運(yùn)行：

審核編輯：劉清