创建工作空间、功能包

catkin_create_pkg laserscan_to_pointcloud sensor_msgs roscpp pcl_conversions pcl_ros 

功能包文件 package.xml

<?xml version="1.0"?>
<package format="2"><name>laserscan_to_pointcloud</name><version>0.0.0</version><description>The laserscan_to_pointcloud package</description><maintainer email="xiaoqiuslam@todo.todo">wason</maintainer><license>TODO</license><buildtool_depend>catkin</buildtool_depend><build_depend>pcl_conversions</build_depend><build_depend>pcl_ros</build_depend><build_depend>roscpp</build_depend><build_depend>sensor_msgs</build_depend><build_export_depend>pcl_conversions</build_export_depend><build_export_depend>pcl_ros</build_export_depend><build_export_depend>roscpp</build_export_depend><build_export_depend>sensor_msgs</build_export_depend><exec_depend>pcl_conversions</exec_depend><exec_depend>pcl_ros</exec_depend><exec_depend>roscpp</exec_depend><exec_depend>sensor_msgs</exec_depend><export></export>
</package>

头文件、源码文件

laserscan_to_pointcloud.h

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <pcl_ros/point_cloud.h>    
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>class ScanToPointCloud
{private:ros::NodeHandle node_handle_;ros::Subscriber laser_scan_subscriber_;ros::Publisher pointcloud2_publisher_;pcl::PointXYZ invalid_point_;public:ScanToPointCloud();~ScanToPointCloud();void ScanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_msg);
};

laserscan_to_pointcloud.cc

#include "laserscan_to_pointcloud/laserscan_to_pointcloud.h"
#include <limits>ScanToPointCloud::ScanToPointCloud() 
{laser_scan_subscriber_ = node_handle_.subscribe("laser_scan", 1, &ScanToPointCloud::ScanCallback, this);pointcloud2_publisher_ = node_handle_.advertise<pcl::PointCloud<pcl::PointXYZ>>("point_cloud", 1, this);invalid_point_.x = std::numeric_limits<float>::quiet_NaN();invalid_point_.y = std::numeric_limits<float>::quiet_NaN();invalid_point_.z = std::numeric_limits<float>::quiet_NaN();
}ScanToPointCloud::~ScanToPointCloud()
{
}void ScanToPointCloud::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_msg = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>());cloud_msg->points.resize(scan_msg->ranges.size());for (unsigned int i = 0; i < scan_msg->ranges.size(); ++i){pcl::PointXYZ &point_tmp = cloud_msg->points[i];float range = scan_msg->ranges[i];if (!std::isfinite(range)){point_tmp = invalid_point_;continue;}if (range > scan_msg->range_min && range < scan_msg->range_max){float angle = scan_msg->angle_min + i * scan_msg->angle_increment;point_tmp.x = range * cos(angle);point_tmp.y = range * sin(angle);point_tmp.z = 0.0;}elsepoint_tmp = invalid_point_;}cloud_msg->width = scan_msg->ranges.size();cloud_msg->height = 1;cloud_msg->is_dense = false;pcl_conversions::toPCL(scan_msg->header, cloud_msg->header);pointcloud2_publisher_.publish(cloud_msg);
}int main(int argc, char **argv)
{ros::init(argc, argv, "laserscan_to_pointcloud");ScanToPointCloud laserscan_to_pointcloud;ros::spin();return 0;
}

代码解读

在这行代码中：

pcl::PointXYZ &point_tmp = cloud_msg->points[i];

cloud_msg->points[i] 表示点云中的第 i 个点，而 pcl::PointXYZ &point_tmp 则创建了一个指向这个点的引用。让我们拆分这个操作：

1. cloud_msg->points[i]：这部分代码访问了 cloud_msg 中的点云数据。cloud_msg 是一个 pcl::PointCloud<pcl::PointXYZ> 类型的指针，而 points 是这个点云对象中存储点的容器。

2. [i]：这是通过索引操作符访问容器中的第 i 个元素，即点云中的第 i 个点。

3. pcl::PointXYZ &point_tmp：这部分代码创建了一个 pcl::PointXYZ 类型的引用变量 point_tmp。引用是 C++ 中的一种别名，它允许我们使用一个变量的别名来访问该变量。

通过将 point_tmp 初始化为 cloud_msg->points[i] 的引用，现在 point_tmp 和 cloud_msg->points[i] 指向相同的点。这就意味着，对 point_tmp 的任何更改都会反映在原始点云数据中。

为什么这有用呢？使用引用可以简化代码并提高可读性。以后在代码中引用 point_tmp 就相当于引用了点云中的第 i 个点，这样可以更方便地进行操作，而不必每次都使用较长的 cloud_msg->points[i]。

编译文件 CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(laserscan_to_pointcloud)find_package(catkin REQUIRED COMPONENTSpcl_conversionspcl_rosroscppsensor_msgs
)find_package(PCL REQUIRED QUIET)catkin_package(
)include_directories(include${catkin_INCLUDE_DIRS}${PCL_INCLUDE_DIRS}
)add_executable(laserscan_to_pointcloud src/laserscan_to_pointcloud.cc)
target_link_libraries(laserscan_to_pointcloud ${catkin_LIBRARIES})

运行效果 截图

roscore
rosrun laserscan_to_pointcloud laserscan_to_pointcloud
rosbag play x.bag这里播放一个数据包，数据包中雷达的话题名字/laser_scan要和代码中的订阅的名字/laser_scan一致rostopic list 
/laser_scan
/point_cloudrostopic echo /point_cloud