被鱼粉直呼小鱼牛逼的代码,来看ROS2如何进行点云PCL处理(订阅、转换、保存)

昨天在交流群里有个鱼粉问小鱼关于ROS2订阅点云数据和使用PCL相关的问题。记得小鱼最早接触PCL是大一的时候，至今已经过去了多年。

ROS1里PCL相关程序很多，但是在ROS2相关的代码和例程目前还没有写过，针对这一问题，小鱼接着国外大佬的代码，做了一个叫做ROS2代码模板文档，复制粘贴，不要太爽。

接着说回点云，主要分享点云订阅，发布，生成，以及Cmake配置。

完整的相关代码可以到社区（fishros.org.cn）搜索点云。

前置安装

sudo apt install ros-你的ROS版本代号-pcl*

CMakeLists配置

cmake_minimum_required(VERSION 3.5)
project(ros2pcl_test)

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(PCL REQUIRED COMPONENTS common io)

link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(ros2pcl_test_sub 
  src/subscription_pcl.cpp
)
ament_target_dependencies(ros2pcl_test_sub 
  rclcpp 
  sensor_msgs
)

target_link_libraries(ros2pcl_test_sub ${PCL_LIBRARIES})

ament_package()

点云订阅

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>

class PclSub : public rclcpp::Node
{

public:
    PclSub(std::string name)
        : Node(name)
    {
        sub_novel = this->create_subscription<sensor_msgs::msg::PointCloud2>("sexy_girl", 10, std::bind(&PclSub::topic_callback, this, std::placeholders::_1));
    }

private:
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_novel;

    void topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(), "points_size(%d,%d)",msg->height,msg->width);
    };
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PclSub>("pclsub");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

点云转换（ROS2->PCL）

在点云订阅代码中增加下面的代码即可。

void topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud =
     boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
	 pcl::fromROSMsg(*msg, *cloud);
        RCLCPP_INFO(this->get_logger(), "points_size(%d,%d)",msg->height,msg->width);
    };

点云保存

void topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud =
     boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
	 pcl::fromROSMsg(*msg, *cloud);
	 pcl::io::savePCDFileASCII ("asd.pcd", *cloud);
    RCLCPP_INFO(this->get_logger(), "points_size(%d,%d)",msg->height,msg->width);
    };

点云生成PointCloud2迭代器

使用PointCloud2迭代器赋值sensor_msgs::msg::PointCloud2 。

#include "sensor_msgs/msg/point_cloud2.hpp"
#include "sensor_msgs/point_cloud2_iterator.hpp"

sensor_msgs::msg::PointCloud2 msg;

// Fill in the size of the cloud
msg.height = 480;
msg.width = 640;

// Create the modifier to setup the fields and memory
sensor_msgs::PointCloud2Modifier mod(msg);

// Set the fields that our cloud will have
mod.setPointCloud2FieldsByString(2, "xyz", "rgb");

// Set up memory for our points
mod.resize(msg.height * msg.width);

// Now create iterators for fields
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(cloud_msg, "r");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(cloud_msg, "g");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(cloud_msg, "b");

for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b)
{
  *iter_x = 0.0;
  *iter_y = 0.0;
  *iter_z = 0.0;
  *iter_r = 0;
  *iter_g = 255;
  *iter_b = 0;
}

点云转换（PCL->ROS2）

#include "pcl_conversions/pcl_conversions.h"

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
sensor_msgs::msg::PointCloud2 msg;

pcl::toROSMsg(*cloud, msg);
cloud_publisher->publish(msg);

今天就到这里，记得关注小鱼领取机器人、点云相关资料大礼包哦~