简单范例

功能描述

使用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
│   └── pcl_reg
├── 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"的topicpublisher_ = 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 mapbool 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"的topicpublisher_->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);// 计时结束并计算costauto 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;//100int icpMaximumIterations_=1;//100double icpTransformationEpsilon_=1e-10;double icpEuclideanFitnessEpsilon_=0.001;double icpFitnessScoreThresh_=0.3;//0.3double 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_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(reg_pcl rclcpp sensor_msgs)
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 img2ptodom_pre# pt_show# EXPORT export_${PROJECT_NAME}DESTINATION lib/${PROJECT_NAME})##安装launch文件,无论该功能包的launch目录下有多少个launch文件，launch相关配置只需要设置一次即可。ros2 launch <包名> <launch文件名，无需带上launch文件夹地址>
install(DIRECTORY launchconfigDESTINATION 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

from launch import LaunchDescription
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directorydef generate_launch_description():config = os.path.join(get_package_share_directory('pcl_reg'),'config','img2pt.yaml')return LaunchDescription([Node(package='pcl_reg',node_executable='img2pt',name='point_cloud_publisher',output="screen",emulate_tty=True,parameters=[config],# parameters=[#     {"voxel_grid_x":0.08},#     {"voxel_grid_y":0.12},#     {"voxel_grid_z":0.66}# ]),])

reg_pcl.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directorydef 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],# parameters=[#     {"voxel_grid_x":0.08},#     {"voxel_grid_y":0.12},#     {"voxel_grid_z":0.66}# ]),])

mapping.launch.py

from launch import LaunchDescription
from launch_ros.actions import Nodedef 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

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Nodedef 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')return LaunchDescription([Node(package='rviz2',node_executable='rviz2',# name='rviz2', arguments=['-d', rviz_config])])

mapping_sub.launch.py

import osfrom ament_index_python.packages import get_package_share_directoryfrom launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSourcedef 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 #是否使用仿真时间，一般为falsevoxel_grid_x: 0.05voxel_grid_y: 0.05voxel_grid_z: 0.85

reg_pcl.yaml

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

使用rviz2订阅topic，查看点云

编写代码订阅topic，查看点云

报错经验总结

ros1系统性卡死

rosnode list #查看节点
rosnode kill #杀死节点
rosnode cleanup #清除无法访问节点的注册信息:杀死kill杀不死的节点

空间或内存或log空间不够时（也是gazebo相关报错，使用gazebo时需要内存和空间）

conda clean --all #清除conda缓存
top #或者top查看进程中谁占用内存较大，并kill掉
rm -rf ~/.ros #清除ros缓存/也可以避免~/.ros/log权限问题

ros1和ros2切换

source /opt/ros/eloquent/setup.bash #ROS2对应ubuntu18版本为eloquent
source /opt/ros/melodic/setup.bash #ROS1对应ubuntu18版本为melodic
一个终端内尽量不要来回切换版本，如果出现ROS_DISTRO相关提示，请关闭终端重新打开