在上一篇ROS中点云学习(三):使用PCL接收点云,给没有颜色的点云增加颜色变为彩色点云中我们说道,原本的/velodyne_points主题下除去XYZ信息,还有intensity(反射强度)和ring(第几圈)信息,但我们把它们舍弃了。
如果现在我们需要这些信息,同时还需要颜色、标签等信息,这就需要自己定义一个点云类型了。
为了主程序的简洁,我们建立一个头文件myPointType.h,在这里面我们定义我们所需要的点云类型,具体代码如下:
#ifndef PCL_NO_PRECOMPILE
#define PCL_NO_PRECOMPILE
#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
/*
*一个具有XYZ、intensity、ring的点云类型
*/
struct PointXYZIR
{
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR,
(float, x, x) (float, y, y)
(float, z, z) (float, intensity, intensity)
(uint16_t, ring, ring)
)
/*
* 一个具有XYZ、RGB、intensity、ring的点云类型
*/
struct PointXYZRGBIR
{
PCL_ADD_POINT4D;
PCL_ADD_RGB;
PCL_ADD_INTENSITY;
uint16_t ring;
uint16_t label;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZRGBIR,
(float, x, x)
(float, y, y)
(float, z, z)
(float, rgb, rgb)
(float, intensity, intensity)
(uint16_t, label, label)
(uint16_t, ring, ring)
)
#endif //PCL_NO_PRECOMPILE
接着建立文件pcl_structure.cpp,在这里面我们使用我们定义的点云类型,具体代码如下
#include "myPointType.h"
class pcl_structure
{
private:
ros::NodeHandle n;
ros::Subscriber subCloud;
ros::Publisher pubCloud;
sensor_msgs::PointCloud2 msg; //接收到的点云消息
sensor_msgs::PointCloud2 structure_msg; //等待发送的点云消息
public:
pcl_structure():
n("~"){
subCloud = n.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 1, &pcl_structure::getcloud, this); //接收velodyne点云数据,进入回调函数getcloud()
pubCloud = n.advertise<sensor_msgs::PointCloud2>("/structure_cloud", 1000); //建立了一个发布器,主题是/adjustd_cloud,方便之后发布加入颜色之后的点云
}
//回调函数
void getcloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
pcl::PointCloud<PointXYZRGBIR>::Ptr structure_pcl_ptr (new pcl::PointCloud<PointXYZRGBIR>); //放在这里是因为,每次都需要重新初始化,舍弃了原有但没用的两个通道intensity、ring
pcl::PointCloud<PointXYZIR>::Ptr raw_pcl_ptr (new pcl::PointCloud<PointXYZIR>); //VLP-16的点云消息包含xyz和intensity、ring的,这里没有加ring不知道为啥也可以,需要的话要自己定义一个点类型PointXYZIR
pcl::fromROSMsg(*laserCloudMsg, *raw_pcl_ptr); //把msg消息指针转化为点云指正
for (int i = 0; i < raw_pcl_ptr->points.size(); i++)
//把接收到的点云位置不变,颜色设置为红色
{
PointXYZRGBIR p;
p.x=raw_pcl_ptr->points[i].x;
p.y=raw_pcl_ptr->points[i].y;
p.z=raw_pcl_ptr->points[i].z;
p.r = 0;
p.g = 0;
p.b = 255;
p.label = (rand() % (10+1)); //设置10个标签,标签随机
p.intensity = raw_pcl_ptr->points[i].intensity; //继承之前点云的intensity
p.ring = raw_pcl_ptr->points[i].ring; //继承之前点云的ring
structure_pcl_ptr->points.push_back(p);
}
structure_pcl_ptr->width = 1;
structure_pcl_ptr->height = raw_pcl_ptr->points.size();
pcl::toROSMsg( *structure_pcl_ptr, structure_msg); //将点云转化为消息才能发布
structure_msg.header.frame_id = "velodyne";//帧id改成和velodyne一样的
pubCloud.publish( structure_msg); //发布调整之后的点云数据,主题为/adjustd_cloud
}
~pcl_structure(){
}
};
int main(int argc, char** argv) {
ros::init(argc, argv, "structure"); //初始化了一个节点,名字为structure
pcl_structure ps;
ros::spin();
return 0;
}
CMakeLists.txt文件和之前差不多,具体为:
cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
project(pcl_structure)
set(PACKAGE_DEPENDENCIES
roscpp
sensor_msgs
pcl_ros
pcl_conversions
std_srvs
message_generation
std_msgs
)
find_package(PCL 1.3 REQUIRED COMPONENTS common io)
find_package(catkin REQUIRED COMPONENTS ${
PACKAGE_DEPENDENCIES})
include_directories(include ${
catkin_INCLUDE_DIRS} ${
PCL_INCLUDE_DIRS})
link_directories(${
PCL_LIBRARY_DIRS})
add_definitions(${
PCL_DEFINITIONS})
add_executable(pcl_structure pcl_structure.cpp)
target_link_libraries(pcl_structure ${
PCL_LIBRARIES} ${
catkin_LIBRARIES} )
和上一篇一样,rosbag我们的velodyne包,就可以调用rviz来查看最后的效果。我们可以发现Color Transformer不仅可以使用RGB8查看(效果为蓝色),还可以使用intensity查看,且通道里面多了intensity(反射强度)、ring(第几圈)、labe(标签)信息。
成功!