ros2 学习launch文件组织工程 yaml配置文件

Freja_Issac

已于 2023-09-14 10:59:48 修改

阅读量889

点赞数

文章标签：学习 ros2 launch yaml

于 2023-09-13 18:03:02 首次发布

本文链接：https://blog.csdn.net/sinat_21699465/article/details/132860404

版权

简单范例

功能描述

使用launch文件，统一管理工程，实现img转点云，发送到img_pt的topic，然后用reg_pcl节点进行subscribe，进行点云配准处理，输出融合后的点云到map_pt的topic。最后由rviz2进行点云展示。

环境准备

ubuntu18.04+ros2（eloquent此版本需要于Ubuntu对应）+ Python（version=3.6需要于eloquent对应）+PCL+EIGEN+rviz2
具体安装库和依赖不做赘述

代码实现

代码结构

代码结构编译前

pcl_reg
├── CMakeLists.txt
├── config
│   ├── img2pt.yaml
│   └── reg_pcl.yaml
├── include
│   └── 空
├── launch
│   ├── img2pt.launch.py
│   ├── mapping.launch.py
│   ├── mapping_sub.launch.py
│   ├── reg_pcl.launch.py
│   └── showviz.launch.py
├── package.xml
├── rviz
│   └── 空
└── src
    ├── img2pt.cpp
    ├── odom_pre.cpp
    ├── other.txt
    ├── pt_show.cpp
    └── reg_pcl.cpp

代码内容

img转pt的部分，图像为rgb三通道的语义图，转换为点云，点云为pcl::PointXYZRGB的格式，XYZ为点云位置，RGB对应图像的三通道rgb。其中XY可以通过图像的u，v坐标转换得到，因为是BEV视角，Z坐标统一置零即可。然后由于语义图上的点云数量过多，在后续匹配的过程中中会降低效率。可以做一些预处理，例如下采样，设置权值等。
map2pt.cpp

#include <iostream>
#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include "pcl_conversions/pcl_conversions.h"//for pcl::toROSMsg
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
// typedef pcl::PointXYZRGB pcl::PointXYZRGB;
using namespace std::chrono_literals;

