[ROS] Pythonで色付き点群をpublish

# 本文について
ROSでPointCloud2 msgを作るとき、色付けない場合
sensor_msgs.point_cloud2 中の関数で利用すれば簡単に
```python
import sensor_msgs.point_cloud2 as pcd2
msg = pcd2.create_cloud_xyz32(header, points)
```
でもPythonのsensor_msgs.point_cloud2中は直接で色付き点群、つまりRGB PointCloud Messageを作る関数がない。
本文では　PythonでこのRGB PointCloud Messageの作る方法を説明
本文のコード

https://github.com/geturin/rgbpcd

![rgbpcd.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1775186/d74d8f2c-e96c-3928-2d7b-3158d8c2220d.png)

# データを用意
この部分は本文で使った色付き点群データの用意方法について説明する
もう色付き点群データがある方ではパスしてもよい


本文で使ったデータは前の文章

https://qiita.com/KEROLL5/items/507bfef24c8af1e5f44b

で処理して取ったデータ


RGB画像:
![0000000150.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1775186/2ae90bb9-ccb1-822b-8e11-02620f9b8283.png)

デプス画像:
![depth.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1775186/103fe26d-a93b-d1d4-a35b-6ea2aea9ab21.png)

そしてデプス画像を前々回の文章

https://qiita.com/KEROLL5/items/28a1438a4425dd83277e

中のコードをもう一度点群に変換

```python
    import numpy as np
    import cv2

    #depth2pcd 
    depth2pcd =test_depth_to_pcd(1)

    #load image and depth image
    bgr = cv2.imread("./data/0000000150.png")
    depth = cv2.imread("./data/depth.png")

    depth = depth[:,:,0]
    rgb = cv2.cvtColor(bgr,cv2.COLOR_BGR2RGB)

    #get pointcloud　(N x 3 ) and rgb data (N x 3 )
    pointcloud = depth2pcd.get_pcd(depth)
    rgb_list = rgb.reshape(-1,3)
```


すると、色付き点群必要の　点群と色は手に入れる

まだ、このデプス画像は低密度なので、画像で一部分だけデプス値があるため、無効の点を削除
```python
    depth = depth.reshape(-1,1)
    filter = np.where(depth!=0)

    pointcloud = pointcloud[filter[0],:]
    rgb_list = rgb_list[filter[0],:]
```

# RGB PointCloud Message  
## 1:RGBF32
sensor_msgs.point_cloud2中のcreate_cloud_xyz32関数を参考しよう
```python
def create_cloud_xyz32(header, points):
    """
    Create a L{sensor_msgs.msg.PointCloud2} message with 3 float32 fields (x, y, z).

    @param header: The point cloud header.
    @type  header: L{std_msgs.msg.Header}
    @param points: The point cloud points.
    @type  points: iterable
    @return: The point cloud.
    @rtype:  L{sensor_msgs.msg.PointCloud2}
    """
    fields = [PointField('x', 0, PointField.FLOAT32, 1),
              PointField('y', 4, PointField.FLOAT32, 1),
              PointField('z', 8, PointField.FLOAT32, 1)]
    return create_cloud(header, fields, points)
```
とても短くな関数
要するには点群のfieldsを定義して、それで点群データ、headerと一緒にcreate_cloud関数に入れる
点群fieldsではPointFieldの組み合わせ
PointFieldでは

```python

PointField(name , offset, datatype, count)
"""
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
"""
```
nameとdatatypeがわかりやすい、countも1をすればよい
offsetではバイト位置

先のXYZ32関数では、float32を使い
float32は4バイト
だからバイト位置としては0 4 8

それをわかる以上、xyzrgbのfieldsを作る

```python
    from sensor_msgs.msg import PointCloud2,PointField
    ##RGBF32
    fields = [  PointField("x",0,PointField.FLOAT32,1),
                    PointField("y",4,PointField.FLOAT32,1),
                    PointField("z",8,PointField.FLOAT32,1),
                    PointField("r",12,PointField.FLOAT32,1),
                    PointField("g",16,PointField.FLOAT32,1),
                    PointField("b",20,PointField.FLOAT32,1)]
```

画像でrgb値を保存するdate typeでは uint8 [0~255]
でもここでr,g,bデータはfloat32定義するので、さき画像からのデータを float32 [0~1]に変換

