ROS——基于PCL实现点云处理

sudo apt-get install ros-melodic-navigation

IPM-ROS

git clone https://github.com/Irvingao/IPM-mapping-ros.git
# catkin_create_pkg IPM-mapping-ros rospy rosmsg roscpp

编译

cd ~/your_ws
catkin_make -DCATKIN_WHITELIST_PACKAGES="IPM-mapping-ros"

打开Realsense发布点云数据

• Ubuntu18.04——基于X86和Arm安装并配置Realsense-ros环境

roslaunch realsense2_camera rs_camera.launch filters:=pointcloud

rviz

PCL点云处理

• ROS中PCL数据转换
• Pcl::PointCloud和pcl::PointCloud::Ptr类型的转换
• 使用PCL接收点云，操作之后重新发送

接收点云信息并用pcl显示

• 参考文章：使用ros订阅点云并在pcl::visualization中实时显示

pcl_view.cpp

//common headers
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

//headers of ros
#include 
#include 
#include 
#include 
#include 

//headers of pcl
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("realtime pcl"));

ros::Publisher pub;

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
   // 声明存储原始数据与滤波后的数据的点云的格式
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;    //原始的点云的数据格式
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  // 转化为PCL中的点云的数据格式
  pcl_conversions::toPCL(*input, *cloud);

  pub.publish (*cloud);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1;
  cloud1.reset (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg (*input, *cloud1);

//  pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
//  viewer.showCloud(cloud1,"Simple Cloud Viewer");

  viewer1->removeAllPointClouds();  // 移除当前所有点云
  viewer1->addPointCloud(cloud1, "realtime pcl");
  viewer1->updatePointCloud(cloud1, "realtime pcl");
  viewer1->spinOnce(0.001);
}

int main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "pcl");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  //"/camera/depth/color/points"realsense发布的点云数据
  ros::Subscriber sub = nh.subscribe ("/camera/depth/color/points", 10, cloud_cb);
  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<pcl::PCLPointCloud2> ("output", 10);
  // Spin
  ros::spin ();
}

CMakeLists.txt

find_package(PCL REQUIRED)

catkin_package()

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
)

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

接收到点云并进行色彩处理并发布

pcl_process.cpp

//common headers
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

//headers of ros
#include 
#include 
#include 
#include 
#include 

//headers of pcl
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 


class pcl_sub
{
private:
  ros::NodeHandle n;
  ros::Subscriber subCloud;
  ros::Publisher pubCloud;
  sensor_msgs::PointCloud2 msg;  //接收到的点云消息
  sensor_msgs::PointCloud2 adjust_msg;  //等待发送的点云消息
  pcl::PointCloud<pcl::PointXYZRGB> adjust_pcl;   //建立了一个pcl的点云，作为中间过程

public:
  pcl_sub():
    n("~"){
    subCloud = n.subscribe<sensor_msgs::PointCloud2>("/camera/depth/color/points", 1, &pcl_sub::getcloud, this); //接收点云数据，进入回调函数getcloud()
    pubCloud = n.advertise<sensor_msgs::PointCloud2>("/adjustd_cloud", 1000);  //建立了一个发布器，主题是/adjusted_cloud，方便之后发布调整后的点云
  }

  //回调函数
  void getcloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr adjust_pcl_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);   //放在这里是因为，每次都需要重新初始化
    pcl::fromROSMsg(*laserCloudMsg, *adjust_pcl_ptr);  //把msg消息转化为点云
    adjust_pcl = *adjust_pcl_ptr;  
    for (int i = 0; i < adjust_pcl.points.size(); i++)
    //把接收到的点云位置不变，颜色全部变为绿色
    {
      adjust_pcl.points[i].r = 0;
      adjust_pcl.points[i].g = 255;
      adjust_pcl.points[i].b = 0;
    }
    pcl::toROSMsg(adjust_pcl, adjust_msg);  //将点云转化为消息才能发布
    pubCloud.publish(adjust_msg); //发布调整之后的点云数据，主题为/adjustd_cloud
  }

  ~pcl_sub(){}

};