class PointCloudPublisher : public rclcpp::Node
{
public:
    PointCloudPublisher() : Node("point_cloud_publisher")
    {
        count = 1380;
        param_respond();//参数初始化
        // timer_param_ = this->create_wall_timer(500ms, std::bind(&PointCloudPublisher::param_respond, this));
        // 创建一个Publisher，发布PointCloud2消息到名为"pt"的topic
        publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("img_pt", 10);

        // 创建一个定时器，每秒钟发布一次PointCloud2消息  std::chrono::seconds(1)
        timer_ = this->create_wall_timer(1s, std::bind(&PointCloudPublisher::publishPointCloud, this));
    }

private:
    //使用yaml进行参数配置，此处参数只会在初始化时生效一次。如果需要参数随时变化，需要做其他处理
    void param_respond()
    {
        //定义可配置参数，与yaml文件或launch文件中参数名需要对应，需要注意的是参数类型需要匹配，否则会报错。并给出参数默认值。
        this->declare_parameter<float>("voxel_grid_x", 0.05);
        this->declare_parameter<float>("voxel_grid_y", 0.05);
        this->declare_parameter<float>("voxel_grid_z", 0.05);
        //获取参数,并将参数值赋值到对应变量
        this->get_parameter("voxel_grid_x", voxel_grid_x_);
        this->get_parameter("voxel_grid_y", voxel_grid_y_);
        this->get_parameter("voxel_grid_z", voxel_grid_z_);
        //打印
        RCLCPP_INFO(this->get_logger(), "Hello voxel_grid_x:%f, voxel_grid_y:%f, voxel_grid_z:%f", voxel_grid_x_,voxel_grid_y_,voxel_grid_z_);
        //将参数统一放到std::vector<rclcpp::Parameter>中并set到节点里，此时参数文件yaml或launch文件中的数值开始生效。
        std::vector<rclcpp::Parameter> all_new_parameters{rclcpp::Parameter("voxel_grid_x", voxel_grid_x_),rclcpp::Parameter("voxel_grid_y", voxel_grid_y_),rclcpp::Parameter("voxel_grid_z", voxel_grid_z_)};
        this->set_parameters(all_new_parameters);
    }
    void publishPointCloud()
    {
        // 创建一个PointCloud对象
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_src(new pcl::PointCloud<pcl::PointXYZRGB>);
        std::string path_img = "/home/apollo/zr/code/data/sim_data/camera1-230829-164731_Lanes/"+std::to_string(count)+".jpg";
        cv::Mat image = cv::imread(path_img);

        // 检查图片是否成功加载
        if (image.empty()) {
            std::cout << "无法加载图片" << std::endl;
            // return -1;
        }
        RCLCPP_INFO(this->get_logger(), "Publishing: '%d'", count);
        count=count+12;
        

        pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
        Eigen::Matrix3f matK_ipm_inverse = Eigen::Matrix3f::Identity(3, 3);
        //bias表示车后轴中心在图像像素位置
        //k_cam表示图像像素和实际尺度m的比例关系
        float k_cam = 0.0325;
        float x_bias = 480*4.0/5.0*k_cam;
        float y_bias = 640/2.0*k_cam;
        matK_ipm_inverse << 0,-k_cam,x_bias,
                    -k_cam,0,y_bias,
                    0,0,1;

        pointCloud = ipmToPointCloud(image,matK_ipm_inverse);

        // /*
        //  点云降采样
        //  create new keyFrame, and add pointCloud to global map
        bool make_global_map = true;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsampled_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
        if(make_global_map){
            // 创建 VoxelGrid 对象
            pcl::VoxelGrid<pcl::PointXYZRGB> voxel_grid;
            voxel_grid.setInputCloud(pointCloud);
            voxel_grid.setLeafSize(voxel_grid_x_, voxel_grid_y_, voxel_grid_z_);  // 设置体素大小

            // 执行下采样
            voxel_grid.filter(*downsampled_cloud);
            // *globalFeatureCloud=globalMapDS;
            RCLCPP_INFO(this->get_logger(), "globalFeatureCloud size is: '%d'", (int)downsampled_cloud->size());
        }
        // */
        // 创建一个PointCloud2消息
        sensor_msgs::msg::PointCloud2::UniquePtr cloud_msg(new sensor_msgs::msg::PointCloud2);
        pcl::toROSMsg(*downsampled_cloud, *cloud_msg);
        // pcl::toROSMsg(*pointCloud, *cloud_msg);
        cloud_msg->header.frame_id = "base_link";  // 设置坐标系
        cloud_msg->header.stamp = this->now();// 设置PointCloud2消息的时间戳

        // 发布PointCloud2消息到"pt"的topic
        publisher_->publish(std::move(cloud_msg));
    }
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr ipmToPointCloud(const cv::Mat& ipmImage,
                                                const Eigen::Matrix3f matK_ipm_inv)
    {
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudOut(new pcl::PointCloud<pcl::PointXYZRGB>);
        for (int y = 0; y < ipmImage.rows; ++y) 
        {
            for (int x = 0; x < ipmImage.cols; ++x) 
            {
                cv::Vec3b pixel = ipmImage.at<cv::Vec3b>(y, x);
                if (!(pixel[0]==0 && pixel[1]==0 && pixel[2]==0)) 
                {
                    pcl::PointXYZRGB pclPoint;
                    Eigen::Vector3f uv1(x,y,1);
                    Eigen::Vector3f xyz=matK_ipm_inv*uv1;
                    pclPoint.x = xyz(0);
                    pclPoint.y = xyz(1);
                    pclPoint.z = 0;
                    pclPoint.r = pixel[0];
                    pclPoint.g = pixel[1];
                    pclPoint.b = pixel[2];
                    pointCloudOut->push_back(pclPoint);
                    // if(pixel[0]==233, pixel[1]==238, pixel[2]==12)
                    // {
                    //     std::cout<<"u,v:["<<x<<","<<y<<"]  x,y:["<<pclPoint.x<<","<<pclPoint.y<<"]"<<std::endl;
                    // }
                    
                }
            }
        }
        return pointCloudOut;
    }
    int count;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;

    float voxel_grid_x_;
    float voxel_grid_y_;
    float voxel_grid_z_;
    // rclcpp::TimerBase::SharedPtr timer_param_;
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PointCloudPublisher>();
    rclcpp::spin(node);
    rclcpp::shutdown();

