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

1 点云基本概念

1.1 点云结构公共字段

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

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

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

PointCloud⟺PointCloud2⟺pcl::PointCloud

ROS中sensor_msgs::PointCloud2类型消息解读，参考：https://blog.csdn.net/weixin_40826634/article/details/108767704
上述变换的实现：
1、PointCloud2to PointCloud

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

2、PointCloudto PointCloud2

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

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);
}

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);
}

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

2.1 创建功能包

catkin_create_pkg point_cloud_pkg std_msgs rospy roscpp sensor_msgs pcl_ros pcl_conversions std_srvs message_generation

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;
}

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;
}

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} )

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>

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"/>

注意：$(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