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"的topic
    publisher_ = 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"的topic
    publisher_->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_pcl
  DESTINATION lib/${PROJECT_NAME})

#####################################################

install(TARGETS 
  reg_pcl 
  # pt_show
  EXPORT 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