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

被折叠的 条评论
为什么被折叠?



