ROS2&AI电脑摄像头intel-D435，利用ros2发布订阅图像(Python)

Posted 2022-10-25 [十七画生]

本文欲分享两个代码来实现图像的传输，利用ros2，ROS2～

配置：Ubuntu20.04 ; Python ;ROS2 foxy ; opencv ；电脑相机 or Intel-D435相机

与传统的传输列表、字符串msg不同（定义消息类型直接发送即可），利用ros2传输图像需要把图像frame转为image类型的msg。

流程如下：
opencv或者realsense获取得到的图像（frame）-->  np.array --> msg消息类型（利用CvBridge）

1. 利用电脑摄像头 传输图像，直接上代码

1.1 发送端 如下

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
import cv2
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
import time
from sensor_msgs.msg import Image

class NodePublisher(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info("大家好，我是%s!" % name)

def main(args=None):
    height = 480
    width =  640
    capture = cv2.VideoCapture(0)
    #这里opencv的图像大小与ros发布的图像大小一致
    capture.set(cv2.CAP_PROP_FRAME_WIDTH, width)    
    capture.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
    capture.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'))  

    rclpy.init()

    node = NodePublisher("Camera_image") # 实例化创建一个节点
    # 创建一个话题--image_data，定义其中的消息类型为Image
    image_pub = node.create_publisher(Image,"image_data",10) 

    bridge = CvBridge() # 转换为ros2的消息类型(imgmsg)的工具

    while True:       
        # 以下三行为图像的消息转换，frame --> np.array --> imgmsg(可直接ros2发布)
        ret, frame = capture.read()        
        frame = np.array(cv2.flip(frame,1))   # 镜像操作,且转为numpy.array   
        # 转换为ros2消息类型，且解码方式为b(blue)、g(green)、r(red)        
        data = bridge.cv2_to_imgmsg(frame,encoding="bgr8") 

        image_pub.publish(data) # 发布 转换好的 图像类型消息

1.2 接收端 如下

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image

bridge = CvBridge() # 转换为ros2的消息类型(imgmsg)的工具

class NodeSubscribe(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info("大家好，我是%s!" % name)

    def callback(self,data):
        global bridge
        # ros2消息类型(imgmsg)转换为np.array
        cv_img = bridge.imgmsg_to_cv2(data, "bgr8") 
       
        cv2.imshow("frame" , cv_img) # 显示接受到的图像数据
        cv2.waitKey(1)


def main(args=None):
    rclpy.init()
    node = NodeSubscribe("image_node") # 实例化创建一个节点--image_node
    # 创建一个话题(image_data)得与发送端一致，定义其中的消息类型为Image。利用callback函数持续接收
    node.create_subscription(Image,'image_data', node.callback, 10)
    
    rclpy.spin(node)
    rclpy.shutdown()

1.3 运行演示

(ROS2 需要建立工作空间，把代码放在相应的工作区并配置好配置文件)：

---

 2. 利用D435 传输图像，直接上代码

首先需要通过realsense库获取D435相机的视频流frame，接下来就是和上述传输视频流一样了。详细的解释已在代码中注释，直接看代码即可。

2.1 发送端 如下

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
import cv2
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
from sensor_msgs.msg import Image

import pyrealsense2 as rs
# ========== 以下为d435相机的配置 ========
pipe = rs.pipeline()
cfg = rs.config()
cfg.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
cfg.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
profile = pipe.start(cfg)
align_to = rs.stream.color
align = rs.align(align_to)  # combine depth map and color image
# =====================================


class NodePublisher(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info("大家好，我是%s!" % name)


def main(args=None):
    rclpy.init(args=None)

    node = NodePublisher("pub_image_node")
    image_pub = node.create_publisher(Image,"image_data",10)
    image_pub2 = node.create_publisher(Image,"image_data2",10)

    bridge = CvBridge()

    while True:

        frame = pipe.wait_for_frames()

        # ==== 得到d435的 彩色图像流 ===
        color_frame = align.process(frame).get_color_frame()
        color_frame = np.asanyarray(color_frame.get_data())

        # ==== 得到d435的 深度图像流 ===
        colorizer = rs.colorizer()
        depth_frame = align.process(frame).get_depth_frame()
        colored_depth_frame = np.asanyarray(colorizer.colorize(depth_frame).get_data())

        # ==== 发布 彩色图像流 消息 ====
        img_msg = bridge.cv2_to_imgmsg(color_frame, encoding = "bgr8")
        image_pub.publish(img_msg)
 
        # ==== 发布 深度图像流 消息 ====
        # img_msg2 = bridge.cv2_to_imgmsg(depth_frame, encoding = "16UC1") 
        img_msg2 = bridge.cv2_to_imgmsg(colored_depth_frame, encoding = "bgr8")
        image_pub2.publish(img_msg2) 


if __name__ =="__main__":
    main()

2.2 接收端 如下

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image

bridge = CvBridge()


class NodeSubscribe(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info("大家好，我是%s!" % name)

    def callback(self,data):
        cv_img = bridge.imgmsg_to_cv2(data, "bgr8")
        cv2.imshow("color_frame" , cv_img)
        cv2.waitKey(1)
        
    def callback2(self,data):
        cv_img = bridge.imgmsg_to_cv2(data, "bgr8")
        cv2.imshow("color_depth_frame" , cv_img)
        cv2.waitKey(1)


def main(args=None):
    rclpy.init()
    # 建立一个节点(sub_image_node)用来接受以下两个话题中的图像数据
    node = NodeSubscribe("sub_image_node")
    # 接收话题image_data中的图像数据，并可视化
    node.create_subscription(Image,'image_data', node.callback, 10)
    # 接收话题image_data2中的图像数据，并可视化
    node.create_subscription(Image,'image_data2', node.callback2, 10)
    
    rclpy.spin(node)
    rclpy.shutdown()

2.3 运行演示

(同样需要利用ROS2建立工作空间，把代码放在相应的工作区并配置好配置文件，方可运行)：

3.总结

掌握好ros2基本的发布订阅框架之后，把获取的图像frame通过cvridge转化为ros2的消息类型之后，即可发布。同样接收端用把接收到的消息用cvbridge转化后，即可做后续的图像处理操作等。

详细的解释，已经在代码中的注释一一列出。有用点个赞，准备下期出个--完全零基础的基于ros2的发布订阅教程(Python)。