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

boss-dog

于 2024-10-03 23:26:19 发布

阅读量277

点赞数 1

分类专栏： ROS 文章标签： ROS2 pcl

本文链接：https://blog.csdn.net/qq_45445740/article/details/142697727

版权

ROS 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

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