系统：Ubuntu22.04
ROS2版本：ROS2 humble
1.订阅ROS2中相机的相关话题并保存RGB图、深度图和点云图
ros2 topic list
/stellar_1/rgb/image_raw
/camera/depth/image_raw
/stellar_1/points2
CMakeLists.txt
cmake_minimum_required(VERSION 3.15)
project(bag_to_image)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(OpenCV REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
add_executable(bag_to_image_node src/bag_to_image_node.cpp)
# ROS2中指定节点或库的依赖项
ament_target_dependencies(bag_to_image_node rclcpp) # 核心C++客户端库，旨在简化开发和实现机器人应用程序
ament_target_dependencies(bag_to_image_node sensor_msgs) # 消息包，提供了一系列标准化的消息类型，用于传输来自各种传感器的数据
ament_target_dependencies(bag_to_image_node cv_bridge) # 用于连接ROS与OpenCV，提供在ROS消息格式与OpenCV图像格式之间的转换
ament_target_dependencies(bag_to_image_node OpenCV)
install(TARGETS
bag_to_image_node
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()
bag_to_image_node.cpp
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <filesystem>
#include <fstream>
#include <queue>
class ImageSaver : public rclcpp::Node
{
public:
ImageSaver() : Node("image_saver"), frame_count(0)
{
// 新建RGB图像、深度图像、点云图像文件夹
std::filesystem::create_directories("rgb_images");
std::filesystem::create_directories("depth_images");
std::filesystem::create_directories("point_clouds");
// 订阅话题
rgb_subscriber = this->create_subscription<sensor_msgs::msg::Image>(
"/stellar_1/rgb/image_raw",
10,
std::bind(&ImageSaver::rgb_callback, this, std::placeholders::_1));
depth_subscriber = this->create_subscription<sensor_msgs::msg::Image>(
"/camera/depth/image_raw",
10,
std::bind(&ImageSaver::depth_callback, this, std::placeholders::_1));
point_cloud_subscriber = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"/stellar_1/points2",
10,
std::bind(&ImageSaver::point_cloud_callback, this, std::placeholders::_1));
}
private:
std::queue<sensor_msgs::msg::Image::SharedPtr> rgb_queue;
std::queue<sensor_msgs::msg::Image::SharedPtr> depth_queue;
void rgb_callback(const sensor_msgs::msg::Image::SharedPtr msg)
{
rgb_queue.push(msg);
process_frames();
}
void depth_callback(const sensor_msgs::msg::Image::SharedPtr msg)
{
depth_queue.push(msg);
process_frames();
}
void point_cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{
// 保存点云图为.bin格式
std::ofstream outfile("point_clouds/frame_" + std::to_string(frame_count) + ".bin", std::ios::binary);
outfile.write(reinterpret_cast<const char *>(msg->data.data()), msg->data.size());
outfile.close();
frame_count++;
process_frames(); // Check if we can process frames after saving point cloud
}
void process_frames()
{
while (!rgb_queue.empty() && !depth_queue.empty() && frame_count > 0) {
auto rgb_msg = rgb_queue.front();
auto depth_msg = depth_queue.front();
// 计算时间戳差值
int64_t rgb_time = rgb_msg->header.stamp.sec * 1e9 + rgb_msg->header.stamp.nanosec;
int64_t depth_time = depth_msg->header.stamp.sec * 1e9 + depth_msg->header.stamp.nanosec;
// 检查时间戳是否足够接近
if (std::abs(rgb_time - depth_time) < 100000000) { // 100 ms
cv_bridge::CvImagePtr cv_rgb_ptr = cv_bridge::toCvCopy(rgb_msg, sensor_msgs::image_encodings::BGR8);
cv::imwrite("rgb_images/frame_" + std::to_string(frame_count) + ".png", cv_rgb_ptr->image);
// TYPE_32FC1：
// 32F：表示每个元素是 32 位（4 字节）浮点数（float）。
// C1：表示单通道（Channel 1），即每个像素只有一个值。常用于灰度图像或单通道浮点数据。
// TYPE_16UC1：
// 16U：表示每个元素是 16 位（2 字节）无符号整数（unsigned int）。
// C1：同样表示单通道，即每个像素只有一个值。常用于灰度图像或单通道整数数据，通常用于深度图等场景。
cv_bridge::CvImagePtr cv_depth_ptr =
cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_16UC1);
cv::imwrite("depth_images/frame_" + std::to_string(frame_count) + ".png", cv_depth_ptr->image);
rgb_queue.pop();
depth_queue.pop();
} else if (rgb_time < depth_time) {
rgb_queue.pop(); // 移除旧的 RGB 消息
} else {
depth_queue.pop(); // 移除旧的深度消息
}
}
}
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr rgb_subscriber;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr depth_subscriber;
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_subscriber;
int frame_count;
};
int main(int argc, char **argv)
{
std::cout << "bag to image node start.." << std::endl;
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<ImageSaver>());
rclcpp::shutdown();
return 0;
}
运行：
source install/setup.bash
ros2 run bag_to_image bag_to_image_node
2.将点云图.bin格式转换为.pcd格式
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <thread>
// 将点云图.bin转换为.pcd
void convert_bin_to_pcd(const std::string &bin_file, const std::string &pcd_file)
{
// 读取二进制点云数据
std::ifstream infile(bin_file, std::ios::binary);
if (!infile) {
std::cerr << "Error opening binary file!" << std::endl;
return;
}
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::PointXYZ point;
// 假设每个点包含x, y, z坐标
while (infile.read(reinterpret_cast<char *>(&point), sizeof(pcl::PointXYZ))) {
cloud.points.push_back(point);
}
cloud.width = cloud.points.size();
cloud.height = 1; // 单个点云
infile.close();
// 保存为 PCD 文件
pcl::io::savePCDFileASCII(pcd_file, cloud);
std::cout << "Converted to PCD file: " << pcd_file << std::endl;
}
int main(int argc, char **argv)
{
convert_bin_to_pcd("frame_0.bin", "frame_0.pcd");
return 0;
}
