ROS2 lidar data轉成point cloud data
===
[TOC]

###### tags: `ROS 2`

* lidar data:
* type: sensor_msgs/msg/laser_scan
* topic: /scan
* point cloud:
* type: sensor_msgs/msg/point_cloud2
* topic:

## Step 1: 訂閱lidar data

function<void(const sensor_msgs::msg::LaserScan::SharedPtr)> scan_callback = [this](const sensor_msgs::msg::LaserScan::SharedPtr _scan) -> void {

    scan_data_.resize(_scan->ranges.size());
    for(long unsigned int i= 0; i< _scan->ranges.size(); ++i)
        scan_data_[i] = _scan->ranges[i];
    r_max= _scan->range_max;
    cur_header= _scan->header;


};             
            
scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>("scan", 10, scan_callback);
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;

## Step 2: 轉換成point cloud
### Step 2.1: 轉換data
laser data放在topic type的ranges中，ranges是大小為360的陣列，ex: ranges[0]= 1.5543，表示以sensor為中心，0度的位置距離1.5543有掃到東西，ranges[1]= inf，1度的位置沒掃到東西...以此類推
依下面公式做轉換，r = ranges[i],i是degree，theta要做轉換，$theta= degree*\frac{pi}{180}$

vector<vector<float>> transformLaserScanToPointCloud()
{
    vector<vector<float>> res;
    for(long unsigned int i= 0; i< scan_data_.size(); ++i)
    {
        if(scan_data_[i] >= r_max)
            continue;
        auto cur_r = scan_data_[i];
        auto cur_d = i* M_PI/ 180.0;
        //x, y, z
        vector<float> pc_data(3, 0);
        pc_data[0] = cur_r* cos(cur_d);
        pc_data[1] = cur_r* sin(cur_d);    
          

        res.push_back(pc_data);
    }
    return res;
}

### Step 2.2 將資料放進PointCloud2
首先要先搞懂點雲，他存放的座標軸x, y, z表示這個點為1(有東西)，在來看他的topic type

# This message holds a collection of N-dimensional points, which may
# contain additional information such as normals, intensity, etc. The
# point data is stored as a binary blob, its layout described by the
# contents of the "fields" array.

# The point cloud data may be organized 2d (image-like) or 1d
# (unordered). Point clouds organized as 2d images may be produced by
# camera depth sensors such as stereo or time-of-flight.

# Time of sensor data acquisition, and the coordinate frame ID (for 3d
# points).
Header header

# 2D structure of the point cloud. If the cloud is unordered, height is
# 1 and width is the length of the point cloud.
uint32 height
uint32 width

# Describes the channels and their layout in the binary data blob.
PointField[] fields

bool    is_bigendian # Is this data bigendian?
uint32  point_step   # Length of a point in bytes
uint32  row_step     # Length of a row in bytes
uint8[] data         # Actual point data, size is (row_step*height)

bool is_dense        # True if there are no invalid points

fileds用來表示每一軸的資料格式，先看一下PointField的topic type

# This message holds the description of one point entry in the
# PointCloud2 message format.
uint8 INT8    = 1
uint8 UINT8   = 2
uint8 INT16   = 3
uint8 UINT16  = 4
uint8 INT32   = 5
uint8 UINT32  = 6
uint8 FLOAT32 = 7
uint8 FLOAT64 = 8

string name      # Name of field
uint32 offset    # Offset from start of point struct
uint8  datatype  # Datatype enumeration, see above
uint32 count     # How many elements in the field

如果三通道(x, y, z)，座標是float表示，則

fields[0].name = "x";
fields[0].offset = 0;
fields[0].datatype= 7;
fields[0].count = 1;

fields[0].name = "y";
fields[0].offset = 4;
fields[0].datatype= 7;
fields[0].count = 1;

fields[0].name = "z";
fields[0].offset = 8;
fields[0].datatype= 7;
fields[0].count = 1;

