订阅ROS2中相机的相关话题并保存RGB、深度和点云图

系统：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(TARGETSbag_to_image_nodeDESTINATION 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 mscv_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;
}