livox/CustomMsg消息从ROS1 bag转换成ROS2

首先把ROSbag转化成ros2的db3格式

rosbags-convert --src Shield_tunnel1_gamma.bag --dst shield1

由于消息格式livox_ros_driver/msg/CustomMsg不会自动转化成livox_ros_driver2/msg/CustomMsg，在ros2 bag play时是不识别的，因此需要用脚本转换：

from pathlib import Path
from rosbags.rosbag2 import Reader, Writer
from rosbags.typesys import Stores, get_typestore
from rosbags.typesys.msg import get_types_from_msg  # 确保从这里导入
import shutil
import numpy as np

# --- 请准确复制你的 CustomPoint.msg 和 CustomMsg.msg 内容 ---
CUSTOM_POINT_MSG = """
uint32 offset_time
float32 x
float32 y
float32 z
uint8 reflectivity
uint8 tag
uint8 line
"""

CUSTOM_MSG = """
std_msgs/Header header
uint64 time_base
uint32 point_num
uint8 lidar_id
uint8[3] rsvd
CustomPoint[] points
"""
# --- 结束 ---

def convert_bag(input_path: str, output_path: str):
    # 使用你的 ROS2 版本（Humble 用 ROS2_HUMBLE）
    typestore = get_typestore(Stores.ROS2_HUMBLE)  # Iron 用 ROS2_IRON 等

    # 注册类型
    typestore.register({
        **get_types_from_msg(CUSTOM_POINT_MSG, 'livox_ros_driver/msg/CustomPoint'),
        **get_types_from_msg(CUSTOM_MSG, 'livox_ros_driver/msg/CustomMsg'),
        **get_types_from_msg(CUSTOM_POINT_MSG, 'livox_ros_driver2/msg/CustomPoint'),
        **get_types_from_msg(CUSTOM_MSG, 'livox_ros_driver2/msg/CustomMsg'),
    })

    # 先创建输出目录（Writer 需要它存在）
    if Path(output_path).exists():
        shutil.rmtree(output_path)
    # Path(output_path).mkdir(parents=True, exist_ok=True)

    with Reader(input_path) as reader:
        with Writer(path=output_path,version=9) as writer:  # 这里修正：用 keyword path=
            conn_map = {}
            for conn in reader.connections:
                if conn.topic == '/livox/lidar':
                    new_conn = writer.add_connection(
                        conn.topic,
                        'livox_ros_driver2/msg/CustomMsg',
                        serialization_format='cdr',
                        typestore=typestore,
                        # offered_qos_profiles=conn.offered_qos_profiles,
                    )
                    conn_map[conn.id] = new_conn
                else:
                    new_conn = writer.add_connection(
                        conn.topic,
                        conn.msgtype,
                        serialization_format='cdr',
                        typestore=typestore,
                        # offered_qos_profiles=conn.offered_qos_profiles,
                    )
                    conn_map[conn.id] = new_conn

            # 获取类型类
            OldMsg = typestore.types['livox_ros_driver/msg/CustomMsg']
            NewMsg = typestore.types['livox_ros_driver2/msg/CustomMsg']
            NewPoint = typestore.types['livox_ros_driver2/msg/CustomPoint']

            for conn, timestamp, rawdata in reader.messages():
                if conn.topic == '/livox/lidar':
                    old_msg = typestore.deserialize_cdr(rawdata, 'livox_ros_driver/msg/CustomMsg')

                    new_msg = NewMsg(
                        header=old_msg.header,
                        time_base=old_msg.time_base,
                        point_num=old_msg.point_num,
                        lidar_id=old_msg.lidar_id,
                        rsvd=np.array(old_msg.rsvd, dtype=np.uint8),  # 固定长度数组转 list
                        points=[NewPoint(
                            offset_time=p.offset_time,
                            x=p.x, y=p.y, z=p.z,
                            reflectivity=p.reflectivity,
                            tag=p.tag,
                            line=p.line
                        ) for p in old_msg.points]
                    )

                    new_data = typestore.serialize_cdr(new_msg, 'livox_ros_driver2/msg/CustomMsg')
                    writer.write(conn_map[conn.id], timestamp, new_data)
                else:
                    writer.write(conn_map[conn.id], timestamp, rawdata)

    print(f"转换完成！新 bag 保存到: {output_path}")
    print("现在可以用 ros2 bag play {output_path} --clock 直接播放，你的 lightning 程序能正常订阅 /livox/lidar。")

if __name__ == '__main__':
    input_bag = 'offroad4'                # 输入 bag 目录
    output_bag = 'offroad4_'     # 输出 bag 目录

    convert_bag(input_bag, output_bag)

运行python3 convertlivox_ros12.py