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

编程语言 2022-06-18 04:20:30 阅读次数: 0

文章目录

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发布点云数据

roslaunch realsense2_camera rs_camera.launch filters:=pointcloud

rviz

在这里插入图片描述

PCL点云处理

接收点云信息并用pcl显示

pcl_view.cpp

//common headers
#include <iostream>
#include <algorithm>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <boost/thread/thread.hpp>
#include <fstream>
#include <string>

//headers of ros
#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/UInt16.h>
#include <sensor_msgs/PointCloud2.h>

//headers of pcl
#include <pcl/common/common.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/search/kdtree.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/ros/conversions.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_normal_sphere.h>
#include <pcl/sample_consensus/sac_model_cylinder.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/principal_curvatures.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/features/moment_of_inertia_estimation.h>

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 <iostream>
#include <algorithm>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <boost/thread/thread.hpp>
#include <fstream>
#include <string>

//headers of ros
#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/UInt16.h>
#include <sensor_msgs/PointCloud2.h>

//headers of pcl
#include <pcl/common/common.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/search/kdtree.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/ros/conversions.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_normal_sphere.h>
#include <pcl/sample_consensus/sac_model_cylinder.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/principal_curvatures.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/features/moment_of_inertia_estimation.h>


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 <iostream>
#include <algorithm>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <boost/thread/thread.hpp>
#include <fstream>
#include <string>

//headers of ros
#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/UInt16.h>
#include <sensor_msgs/PointCloud2.h>

//headers of opencv
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

//headers of pcl
#include <pcl/common/common.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/search/kdtree.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/ros/conversions.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_normal_sphere.h>
#include <pcl/sample_consensus/sac_model_cylinder.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/principal_curvatures.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/features/moment_of_inertia_estimation.h>


// 全局变量
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 <iostream>
#include <algorithm>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <boost/thread/thread.hpp>
#include <fstream>
#include <string>

//headers of ros
#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/UInt16.h>
#include <sensor_msgs/PointCloud2.h>

//headers of opencv
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

//headers of pcl
#include <pcl/common/common.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/search/kdtree.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/ros/conversions.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_normal_sphere.h>
#include <pcl/sample_consensus/sac_model_cylinder.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/principal_curvatures.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/features/moment_of_inertia_estimation.h>

// 全局变量
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;
}

在这里插入图片描述

猜你喜欢

转载自blog.csdn.net/qq_45779334/article/details/124260234
今日推荐
openKylin 社区生态委员会第六次会议圆满召开
阿里云正式发布通义千问 2.5
Python 3.13 发布首个 Beta：实验性自由线程模式和 JIT、改进交互式解释器
Stack Overflow 拿我的代码去训练 AI 大模型，还封了我的账号
Pop!_OS 的 COSMIC 桌面完成 App Store 上架工作
报告：Django 仍然是 74% 开发者的首选
《2024 年一季度互联网投融资运行情况》研究报告
15 年前上了“FFmpeg 耻辱柱”，今天他还得谢谢咱——腾讯QQPlayer一雪前耻？
TIOBE 5 月榜单：Fortran “复活”进入 Top 10
GCC 14.1 发布
面壁智能发布 Eurux-8x22B 开源大模型 —— 堪称「理科状元」
开源日报 | 谷歌扶持鸿蒙上位；开源Rabbit R1；Docker加持的安卓手机；微软的焦虑和野心；海尔电器把开放平台关了
周排行
计算机组成与设计（七）—— 除法器
Integer Approximation(分治+枚举)
大话数据库索引
windows10系统JDK的配置及下载地址
mysql实现秒值转换中原六仔平台搭建
Codeforces Round #556 (Div. 1)
百练1064 网线主管
Codeforces 995F Cowmpany Cowmpensation
子集生成之增量构造法，位向量法，二进制法
ERROR: cmd.exe failed with args /c "/APK\gradle\rungradle.bat...
每日归档
更多
2024-05-10(38)
2024-05-09(35)
2024-05-08(42)
2024-05-07(14)
2024-05-06(40)
2024-05-05(0)
2024-05-04(7)
2024-05-03(19)
2024-05-02(0)
2024-05-01(4)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022