• ROS知识点——生成点云，发布、订阅ROS点云话题

    文章目录

    1 点云基本概念

    1.1 点云结构公共字段

    PCL包含一个重要的数据结构，被设计成一个模板类，把点的类型当做模板类的参数。

    header：pcl::PCLHeader 记录了点云的获取时间
    points：std::vector储存所有点的容器。vector定义中的PointT对应的类的模板参数，即点的类型
    width：指定点云组织成图像时的宽度
    height：指定点云组成图像时的高度
    is_dense: 指定点云中是否有无效值
    sensor_origin_:是Eigen::Vector4f类型，传感器相对于原点平移所得的位姿
    sensor_orientation_:是Eigen::Quaternionf类型，定义传感器旋转所得的位姿

    1.2 点云类型

    PointT是pcl::PointCloud类的模板参数，定义点云的类型

    pcl::PointXYZ 位置
    pcl::PointXYZI 位置+亮度
    pcl::PointXYZRGBA 位置+颜色+透明度
    pcl::PointXYZRGB 位置+颜色
    pcl::Normal 表示曲面上给定点处的法线以及测量的曲率
    pcl::PointNormal 曲率信息+位置

    在这里插入图片描述

    1.3 ROS的PCL接口

    定义不同的消息类型去处理点云的数据

    std_msgs::Header 不是真的消息类型，它包含发送的时间、序列号等
    sensor_msgs::PointCloud2 用来转换pcl::PointCloud类型
    pcl_msgs::PointIndices 储存点云的索引
    pcl_msgs::PolygonMesh 保存了描绘网格、定点和多边形
    pcl_msgs::Vertices 将一组定点的索引保存在数组中
    pcl_msgs::ModelCoefficients 储存一个模型的不同系数，如描述一个平面需要四个参数

    1.4 pcl-ros点云格式转换

    三种格式:

    sensor_msgs::PointCloud
sensor_msgs::PointCloud2
pcl::PointCloud

    • 1
    • 2
    • 3

    其中，PointCloud2和pcl::PointCloud可以相互转换，PointCloud和PointCloud2可以相互转换，PointCloud和pcl::PointCloud的转换需要使用PointCloud2中转。

    PointCloud⟺PointCloud2⟺pcl::PointCloud

    • 1

    上述变换的实现：
    1、PointCloud2to PointCloud

    #include "sensor_msgs/point_cloud_conversion.h"
static inline bool convertPointCloud2ToPointCloud (
		const sensor_msgs::PointCloud2 &input, sensor_msgs::PointCloud &output);

    • 1
    • 2
    • 3

    2、PointCloudto PointCloud2

    #include "sensor_msgs/point_cloud_conversion.h"
static inline bool convertPointCloudToPointCloud2 (
		const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)

    • 1
    • 2
    • 3

    3、pcl::PointCloudto PointCloud2

    #include "pcl_conversions/pcl_conversions.h"
