Luminarの点群データをROS2で可視化してみる

# モチベーション
Luminarの点群データを確認してみたかったので、ROS2にて可視化してみた

# Reference
TRIが公開しているデータセットがLuminar H2を4台搭載したものを公開しているのでそちらを使用する

[TRI-ML/DDAD](https://github.com/TRI-ML/DDAD)
[TRI-ML/dgp](https://github.com/TRI-ML/dgp)

# Code
議事録的として貼り付けておく(コードはぐちゃぐちゃ)

<details><summary>サンプルコード</summary><div>

```python
import os
import sys
import time
from tqdm import tqdm
import numpy as np
import cv2

sys.path.append('/path/dgp')
from dgp.datasets.synchronized_dataset import SynchronizedSceneDataset

import rclpy
from rclpy.time import Time
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, PointField, Image
from std_msgs.msg import Header

from cv_bridge import CvBridge

image_01 = None
depth_01 = None

class Publisher(Node):
    def __init__(self):
        super().__init__('ddad_publisher')
        self.lidar_publisher = self.create_publisher(PointCloud2, "luminar")
        self.image_publisher = self.create_publisher(Image, "image01")
        self.depth_publisher = self.create_publisher(Image, "depth01")
        self.image_bridge = CvBridge()
        self.depth_bridge = CvBridge()

    def array_to_pointcloud2(self, points, stamp=None, parent_frame="luminar"):
        ros_dtype = PointField.FLOAT32
        dtype = np.float32
        itemsize = np.dtype(dtype).itemsize
        data = points.astype(dtype).tobytes()

        fields = [PointField(
            name=n, offset=i*itemsize, datatype=ros_dtype, count=1)
            for i, n in enumerate('xyz')]

        header = Header(frame_id=parent_frame)

        return PointCloud2(
            header=header,
            height=1,
            width=points.shape[0],
            is_dense=False,
            is_bigendian=False,
            fields=fields,
            point_step=(itemsize * 3),
            row_step=(itemsize * 3 * points.shape[0]),
            data=data
        )

    def pil2cv(self, image):
        ''' PIL型 -> OpenCV型 '''
        new_image = np.array(image, dtype=np.uint8)
        if new_image.ndim == 2:  # モノクロ
            pass
        elif new_image.shape[2] == 3:  # カラー
            new_image = cv2.cvtColor(new_image, cv2.COLOR_RGB2BGR)
        elif new_image.shape[2] == 4:  # 透過
            new_image = cv2.cvtColor(new_image, cv2.COLOR_RGBA2BGRA)
        return new_image

    def dataloader(self):
        st = time.time()
        dataset = SynchronizedSceneDataset(
            'path/ddad.json',
            datum_names=('lidar', 'CAMERA_01', 'CAMERA_05'),
            generate_depth_from_datum='lidar',
            split='train'
            )
        print('Loading dataset took {:.2f} s'.format(time.time() - st))
        # Iterate through the dataset.
        for sample in tqdm(dataset, desc='Loading samples from the dataset'):
            lidar, camera_01, camera_05 =sample[0:3]
            point_cloud = lidar['point_cloud']
            image_01 = camera_01['rgb']
            depth_01 = camera_01['depth']

            print(depth_01.shape)

            pc2 = self.array_to_pointcloud2(point_cloud)
            self.lidar_publisher.publish(pc2)

            image_cv_01 = self.pil2cv(image_01)
            depth_cv_01 = self.pil2cv(depth_01)

            image_msg_01 = self.image_bridge.cv2_to_imgmsg(image_cv_01, encoding="bgr8")
            depth_msg_01 = self.depth_bridge.cv2_to_imgmsg(depth_cv_01, encoding="8UC1")

            self.image_publisher.publish(image_msg_01)
            self.depth_publisher.publish(depth_msg_01)

def main(args=None):
    rclpy.init(args=args)
    ddad_publisher = Publisher()
    ddad_publisher.dataloader()

if __name__ == '__main__':
    main()
```
</div></details>

# GIF
GIFにしてみましたが、見づらいですね

![ezgif.com-video-to-gif.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/239786/2ecfae8a-14ec-fc8c-e8f7-bc7ed9556d92.gif)

![ezgif.com-video-to-gif (2).gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/239786/568d48b0-d0ff-f19e-117b-4e1658347487.gif)


# IMAGE

各Grid Cellは5m刻み

## Scene1
![Screenshot from 2020-06-02 02-57-00.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/239786/1e2737f5-e238-c189-aee1-df332086f13e.png)

## Scene2
![Screenshot from 2020-06-02 02-58-45.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/239786/9f9ee200-2e85-f26e-b9be-dfc95f532d98.png)
![Screenshot from 2020-06-02 02-59-05.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/239786/bf74ac92-d815-7cb5-bd64-8276aad06645.png)
![Screenshot from 2020-06-02 03-00-20.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/239786/759ba3aa-ff15-2e90-a549-d8a5d2a564bc.png)

## Scene3
![Screenshot from 2020-06-02 03-01-58.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/239786/244d181e-dcc5-e22a-2da1-aef422d78aef.png)
![Screenshot from 2020-06-02 03-02-30.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/239786/07ce6bca-2621-9d07-2c89-b1d9f9228ac8.png)

## Scene4
![Screenshot from 2020-06-02 03-04-00.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/239786/ff7173c7-718f-f708-1514-901498b81d22.png)
![Screenshot from 2020-06-02 03-04-18.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/239786/dfb1df9a-d0be-73f8-2757-3018b9e1bf9c.png)

## Scene5
![Screenshot from 2020-06-02 03-07-58.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/239786/2a73aa5d-950f-8ce2-820e-cc765e14a219.png)
![Screenshot from 2020-06-02 03-08-30.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/239786/c38e508f-afbc-13b0-9f8b-631fc2635261.png)


# まとめ
LuminarがVelodyneのように一般ユーザにも使えるようになると
研究室やField Roboticsにおいてもありがたいですね