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

7234 ワード
Gazebo PointCloud ROS PCL PCL テキストリンク

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

モチベーション

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

準備

  • pcl_rosをインストールしておく。本記事ではkineticで確認した。
sudo apt-get install ros-kinetic-pcl-ros

PointCloud2 -> pcd

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

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のexample.launchを使っています。このサンプルを起動する場合は、別途インストールしてください。
  • roslaunch sample_to_pcd.launchで起動させると、次のようなGAZEBO画面が立ち上がり、点群がpcdファイルに保存されていきます。
  • 保存先はprefixで指定します。上記のサンプルだと.ros/velodyne_[数値].pcdとして保存されます。絶対パスを指定して，特定のフォルダに保存することもできます。数値には、PointCloud2のheader.stampが反映されているようです。
  • fixed_frameは、pcdファイルに保存するとき、ヘッダーのVIEWPOINTに、fixed_frameから点群データへの変換パラメタを挿入します。点群データ自体は座標変換せず、そのままの数値が保存されるようです。そのため、別の座標系で保存したいときは、別途座標変換ノードを挟む必要があります。
  • pcl_viewerで保存されていることを確認します。pcl_viewer (ファイルパス).pcdとすると、以下のように点群を可視化できます（ファイルパスは各自でご確認ください）。視点をグリグリできます。

pcd ｰ> PointCloud2

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

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>
  • 上記のサンプルでは、例のウサギを使用しています。配信したいファイルにパスを変更してください。
  • roslaunch sample_from_pcd.launchで起動させると、rvizが立ち上がるので、手動で点群トピックを追加すると、確かにウサギが確認できます。

実装の該当箇所

参考サイト