订阅深度图

最新推荐文章于 2025-12-31 10:37:36 发布

原创最新推荐文章于 2025-12-31 10:37:36 发布 · 107 阅读

0 ·

CC 4.0 BY-SA版权

文章标签：

#ubuntu #vscode #c++

本文描述了一个ROS节点的实现过程，该节点订阅RealSense相机的深度图像，使用PCL库进行点云数据的处理，包括将数据保存为PCD文件和应用voxel_grid滤波器。此外，展示了如何设置回调函数以处理图像消息，并在控制台上打印数据信息。

1 连接相机

2 打开roscore

3 在工作空间

source devel/setup.bash

4打开相机

roslaunch realsense2_camera rs_camera.launch

5修改cpp代码

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>//消息类型
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
//添加
#include <pcl/PCLPointCloud2.h>
#include <pcl/filters/voxel_grid.h>
#include <iostream>

using namespace std;
 
ros::Publisher pub;//为了之后发布处理后的点云
 

// void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
//void cloud_cb (const sensor_msgs::ImageConstPtr & cloud_msg)
//{
  // Container for original & filtered data
  // pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; //创建一个点云来存储数据
  // pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);//转为常量指针
  // pcl::PCLPointCloud2 cloud_filtered;
 
  //pcl::io::savePCDFile("./pointcloud_init.pcd", *cloud_msg);//把cloud_msg存储为pcd文件
  //cout<<"publish point_cloud height = "<<cloud_msg->height<<endl;
  //cout<<"publish point_cloud width = "<<cloud_msg->width<<endl;
  //ROS_INFO("ok！");
//}
 void cloud_cb (const sensor_msgs::Image::ConstPtr & cloud_msg)
 {
  
  ROS_INFO("width:%d,height:%d,header.frame_id:%s",cloud_msg->width,cloud_msg->height,cloud_msg->header.frame_id);

  ROS_INFO("data1:%d",cloud_msg->data[1]);

  // 在 C++ 中使用数组索引打印数组的内容
    size_t n = sizeof(cloud_msg->data);

    //循环遍历数组元素
    for (size_t i = 0; i < 100; i++) { 
        ROS_INFO(" %d",cloud_msg->data[i]);
    }


}

int main (int argc, char** argv)
{
  setlocale(LC_ALL,"");
  
  // Initialize ROS
  ros::init (argc, argv, "VoxelGrid");
  ros::NodeHandle nh;//方便发布信息
  // 定义发布对象sub，发布消息的渠道
  //<sensor_msgs::PointCloud2>表示发送信息的格式
  //"/camera/depth/image_rect_raw"为发布信息的主题，1表示处理队列大小，cloud_cb表示回调函数
  ros::Subscriber sub = nh.subscribe("/camera/depth/image_rect_raw", 1, cloud_cb);//<sensor_msgs::Image> ("/camera/depth/image_rect_raw", 1, cloud_cb);//PointCloud2>("/point_cloud_publisher_topic",1,cloud_cb); // ("/camera/depth/image_rect_raw", 1, cloud_cb);
   ROS_INFO("success!");
  // Spin
  ros::Rate rate=1;
  while (ros::ok())
  {
    rate.sleep();
    ros::spinOnce();
  }
  
  // ros::spin ();//在调用回调函数的同时查看是否有新来的消息包
}

6运行节点即可