    /*打开图片: 单独实线功能，不使用ros2系统 */
    // cv::Mat image = cv::imread("/home/apollo/zr/code/data/sim_data/camera1-230829-164731_Lanes/1440.jpg");

    // // 检查图片是否成功加载
    // if (image.empty()) {
    //     std::cout << "无法加载图片" << std::endl;
    //     return -1;
    // }

    
    // pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    // Eigen::Matrix3f matK_ipm = Eigen::Matrix3f::Identity(3, 3);
    // matK_ipm << 0,40,1.798070000000000057e+00,
    //             -40,0,0,
    //             0,0,1;

    // pointCloud = ipmToPointCloud(image,matK_ipm);

    // // 创建窗口并显示图片
    // cv::namedWindow("Image", cv::WINDOW_NORMAL);
    // cv::imshow("Image", image);

    // // 等待按键退出
    // cv::waitKey(0);
    return 0;
}

reg_pcl.cpp

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include <iostream>
#include <pcl/registration/icp.h>
#include <pcl/registration/ndt.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>

// 处理点云的函数


class PointCloudProcessor : public rclcpp::Node
{
public:
    PointCloudProcessor() : Node("point_cloud_processor")
    {
        param_respond();//参数初始化
        init_flag = true;//第一帧标志位，第一帧无法进行点云配准
        count = 0;//匹配次数，为了保存结果点云计数，可选用
        map_RT = Eigen::Matrix4f::Identity();//建图功能累计的位姿矩阵
        cloud_local_map = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
        // 创建订阅者，订阅 'img_pt' 话题的点云消息
        subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "img_pt", rclcpp::QoS(10),
            std::bind(&PointCloudProcessor::pointCloudCallback, this, std::placeholders::_1));

        // 创建发布者，发布处理后的点云到 'map_pt' 话题
        publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("map_pt", rclcpp::QoS(10));
    }

