ROS 发布三维激光点云

1.创建功能包

cd src
catkin_create_pkg roscpp rospy sensor_msgs std_msgs 
cd ..
catkin_make

2.编辑主函数（版本一）

创建  pointcloud.cpp  文件然后粘贴以下代码

#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "point_cloud_publisher");
  ros::NodeHandle n;
  ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);
  unsigned int num_points = 100;
  ros::Rate r(1.0);
  while(n.ok()){
    sensor_msgs::PointCloud cloud;
    cloud.header.stamp = ros::Time::now();
    cloud.header.frame_id = "sensor_frame";
    cloud.points.resize(num_points);
    //we'll also add an intensity channel to the cloud
    cloud.channels.resize(1);
    cloud.channels[0].name = "rgb";
    cloud.channels[0].values.resize(num_points);

    //generate some fake data for our point cloud
    for(unsigned int i = 0; i < num_points; ++i){
      cloud.points[i].x = 0.1*i;
      cloud.points[i].y = 0.1*i;
      cloud.points[i].z = 5;
      cloud.channels[0].values[i] = 255;
    }
    cloud_pub.publish(cloud);
    r.sleep();
  }
}

此程序对wiki上http://wiki.ros.org/cn/navigation/Tutorials/RobotSetup/Sensors的例子进行了修改。
cloud.channels[0].name = “rgb”,将名字改为rgb。另外固定点云位置为一条直线。

3.编辑CMakeLists.txt

在最后加入
add_executable(pointcloud src/pointcloud.cpp)
target_link_libraries(pointcloud ${catkin_LIBRARIES})

4.rviz显示

在rviz 中fix frame 设置为sensor_frame
其他设置及运行效果如图

 版本2

1.创建ROS包

cd catkin_ws/src
catkin_create_pkg point_cloud_processing pcl_ros roscpp rospy sensor_msgs std_msgs 
cd ..
catkin_make 

2.添加.cpp文件

#include <iostream>
#include <string>
#include <ros/ros.h> 
#include <pcl/point_cloud.h> 
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h> 
#include <pcl_conversions/pcl_conversions.h> 
#include <sensor_msgs/PointCloud2.h> 
using namespace std; 

int main(int argc,char ** argv)
{
    ROS_INFO("Log In Success!!!!!!!");
	ros::init (argc, argv, "pcdp"); 
	ros::NodeHandle n; 
	ros::Publisher pcl_pub;
	pcl_pub = n.advertise<sensor_msgs::PointCloud2> ("pcdp_output", 1);    
	pcl::PointCloud<pcl::PointXYZ> testcloud; //point cloud msg  
	sensor_msgs::PointCloud2 output; //PointCloud2 msg

	testcloud.width=50000;
	testcloud.height=2;
	testcloud.points.resize(testcloud.width*testcloud.height);

	while (ros::ok())
	{
		for(size_t i=0; i<testcloud.points.size(); ++i)
		{
				testcloud.points[i].x=1024*rand()/(RAND_MAX+1.0f);
				testcloud.points[i].y=1024*rand()/(RAND_MAX+1.0f);
				testcloud.points[i].z=1024*rand()/(RAND_MAX+1.0f);
		}
		pcl::toROSMsg(testcloud,output); //point cloud msg -> ROS msg
		output.header.frame_id="pcdp";
		pcl_pub.publish(output); //publish
	}
	ros::spin();
    return 0;
}

3.编辑CMakeLists.txt文件

cmake_minimum_required(VERSION 2.8.3)
project(point_cloud_processing)


find_package(catkin REQUIRED COMPONENTS
  pcl_ros
  roscpp
  rospy
  sensor_msgs
  std_msgs
)
find_package(PCL REQUIRED) 

catkin_package(
  CATKIN_DEPENDS pcl_ros roscpp rospy sensor_msgs std_msgs
)

include_directories(
  ${catkin_INCLUDE_DIRS}
	${PCL_INCLUDE_DIRS}
)

add_executable(pcdp src/pcdp.cpp)
target_link_libraries(pcdp ${catkin_LIBRARIES} ${PCL_LIBRARIES})

3.编译、并在RViz中显示点云数据

cd catkin_ws/
catkin_make 
roscore

 重新打开一个终端，输入：

rosrun point_cloud_processing pcdp

 

参考文献
http://wiki.ros.org/cn/navigation/Tutorials/RobotSetup/Sensors
http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud.html

ROS创建点云数据并在rviz中显示
ROS－创建、发布和显示PCL点云