pcl_ros学习之日常套路框架

 １．pcl_tutorial 官方给的模板

int main(int argc, char** argv)
{
  pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
  pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
  //建立两个容器，容纳输入点云和输出点云，pcl格式．
  
  //Fill in the point cloud  
  pcl::PCDReader::reader;
  reader.read ("filename.pcd", *cloud);  //把pcd格式的点云作为输入点云  
  std::cerr << "pointcloud number before filtering:"<< cloud->width * cloud->height <<"data points ("<< pcl::getFieldsList (*cloud) << ")."
  //cloud->width,cloud->height 点云的宽高相乘为点云的数量．
  //pcl::getFieldsList()  用于获得*cloud点云的包含的点云信息种类　（x,y,z,intensity）
  
  //create the filtering object
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
  sor.setInputCloud (cloud);
  sor.setLeafSize (0.01f,0.01f,0.01f);
  sor.filter (*cloud_filtered);
　
　std::cerr << "pointcloud number after filtering" << cloud_filtered->width * cloud_filtered->height << "(" << pcl::getFeildsList (*cloud_filtered) <<").";
  
  pcl::PCDWriter writer;
  writer.writer ("output_filename.pcd", *cloud_filtered ,Eigen::Vector4f::zero () , Eigen::Quaternion f::Identify (), false);

  return (0);
}

  

编写一个点云处理程序的步骤：

１．主函数

２．编写点云输入输出容器

３．读取文件并赋值给输入容器

４．点云处理并赋值给输出容器

５．输出并保存

---

---

 

２．结合ros的pcl.

#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>

ros::Publisher pub

void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input)
{
  //create a container for data
  sensor_msgs::PointCloud2 output;
  
  //do data processing here...
  output = *input;
  
  //publish the data
  pub.publish(output);
}


int main (int argc , char** argv)
{
  //initialize ros
  ros::init (argc, argv, "package_name");
  ros::NodeHandel nh;

  //create a ros subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe("input",1,cloud_cb);
  //create a ros publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2> ("output",1);

  //spin
  ros::spin ();
}

编写主函数节点

int main (int argc,char** argv)

１．初始化节点　ros::init ( )

２．实例化节点    ros::NodeHandle nh;

３．订阅话题　　ros::Subsriber sub=nh.subscribe ("input",1,cloud_cb);

４．发布话题　　ros::Publisher pub=nh.advertise<sensor_msgs::PointCloud2> ("output",1)

５．ros::spin ()

编写点云处理总函数

void cloud_cb (const sensor_msgs::PointCloud2ConsrPtr& input)

1.输出容器

2.点云处理（包括点云数据类型转换）

3.发布话题？？？这个不完善啊，我去看一下自己的代码．

---

---

---

３．90度点云分割　(ros+rviz)

１）头文件　headfilename.h

头文件里包含　要引用的头文件；常数；结构体；类．等

#pragma once

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>

#include <pcl/filters/extract_indices.h>

#include <sensor_msgs/PointCloud2.h>
#include<pcl/io/pcd_io.h>//which contains the required definitions to load and store point clouds to PCD and other file formats.

#define CLIP_COR 0 //     x  zuobiao

Class PclTestCore
{
  
private:
  ros::Subscriber sub_point_cloud_;
  ros::Publisher pub_point_cloud_;
  
  void point_cb (const sensor_msgs::PointCloud2ConstPtr &in_cloud);
  void clip_above (double CLIP_COR, const pcl::PointCloud<pcl::PointXYZI>::Ptr in,const pcl::PointCloud<pcl::PointXYZI>::Ptr out)
  void clip_xy (const pcl::PointCloud<pcl::PointXYZI>::Ptr in,const pcl::PointCloud<pcl::PointXYZI>::Ptr out)

public:
  PclTestCore (ros::NodeHandle &nh);
  ~PclTestCore();
  void Spin();
};
  

1.头文件编写

2.常数

3.编写类

Class PclTestCore

private:

ros::Subscriber sub_point_cloud_;ros::Publisher pub_point_cloud_;void point_cb(const sensor_msgs::PointCloud2ConstPtr &in_cloud);void clip_above(double CLIP_COR,const pcl::PointCloud<PointXYZI>::Ptr in,const pcl::PointCloud<PointXYZI>::Ptr out) 和其他点云处理函数

