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 timedef 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_msgdef 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 = 0with 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 = 0for topic, msg, t in in_bag.read_messages():processed_messages += 1if processed_messages % 100 == 0:percent = (processed_messages / total_messages) * 100print(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 += 1else:# 其他类型消息直接复制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" ]; thenecho "Usage: $0 <bag_path>"exit 1
fi# 判断文件是否存在
if [ ! -f "$BAG_PATH" ]; thenecho "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