通过roslibpy订阅ROS图像话题，格式sensor_msgs/Image，手动解码转换

最新推荐文章于 2025-09-19 17:17:46 发布

原创最新推荐文章于 2025-09-19 17:17:46 发布 · 326 阅读

0 ·

CC 4.0 BY-SA版权

文章标签：

#opencv #计算机视觉

基础架构原理
roslibpy通过WebSocket与ROS Bridge通信时，sensor_msgs/Image消息会被自动序列化为JSON格式传输，其中包含width/height等元数据和base64编码的像素数据。需特别注意原始图像数据采用行优先(row-major)存储方式。
完整实现代码


import roslibpy
import base64
import numpy as np
import cv2
from io import BytesIO

class ROSImageConverter:
    def __init__(self, host='localhost', port=9090):
        self.client = roslibpy.Ros(host=host, port=port)
        self.client.run()
        
    def _decode_image(self, msg):
        # 提取图像元数据
        height = msg['height']
        width = msg['width']
        encoding = msg['encoding']
        
        # Base64解码
        byte_data = base64.b64decode(msg['data'])
        np_arr = np.frombuffer(byte_data, dtype=np.uint8)
        
        # 根据编码重建图像
        if encoding == 'rgb8':
            cv_image = np_arr.reshape((height, width, 3))
            cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR)
        elif encoding == 'bgr8':
            cv_image = np_arr.reshape((height, width, 3))
        elif encoding == 'mono8':
            cv_image = np_arr.reshape((height, width))
        else:
            raise ValueError(f"不支持的编码格式: {encoding}")
            
        return cv_image

    def subscribe(self, topic):
        listener = roslibpy.Topic(
            self.client,
            topic,
            'sensor_msgs/Image'
        )
        listener.subscribe(lambda msg: self._process_image(msg))
        
    def _process_image(self, msg):
        try:
            cv_image = self._decode_image(msg)
            cv2.imshow('ROS Image', cv_image)
            cv2.waitKey(1)
        except Exception as e:
            print(f"图像处理错误: {str(e)}")

if __name__ == '__main__':
    converter = ROSImageConverter()
    converter.subscribe('/camera/image_raw')
    try:
        while converter.client.is_connected:
            pass
    finally:
        converter.client.terminate()
        cv2.destroyAllWindows()