template
void toROSMsg(const pcl::PointCloud &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
{
    pcl::PCLPointCloud2 pcl_pc2;
    pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);
    pcl_conversions::moveFromPCL(pcl_pc2, cloud);
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8

    4、PointCloud2to pcl::PointCloud

    #include "pcl_conversions/pcl_conversions.h"
template
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud &pcl_cloud)
{
    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(cloud, pcl_pc2);
    pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8

    2 创建点云并发布ROS点云话题

    2.1 创建功能包

    catkin_create_pkg point_cloud_pkg std_msgs rospy roscpp sensor_msgs pcl_ros pcl_conversions std_srvs message_generation

    • 1

    2.2 发布ROS点云话题

    create_point_cloud_pub.cpp

    #include
#include
#include
#include
#include

int main(int argc, char** argv)
{
    ros::init(argc, argv, "point_cloud_node");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("/point_cloud_publisher_topic", 1000);
    ros::Rate rate(10);
    unsigned int num_points = 10;

    //建立pcl点云
    pcl::PointCloud<pcl::PointXYZRGB> cloud;
    // 点云初始化
    cloud.points.resize(num_points);

    //建立ros点云
    sensor_msgs::PointCloud2 output_msg;
    
    while(ros::ok()){
        // 调用ros获取时间的接口，获取系统时间，作为时间戳stamp
        output_msg.header.stamp=ros::Time::now();
        // 创建num_points个绿色的点
        for(int i=0;i<num_points;i++)
        {
            cloud.points[i].x=i;
            cloud.points[i].y=i;
            cloud.points[i].z=i;
            cloud.points[i].r=0;
            cloud.points[i].g=255;
            cloud.points[i].b=0;
        }
        //将pcl点云转换到ros消息对象
        pcl::toROSMsg(cloud, output_msg);
        // 发布的点云坐标系
        output_msg.header.frame_id="point_cloud_frame_id";
        pub.publish(output_msg);
        rate.sleep();
    }
    ros::spin();

    return 0;
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46

    2.3 订阅ROS点云话题

    pub_sub_pcl_topic_pkg.cpp

    #include
#include
#include
#include
#include
#include
#include

class pcl_sub
{
private:
    ros::NodeHandle n;
    ros::Subscriber subCloud;
    ros::Publisher pubCloud;
    // 接收到的点云消息
    sensor_msgs::PointCloud2 msg;   
    // 等待发布的点云消息             
    sensor_msgs::PointCloud2 adjust_msg; 
    // 建立了一个pcl的点云，用于完成点云的中间处理过程         
    pcl::PointCloud<pcl::PointXYZRGB> adjust_pcl; 

public:
    // https://blog.csdn.net/u014587147/article/details/75647002 构造函数
    pcl_sub():n("~")
    {   
        // 接收点云数据，进入回调函数 getcloud() /point_cloud_topic 为订阅的点云话题名
        // 1 为待处理信息队列大小，一次只处理一个消息
        // &pcl_sub::getcloud 调用的函数指针，即回调函数。
        // this 回调函数所在的类
        subCloud = n.subscribe<sensor_msgs::PointCloud2>("/point_cloud_publisher_topic", 1, &pcl_sub::getcloud, this); 
        // 发布位姿变换后的点云/adjusted_cloud
        pubCloud = n.advertise<sensor_msgs::PointCloud2>("/adjustd_cloud", 1);                                     
    }

    // 回调函数
    void getcloud(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
    {   
        // 放在这里是因为，每次都需要重新初始化
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr adjust_pcl_ptr(new pcl::PointCloud<pcl::PointXYZRGB>); 
        // 把msg消息转化为点云
        pcl::fromROSMsg(*laserCloudMsg, *adjust_pcl_ptr);                                             

        // 定义一个旋转矩阵 虽然称为3d，实质上是4x4的矩阵(旋转R+平移t)
        Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
        Eigen::AngleAxisd rotationVector(M_PI/4,Eigen::Vector3d(0,0,1));
        Eigen::Matrix3d rotationMatrix=Eigen::Matrix3d::Identity();
        rotationMatrix=rotationVector.toRotationMatrix();
        //旋转部分赋值
        T.linear() = rotationMatrix;
        //平移部分赋值
        T.translation() = Eigen::Vector3d(0, 0, 0);

        // 执行变换，并将结果保存在新创建的 transformed_cloud 中
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
        pcl::transformPointCloud (*adjust_pcl_ptr, *transformed_cloud, T.matrix());

        adjust_pcl = *transformed_cloud;
        for (int i = 0; i < adjust_pcl.points.size(); i++)
        //把接收到的点云位置不变，颜色全部变为红色
        {
            adjust_pcl.points[i].r = 255;
            adjust_pcl.points[i].g = 0;
            adjust_pcl.points[i].b = 0;
        }

        // 将点云转化为消息才能发布
        pcl::toROSMsg(adjust_pcl, adjust_msg); 
        // adjust_msg.header.frame_id="point_cloud_frame_id";
        // 发布调整之后的点云数据 主题为/adjustd_cloud
        pubCloud.publish(adjust_msg);          
    }

    // 析构函数
    ~pcl_sub() {}
};


int main(int argc, char **argv)
{
    // 初始化了一个节点，名字为colored
    ros::init(argc, argv, "point_cloud_transform"); 
    // 创建一个对象，回调函数放在了构造函数中，所以创建对象的时候会顺序调用构造函数，回调函数，然后点云的接收、转换、发布就完成了。
    pcl_sub ps;
    ros::spin();
    return 0;
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86

    2.4 CMakeLists.txt

    cmake_minimum_required(VERSION 3.0.2)
project(point_cloud_pkg)

find_package(catkin REQUIRED COMPONENTS
  message_generation
  pcl_conversions
  pcl_ros
  roscpp
  rospy
  sensor_msgs
  std_msgs
  std_srvs
)

find_package(PCL REQUIRED COMPONENTS common io)
find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})

catkin_package( )

include_directories(
  ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}
)

link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(point_cloud_publisher src/create_point_cloud_pub.cpp)
target_link_libraries(point_cloud_publisher ${PCL_LIBRARIES}  ${catkin_LIBRARIES} )

add_executable(point_cloud_transform src/pub_sub_pcl_topic_pkg.cpp)
target_link_libraries(point_cloud_transform ${PCL_LIBRARIES}  ${catkin_LIBRARIES} )

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31

    2.5 point_cloud.launch

    <launch>
<node pkg="rviz" type="rviz" name="rviz" args="-d ${find point_cloud_pkg}/rviz/point_cloud.rviz">node>
<node pkg="point_cloud_pkg" type="point_cloud_publisher" name="point_cloud_node" output="screen">node>
<node pkg="point_cloud_pkg" type="point_cloud_transform" name="point_cloud_transform" output="screen">node>

launch>

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9

    2.6 运行launch文件

    实现点云的发布和订阅，并用rviz显示

    请添加图片描述点击左下方Add，选择PointCloud2，重复两次操作。分别将生成的PointCloud2模块对应的Topic改为，发布和订阅的topic。

    Fixed frame对应内容修改为代码中： output_msg.header.frame_id对应的值"point_cloud_frame_id"

    2.7 保存、加载rviz配置

    如2.6所示的效果图，不想每次都配置rviz内容，如FixedFrame名字，点add配置topic名字等等。

    保存：这时可以使用Ctrl+Shift+S将目前配置的配置保存下来，可以放到对应功能包下面的rviz文件夹下（若没有，则新建）。
    加载：在launch文件中，将rviz对应的node的args指定为该待需要加载的rviz即可。

    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find point_cloud_pkg)/rviz/point_cloud.rviz"/>

    • 1

    注意：$(find point_cloud_pkg)这里是小括号

    参考

    [1] https://blog.csdn.net/HUASHUDEYANJING/article/details/123367811
    [2] https://blog.csdn.net/qq_21950671/article/details/119819293
    [3] https://blog.csdn.net/Leslie___Cheung/article/details/112007715

  • 相关阅读:
    SpringBoot SpringBoot 运维实用篇 2 配置高级 2.1 临时属性
    家用汽车使用相关的英语
    GB/T 20909-2017 钢门窗检测
    多线程&并发篇---第十篇
    WebGL编程指南10-通过组合变化绘制旋转三角形
    【《C Primer Plus》读书笔记】第11章：字符串和字符串函数
    神经网络仿真软件是什么,神经网络仿真软件下载
    解决ffmpeg的播放摄像头的延时优化问题(项目案例使用有效)
    Nautilus Chain上线主网，为DeFi和流支付的未来构建基础
    Linux笔记系列
  • 原文地址：https://blog.csdn.net/qq_46515446/article/details/126240673