如何在ROS2的环境下使用Pcl_ros以及pcl 功能包

richard491067290

您好，

我现在正在将一个项目从ROS像ROS2迁徙，在其中用到了PCL相关的功能包，但是我不知道该如何使用这些功能在ROS2中，

十分期待您的回复！
谢谢!

小鱼

@richard491067290 之前分享过相关文章，你可以试试

原文链接：https://mp.weixin.qq.com/s/_zp8pSJ9f766ZyATiiapVQ

前置安装

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);

richard491067290

谢谢回复!我之前也看到这个帖子了，但是```
code_text

#include <iostream>
#include <vector>
#include <Eigen/Core>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/TransformStamped.h>
#include <pcl_ros/impl/transforms.hpp>
#include <pcl_ros/point_cloud.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/filters/passthrough.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include "tf2_ros/static_transform_broadcaster.h"


class PCHandler
{
protected:
    ros::NodeHandle m_nh;
    sensor_msgs::PointCloud2 m_cloud;
    sensor_msgs::PointCloud2 m_cropped_cloud;
    geometry_msgs::PointStamped m_bottle_center;
    Eigen::Vector4f m_centroid;


public:
    ros::Subscriber m_pointcloud_sub, m_center_sub;
    ros::Publisher m_croppedcloud_pub, m_goalpose_pub;
    tf::TransformListener* m_tf_listener = NULL;
    tf2_ros::StaticTransformBroadcaster m_br;

PCHandler(void)
{
    m_pointcloud_sub = m_nh.subscribe("/camera/depth/color/points", 1, &PCHandler::getPC, this);
    m_center_sub = m_nh.subscribe("/point_in_3d", 1, &PCHandler::getCenter, this);

    m_goalpose_pub = m_nh.advertise<geometry_msgs::PoseStamped>("/bottle_detection/goalpose", 1);
    
    //Initialize tf_listener for pointcloud transformation
    m_tf_listener = new(tf::TransformListener);
    m_tf_listener->waitForTransform("/base_link", "/camera_depth_optical_frame", ros::Time::now(), ros::Duration(5.0));
    ROS_INFO("Transform to camera found.");
}

~PCHandler(void)
{
}
void getPC(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
    m_cloud = *msg;
    ROS_INFO("successfully get PointCloud");
    m_tf_listener->waitForTransform("/base_link", m_cloud.header.frame_id, m_cloud.header.stamp, ros::Duration(3.0));
    pcl_ros::transformPointCloud("base_link", m_cloud, m_cloud, *m_tf_listener);
}

void getCenter(const geometry_msgs::PointStamped::ConstPtr& msg)
{
    m_bottle_center = *msg;
    pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(m_cloud, *pointcloud_ptr);
    ROS_INFO("size of original PointCloud: %lu", pointcloud_ptr->size());
    ////define a 3d volume around bottle
    float d = 0.1;
    pcl::PointCloud<pcl::PointXYZ>::Ptr boundingbox_ptr (new pcl::PointCloud<pcl::PointXYZ>);
    boundingbox_ptr->push_back(pcl::PointXYZ(m_bottle_center.point.x-d, m_bottle_center.point.y-d, m_bottle_center.point.z+0.3));
    boundingbox_ptr->push_back(pcl::PointXYZ(m_bottle_center.point.x+d, m_bottle_center.point.y-d, m_bottle_center.point.z+0.3));
    boundingbox_ptr->push_back(pcl::PointXYZ(m_bottle_center.point.x-d, m_bottle_center.point.y+d, m_bottle_center.point.z+0.3));
    boundingbox_ptr->push_back(pcl::PointXYZ(m_bottle_center.point.x+d, m_bottle_center.point.y+d, m_bottle_center.point.z+0.3));
    boundingbox_ptr->push_back(pcl::PointXYZ(m_bottle_center.point.x-d, m_bottle_center.point.y-d, m_bottle_center.point.z-0.1));
    boundingbox_ptr->push_back(pcl::PointXYZ(m_bottle_center.point.x+d, m_bottle_center.point.y-d, m_bottle_center.point.z-0.1));
    boundingbox_ptr->push_back(pcl::PointXYZ(m_bottle_center.point.x-d, m_bottle_center.point.y+d, m_bottle_center.point.z-0.1));
    boundingbox_ptr->push_back(pcl::PointXYZ(m_bottle_center.point.x+d, m_bottle_center.point.y+d, m_bottle_center.point.z-0.1));

    pcl::ConvexHull<pcl::PointXYZ> hull;
    hull.setInputCloud(boundingbox_ptr);
    hull.setDimension(3);
    std::vector<pcl::Vertices> polygons;
    pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull (new pcl::PointCloud<pcl::PointXYZ>);
    hull.reconstruct(*surface_hull, polygons);
    pcl::PointCloud<pcl::PointXYZ>::Ptr objects (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::CropHull<pcl::PointXYZ> bb_filter;
    bb_filter.setDim(3);
    bb_filter.setInputCloud(pointcloud_ptr);
    bb_filter.setHullIndices(polygons);
    bb_filter.setHullCloud(surface_hull);
    bb_filter.filter(*objects);

//test, get the maximun and minimun of a pointcloud
    pcl::PointXYZ minPt, maxPt;
    pcl::getMinMax3D(*objects, minPt, maxPt);

//filter out points which belongs to the bottle cap
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(objects);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(maxPt.z-0.005, maxPt.z);
    pass.filter(*objects);

    ROS_INFO("size of cap cloud: %lu", objects->size());

//calculate centroid of bottle cap
    if(objects->size()>300){
    pcl::compute3DCentroid(*objects, m_centroid);
    ROS_INFO("center of the point cloud: %f, %f, %f", m_centroid(0), m_centroid(1), m_centroid(2));
    }
//broadcast frame of goalpose
    // static tf::TransformBroadcaster br;
    // br.sendTransform(
    //     tf::StampedTransform(
    //         tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(m_centroid[0], m_centroid[1], m_centroid[2])),
    //         ros::Time::now(), "base_link", "cap_frame"));

//publish goalpose

   tf2::Quaternion q;
   q.setRPY(-3.1415926/2, 0, 0);

   if(m_centroid(2)>=0.20 && m_centroid(2)<=0.40){
   geometry_msgs::PoseStamped goalpose;

   goalpose.pose.orientation.x = q.x();
   goalpose.pose.orientation.y = q.y();
   goalpose.pose.orientation.z = q.z();
   goalpose.pose.orientation.w = q.w();
   goalpose.pose.position.x = m_centroid[0];
   goalpose.pose.position.y = m_centroid[1];
   goalpose.pose.position.z = 0.4;
   goalpose.header.frame_id = "base_link";
   goalpose.header.stamp = ros::Time::now();

   m_goalpose_pub.publish(goalpose);
   ROS_INFO("goal pose is published!");

//Publish transform
   geometry_msgs::TransformStamped transform;
   transform.header.stamp = ros::Time::now();
   transform.header.frame_id = "base_link";
   transform.child_frame_id = "goal_position_0";
   transform.transform.translation.x = goalpose.pose.position.x;
   transform.transform.translation.y = goalpose.pose.position.y;
   transform.transform.translation.z = goalpose.pose.position.z;
   transform.transform.rotation.x = goalpose.pose.orientation.x;
   transform.transform.rotation.y = goalpose.pose.orientation.y;
   transform.transform.rotation.z = goalpose.pose.orientation.z;
   transform.transform.rotation.w = goalpose.pose.orientation.w;
   m_br.sendTransform(transform);
   }
   
//visualize the result
    // pcl::PointCloud<pcl::PointXYZ>::Ptr centroid_ptr (new pcl::PointCloud<pcl::PointXYZ>);
    // centroid_ptr->push_back(pcl::PointXYZ(m_centroid(0), m_centroid(1), m_centroid(2)));
    // pcl::visualization::PCLVisualizer viewer("Centroid example");
    // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler(objects,255, 255, 255);
    // viewer.addPointCloud(objects, source_cloud_color_handler, "original_cloud");
    // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> centroid_cloud_color_handler(centroid_ptr,230, 20, 20);
    // viewer.addPointCloud(centroid_ptr, centroid_cloud_color_handler, "cap_centroid");
    // //viewer.addCoordinateSystem(1.0, "cloud", 0);
    // viewer.setBackgroundColor(0.05, 0.05, 0.05, 0);
    // viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
    // viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cap_centroid");
    // while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
    // viewer.spinOnce ();
    // }

}

};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pc_handle_node");
    ros::NodeHandle nh("~");
    PCHandler pc_handler;
    ros::spin();

    return 0;
}

但是在我ROS1的版本中我运用了例如PCl_ros包和PCl包中的一些功能，我不知道该如何改写这些功能？是否能请您给我一个相关的改写例子或者其他的指导呢？万分感谢！！

pcl_ros::transformPointCloud("base_link", m_cloud, m_cloud, *m_tf_listener);

pcl::ConvexHull<pcl::PointXYZ> hull;

小鱼

@richard491067290 不知道是否解决了哈,ros2中方法类似

https://github.com/ros-perception/perception_pcl/blob/ros2/pcl_ros/src/transforms.cpp

template void pcl_ros::transformPointCloud<pcl::PointXYZ>(
  const pcl::PointCloud<pcl::PointXYZ> &,
  pcl::PointCloud<pcl::PointXYZ> &,
  const geometry_msgs::msg::TransformStamped &);

庙堂不见江湖见

@小鱼大佬请问一下有完整的工作空间可以提供学习一下吗，按照您这个流程编写功能包无法正常编译，会报几个类似下面的warning
** WARNING ** io features related to openni2 will be disabled
然后无法通过正常编译
我的ros2版本是foxy，非常感谢！！！

GENE

@1400276587 同样的问题，请问解决了吗？

大预言家·福尔摩斯·睿

此回复已被删除！