int main(int argc, char** argv) {

  ros::init(argc, argv, "colored");  //初始化了一个节点，名字为colored

  pcl_sub ps;

  ros::spin();
  return 0;
}

接收到图片进行处理并转换为彩色点云发布

//common headers
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

//headers of ros
#include 
#include 
#include 
#include 
#include 

//headers of opencv
#include 
#include 
#include 

//headers of pcl
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 


// 全局变量
ros::Publisher pubCloud;
int R, G, B; // R,G,B值
cv::Mat image; // 接收到图像信息
pcl::PointCloud<pcl::PointXYZRGB> cloud;  //建立了一个pcl的点云中间量（不能直接发布）
sensor_msgs::PointCloud2 output_msg;  //建立一个可以直接发布的点云

void imagepointcloudCallback(const sensor_msgs::ImageConstPtr& msg)
{
  try
  {
    image = cv_bridge::toCvShare(msg, "bgr8") -> image; // 获取图像

    pcl::PointCloud<pcl::PointXYZRGB> cloud;  //建立了一个pcl的点云（不能直接发布）
    cloud.width = image.rows;
    cloud.height =image.cols;
    cloud.points.resize(cloud.width * cloud.height); // 创建等于像素个数的点云

    sensor_msgs::PointCloud2 output_msg;  //建立一个可以直接发布的点云
    output_msg.header.stamp = ros::Time::now();

    int n = 0; // 点云中的点的标号
    for (int i = 0; i < image.rows; i++)
    { 
      for(int j=0;j<image.cols;j++)
      {
        R = image.at<cv::Vec3b>(i, j)[2];
        G = image.at<cv::Vec3b>(i, j)[1];
        B = image.at<cv::Vec3b>(i, j)[0];
        if (R==0 && G==0 && B==0){
          continue;
        } else {
          cloud.points[n].x = 5 * j / (double)cloud.height; // 放缩比例长
          cloud.points[n].y = 5 * i / (double)cloud.width; // 宽
          cloud.points[n].z = 0;
          cloud.points[n].r = R; // R通道
          cloud.points[n].g = G; // G通道
          cloud.points[n].b = B; // B通道
        }
        n = n + 1;
      }
    }
    pcl::toROSMsg(cloud, output_msg); //将点云转化为消息才能发布
    output_msg.header.frame_id = "map";
    pubCloud.publish(output_msg); //发布调整之后的点云数据，主题为/adjustd_cloud
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
  cv::waitKey(1);
}

int main(int argc, char** argv) {
  // 初始化节点
  ros::init(argc, argv, "pcl_process_node");  //初始化了一个节点，名字为pcl_process_node
  // 局部变量
  ros::NodeHandle n;
  ros::Subscriber subCloud;

  // opencv节点
  image_transport::ImageTransport it(n);
  image_transport::Subscriber sub = it.subscribe("/rgb_camera/image_ipm", 1, imagepointcloudCallback);
  
  // 点云节点
  pubCloud = n.advertise<sensor_msgs::PointCloud2>("/map/pointcloud_ipm", 1000);  //建立了一个发布器，主题是/adjusted_cloud，方便之后发布调整后的点云

  ros::spin();
  return 0;
}

读取图像转成点云并合并已有点云

//common headers
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

//headers of ros
#include 
#include 
#include 
#include 
#include 

//headers of opencv
#include 
#include 
#include 

//headers of pcl
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

// 全局变量
ros::Publisher pubCloud;
// 点云发布相关
cv::Mat image; // 接收到图像信息
int R, G, B; // 像素RGB信息

// 点云接收相关
pcl::PointCloud<pcl::PointXYZRGB> final_rec_cloud;   //建立了一个pcl的点云，作为全局的接收与发送端的合成点云变量


//回调函数
void getcloud(const sensor_msgs::PointCloud2ConstPtr& input_msg){
  pcl::PointCloud<pcl::PointXYZRGB> recive_cloud;   //建立了一个pcl的点云，作为中间过程
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr recive_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>); //建立一个接收ROS信息的点云指针
  pcl::fromROSMsg(*input_msg, *recive_cloud_ptr);  //把msg消息转化为点云
  recive_cloud = *recive_cloud_ptr;  
  final_rec_cloud = recive_cloud; // 将接收到的点云传递给全局点云变量
}

