ROS同步解包
- 引言
- 1 鱼香ROS一键安装ros-docker脚本：
- 2 指定目录映射
- 3 数据解包
- 3.1 解包脚本
- 3.2 依赖安装
- 3.3 运行脚本，解包
引言
总结步骤如下：
- 采集同步数据：ROS录制（推荐），或者代码同步触发采集
- ROS同步解包：一一对应的图片和点云数据
- 相机内参标定
- 激光雷达与相机外参标定
- 标定信息标准化文件生成
- 点云与图像配准映射
参考链接🔗：
1. 激光雷达和相机联合标定保姆级教程
1 鱼香ROS一键安装ros-docker脚本：
wget http://fishros.com/install -O fishros && . fishros
注：alianros为容器名称
2 指定目录映射
因为默认目录映射到/home/user，笔者想指定映射到专用的解包文件夹内
- 提交当前容器为新镜像
首先，将当前运行中的容器alianros提交为一个新的镜像。这将保留容器中的所有更改和数据。
docker commit alianros alian_custom
- 启动新容器并指定新的目录映射
使用新创建的镜像alian_custom启动一个新的容器，并在启动时指定新的目录映射。
docker run -it --name alian_new \
-v /new/path/on/host:/new/path/in/container \
[其他必要的参数] \
alian_custom
- 停止并移除旧容器（可选）
如果新容器运行正常，您可以选择停止并移除旧容器alian。
docker stop alianros
docker rm alianros
- 重命名新容器（可选）
如果需要，您可以将新容器重命名为原容器的名称。
docker rename alian_new alianros
- 设置终端输入容器名来启动容器
打开 shell 配置文件
nano ~/.bashrc # 如果是 bash 用户
在文件的最后，添加如下内容：
alias alianros='docker exec -it alianros /bin/bash'
保存并应用更改
编辑完成后，保存文件并退出 (Ctrl + O 保存，Ctrl + X 退出)。然后运行以下命令使更改生效：
source ~/.bashrc # 如果是 bash 用户
只需在终端中输入 alianros，它就会执行 docker exec -it alian /bin/bash：
alianros
3 数据解包
3.1 解包脚本
满足如下条件
- 能够实现对‘.bag’进行解包，其中包括相机话题‘/use_cam/image_raw’和点云话题‘/lslidar_point_cloud’
- 数据提取使用时间戳命名，并保持图像数据和点云数据命名的时间同步问题，假设点云有700+数据，图像有1000+，以点云的时间戳为基准，寻找最接近的图像数据，两者以相同的时间戳命名，其中图片文件后缀保存为‘.jpg’，点云文件后缀保存为‘.pcd’
- 创建图像和点云的保存目录，‘savepath/images’和’savepath/pointcloud’
- 输入变量包括：.bag路径，相机话题，点云话题，保存路径
- 最终得到一个输出目录，包括两个子目录‘images’和’pointcloud’，子目录下的图片和点云文件一一匹配
新建脚本文件bag_unpacker.py，将下面的代码复制进去
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
author:alian
2024.10.03
安装必要的包
sudo apt-get update
sudo apt-get install python-opencv python-numpy python-pip
sudo apt-get install ros-melodic-cv-bridge # 根据您的 ROS 版本调整
sudo pip install pypcd
本脚本基于 Python 2.7 编写，适用于 Ubuntu 18.04 上的 ROS Melodic
赋予执行权限：
chmod +x bag_unpacker.py
运行脚本：
./bag_unpacker.py --bag_path /path/to/your.bag --camera_topic /use_cam/image_raw --pointcloud_topic /lslidar_point_cloud --save_path /path/to/save
参数说明：
--bag_path：指定要解包的 .bag 文件路径。
--camera_topic：指定相机话题名称，默认值为 /use_cam/image_raw。
--pointcloud_topic：指定点云话题名称，默认值为 /lslidar_point_cloud。
--save_path：指定数据保存的根目录。
"""
import rosbag
import rospy
import os
import cv2
import numpy as np
from cv_bridge import CvBridge
from sensor_msgs.msg import Image, PointCloud2
from pypcd import pypcd
import struct
import sys
from tqdm import tqdm # 导入 tqdm
class BagUnpacker(object): # 1激光雷达+1摄像头同步
def __init__(self, bag_path, camera_topic, pointcloud_topic, save_path):
"""
初始化BagUnpacker类。
:param bag_path: .bag文件路径
:param camera_topic: 相机话题名称
:param pointcloud_topic: 点云话题名称
:param save_path: 数据保存路径
"""
self.bag_path = bag_path
self.camera_topic = camera_topic
self.pointcloud_topic = pointcloud_topic
self.save_path = save_path
# 创建保存目录
self.images_dir = os.path.join(self.save_path, 'images')
self.pointcloud_dir = os.path.join(self.save_path, 'pointcloud')
if not os.path.exists(self.images_dir):
os.makedirs(self.images_dir)
if not os.path.exists(self.pointcloud_dir):
os.makedirs(self.pointcloud_dir)
self.bridge = CvBridge()
def unpack(self):
"""
解包.bag文件，提取指定话题的数据，并进行时间同步保存。
"""
print("开始解包.bag文件...")
bag = rosbag.Bag(self.bag_path, 'r')
# 收集相机数据
print("收集相机数据...")
image_msgs = []
for topic, msg, t in tqdm(bag.read_messages(self.camera_topic)):
timestamp = msg.header.stamp.to_sec()
cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
image_msgs.append((timestamp, cv_image))
# 收集点云数据
print("收集点云数据...")
pointcloud_msgs = []
for topic, msg, t in tqdm(bag.read_messages(self.pointcloud_topic)):
timestamp = msg.header.stamp.to_sec()
pointcloud = self.pointcloud2_to_xyz(msg)
pointcloud_msgs.append((timestamp, pointcloud))
bag.close()
# 排序数据
image_msgs.sort(key=lambda x: x[0])
pointcloud_msgs.sort(key=lambda x: x[0])
print("开始时间同步和保存数据...")
image_timestamps = [msg[0] for msg in image_msgs]
image_data = [msg[1] for msg in image_msgs]
# 使用 tqdm 显示点云处理进度
for pc_timestamp, pc_data in tqdm(pointcloud_msgs):
# 寻找最接近的图像时间戳
closest_idx = self.find_closest_timestamp(pc_timestamp, image_timestamps)
closest_image = image_data[closest_idx]
closest_image_timestamp = image_timestamps[closest_idx]
# 使用点云的时间戳进行命名
filename = "{0:.6f}".format(pc_timestamp)
# 保存图像
image_filename = os.path.join(self.images_dir, filename + ".jpg")
cv2.imwrite(image_filename, closest_image)
# 保存点云
pcd_filename = os.path.join(self.pointcloud_dir, filename + ".pcd")
self.save_pcd(pc_data, pcd_filename)
# 可选：打印保存的信息
# print("保存文件: ", filename)
print("解包完成。")
def find_closest_timestamp(self, target, timestamps):
"""
寻找最接近的时间戳索引。
:param target: 目标时间戳
:param timestamps: 时间戳列表
:return: 最接近的时间戳索引
"""
closest_idx = np.argmin(np.abs(np.array(timestamps) - target))
return closest_idx
def pointcloud2_to_xyz(self, pointcloud_msg):
"""
将PointCloud2消息转换为XYZ点云。
:param pointcloud_msg: sensor_msgs/PointCloud2消息
:return: Nx3的numpy数组
"""
# 使用pypcd解析PointCloud2
try:
# pypcd需要PCD格式的数据，这里需要手动转换
# 先将PointCloud2转换为PCD格式的内存数据
header = pointcloud_msg.header
width = pointcloud_msg.width
height = pointcloud_msg.height
fields = pointcloud_msg.fields
is_bigendian = pointcloud_msg.is_bigendian
point_step = pointcloud_msg.point_step
row_step = pointcloud_msg.row_step
data = pointcloud_msg.data
is_dense = pointcloud_msg.is_dense
# 将数据写入临时文件
temp_pcd_path = "/tmp/temp_pointcloud.pcd"
with open(temp_pcd_path, 'w') as f:
f.write("# .PCD v0.7 - Point Cloud Data file format\n")
f.write("VERSION 0.7\n")
f.write("FIELDS x y z\n")
f.write("SIZE 4 4 4\n")
f.write("TYPE F F F\n")
f.write("COUNT 1 1 1\n")
f.write("WIDTH {}\n".format(width))
f.write("HEIGHT {}\n".format(height))
f.write("VIEWPOINT 0 0 0 1 0 0 0\n")
f.write("POINTS {}\n".format(width * height))
f.write("DATA ascii\n")
# Extract x, y, z from each point
for i in range(width * height):
offset = i * point_step
x = struct.unpack('f', data[offset:offset+4])[0]
y = struct.unpack('f', data[offset+4:offset+8])[0]
z = struct.unpack('f', data[offset+8:offset+12])[0]
f.write("{0} {1} {2}\n".format(x, y, z))
# 使用pypcd读取临时PCD文件
pc = pypcd.PointCloud.from_path(temp_pcd_path)
xyz = np.vstack((pc.pc_data['x'], pc.pc_data['y'], pc.pc_data['z'])).transpose()
# 删除临时文件
os.remove(temp_pcd_path)
return xyz
except Exception as e:
print("转换点云时出错: ", e)
return np.array([])
def save_pcd(self, pointcloud, filename):
"""
将点云数据保存为PCD文件。
:param pointcloud: Nx3的numpy数组
:param filename: 保存路径
"""
if pointcloud.size == 0:
print("空点云数据，跳过保存。")
return
# 创建 PCD 对象
pointcloud = np.array(pointcloud, dtype=np.float32)
pc = pypcd.make_xyz_point_cloud(pointcloud)
pc.save_pcd(filename, compression='binary')
class BagUnpacker_2cam(object): # 1激光雷达+2摄像头同步
def __init__(self, bag_path, camera0_topic,camera1_topic, pointcloud_topic, save_path):
"""
初始化BagUnpacker类。
:param bag_path: .bag文件路径
:param camera_topic: 相机话题名称
:param pointcloud_topic: 点云话题名称
:param save_path: 数据保存路径
"""
self.bag_path = bag_path
self.camera_topic0 = camera0_topic
self.camera_topic1 = camera1_topic
self.pointcloud_topic = pointcloud_topic
self.save_path = save_path
# 创建保存目录
self.images0_dir = os.path.join(self.save_path, 'images0')
self.images1_dir = os.path.join(self.save_path, 'images1')
self.pointcloud_dir = os.path.join(self.save_path, 'pointcloud')
if not os.path.exists(self.images0_dir):
os.makedirs(self.images0_dir)
if not os.path.exists(self.images1_dir):
os.makedirs(self.images1_dir)
if not os.path.exists(self.pointcloud_dir):
os.makedirs(self.pointcloud_dir)
self.bridge = CvBridge()
def unpack(self):
"""
解包.bag文件，提取指定话题的数据，并进行时间同步保存。
"""
print("开始解包.bag文件...")
bag = rosbag.Bag(self.bag_path, 'r')
# 收集相机数据
print("收集相机1数据...")
image0_msgs = []
for topic, msg, t in tqdm(bag.read_messages(self.camera_topic0)):
timestamp = msg.header.stamp.to_sec()
cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
image0_msgs.append((timestamp, cv_image))
print("收集相机2数据...")
image1_msgs = []
for topic, msg, t in tqdm(bag.read_messages(self.camera_topic1)):
timestamp = msg.header.stamp.to_sec()
cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
image1_msgs.append((timestamp, cv_image))
# 收集点云数据
print("收集点云数据...")
pointcloud_msgs = []
for topic, msg, t in tqdm(bag.read_messages(self.pointcloud_topic)):
timestamp = msg.header.stamp.to_sec()
pointcloud = self.pointcloud2_to_xyz(msg)
pointcloud_msgs.append((timestamp, pointcloud))
bag.close()
# 排序数据
image0_msgs.sort(key=lambda x: x[0])
image1_msgs.sort(key=lambda x: x[0])
pointcloud_msgs.sort(key=lambda x: x[0])
print("开始时间同步和保存数据...")
image0_timestamps = [msg[0] for msg in image0_msgs]
image0_data = [msg[1] for msg in image0_msgs]
image1_timestamps = [msg[0] for msg in image1_msgs]
image1_data = [msg[1] for msg in image1_msgs]
# 使用 tqdm 显示点云处理进度
for pc_timestamp, pc_data in tqdm(pointcloud_msgs):
# 寻找最接近的图像时间戳
closest_idx0 = self.find_closest_timestamp(pc_timestamp, image0_timestamps)
closest_image0 = image0_data[closest_idx0]
closest_idx1 = self.find_closest_timestamp(pc_timestamp, image1_timestamps)
closest_image1 = image1_data[closest_idx1]
# 使用点云的时间戳进行命名
filename = "{0:.6f}".format(pc_timestamp)
# 保存图像
image0_filename = os.path.join(self.images0_dir, filename + ".jpg")
cv2.imwrite(image0_filename, closest_image0)
image1_filename = os.path.join(self.images1_dir, filename + ".jpg")
cv2.imwrite(image1_filename, closest_image1)
# 保存点云
pcd_filename = os.path.join(self.pointcloud_dir, filename + ".pcd")
self.save_pcd(pc_data, pcd_filename)
# 可选：打印保存的信息
# print("保存文件: ", filename)
print("解包完成。")
def find_closest_timestamp(self, target, timestamps):
"""
寻找最接近的时间戳索引。
:param target: 目标时间戳
:param timestamps: 时间戳列表
:return: 最接近的时间戳索引
"""
closest_idx = np.argmin(np.abs(np.array(timestamps) - target))
return closest_idx
def pointcloud2_to_xyz(self, pointcloud_msg):
"""
将PointCloud2消息转换为XYZ点云。
:param pointcloud_msg: sensor_msgs/PointCloud2消息
:return: Nx3的numpy数组
"""
# 使用pypcd解析PointCloud2
try:
# pypcd需要PCD格式的数据，这里需要手动转换
# 先将PointCloud2转换为PCD格式的内存数据
header = pointcloud_msg.header
width = pointcloud_msg.width
height = pointcloud_msg.height
fields = pointcloud_msg.fields
is_bigendian = pointcloud_msg.is_bigendian
point_step = pointcloud_msg.point_step
row_step = pointcloud_msg.row_step
data = pointcloud_msg.data
is_dense = pointcloud_msg.is_dense
# 将数据写入临时文件
temp_pcd_path = "/tmp/temp_pointcloud.pcd"
with open(temp_pcd_path, 'w') as f:
f.write("# .PCD v0.7 - Point Cloud Data file format\n")
f.write("VERSION 0.7\n")
f.write("FIELDS x y z\n")
f.write("SIZE 4 4 4\n")
f.write("TYPE F F F\n")
f.write("COUNT 1 1 1\n")
f.write("WIDTH {}\n".format(width))
f.write("HEIGHT {}\n".format(height))
f.write("VIEWPOINT 0 0 0 1 0 0 0\n")
f.write("POINTS {}\n".format(width * height))
f.write("DATA ascii\n")
# Extract x, y, z from each point
for i in range(width * height):
offset = i * point_step
x = struct.unpack('f', data[offset:offset+4])[0]
y = struct.unpack('f', data[offset+4:offset+8])[0]
z = struct.unpack('f', data[offset+8:offset+12])[0]
f.write("{0} {1} {2}\n".format(x, y, z))
# 使用pypcd读取临时PCD文件
pc = pypcd.PointCloud.from_path(temp_pcd_path)
xyz = np.vstack((pc.pc_data['x'], pc.pc_data['y'], pc.pc_data['z'])).transpose()
# 删除临时文件
os.remove(temp_pcd_path)
return xyz
except Exception as e:
print("转换点云时出错: ", e)
return np.array([])
def save_pcd(self, pointcloud, filename):
"""
将点云数据保存为PCD文件。
:param pointcloud: Nx3的numpy数组
:param filename: 保存路径
"""
if pointcloud.size == 0:
print("空点云数据，跳过保存。")
return
# 创建 PCD 对象
pointcloud = np.array(pointcloud, dtype=np.float32)
pc = pypcd.make_xyz_point_cloud(pointcloud)
pc.save_pcd(filename, compression='binary')
if __name__ == "__main__":
import argparse
parser = argparse.ArgumentParser(description="解包ROS .bag文件，提取相机和点云数据并同步保存。")
parser.add_argument('--bag_path', type=str, required=True, help='.bag文件路径')
parser.add_argument('--camera0_topic', type=str, default='/use_cam/image_raw', help='相机话题名称')
parser.add_argument('--camera1_topic', type=str, default='/use_cam/image_raw', help='相机话题名称')
parser.add_argument('--pointcloud_topic', type=str, default='/lslidar_point_cloud', help='点云话题名称')
parser.add_argument('--save_path', type=str, required=True, help='数据保存路径')
args = parser.parse_args()
# 1激光雷达+1摄像头同步
# unpacker = BagUnpacker(
# bag_path=args.bag_path,
# camera_topic=args.camera0_topic,
# pointcloud_topic=args.pointcloud_topic,
# save_path=args.save_path
# )
# unpacker.unpack()
# 1激光雷达+2摄像头同步
unpacker = BagUnpacker_2cam(
bag_path=args.bag_path,
camera0_topic=args.camera0_topic,
camera1_topic=args.camera1_topic,
pointcloud_topic=args.pointcloud_topic,
save_path=args.save_path
)
unpacker.unpack()
"""
终端命令：
roscore
rosbag info xxx.bag # 查看话题名称
python ROS_bag/bag_unpacker.py --bag_path ROS_bag/cali_lidar_camera.bag --camera1_topic /usb_cam/image_raw --pointcloud_topic /lslidar_point_cloud --save_path ROS_bag/cali
python ROS_bag/bag_unpacker.py --bag_path ROS_bag/2024-07-08-19-54-56_lidar2cam_12.bag --camera0_topic /img0 --camera1_topic /img1 --pointcloud_topic /benewake//SyncX2PointCloud --save_path ROS_bag/cali_12
"""
3.2 依赖安装
确保进入自定义的ROS1容器
查看容器的python版本，如图为2.7
- 安装pip及必要依赖
sudo apt-get update
sudo apt-get install python-opencv python-numpy python-pip
sudo apt-get install ros-melodic-cv-bridge # 根据您的 ROS 版本调整
如果您的容器中没有 pip，您可以按照以下步骤安装 pip：
下载 get-pip.py 脚本：
curl https://bootstrap.pypa.io/pip/2.7/get-pip.py -o get-pip.py
使用 Python 2.7 运行该脚本：
python get-pip.py
- 在 Python 2.7 容器中安装 tqdm(进度条)/pypcd
pip install tqdm
安装完成后，您可以验证 tqdm 是否成功安装：
python -c "import tqdm; print(tqdm.__version__)"
# 安装 pypcd
pip install pypcd
- 在 Docker 容器中安装 vim nano gedit等终端文本编辑器（后续有用）
vim 通常也很适合在容器中使用。如果你想用 vim，可以通过以下命令安装：
apt-get update
apt-get install vim nano gedit
安装完成后，通过以下命令打开并编辑 .py 文件：
vim your_script.py
vim 的基本使用步骤和常用命令说明：
- 打开文件 在终端中输入以下命令来打开文件： vim your_script.py 如果文件不存在，vim 会创建一个新的文件。
- 进入插入模式（编辑模式） 默认情况下，vim 启动时是 命令模式，你无法直接编辑文件。要开始编辑，需要进入 插入模式： 按下 i 键，进入插入模式。此时可以输入、修改文件的内容。
- 保存文件并退出 编辑完成后，需要返回 命令模式 来保存文件并退出。按如下步骤操作： 退出插入模式：按下 Esc 键。 保存并退出： 输入 :wq 然后按 Enter 保存文件并退出。 如果只想保存文件但不退出，可以输入 :w 然后按 Enter。
如果想退出但不保存，可以输入 :q! 然后按 Enter。- 常用命令 i：进入插入模式（在当前光标位置编辑）。 Esc：退出插入模式，回到命令模式。 :w：保存文件但不退出。 :wq 或 ZZ：保存并退出。 :q!：强制退出（不保存修改）。 x：删除当前光标处的字符。 dd：删除当前行。 yy：复制当前行。
p：在光标下方粘贴。 u：撤销上一次操作。- 基本工作流程 打开文件：vim your_file.py 进入插入模式：按 i，编辑文件。 完成编辑后：按 Esc 退出插入模式。 保存并退出：输入 :wq 然后按 Enter。
- 查找内容 在命令模式下，按 / 键然后输入你要查找的内容： /find_this_text 然后按 Enter，vim 会跳到匹配的地方。使用 n 和 N 可以跳到下一个或上一个匹配项。
- 常见问题 如果 vim 在启动时显示成只读模式，使用 :w! 可以强制保存文件。
3.3 运行脚本，解包
执行指令如下：
#打开一个终端（进入容器内）
roscore
# 打开第二个终端（进入容器内）
rosbag info xxx.bag # 查看话题名称
笔者以1个激光雷达+2个相机录制的Bag包为例：
# 1个激光雷达+1个相机执行指令
python ROS_bag/bag_unpacker.py --bag_path ROS_bag/cali_lidar_camera.bag --camera1_topic /usb_cam/image_raw --pointcloud_topic /lslidar_point_cloud --save_path ROS_bag/cali
# 1个激光雷达+2个相机执行指令
python ROS_bag/bag_unpacker.py --bag_path ROS_bag/2024-07-08-19-54-56_lidar2cam_12.bag --camera0_topic /img0 --camera1_topic /img1 --pointcloud_topic /benewake//SyncX2PointCloud --save_path ROS_bag/cali_12
成功解包如下：
BUG解决
BUG1：
BUG2：
解决方法：
cd /usr/local/lib/python2.7/dist-packages/pypcd
vim pypcd.py
修改如下：