public:

PclTestCore(ros::NodeHandle &nh);~PclTestCore(); void Spin()

2)  _node:主函数节点

#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>

#include <pcl/filters/extract_indices.h>

#include <sensor_msgs/PointCloud2.h>
#include<pcl/io/pcd_io.h>//which contains the required definitions to load and store point clouds to PCD and other file formats.

#define CLIP_COR 0 //     x  zuobiao

int main(int argc , char **argv)
{
  ros::init(argc,argv,"package_name");
  ros::NodeHandle nh;
  PclTestCore core(nh);
  return 0;
}

主节点编写：

１．节点初始化　ros::init(argc,argv,"package_name")

２．节点实例化　ros::NodeHandle nh

３．对节点进行操作　PclTestCore core(nh)

４．return 0

３）core_node 节点编写

#include "   .h"
PclTestCore::PclTestCore(ros::NodeHandel &nh)
{
  sub_point_cloud_ = nh.Subcriber("/velodyne_points",10,&PclTestCore::point_cb,this)
  in_publisher = nh.advertise<sensor_msgs::PointCloud2>("/cloud_msg",10)
 
  ros::spin();
}

PclTestCore::~PclTestCore(){ }
void PclTestCore::spin() { }

void PclTestCore::clip_above(double clip_cor,const pcl::PointCloud<pcl::PointXYZ>::Ptr in,const pcl::PointCloud<pcl::PointXYZI>::Ptr out)
{
  pcl::ExtractIndices<pcl::PointXYZI> cliper;
  cliper.SetInputCloud(in);
  pcl::PointIndices indices;
  for (size_t i=0; i<in->points.size();i++)
  {
    if(in->points[i].x >clip_cor)
    {
      indices.indices.push_back(i);
    }
  }
  cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices));
  cliper.setNegative(true);
  cliper.filter(*out);
  
}

void PclTestCore::clip_xy (const pcl::PointCloud<pcl::PointXYZI>::Ptr in,const pcl::PointCloud<PointXYZI>::Ptr out)
{
  pcl::ExtractIndices<pcl::PointXYZI> cliper;
  pcl::setPointCloud(in);
  pcl::PointIndices indices;
  
  for(size_t i=0;i<in->points.size();i++)
  {
    if(in->points[i].x<in->points[i].y)
    {
      indices.indices.push_back(i);
    }
  }

  cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices));
  cliper.setNegative(true);
  cliper.filter(*out);
}


ros::Publisher in_publisher;
void PclTestCore::point_cb(const sensor_msgs::PointCloud2ConstPtr &in_cloud_ptr)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr current_pc_ptr(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cliped_pc_ptr(new pcl::PointCloud<pcl::PointXYZI>);

    pcl::fromROSMsg(*in_cloud_ptr, *current_pc_ptr);

    clip_above(CLIP_COR, current_pc_ptr, cliped_pc_ptr);
    
    pcl::PointCloud<pcl::PointXYZI>::Ptr remove_close(new pcl::PointCloud<pcl::PointXYZI>);

    clip_xy(cliped_pc_ptr, remove_close);

    sensor_msgs::PointCloud2 cloud_msg;
    pcl::toROSMsg(*remove_close, cloud_msg);
    in_publisher.publish(cloud_msg);
}

这个框架：

１．头文件.h

class private:订阅发布话题，pointcb，其他点云处理函数的定义；public:PclTestCore(ros::NodeHandle &nh),~PclTestCore(),void spin()

２．core

PclTestCore::PclTestCore(ros::NodeHandel &nh) {订阅发布话题具体的，ros::spin()}

PclTestCore::~PclTestCore(){}

void PclTestCore::spin(){}

void PclTestCore::其他函数具体定义

void PclTestCore::point_cb()

3. _node

int main(int argc,char **argv)    {节点初始化，实例化，core，return}

４）建立功能包，编译

功能包ninety(download baiduyun).编译

解决了找不到头文件的问题

写个launch文件 ninety.launch(mingtianxie)

或者直接

roscore

rosbag play --clock test.bag

rosrun ninety ninety_node

rosrun rviz rviz

fixed_frame velodyne

add pointcloud2 /cloud_msg