ubuntu播放rosbag包（可鼠标交互）

1 前言

众所周知，ubuntu中播放bag包最主要的工具是rviz，然而rviz有一个无法忍受的缺陷就是不支持鼠标回滚，并且显示的时间的ros时间，不是世界时间，因此在遇到相关bug时不能与对应的世界时间对应。基于以上，解决方案如下：

2 依托工具

foxglove,这个工具可以实现bag包的播放，相关安装下载的可以网上搜索一下，相关资料很多。

3 相关修改

fox-glove是一个很好用的工具，但是对于ros1不是很友好，对于点云格式为pointcloud的消息不能显示，要想显示有两种方式，一是写一个foxglove的插件，二是将pointcloud转换为pointcloud2格式，写插件相对复杂，因此选用更方便的改写方式，python脚本如下:

#!/usr/bin/env python3
import rosbag
from sensor_msgs.msg import PointCloud, PointCloud2, PointField
import struct
import sys
import time

def convert_pointcloud_to_pointcloud2(pc_msg):
    """将 PointCloud 消息转换为 PointCloud2 格式"""
    # 创建 PointCloud2 消息
    pc2_msg = PointCloud2()
    
    # 复制头信息
    pc2_msg.header = pc_msg.header
    
    # 设置点云属性
    pc2_msg.height = 1  # 无序点云
    pc2_msg.width = len(pc_msg.points)  # 点的数量
    pc2_msg.is_dense = True  # 所有点都有效
    pc2_msg.is_bigendian = False  # 小端字节序
    
    # 设置字段（x, y, z）
    pc2_msg.fields = [
        PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
        PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
        PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
    ]
    
    # 计算每个点的大小
    pc2_msg.point_step = 16  # 4 bytes * 4 (x, y, z, 填充字段)
    pc2_msg.row_step = pc2_msg.point_step * pc2_msg.width
    
    # 构建点云数据缓冲区
    buffer = bytearray()
    for point in pc_msg.points:
        # 打包每个点的坐标 (x, y, z)
        buffer += struct.pack('fff', point.x, point.y, point.z)
        # 添加填充字段 (通常用于颜色或强度数据)
        buffer += struct.pack('f', 0.0)
    
    pc2_msg.data = bytes(buffer)
    return pc2_msg

def convert_bag(input_bag_path, output_bag_path):
    """转换整个 bag 文件"""
    print(f"开始转换: {input_bag_path} -> {output_bag_path}")
    start_time = time.time()
    
    # 跟踪转换进度
    pointcloud_topics = set()
    converted_messages = 0
    
    with rosbag.Bag(input_bag_path, 'r') as in_bag, \
         rosbag.Bag(output_bag_path, 'w') as out_bag:
        
        # 读取并转换消息
        total_messages = in_bag.get_message_count()
        processed_messages = 0
        
        for topic, msg, t in in_bag.read_messages():
            processed_messages += 1
            if processed_messages % 100 == 0:
                percent = (processed_messages / total_messages) * 100
                print(f"进度: {percent:.1f}% ({processed_messages}/{total_messages})")
            
            if msg._type == 'sensor_msgs/PointCloud':
                # 转换点云消息
                pc2_msg = convert_pointcloud_to_pointcloud2(msg)
                new_topic = f"{topic}_pc2"  # 使用新话题名称
                out_bag.write(new_topic, pc2_msg, t)
                pointcloud_topics.add(topic)
                converted_messages += 1
            else:
                # 其他类型消息直接复制
                out_bag.write(topic, msg, t)
    
    end_time = time.time()
    print(f"\n转换完成! 耗时: {end_time - start_time:.2f}秒")
    print(f"转换了 {converted_messages} 条 PointCloud 消息")
    print("包含 PointCloud 的话题:")
    for topic in pointcloud_topics:
        print(f"  - {topic} -> {topic}_pc2")

if __name__ == "__main__":
    if len(sys.argv) != 3:
        print("用法: python3 convert_pointcloud_to_pointcloud2.py 输入.bag 输出.bag")
        print("示例: python3 convert_pointcloud_to_pointcloud2.py new.bag converted.bag")
        sys.exit(1)
    
    input_bag = sys.argv[1]
    output_bag = sys.argv[2]
    
    try:
        convert_bag(input_bag, output_bag)
        print("\n请使用以下命令验证转换结果:")
        print(f"rosbag info {output_bag} | grep PointCloud")
        print(f"rosbag play {output_bag}")
    except Exception as e:
        print(f"转换出错: {e}")
        sys.exit(1)

转换完的bag包就可以使用fox-glove播放了，为了更方便的播放，写相关shell脚本如下:

#!/usr/bin/env bash
set -e

# 脚本入参：原始bag路径
BAG_PATH="$1"

# 判断是否传入参数
if [ -z "$BAG_PATH" ]; then
    echo "Usage: $0 <bag_path>"
    exit 1
fi

# 判断文件是否存在
if [ ! -f "$BAG_PATH" ]; then
    echo "Error: Bag file '$BAG_PATH' does not exist."
    exit 1
fi

# 提取不带后缀的文件名
BAG_DIR=$(dirname "$BAG_PATH")
BAG_BASE=$(basename "$BAG_PATH" .bag)

# 定义新的bag文件名
NEW_BAG_PATH="${BAG_DIR}/${BAG_BASE}_converted.bag"

echo "Starting conversion:"
echo "  Input bag: $BAG_PATH"
echo "  Output bag: $NEW_BAG_PATH"
#bagconvert是上述的py脚本使用pyinstaller打包成的可执行文件，相关资料也很多，实在不会的直接下载我打包好的
bagconvert "$BAG_PATH" "$NEW_BAG_PATH"
echo "Opening with foxglove-studio: $NEW_BAG_PATH"

rm -f "$BAG_PATH"
foxglove-studio $NEW_BAG_PATH