再回到點雲，point_step表示一個點雲的資料大小，這邊填16，雖然3個座標每個4byte為`3*4= 12byte`，但須要多留4byte給rviz，所以是16，row_step表示總共有多少顆點雲，data就是放每一顆點雲的座標，表示方式是這樣(如果是3通道)，需要把座標軸(float)放到4個uint_8中，所以需要做轉換，
data[0]到data[3]表示pc~0~^x^(pointcloud 0 的x軸)，
data[4]到data[7]表示pc~0~^y^(pointcloud 0 的y軸)，
data[8]到data[11]表示pc~0~^z^(pointcloud 0 的z軸)，
data[12]到data[15]保留，
data[16]到data[19]表示pc~1~^x^(pointcloud 1 的x軸)，
data[20]到data[23]表示pc~1~^y^(pointcloud 1 的y軸)，
data[24]到data[27]表示pc~1~^z^(pointcloud 1 的z軸)，
data[28]到data[31]保留，以此類推

sensor_msgs::msg::PointCloud2 PreparePointCloud2Message(const vector<vector<float>>& data_)
{
    sensor_msgs::msg::PointCloud2 msg;
    msg.header = cur_header;
    msg.height = 1;
    msg.width = data_.size();
    msg.fields.resize(3);
    msg.fields[0].name = "x";
    msg.fields[0].offset = 0;
    msg.fields[0].datatype = 7; 
    msg.fields[0].count = 1;
    msg.fields[1].name = "y";
    msg.fields[1].offset = 4;
    msg.fields[1].datatype = 7;
    msg.fields[1].count = 1;
    msg.fields[2].name = "z";
    msg.fields[2].offset = 8;
    msg.fields[2].datatype = 7;
    msg.fields[2].count = 1;
    msg.is_bigendian = false;
    msg.point_step = 16;
    msg.row_step = 16 * msg.width;
    msg.is_dense = true;
    msg.data.resize(16 * data_.size());

    long unsigned int msg_data_idx= 0, laser_data_idx= 0, laser_axis= 0;
    //0 for x, 1 for y, 2 for z, 3 for reserve
    while(msg_data_idx < msg.data.size())
    {
        if(laser_axis != 3)
        {
            uint8_t* temp;
            float cur_data= data_[laser_data_idx][laser_axis];
            temp= reinterpret_cast<uint8_t*>(&cur_data);
            for(int i= 0; i< 4; ++i)
                msg.data[msg_data_idx + i]= temp[i];                    
        }

        ++laser_axis;
        laser_axis %= 4;
        if(laser_axis == 0) ++laser_data_idx;
        msg_data_idx+= 4;
    }           

    return msg;
}

## Step 3: 發布point cloud data

用計時器去publish

auto timer_callback = [this]() -> void {

    auto laser2pc = transformLaserScanToPointCloud();                
    auto pc_msg = PreparePointCloud2Message(laser2pc);                
    pc_pub_->publish(pc_msg);               

};  

pc_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("myPC", 10);


timer_ = this->create_wall_timer(1ms, timer_callback);

## 所有的code

#include <memory>

// #include "opencv2/opencv.hpp"
// #include <opencv2/imgproc/types_c.h>
// #include <ament_index_cpp/get_package_share_directory.hpp>
#include "rclcpp/rclcpp.hpp"
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>

