ROS中点云学习(四):使用自己定义的PCL点云

在上一篇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(标签)信息。
成功!

  • 10
    点赞
  • 23
    收藏
    觉得还不错? 一键收藏
  • 3
    评论
PCL转换为ROS格式需要进行以下步骤: 1. 创建一个ROS节点和发布者对象。 ```cpp ros::NodeHandle nh; ros::Publisher cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("point_cloud", 1); ``` 2. 创建一个PCL对象和ROS对象。 ```cpp pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZ>); sensor_msgs::PointCloud2 ros_cloud; ``` 3. 将PCL对象转换为ROS对象。 ```cpp pcl::toROSMsg(*pcl_cloud, ros_cloud); ``` 4. 设置ROS对象的header信息。 ```cpp ros_cloud.header.frame_id = "base_link"; ros_cloud.header.stamp = ros::Time::now(); ``` 5. 发布ROS对象。 ```cpp cloud_pub.publish(ros_cloud); ``` 完整代码示例: ```cpp #include <ros/ros.h> #include <sensor_msgs/PointCloud2.h> #include <pcl_conversions/pcl_conversions.h> #include <pcl/point_cloud.h> #include <pcl/point_types.h> int main(int argc, char **argv) { // 初始化ROS节点 ros::init(argc, argv, "pcl_to_ros_publisher"); ros::NodeHandle nh; // 创建发布者对象 ros::Publisher cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("point_cloud", 1); // 创建PCL对象 pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZ>); // 填充PCL对象 // 将PCL对象转换为ROS对象 sensor_msgs::PointCloud2 ros_cloud; pcl::toROSMsg(*pcl_cloud, ros_cloud); // 设置ROS对象的header信息 ros_cloud.header.frame_id = "base_link"; ros_cloud.header.stamp = ros::Time::now(); // 发布ROS对象 cloud_pub.publish(ros_cloud); // 循环等待 ros::spin(); return 0; } ```

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值