win10 + python3 + 安裝rosbag & cv_bridge & sensor_msgs+解析bag文件生成pcd和圖片

anaconda

需要在anaconda 環境下進行安裝，推薦使用。

• 官網：https://www.anaconda.com/
• 用法：https://www.cnblogs.com/yunhgu/p/15498134.html

安裝rosbag

進入anaconda環境下,運行如下命令，可能會報超時錯誤，跟你網絡有關系。需要你修改hosts來訪問github。
其他法子：
https://blog.csdn.net/South_Li/article/details/129188984

pip install --extra-index-url https://rospypi.github.io/simple/ rosbag
pip install roslz4 --extra-index-url https://rospypi.github.io/simple/

安裝成功后，測試

安裝cv_brigde

cv_bridge下載(源代碼https://github.com/ros-perception/vision_opencv)：https://codeload.github.com/ros-perception/vision_opencv/zip/refs/heads/noetic
下載完成后cd至cv_bridge文件夾，然后cmd打開命令行窗口：

python setup.py install

安裝成功后測試

whl包

https://files.cnblogs.com/files/yunhgu/rosbag_cv_bridge.zip

安裝sensor_msgs

這個比較簡單直接使用pip就可以了

pip install sensor_msgs --extra-index-url https://rospypi.github.io/simple/

問題

實際使用過程中遇到了一個問題，在解析圖片信息的時候

from cv_bridge.boost.cv_bridge_boost import cvtColor2

上面這個導入報了錯
No module named 'cv_bridge.boost'

解決方法：
下載這個
https://github.com/rospypi/simple/raw/any/cv-bridge/cv_bridge-1.13.0.post0-py2.py3-none-any.whl
然后進入anconda界面

pip install cv_bridge-1.13.0.post0-py2.py3-none-any.whl

從信息得出可能是版本的問題。

python3 代碼解析bag文件

# -*- coding: utf-8 -*-#
# -------------------------------------------------------------------------------
# Name:         parse_bag
# Author:       yunhgu
# Date:         2022/1/10 11:01
# Description: 
# -------------------------------------------------------------------------------
import copy
import struct
from pathlib import Path
from traceback import format_exc

import cv2
import numpy as np
import rosbag
import sensor_msgs.point_cloud2 as pc2
from cv_bridge import CvBridge

PCD_ASCII_TEMPLATE = """VERSION 0.7
FIELDS x y z intensity
SIZE 4 4 4 2
TYPE F F F U
COUNT 1 1 1 1
WIDTH {}
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS {}
DATA ascii
"""

PCD_BINARY_TEMPLATE = """VERSION 0.7
FIELDS x y z intensity
SIZE 4 4 4 4
TYPE F F F F
COUNT 1 1 1 1
WIDTH {}
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS {}
DATA binary
"""


class BagExtractor:
    def __init__(self, bag_folder, dst_folder):
        self.bag_folder = Path(bag_folder)
        self.dst_folder = Path(dst_folder)
        self.bridge = CvBridge()

    def extract_pcd_img(self, pcd_topic_list: list, img_topic_list: list):
        """
        :param pcd_topic_list: 點雲文件topic名字列表
        :param img_topic_list: 圖片文件topic名字列表
        :return:
        """
        for bag_file in self.bag_folder.rglob("*.bag"):
            output_file = self.dst_folder.joinpath(bag_file.relative_to(self.bag_folder))
            output_folder = output_file.parent.joinpath(f"{output_file.stem}")
            output_folder.mkdir(parents=True, exist_ok=True)
            with rosbag.Bag(bag_file, 'r') as bag:
                info = bag.get_type_and_topic_info()
                print(info.topics)
                pcd_number = self.get_pcd_img_number(info, pcd_topic_list)
                img_number = self.get_pcd_img_number(info, img_topic_list)
                print(f"解析{bag_file.name} pcd總數:{pcd_number} 圖片總數:{img_number}")
                for topic, msg, t in bag.read_messages():
                    time_str = "%.9f" % msg.header.stamp.to_sec()
                    if topic in pcd_topic_list:  # 點雲的topic
                        pcd_path = output_folder.joinpath(f"{time_str}.pcd")
                        # self.to_pcd_ascii(pcd_path, msg)
                        self.to_pcd_binary(pcd_path, msg)
                        print(f"Extract pcd file {time_str}.pcd")
                    if topic in img_topic_list:  # 圖片的topic
                        img_path = output_folder.joinpath(f"{time_str}.png")
                        self.to_img(img_path, msg)
                        print(f"Extract img file {time_str}.png")

    @classmethod
    def get_pcd_img_number(cls, info, topic_list):
        try:
            for topic in topic_list:
                topic_ob = info.topics.get(topic, None)
                if topic_ob:
                    return topic_ob.message_count
            return 0
        except Exception as e:
            print(f"獲取pcd|img幀數出錯:{e}")
            return 0

    @classmethod
    def to_pcd_ascii(cls, pcd_path, msg):
        with open(pcd_path, 'w') as f:
            points_data = np.array(list(pc2.read_points(msg)))
            lidar = list(np.delete(points_data, np.where(np.isnan(points_data))[0], axis=0))
            header = copy.deepcopy(PCD_ASCII_TEMPLATE).format(len(lidar), len(lidar))
            f.write(header)
            for pi in lidar:
                f.write(' '.join([str(i) for i in pi]) + '\n')

    @classmethod
    def to_pcd_binary(cls, pcd_path, msg):
        with open(pcd_path, 'wb') as f:
            points_data = np.array(list(pc2.read_points(msg)))
            lidar = list(np.delete(points_data, np.where(np.isnan(points_data))[0], axis=0))
            header = copy.deepcopy(PCD_BINARY_TEMPLATE).format(len(lidar), len(lidar))
            f.write(header.encode())
            for pi in lidar:
                h = struct.pack('ffff', pi[0], pi[1], pi[2], pi[3])
                f.write(h)

    def to_img(self, img_path, msg):
        try:
            # cv_image = self.bridge.compressed_imgmsg_to_cv2(msg)
            cv_image = self.bridge.imgmsg_to_cv2(msg)
            cv2.imencode('.png', cv_image)[1].tofile(str(img_path))
        except Exception as e:
            print(f"生成圖片失敗:{e}")


if __name__ == '__main__':
    try:
        bag_path = r'C:\Users\pc\Desktop\bag測試數據\data'  # bag文件路徑
        dst_path = r'C:\Users\pc\Desktop\bag測試數據\result'  # 結果路徑
        extractor = BagExtractor(bag_path, dst_path)
        extractor.extract_pcd_img(pcd_topic_list=['/rslidar_points', '/zvision_lidar_points'],
                                  img_topic_list=['/usb_cam/image_raw', '/zed2/zed_node/left/image_rect_color'])
    except Exception as ee:
        print(f"運行失敗,無法解決請連續開發人員!{format_exc()}{ee}")

rospy 路徑

https://rospypi.github.io/simple