#define _USE_MATH_DEFINES 
#include <cmath>
//M_PI for pi
using namespace std;
class PoseSubscriber : public rclcpp::Node
{
    public:
        PoseSubscriber(): rclcpp::Node("pose_sub")
        {
            robot_pose_.resize(3);
            
            //subscription data at odom and laser
            function<void(const nav_msgs::msg::Odometry::SharedPtr)> odom_callback = [this](const nav_msgs::msg::Odometry::SharedPtr _odom) -> void {
                
                robot_pose_[0] = _odom->pose.pose.position.x;
                robot_pose_[1] = _odom->pose.pose.position.y;
                robot_pose_[2] = _odom->pose.pose.position.z;
                
            };

            function<void(const sensor_msgs::msg::LaserScan::SharedPtr)> scan_callback = [this](const sensor_msgs::msg::LaserScan::SharedPtr _scan) -> void {
                // RCLCPP_INFO(this->get_logger(), "scan sub start");
                scan_data_.resize(_scan->ranges.size());
                for(long unsigned int i= 0; i< _scan->ranges.size(); ++i)
                    scan_data_[i] = _scan->ranges[i];
                r_max= _scan->range_max;
                cur_header= _scan->header;


            };             
            odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>("odom", 10, odom_callback);
            scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>("scan", 10, scan_callback);

            //publish data to pointcloud2
            auto timer_callback = [this]() -> void {
                
                auto laser2pc = transformLaserScanToPointCloud();                
                auto pc_msg = PreparePointCloud2Message(laser2pc);                
                pc_pub_->publish(pc_msg);               
                
            };  

            pc_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("myPC", 10);
            

            timer_ = this->create_wall_timer(1ms, timer_callback);



        } 
    private:
        sensor_msgs::msg::PointCloud2 PreparePointCloud2Message(const vector<vector<float>>& data_)
        {
            sensor_msgs::msg::PointCloud2 msg;
            msg.header = cur_header;
            msg.height = 1;
            msg.width = data_.size();
            msg.fields.resize(3);
            msg.fields[0].name = "x";
            msg.fields[0].offset = 0;
            msg.fields[0].datatype = 7; 
            msg.fields[0].count = 1;
            msg.fields[1].name = "y";
            msg.fields[1].offset = 4;
            msg.fields[1].datatype = 7;
            msg.fields[1].count = 1;
            msg.fields[2].name = "z";
            msg.fields[2].offset = 8;
            msg.fields[2].datatype = 7;
            msg.fields[2].count = 1;
            msg.is_bigendian = false;
            msg.point_step = 16;
            msg.row_step = 16 * msg.width;
            msg.is_dense = true;
            msg.data.resize(16 * data_.size());
            // RCLCPP_INFO(this->get_logger(), "a");
            long unsigned int msg_data_idx= 0, laser_data_idx= 0, laser_axis= 0;
            //0 for x, 1 for y, 2 for z, 3 for reserve
            while(msg_data_idx < msg.data.size())
            {
                if(laser_axis != 3)
                {
                    uint8_t* temp;
                    float cur_data= data_[laser_data_idx][laser_axis];
                    temp= reinterpret_cast<uint8_t*>(&cur_data);
                    for(int i= 0; i< 4; ++i)
                        msg.data[msg_data_idx + i]= temp[i];                    
                }
                
                ++laser_axis;
                laser_axis %= 4;
                if(laser_axis == 0) ++laser_data_idx;
                msg_data_idx+= 4;
            }           
            // RCLCPP_INFO(this->get_logger(), "b");
            return msg;
        }        
        //output positions at x, y ,z
        vector<vector<float>> transformLaserScanToPointCloud()
        {
            vector<vector<float>> res;
            for(long unsigned int i= 0; i< scan_data_.size(); ++i)
            {
                if(scan_data_[i] >= r_max)
                    continue;
                auto cur_r = scan_data_[i];
                auto cur_d = i* M_PI/ 180.0;
                //x, y, z
                vector<float> pc_data(3, 0);
                pc_data[0] = cur_r* cos(cur_d);
                pc_data[1] = cur_r* sin(cur_d);    
                // pc_data[2] = 0;    
                            
                res.push_back(pc_data);
            }
            return res;
        }

        vector<float> robot_pose_;
        vector<float> scan_data_;
        std_msgs::msg::Header cur_header;
        float r_max;

        rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
        rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pc_pub_;
        rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<PoseSubscriber>());
    rclcpp::shutdown();
    return 0;
}

## 參考資料

pointcloud data type:
* https://www.cswamp.com/post/110
* https://zhuanlan.zhihu.com/p/407352357
* https://www.cnblogs.com/sdu20112013/p/11543510.html

轉換公式:
http://library.isr.ist.utl.pt/docs/roswiki/laser_geometry.html