一、視覺模塊架構設計

考慮的幾個方面：

傳感器主要使用話題（topic）通信機制持續向外部發布圖像信息；

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

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

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

二、代碼編寫

一、新建工作空間

1. 創建src文件夾以存放源碼；

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

2.1 interfaces_pkg編寫

需要注意的是，在新建interface pkg時，build type暫時只能選擇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中存放的信息是結果中心坐標的msg：

int32 position_x

int32 position_y

至此，interface pkg代碼編寫結束。

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中注冊節點，并分別執行colcon build、source install/local_setup.sh、ros2 run cv_devel_pkg camera_pub。

如圖，正常運行：

(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#這里可以嵌入自己的機器視覺或者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中的節點代碼全部編寫完成后，在setup.py中注冊，然后build & run。

檢測結果展示：

三個節點可正常運行：

審核編輯：劉清