ros2编写pcl节点加载pcd文件

本文主要是介绍ros2编写pcl节点加载pcd文件，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

初次学习ros2和pcl，尝试在ros2中创建节点，加载pcd文件，并在rviz中进行可视化，记录一下整个过程。

编辑环境

ubuntu20.04 + ros2_foxy

创建节点

mkdir -p proj_ws_pcl/src #创建工程文件夹
cd proj_ws_pcl/src #创建源码文件夹
ros2 pkg create --build-type=ament_cmake --node-name reg_pcl # 创建节点

创建好后，文件目录工程如下

proj_ws_pcl
└── src└── pcl_reg├── CMakeLists.txt├── include（可选）│   └── pcl_reg├── package.xml└── src├── other.txt├── pt_show.cpp（可选）└── reg_pcl.cpp

编辑代码

修改pcl_reg/src/reg_pcl.cpp代码

#include <iostream>
#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include "pcl_conversions/pcl_conversions.h"
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>// typedef pcl::PointXYZRGB PointType;
using namespace std::chrono_literals;class PointCloudPublisher : public rclcpp::Node
{
public:PointCloudPublisher() : Node("point_cloud_publisher"){count = 2133;// 创建一个Publisher，发布PointCloud2消息到名为"pt"的topicpublisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("pt", 10);// 创建一个定时器，每秒钟发布一次PointCloud2消息  std::chrono::seconds(1)timer_ = this->create_wall_timer(1s, std::bind(&PointCloudPublisher::publishPointCloud, this));}private:void publishPointCloud(){// 创建一个PointCloud对象pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);// 填充PointCloud数据（此处仅为示例）// 从PCD文件中加载点云数据std::string path_pcd = "/home/liangwx/proj_ws/src/gimbal/RAW_POINTCLOUD_20240509_165837.444_cut.pcd";// std::string path_pcd = "/path/to/point_cloud_name"+std::to_string(count)+".pcd";if (pcl::io::loadPCDFile<pcl::PointXYZRGB>(path_pcd, *cloud) == -1){PCL_ERROR("Failed to load PCD file\n");// return;}count++;RCLCPP_INFO(this->get_logger(), "Publishing: '%d'", count);// 创建一个PointCloud2消息sensor_msgs::msg::PointCloud2::UniquePtr cloud_msg(new sensor_msgs::msg::PointCloud2);pcl::toROSMsg(*cloud, *cloud_msg);cloud_msg->header.frame_id = "base_link";  // 设置坐标系// 设置PointCloud2消息的时间戳cloud_msg->header.stamp = this->now();// 发布PointCloud2消息到"pt"的topicpublisher_->publish(std::move(cloud_msg));}/// @brief int count;rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;rclcpp::TimerBase::SharedPtr timer_;
};int main(int argc, char** argv)
{//  读取点云点云序列并发布名为pt的topic，然后使用rviz2订阅pt并可视化rclcpp::init(argc, argv);auto node = std::make_shared<PointCloudPublisher>();rclcpp::spin(node);rclcpp::shutdown();// 读取单个点云// pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);// int count = 2133;// std::string path_pcd = "/home/apollo/zr/code/dataset_sim_gazebo/point_cloud"+std::to_string(count)+".pcd";// if (pcl::io::loadPCDFile<pcl::PointXYZRGB>(path_pcd, *cloud) == -1)// {//     PCL_ERROR("Failed to load PCD file\n");//     // return;// }return 0;
}

编辑CMakeList.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 dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)#########################################################
# 寻找依赖库（标准库）
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)# 寻找依赖库（外部库）
find_package(Eigen3 REQUIRED)
# 针对PCL库版本不适配会出现warning，做的取消注释，其实没有解决问题，真正补丁在pcl的github的4431，如果不用补丁需要升级pcl的版本为1.12.我使用的是1.8版本会报错
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}# ${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(reg_pcl src/reg_pcl.cpp)
target_include_directories(reg_pcl PUBLIC$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>$<INSTALL_INTERFACE:include>)install(TARGETS reg_pclDESTINATION lib/${PROJECT_NAME})#####################################################install(TARGETS reg_pcl # pt_showEXPORT export_${PROJECT_NAME}DESTINATION lib/${PROJECT_NAME})#####################################################if(BUILD_TESTING)find_package(ament_lint_auto REQUIRED)# the following line skips the linter which checks for copyrights# uncomment the line when a copyright and license is not present in all source files#set(ament_cmake_copyright_FOUND TRUE)# the following line skips cpplint (only works in a git repo)# uncomment the line when this package is not in a git repo#set(ament_cmake_cpplint_FOUND TRUE)ament_lint_auto_find_test_dependencies()
endif()ament_package()

编译运行

在proj_ws_pcl目录下进行编译

编译代码

cd ..
colcon build

运行节点

source install/setup.bash 
ros2 run pcl_reg reg_pcl

可视化点云

rviz2

参考链接

https://blog.csdn.net/sinat_21699465/article/details/132567374

这篇关于ros2编写pcl节点加载pcd文件的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---