void imagepointcloudCallback(const sensor_msgs::ImageConstPtr& msg) {
  // 在循环时需要清空变量中的点云；因此需要重新定义。
  pcl::PointCloud<pcl::PointXYZRGB> cloud;  //建立了一个pcl的存储图像的点云中间量（不能直接发布）
  pcl::PointCloud<pcl::PointXYZRGB> output_cloud;   //建立了一个pcl的点云，作为合成点云的中间点云变量
  sensor_msgs::PointCloud2 output_msg;  //建立一个可以直接发布的点云
  try {
    image = cv_bridge::toCvShare(msg, "bgr8") -> image; // 获取图像
    // 初始化一个和图像同样大小的点云数据
    cloud.width = image.rows;
    cloud.height =image.cols;
    cloud.points.resize(cloud.width * cloud.height); // 创建这么多个点

    output_msg.header.stamp = ros::Time::now();

    int n = 0; // 点云中的点的标号
    for (int i = 0; i < image.rows; i++) { 
      for(int j=0;j<image.cols;j++) {
        R = image.at<cv::Vec3b>(i, j)[2];
        G = image.at<cv::Vec3b>(i, j)[1];
        B = image.at<cv::Vec3b>(i, j)[0];
        if (R==0 && G==0 && B==0){
          continue;
        } else {
          cloud.points[n].x = 5 * j / (double)cloud.height; // 放缩比例长
          cloud.points[n].y = 5 * i / (double)cloud.width; // 宽
          cloud.points[n].z = 0;
          cloud.points[n].r = R; // R通道
          cloud.points[n].g = G; // G通道
          cloud.points[n].b = B; // B通道
        }
        n = n + 1;
      }
    }
    if (final_rec_cloud.points.size() != 0) {
      ROS_INFO_STREAM("merge two cloud");
      output_cloud.points.resize(final_rec_cloud.points.size() + image.rows*image.cols);
      output_cloud = cloud + final_rec_cloud;
    } else {
      output_cloud = cloud;
    }
    pcl::toROSMsg(output_cloud, output_msg); //将点云转化为消息才能发布
    output_msg.header.frame_id = "camera_link";
    pubCloud.publish(output_msg); //发布调整之后的点云数据
  }
  catch (cv_bridge::Exception& e) {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
  cv::waitKey(1);
}

int main(int argc, char** argv) {
  // 初始化节点
  ros::init(argc, argv, "pcl_process_node");  //初始化了一个节点，名字为pcl_process_node
  // 局部变量
  ros::NodeHandle n;
  ros::Subscriber subCloud;

  // opencv节点
  image_transport::ImageTransport it(n);
  // image_transport::Subscriber sub = it.subscribe("/camera/color/image_raw", 1, imagepointcloudCallback);
  image_transport::Subscriber sub = it.subscribe("/rgb_camera/image_ipm", 1, imagepointcloudCallback);
  
  // 点云节点
  subCloud = n.subscribe<sensor_msgs::PointCloud2>("/camera/depth/color/points", 1, getcloud); //接收点云数据，进入回调函数getcloud()
  pubCloud = n.advertise<sensor_msgs::PointCloud2>("/merge_cloud", 1000);  //建立了一个发布器，主题是/adjusted_cloud，方便之后发布调整后的点云

  ros::spin();
  return 0;
}