private:
    //使用yaml进行参数配置，此处参数只会在初始化时生效一次。如果需要参数随时变化，需要做其他处理
    void param_respond()
    {
        //定义可配置参数，与yaml文件或launch文件中参数名需要对应，需要注意的是参数类型需要匹配，否则会报错。并给出参数默认值。
        this->declare_parameter<bool>("use_icp", true);
        this->declare_parameter<int>("icpMaximumIterations", 1);
        this->declare_parameter<double>("icpMaxCorrespondenceDistance", 1);
        this->declare_parameter<double>("icpTransformationEpsilon", 1e-10);
        this->declare_parameter<double>("icpEuclideanFitnessEpsilon", 0.001);
        this->declare_parameter<double>("icpFitnessScoreThresh", 0.3);
        this->declare_parameter<double>("ndtTransformationEpsilon", 1e-10);
        this->declare_parameter<double>("ndtResolution", 0.1);
        this->declare_parameter<double>("ndtFitnessScoreThresh", 0.3);
        //获取参数,并将参数值赋值到对应变量
        this->get_parameter("use_icp", use_icp_);
        this->get_parameter("icpMaximumIterations", icpMaximumIterations_);
        this->get_parameter("icpMaxCorrespondenceDistance", icpMaxCorrespondenceDistance_);
        this->get_parameter("icpTransformationEpsilon", icpTransformationEpsilon_);
        this->get_parameter("icpEuclideanFitnessEpsilon", icpEuclideanFitnessEpsilon_);
        this->get_parameter("icpFitnessScoreThresh", icpFitnessScoreThresh_);
        this->get_parameter("ndtTransformationEpsilon", ndtTransformationEpsilon_);
        this->get_parameter("ndtResolution", ndtResolution_);
        this->get_parameter("ndtFitnessScoreThresh", ndtFitnessScoreThresh_);
        //调试过程中，如果怀疑参数文件有问题，可以使用下方直接赋值方法，确认其他部分没有问题，定位问题到参数文件yaml或launch文件上
        // icpTransformationEpsilon_ = 1e-10;
        // ndtTransformationEpsilon_ = 1e-10;
        RCLCPP_INFO(this->get_logger(), "Hello use_icp:%d, icpMaximumIterations:%d, icpMaxCorrespondenceDistance:%f", use_icp_,icpMaximumIterations_,icpMaxCorrespondenceDistance_);
        将参数统一放到std::vector<rclcpp::Parameter>中并set到节点里，此时参数文件yaml或launch文件中的数值开始生效。
        std::vector<rclcpp::Parameter> all_new_parameters{rclcpp::Parameter("use_icp", use_icp_),
                                                        rclcpp::Parameter("icpMaximumIterations", icpMaximumIterations_),
                                                        rclcpp::Parameter("icpMaxCorrespondenceDistance", icpMaxCorrespondenceDistance_),
                                                        rclcpp::Parameter("icpTransformationEpsilon", icpTransformationEpsilon_),
                                                        rclcpp::Parameter("icpEuclideanFitnessEpsilon", icpEuclideanFitnessEpsilon_),
                                                        rclcpp::Parameter("icpFitnessScoreThresh", icpFitnessScoreThresh_),
                                                        rclcpp::Parameter("ndtTransformationEpsilon", ndtTransformationEpsilon_),
                                                        rclcpp::Parameter("ndtResolution", ndtResolution_),
                                                        rclcpp::Parameter("ndtFitnessScoreThresh", ndtFitnessScoreThresh_)};
        this->set_parameters(all_new_parameters);

    }
    void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr input_msg)
    {
        std::cout<<"rcv pt msg"<<std::endl;
        // 将接收到的点云消息转换为 pcl::PointCloud<pcl::PointXYZRGB> 类型
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cameraCloudIn(new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::fromROSMsg(*input_msg, *cameraCloudIn);

        if(init_flag)
        {
            //第一帧不做匹配，仅保存
          cloud_src = cameraCloudIn;
          cloud_local_map = cameraCloudIn;
          init_flag = false;
          return ; 
        }
        else
        {
            cloud_tgt = cloud_src;
            cloud_src = cameraCloudIn;
            //计时开始
            rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
            auto start_time = steady_clock_.now();
            // 点云配准
            mapping_local(cloud_src,cloud_tgt);
            // 计时结束并计算cost
            auto cycle_duration = steady_clock_.now() - start_time;
            std::cout<<"icp cost time:"<<cycle_duration.seconds()<<" s"<<std::endl;
            RCLCPP_INFO(get_logger(), "Cost %.4f s", cycle_duration.seconds());
        }
        
          
        // 将处理后的点云转换为 sensor_msgs::msg::PointCloud2 类型
        sensor_msgs::msg::PointCloud2::UniquePtr result_msg(new sensor_msgs::msg::PointCloud2);
        pcl::toROSMsg(*cloud_local_map, *result_msg);
        // pcl::toROSMsg(*cloud_src, *result_msg);
        result_msg->header.frame_id = "base_link2";  // 设置坐标系
        result_msg->header.stamp = this->now();// 设置PointCloud2消息的时间戳
        // 发布处理后的点云到 'map_pt' 话题
        publisher_->publish(std::move(result_msg));

        
    }
    void mapping_local(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& src_cloud,const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& tgt_cloud)
    {


        Eigen::Matrix4f transWorldCurrent = Eigen::Matrix4f::Identity();
        transWorldCurrent(0,3)=1;
        // printMat4(transWorldCurrent);
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr currentFeatureCloudInWorld(new pcl::PointCloud<pcl::PointXYZRGB>());
        if(use_icp_)
        {
            static pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
            icp.setMaxCorrespondenceDistance(icpMaxCorrespondenceDistance_); 
            icp.setMaximumIterations(icpMaximumIterations_);
            icp.setTransformationEpsilon(icpTransformationEpsilon_);
            icp.setEuclideanFitnessEpsilon(icpEuclideanFitnessEpsilon_);

            icp.setInputSource(src_cloud);
            icp.setInputTarget(tgt_cloud);
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr transCurrentCloudInWorld(new pcl::PointCloud<pcl::PointXYZRGB>());
            icp.align(*transCurrentCloudInWorld,transWorldCurrent.matrix());//迭代效果不理想，原因尚未定位到？？？
                
            if (icp.hasConverged() == false || icp.getFitnessScore() > icpFitnessScoreThresh_) 
            {
              std::cout << "ICP locolization failed    the score is   " << icp.getFitnessScore() << std::endl;
              return ;
            } 
            else 
            {
              transWorldCurrent = icp.getFinalTransformation();
              printMat4(transWorldCurrent);
            }
        }
        else
        {
            pcl::NormalDistributionsTransform<pcl::PointXYZRGB, pcl::PointXYZRGB> ndt;
            ndt.setTransformationEpsilon(ndtTransformationEpsilon_);
            ndt.setResolution(ndtResolution_);
            ndt.setInputSource(src_cloud);
            ndt.setInputTarget(tgt_cloud);
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr transCurrentCloudInWorld(new pcl::PointCloud<pcl::PointXYZRGB>());
            ndt.align(*transCurrentCloudInWorld, transWorldCurrent.matrix());//会直接退出程序，原因尚未定位到？？？
            if (ndt.hasConverged() == false || ndt.getFitnessScore() > ndtFitnessScoreThresh_) 
            {
                std::cout << "ndt locolization failed    the score is   " << ndt.getFitnessScore() << std::endl;
                return ;
            } 
            else
            { 
                transWorldCurrent =ndt.getFinalTransformation();
                printMat4(transWorldCurrent);
            }
        }   

        map_RT = map_RT*transWorldCurrent;
        pcl::transformPointCloud (*src_cloud, *currentFeatureCloudInWorld, map_RT);
        /*
        // 保存pcd文件，方便pcl_viewer <pcdfile.pcd>
        count++;
        std::string filename1 = "/home/apollo/zr/code/resultpcd/src_tgt"+std::to_string(count)+".pcd";
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr show_cloud1(new pcl::PointCloud<pcl::PointXYZRGB>);
        *show_cloud1 = *show_cloud1+*src_cloud;
        *show_cloud1 = *show_cloud1+*tgt_cloud;
        pcl::io::savePCDFileBinary(filename1, *show_cloud1);
        std::string filename2 = "/home/apollo/zr/code/resultpcd/tgt_trans"+std::to_string(count)+".pcd";
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr show_cloud2(new pcl::PointCloud<pcl::PointXYZRGB>);
        *show_cloud2 = *show_cloud2+*tgt_cloud;
        *show_cloud2 = *show_cloud2+*currentFeatureCloudInWorld;
        pcl::io::savePCDFileBinary(filename2, *show_cloud2);
        */
        
        // 创建新的帧，并将src点云（后续需要换成匹配点云点对）添加进map点云
        *cloud_local_map = *cloud_local_map+*currentFeatureCloudInWorld;
        RCLCPP_INFO(this->get_logger(), "cloud_local_map size is: '%d'", (int)cloud_local_map->size());
        
        //
    }
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
    rclcpp::Subscription<geometry_msgs::msg::TransformStamped>::SharedPtr subscription_odom_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;

    Eigen::Matrix4f map_RT;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_local_map;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_src; //不需要初始化，后面会赋值
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_tgt;//不需要初始化，后面会赋值
    bool init_flag;
    int count;

    bool use_icp_ = true;
    double icpMaxCorrespondenceDistance_=1;//100
    int icpMaximumIterations_=1;//100
    double icpTransformationEpsilon_=1e-10;
    double icpEuclideanFitnessEpsilon_=0.001;
    double icpFitnessScoreThresh_=0.3;//0.3
    double ndtTransformationEpsilon_=1e-10;
    double ndtResolution_=0.1;
    double ndtFitnessScoreThresh_=0.3;

    void printMat4(Eigen::Matrix4f mat)
    {
        std::cout<<"matrix is :"<<std::endl;
        for(int i=0;i<mat.rows();i++)
        {
            for(int j=0;j<mat.cols();j++)
            {
                std::cout<<mat(i,j)<<" , ";
            }
            std::cout<<std::endl;
        }
    }
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<PointCloudProcessor>());
    rclcpp::shutdown();
    return 0;
}

