realsense example修改为ros2的节点

realsense ros2的节点过于臃肿，会过于占据系统资源（cpu基本在70%以上）。利用realsense example修改为ros2的节点，根据自己所需取所需要的数据，可以大大减少数据量的传输和cpu占用（10%以内）。

以获取realsense的深度点云为例。代码用848*480的分辨率 15hz初始化数据流，获取后降采样并发布出来

// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2019 Intel Corporation. All Rights Reserved.

#include  // Include RealSense Cross Platform API
#include              // for cout
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 


// 定义 PCL 点云类型
using PointT = pcl::PointXYZ;
using CloudT = pcl::PointCloud;

class RS_CAMERA : public rclcpp::Node 
{
public:
    RS_CAMERA() : Node("rs_camera") {

        std::thread([this]{

            try{
                // Create a Pipeline - this serves as a top-level API for streaming and processing frames
                rs2::config cfg;
                cfg.enable_stream(RS2_STREAM_DEPTH, 848, 480, RS2_FORMAT_Z16, 15);

                rs2::pipeline p;
                // Configure and start the pipeline
                p.start(cfg);

                // Declare pointcloud object, for calculating pointclouds and texture mappings
                rs2::pointcloud pc;
                // We want the points object to be persistent so we can display the last cloud when a frame drops
                rs2::points points;

                double downsample_resolution = 0.03;
                std::shared_ptr> voxelgrid(new pcl::VoxelGrid());
                voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
                cut_cloud_pub_ = this->create_publisher("/rs/camera_depth_cloud", 10);
                sensor_msgs::msg::PointCloud2 global_frame_cloud;
                

                while (true)
                {
                    
                    // std::chrono::milliseconds ms = std::chrono::duration_cast< std::chrono::milliseconds >(
                    //     std::chrono::system_clock::now().time_since_epoch()
                    // );
                    // std::cout << "time " << std::fixed << (double)ms.count()/1000.0 << std::endl;
                    // Block program until frames arrive
                    rs2::frameset frames = p.wait_for_frames();

                    // Try to get a frame of a depth image
                    rs2::depth_frame depth = frames.get_depth_frame();

                    // Get the depth frame's dimensions
                    auto width = depth.get_width();
                    auto height = depth.get_height();

                    points = pc.calculate(depth);

                    // Query the distance from the camera to the object in the center of the image
                    float dist_to_center = depth.get_distance(width / 2, height / 2);

                    std::shared_ptr pcl_cloud = convertToPCLOrderedCloud(points, depth);
                    voxelgrid->setInputCloud(pcl_cloud);
                    voxelgrid->filter(*pcl_cloud);

                    pcl::toROSMsg(*pcl_cloud, global_frame_cloud);
                    global_frame_cloud.header.frame_id = "camera";
                    global_frame_cloud.header.stamp = rclcpp::Clock().now();
                    cut_cloud_pub_->publish(global_frame_cloud);

                    // Print the distance
                    std::cout << "width " << width << ",height " << height << ", size " << points.size() << std::endl;
                    std::cout << "The camera is facing an object " << dist_to_center << " meters away" << std::endl ;
                }

                return EXIT_SUCCESS;
            }
            catch (const rs2::error & e)
            {
                std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
                return EXIT_FAILURE;
            }
            catch (const std::exception& e)
            {
                std::cerr << e.what() << std::endl;
                return EXIT_FAILURE;
            }
        }).detach();
    }
    ~RS_CAMERA(){}

    // 转换函数
    std::shared_ptr convertToPCLOrderedCloud(const rs2::points& points, const rs2::depth_frame& depth_frame) {
        // 获取深度图尺寸
        int width = depth_frame.get_width();
        int height = depth_frame.get_height();

        // 创建 PCL 有序点云对象
        std::shared_ptr cloud(new CloudT);
        cloud->width = width;     // 设置宽度（类似图像的列数）
        cloud->height = height;   // 设置高度（类似图像的行数）
        cloud->is_dense = false;  // 可能存在无效点（NaN）
        cloud->resize(width * height);

        // 获取 RealSense 点云顶点数组
        auto vertices = points.get_vertices();

        // 按行优先顺序填充点云
        for (int y = 0; y < height; ++y) {
            for (int x = 0; x < width; ++x) {
                // 计算一维索引（行优先）
                int index = y * width + x;
                auto& vertex = vertices[index];
                auto& pcl_point = cloud->at(x, y);  // 通过 (x,y) 访问有序点云

                // 填充点坐标（跳过 NaN）
                if (vertex.z > 0 && std::isfinite (vertex.z) && vertex.z < 5.6) {  // 可根据需求调整有效性条件
                    pcl_point.x = vertex.x;
                    pcl_point.y = vertex.y;
                    pcl_point.z = vertex.z;
                } else {
                    pcl_point.x = pcl_point.y = pcl_point.z = std::numeric_limits::quiet_NaN();
                }
            }
        }

        return cloud;
    }

private:

    rclcpp::Publisher::SharedPtr cut_cloud_pub_;
};


// Hello RealSense example demonstrates the basics of connecting to a RealSense device
// and taking advantage of depth data
int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared());
    rclcpp::shutdown();

    return 0;
}
    

CMakeLists

cmake_minimum_required(VERSION 3.8)

project(realsense_groundfit)

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(PCL REQUIRED)
find_package(pcl_conversions REQUIRED)

include_directories(
    include
)

set(dependencies 
    PCL
    pcl_conversions
    rclcpp
    sensor_msgs
    )

    
set(rs_dependencies 
    realsense2
)

add_executable(realsense_groundfit src/rs-hello-realsense.cpp)
set_property(TARGET realsense_groundfit PROPERTY CXX_STANDARD 11)
ament_target_dependencies(realsense_groundfit ${dependencies})
target_link_libraries(realsense_groundfit
    ${rs_dependencies}
)

install(TARGETS
realsense_groundfit 
      DESTINATION lib/${PROJECT_NAME}
)

官方example链接

https://github.com/IntelRealSense/librealsense/tree/master/examples

根据自己需要增删即可