ros2+pcl入门范例

Freja_Issac

已于 2023-09-13 14:01:26 修改

阅读量1.4k

点赞数 1

文章标签： ros2 PCL 入门

于 2023-08-30 09:14:11 首次发布

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

版权

简单范例

功能描述

读取一段pcl点云序列，并发布到名为“pt”的topic。然后通过rviz2观察点云

环境准备

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

pip3 install -r packages.txt

保存pip环境下所有库：

pip3 freeze > packages.txt

环境配置

source /opt/ros/eloquent/setup.bash # (如果同时配置了ros1，需要source选定ros2)
rosversion -d #(查看ros版本)
conda activate ros2_env #（需要提前创建pthon虚拟环境，需要安装anaconda3：创建虚拟环境<conda create --name ros2_env python=3.6>）
python --version #(查看python版本)

代码实现

代码结构

代码结构编译前

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

代码结构编译后

project_name
├── build（文件夹，未展开）
├── install（文件夹，未展开）
├── log（文件夹，未展开）
└── src
    └── pcl_reg
        ├── CMakeLists.txt
        ├── include（可选）
        │   └── pcl_reg
        ├── package.xml
        └── src
            ├── other.txt
            ├── pt_show.cpp（可选）
            └── reg_pcl.cpp

代码内容

mkdir project_name #创建工程文件夹
mkdir src & cd src #创建源码文件夹
使用ros2 pkg创建包的框架结构
ros2 pkg create --build-type ament_cmake <package_name>
ros2 pkg create --build-type ament_cmake --node-name <node_name> <package_name>

与上面结构对应

ros2 pkg create --build-type ament_cmake pcl_reg
ros2 pkg create --build-type=ament_cmake --node-name reg_pcl pcl_reg

#此处的<node_name> 在后面编译完成后调用的内容对应,也与CMakeLists.txt内容对应package_name对应project；node_name对应 executable；
#在pcl_reg文件夹下会自动生成pcl_reg.cpp。文件名可以更改，只要与CMakeLists.txt中add_executable里面源文件名对应上就行

#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 = "/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;
}

在pcl_reg.cpp填充内容

对CMakeLists.txt进行适当修改

#是因为ament_target_dependencies里面引入了非ament标准库内容
#ros外部库需要使用target_link_libraries做链接，例如PCL，OPENCV等

# 寻找依赖库（标准库）
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}
  /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}
)

install(TARGETS 
  reg_pcl 
  # pt_show
  EXPORT export_${PROJECT_NAME}
  DESTINATION lib/${PROJECT_NAME})

对package.xml进行适当修改

  <depend>rclcpp</depend>
  <depend>sensor_msgs</depend>
  <depend>EIGEN3</depend>
  <depend>Boost</depend>
  <depend>PCL</depend>

cd project_name(在工程目录下编译)
colcon build --packages-select pcl_reg#编译指定包（如果编译所有src 下面的包可以写为colcon build）
source install/setup.bash (注册当前工程环境)
ros2 run <package_name> <node_name>
ros2 run pcl_reg reg_pcl

使用rviz2订阅topic，查看点云

查看topic相关情况

ros2 topic list -t # 查看姓名和类型
ros2 topic bw /topic_name #带宽
ros2 topic hz /topic_name #频率
ros2 topic info /topic_name #订阅信息
ros2 topic echo /topic_name #显示topic内容

运行rviz2并进行topic相关配置

ros2 run rviz2 rviz2

左侧栏的Fixed Frame 填写坐标系名称（代码里的frame id）
之后点击左下角的Add，弹出create visualization对话框，添加需要现实的点云数据类型，一般是PointCloud2（与代码里msg类型对应）
点击ok后，左侧栏会出现topic，填写topic名称/topic_name
如果此时该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相关提示，请关闭终端重新打开