对CMakeLists.txt进行适当修改

cmake_minimum_required(VERSION 3.5)
project(pcl_reg)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# 寻找依赖库（标准库）
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)

# 寻找依赖库（外部库）
find_package(Eigen3 REQUIRED)
# 针对PCL库版本不适配会出现warning，做的补丁，其实没有解决问题
if(NOT DEFINED CMAKE_SUPPRESS_DEVELOPER_WARNINGS)
  set(CMAKE_SUPPRESS_DEVELOPER_WARNINGS 1 CACHE INTERNAL "No dev warnings")
endif()
find_package(PCL REQUIRED)
find_package(rviz2 REQUIRED)
find_package(OpenCV 3.2.0  REQUIRED)


# 添加包含路径,方便include
include_directories(
  /usr/include
  ${EIGEN3_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
  ${RVIZ2_INCLUDE_DIRS}
  /usr/include/OGRE
  ${OpenCV_INCLUDE_DIRS}
)

# 添加可执行文件
add_executable(reg_pcl src/reg_pcl.cpp)
# 标准库需要使用ament_target_dependencies
ament_target_dependencies(reg_pcl rclcpp sensor_msgs)
# 非标准库需要使用target_link_libraries
target_link_libraries(reg_pcl
  ${PCL_LIBRARIES}
  # ${OpenCV_LIBS}
)
add_executable(img2pt src/img2pt.cpp)
ament_target_dependencies(img2pt rclcpp sensor_msgs)
target_link_libraries(img2pt
  ${PCL_LIBRARIES}
  ${OpenCV_LIBS}
)

add_executable(odom_pre src/odom_pre.cpp)
ament_target_dependencies(odom_pre rclcpp sensor_msgs)
# target_link_libraries(odom_pre
#   ${PCL_LIBRARIES}
#   ${OpenCV_LIBS}
# )

# add_executable(pt_show src/pt_show.cpp)
# ament_target_dependencies(pt_show rclcpp sensor_msgs)
# target_link_libraries(pt_show
#   ${PCL_LIBRARIES}
#   ${RVIZ2_LIBRARIES}
#   # ${OpenCV_LIBS}
# )

##安装可执行文件
install(TARGETS 
  reg_pcl 
  img2pt
  odom_pre
  # pt_show
  # EXPORT export_${PROJECT_NAME}
  DESTINATION lib/${PROJECT_NAME}
  )

##########配置文件#########
# 安装launch文件,无论该功能包的launch目录下有多少个launch文件，launch相关配置只需要设置一次即可。ros2 launch <包名> <launch文件名，无需带上launch文件夹地址>
# 配置文件config同理
# 如果由rviz配置文件也可以同理，或者指定固定位置的文件
# 使用的config文件或者launch文件并不是src文件内的pcl_reg包内的config/launch文件，而是通过install复制到/install/pcl_reg/share/pcl_reg/config文件，，或者，，/install/pcl_reg/share/pcl_reg/launch文件
install(DIRECTORY 
  launch
  config
  rviz2
  DESTINATION share/${PROJECT_NAME}/
)


if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

对package.xml进行适当修改

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>pcl_reg</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="apollo@todo.todo">apollo</maintainer>
  <license>TODO: License declaration</license>

  <depend>rclcpp</depend>
  <depend>sensor_msgs</depend>
  <depend>EIGEN3</depend>
  <depend>PCL</depend>
  <depend>OpenCV</depend>
  <!-- ros2 launch 使得launch file可以被识别 -->
  <exec_depend>ros2launch</exec_depend> 
  <buildtool_depend>ament_cmake</buildtool_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

launch文件
img2pt.launch.py
使用方法： ros2 launch <包名> <launch文件名>
使用方法： ros2 launch pcl_reg img2pt.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    config = os.path.join(
        #获取pcl_reg包的share文件夹的地址，使用的配置文件并不是src文件内的pcl_reg包内的config文件，而是通过install复制到/install/pcl_reg/share/pcl_reg/config文件
        get_package_share_directory('pcl_reg'),
        'config',
        'img2pt.yaml'
    )
    return LaunchDescription([
        Node(
            #package包名，node_executable可执行文件名，（name节点名（可选）），至少提供这两个才能使launch文件运行
            package='pcl_reg',
            node_executable='img2pt',
            name='point_cloud_publisher',
            #普通运行ros2 run，代码中的log或者cout可以打印到屏幕上；使用ros2 launch运行的话，默认打印到屏幕上的输出被关闭了。需要手动设置打开：output/emulate_tty
            output="screen",
            emulate_tty=True,
            parameters=[config],
            # 参数可以使用下面写法，直接在launch文件中设置，也可以通过yaml文件配置，例如上面
            # parameters=[
            #     {"voxel_grid_x":0.08},
            #     {"voxel_grid_y":0.12},
            #     {"voxel_grid_z":0.66}
            # ]
        ),
    ])

reg_pcl.launch.py
使用方法： ros2 launch pcl_reg reg_pcl.launch.py
使用方法与py内容填写同上。注意格式即可

from launch import LaunchDescription
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    config = os.path.join(
        get_package_share_directory('pcl_reg'),
        'config',
        'reg_pcl.yaml'
    )
    return LaunchDescription([
        Node(
            package='pcl_reg',
            node_executable='reg_pcl',
            name='point_cloud_processor',
            output="screen",
            emulate_tty=True,
            parameters=[config],
        ),
    ])

mapping.launch.py
简单使用launch文件将两个可执行文件同时启动。但是这样一个文件内涉及对两个可执行文件的配置，这不利于一个大型工程的管理。

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='pcl_reg',
            node_executable='img2pt',
            name='point_cloud_publisher',
            output="screen",
            emulate_tty=True,
        ),
        Node(
            package='pcl_reg',
            node_executable='reg_pcl',
            name='point_cloud_processor',
            output="screen",
            emulate_tty=True,
        ),
        ##ExecuteProcess是针对有终端输入的情况
        # ExecuteProcess()
    ])