```python
color = (rgb_list/255).astype(np.float32)
```
そして点群データと色データを組み合わせ　色付き点群を作る、一緒にcreate_cloud関数に入れすれば
RGB POINTCLOUD MESSAGEが手に入れる
```python
    from sensor_msgs.msg import PointCloud2,PointField
    from std_msgs.msg import Header
    import sensor_msgs.point_cloud2 as pcd2

    data = np.hstack((pointcloud,color)).astype(np.float32)
    header = Header()
    header.frame_id = "map"
    msg = pcd2.create_cloud(header=header,fields=fields,points=data )
```

ROSにpublish
```python
    import rospy
    
    rospy.init_node("rgbpcd")
    pcd_publish = rospy.Publisher("test_pcd",PointCloud2,queue_size=1)
    rate =rospy.Rate(20)


    while not rospy.is_shutdown():
        msg = pcd2.create_cloud(header=header,fields=fields,points=data )
        pcd_publish.publish(msg)
        rate.sleep()
```
最後はでrviz中で　点群の color transformerをRGBF32を選択すれば、点群は色付きます

![QQ图片20220902182056.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1775186/a39cbd3e-6ee6-83a2-aef5-168b8632a336.png)
![QQ图片20220902182302.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1775186/8603263c-33cb-ceef-decd-6a9819e362aa.png)

## 2.RGB8
先で色付き点群をpublishしましたが
まだ改良点がある

画像からのRGBデータでは元々 uint8 [0~255]　の形
publishするため　float32 [0~1]　に変換した

uint8データサイズは　1バイト　　8ビット
float32 は　4バイト　32ビット

一つ組のRGBデータをfloat32に変換すると　サイズは本来の　3バイトから12バイトに変化する
9バイトを増加
データのバイト量が増加したら、publishするとき使ったband widthも増加する

先RGBF32 , RATE = 20 Hz場合のbandwidthをチェックしよう
![QQ图片20220902185632.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1775186/953005a2-a34d-cd29-3bc4-c9b9e66da2bc.png)
bandwidthは8.5MB/s

これを改良するため、RGBデータを圧縮する
圧縮方法は以下の文章と動画を参考

https://sy-base.com/myrobotics/ros/ros_pointcloud_add_color/

https://www.bilibili.com/video/BV1ae4y1Z7HA?p=3&share_source=copy_web


簡単に言うと、元々のRGBデータ　3つの uint8  3*8=24ビット　変換じゃなく
そのまま一つfloat32の32ビットに入れる

流れは
uint8 r = R R R R R R R R
uint8 g = G G G G G G G G
uint8 b  = B B B B B B B B

uint32 rgb  = 0 0 0 0 0 0 0 0 R R R R R R R R G G G G G G G G B B B B B B B B
float32 rgb =0 0 0 0 0 0 0 0 R R R R R R R R G G G G G G G G B B B B B B B B 

```python
    r,g,b = rgb_list[:,0].astype(np.uint32), rgb_list[:,1].astype(np.uint32), rgb_list[:,2].astype(np.uint32)
    rgb = np.array((r << 16) | (g << 8 ) | (b << 0),dtype=np.uint32)
    color = rgb.reshape(-1,1)
    color.dtype = np.float32
```

そしてr,g,bデータを　一つのfloat32に圧縮した
点群の定義　fieldsを以下のように変更
```python
    fields = [  PointField("x",0,PointField.FLOAT32,1),
                PointField("y",4,PointField.FLOAT32,1),
                PointField("z",8,PointField.FLOAT32,1),
                PointField("rgb",12,PointField.FLOAT32,1)]
```
他の部分は先のRGBF32と同じでもよい、ROSにpublishする

すると、rvizで点群のcolortransformerはRGB8に変更した
![QQ图片20220902192756.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1775186/49acdd9f-7652-0a90-1a9e-e5ed3ec9957d.png)
![QQ图片20220902192904.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1775186/44bb7a6e-6f3a-e58c-897e-e6f4f1fc4058.png)

も一回点群のbandwidthをチェックしよう
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1775186/06ee9135-18e4-6580-4c5b-070ffb84556b.png)

5.6MB/sになった


# 参考資料
https://sy-base.com/myrobotics/ros/ros_pointcloud_add_color/

https://www.bilibili.com/video/BV1ae4y1Z7HA?p=3&share_source=copy_web