简单范例
功能描述
使用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