# def generate_launch_description():
#     ld = LaunchDescription()
    
#     node1 = Node(
#         package='pcl_reg',
#         eloquent版本不能用executable，要使用node_executable
#         node_executable='img2pt',
#         # name='point_cloud_publisher',
#         # output='screen'
#     )
    
#     ld.add_action(node1)
    
#     return ld

showviz.launch.py
launch文件不仅可以启动自己编写的可执行文件，也可以启动ros2的工具，例如rviz2.这个工具可以查看机器人的运行状态，可以查看点云等。
其中查看点云需要配置点云的坐标系与代码中的frame_id保持一致，需要设置节点名与代码中的topic（create_publisher的参数）保持一致
如果想要自动化一点，可以通过保存rviz2的的config文件，来保证效果。

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    rviz_config = os.path.join(
        get_package_share_directory('pcl_reg'),
        'rviz2',
        'map_pt_conf.rviz'
        )
    # rviz_config = os.path.join(
    #     '/home','apollo',
    #     '.rviz2',
    #     'map_pt_conf.rviz'
    #     )
    #下面这个写法也可以直接在终端执行，不难推测出，终端可启动工具都可以用这种方式使用launch文件启动
    return LaunchDescription([
        Node(
            package='rviz2',
            node_executable='rviz2',
            # name='rviz2', 
            arguments=['-d', rviz_config]
        )
    ])

