[ROS] sensor_msgs::PointCloud2をpcdファイルで入出力するときのメモ 

ROSで、sensor_msgs::PointCloud2型のtopicをpcdファイルに保存する。また、その逆を行うときのメモ。

## モチベーション
* sensor_msgs::PointCloud2 -> pcd
    * GAZEBOシミュレーションなどで得た点群データをROS以外のソフトウェア（Matlab）に食わせるときに使う。
* pcd -> sensor_msgs::PointCloud2
    * 静的な点群処理のサンプル・テスト・デバッグに使用する。
    * rosbagを使ってもいいが、.pcdのほうが点群データだということがわかりやすい。あと、pcdのほうが可視化が楽。

## 準備
* [pcl_ros](http://wiki.ros.org/pcl_ros)をインストールしておく。本記事ではkineticで確認した。

```bash
sudo apt-get install ros-kinetic-pcl-ros
```

## PointCloud2 -> pcd
ROSで、sensor_msgs/PointCloud2型のtopicをpcdファイルに保存する。

```xml:sample_to_pcd.launch
<launch>
    <!-- this simulation publish 2 pointCloud2; velodyne_points and velodyne_points2 -->
    <include file="$(find velodyne_description)/launch/example.launch"/>

    <node pkg="pcl_ros" type="pointcloud_to_pcd" name="pointcloud_to_pcd" output="screen">
        <remap from="input" to="velodyne_points"/>
        <!-- prefix : set output folder path. without this, save pcd files in.ros folder. -->
        <param name="prefix" value="velodyne_" />
        <param name="fixed_frame" value="base_footprint" />
        <param name="binary" value="false" />
        <param name="compressed" value="false" />
    </node>
</launch>

```

* 上記のサンプル用のlaunchファイルでは、点群生成シミュレータとして[velodyne_simulator](https://bitbucket.org/DataspeedInc/velodyne_simulator.git)のexample.launchを使っています。このサンプルを起動する場合は、別途インストールしてください。
* `roslaunch sample_to_pcd.launch`で起動させると、次のようなGAZEBO画面が立ち上がり、点群がpcdファイルに保存されていきます。  
![Gazebo_010.jpg](https://qiita-image-store.s3.amazonaws.com/0/260124/142d04db-d429-275e-dbc6-657d2aa8ae86.jpeg)
* 保存先は`prefix`で指定します。上記のサンプルだと`.ros/velodyne_[数値].pcd`として保存されます。絶対パスを指定して，特定のフォルダに保存することもできます。数値には、PointCloud2のheader.stampが反映されているようです。
* `fixed_frame`は、pcdファイルに保存するとき、ヘッダーの`VIEWPOINT`に、`fixed_frame`から点群データへの変換パラメタを挿入します。点群データ自体は座標変換せず、そのままの数値が保存されるようです。そのため、別の座標系で保存したいときは、別途座標変換ノードを挟む必要があります。
* pcl_viewerで保存されていることを確認します。`pcl_viewer (ファイルパス).pcd`とすると、以下のように点群を可視化できます（ファイルパスは各自でご確認ください）。視点をグリグリできます。  
![PCD viewer_011.jpg](https://qiita-image-store.s3.amazonaws.com/0/260124/4ef9fe22-162a-e29a-f46b-c0752b928dc8.jpeg)


## pcd ｰ> PointCloud2
今度は逆に、pcdから点群をロードし、PoincCloud2トピックに配信します。

```xml:sample_from_pcd.launch
<launch>
    <!-- change your file path -->
    <arg name="file_name" value="/home/ken/Documents/bunny.pcd"/>
    <!-- rate : if publish at 10 Hz, set 0.1 -->
    <arg name="rate" value="0.1"/>
    <node pkg="pcl_ros" type="pcd_to_pointcloud" name="pcd_to_pointcloud" args="$(arg file_name) $(arg rate)" output="screen">
        <remap from="cloud_pcd" to="bunny"/>
        <param name="frame_id" value="map" />
    </node>
    <node pkg="rviz" type="rviz" name="rviz"/>
    <!-- After opening rviz, please open PointCloud2 topic by yourself. -->
</launch>
```

* 上記のサンプルでは、[例のウサギ](https://github.com/PointCloudLibrary/pcl/blob/master/test/bunny.pcd)を使用しています。配信したいファイルにパスを変更してください。
* `roslaunch sample_from_pcd.launch`で起動させると、rvizが立ち上がるので、手動で点群トピックを追加すると、確かにウサギが確認できます。
![bunny_pcd.jpg](https://qiita-image-store.s3.amazonaws.com/0/260124/217d71ff-38fc-9923-77c9-4fd634b21713.jpeg)

## 実装の該当箇所
* [perception_pcl/pcl_ros/src/tools/](https://github.com/ros-perception/perception_pcl/tree/melodic-devel/pcl_ros/tools)
 * rosbagからpcdに変換するツールもあるようです。

## 参考サイト
* [PCL関係の素敵なツール！](http://kawauso-festival.blogspot.com/2018/03/pcl.html)