mapping_sub.launch.py
对整个工程的启动代码做统一管理

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource


def generate_launch_description():
   img2pt = IncludeLaunchDescription(
      PythonLaunchDescriptionSource([os.path.join(
         get_package_share_directory('pcl_reg'), 'launch'),
         '/img2pt.launch.py'])
      )
   reg_pcl = IncludeLaunchDescription(
      PythonLaunchDescriptionSource([os.path.join(
         get_package_share_directory('pcl_reg'), 'launch'),
         '/reg_pcl.launch.py'])
      )
   rviz_node = IncludeLaunchDescription(
      PythonLaunchDescriptionSource([os.path.join(
         get_package_share_directory('pcl_reg'), 'launch'),
         '/showviz.launch.py'])
      )

   return LaunchDescription([
      img2pt,
      reg_pcl,
      # rviz_node
   ])

img2pt.yaml

point_cloud_publisher: #节点名、不是excutable文件名
  ros__parameters: #固定形式
    use_sim_time: false #是否使用仿真时间，一般为false
    voxel_grid_x: 0.05
    voxel_grid_y: 0.05
    voxel_grid_z: 0.85

reg_pcl.yaml

point_cloud_processor:
  ros__parameters:
    use_sim_time: false
    use_icp_: true
    icpMaximumIterations: 1 #100 int型请务必写整数
    icpMaxCorrespondenceDistance: 1.0005 #double类型不要直接写1，要写带小数点的
    icpTransformationEpsilon: 1e-10 #支持科学计数法
    icpEuclideanFitnessEpsilon: 0.001
    icpFitnessScoreThresh: 0.3
    ndtTransformationEpsilon: 1e-10
    ndtResolution: 0.1
    ndtFitnessScoreThresh: 0.3