TOPIC 16｜ROSで活用する[2/2]｜点群データとOctomapの生成

【目次】

16.5　 点群データを生成してプレビューする

　16.5.1　 点群データを生成する

　16.5.2　 ROSの起動とRvizの実行

　16.5.3　 RvizでPointCloudをプレビューする

16.6　 点群からOctomapを生成する

　16.6.1　 点群からOctomapをパブリッシュする

　16.6.2　 RvizでOctomapをプレビューする

16.7　 ROSでPLATEAUを利用する場合の注意

16.8　 事例

16.5 _ 点群データを生成してプレビューする

OBJ形式ファイルができたら、これを点群に変換します。

16.5.1 _ 点群データを生成する

まずは、create_sampling_point_cloud.pyを使って、生成したOBJ形式ファイルから点群ファイルに変換します。

cd ~/CityGMLtoRobotMap
python　create_sampling_point_cloud.py -f　~/CG2RM/obj/53394525_bldg_6697_2_op.obj --density 1

-fオプションは変換元のOBJ形式ファイル、--densityの密度で、単位は「個/㎡」です。値が大きいほど、密度が高くなります。上記コマンドを実行すると、指定したOBJファイルが保持する形状の表面がサンプリングされ、点群（PointCloud）が生成されます。生成された点群データは、「~/CG2RM/pointcloud」ディレクトリに、PLYファイルとPCDファイルとして生成されます。ファイル名は、元のファイル名の後ろに「_sample」が付いたものとなります。例えば、53394525_bldg_6697_2_op_sample.pcdなどです。

16.5.2 _ ROSの起動とRvizの実行

ここまでの準備ができたら、ROSを起動します。ターミナル上で、次のようにroscoreコマンドを実行します。

roscore

roscoreは、パブリッシュやサブスクライブの仕組みを提供するROSの本体とも言える機能です。ROSに付随する機能を使用する際には、常にroscoreを実行しておく必要性があります。

続いて、ROSの標準ビューワーであるRvizを起動します。別ターミナルを開き、次のようにrvizコマンドを実行します。

rviz

実行すると、RvizのGUIが起動します。初期状態では点群などのデータは表示されません。あとで表示したい項目を追加します。その方法は、後述します。

16.5.3 _ RvizでPointCloudをプレビューする

さらに別ターミナルを開き、以下のコマンドを入力して、pcl_rosに含まれるpcd_to_pointcloudを動かします。

rosrun pcl_ros pcd_to_pointcloud  ~/CG2RM/pointcloud/53394525_bldg_6697_2_op_sample.pcd  _frame_id:=/map _latch:=true

このコマンドを実行すると、引数に指定した点群ファイルが読み込まれ、パブリッシュされます。デフォルトでは、/cloud_pcdというトピック名でPointCloud2メッセージをパブリッシュします。上記では、オプションとして、_frame_idと_latchを指定しています。_frame_idはフレームIDを指定するもので、後で、octomap_serverから接続するときに必要となる情報です。_latchはサブスクライバ（今回の例では、octomap_serverおよびRviz）がつながったとき、一度だけメッセージをパブリッシュするオプションです。

RvizのGUI左下にある［Add］ボタンをクリックすると、ROS上でパブリッシュされているトピック一覧が表示されます。［By topic］タブから［/cloud_pcd］の［PointCloud2］を選択し、［OK］をクリックすると、pcd_to_pointcloudからパブリッシュされている点群データをRviz上に追加できます。

追加したデータは、［Displays］パネルに表示されます。追加直後の状態では、すべての点が白く表示され、奥行きや高さなどの形状を視認しにくいので、［Displays］パネルの［PointCloud2］をクリックしてツリーを開き、［Style］を［Points］に、［Size］を［１］に、［Color Transformer］を［AxisColor］に変更します。

設定を変更すると、図16-10のように高さ方向に従って段階的に色が付与された状態の点群が表示されます。

このように現実世界を詳細に表現した点群地図は、LiDARを使った自己位置推定に利用できます。PCLライブラリなどで実装されているICP（Iterative Closest Point）やNDT（Normal Distribution Transform）などの位置合わせ（registration）アルゴリズムを使って、LiDARのスキャンと点群地図とを位置合わせすることで、ロボットが自己位置を推定できます。

点群地図とLiDARを使って自己位置推定を行う例として、A ROS-based NDT localizer with multi-sensor state estimation、amcl3d、hdl_localizationなどがあります。

16.6 _ 点群からOctomapを生成する

次に、生成した点群をボクセル化する方法を説明します。

16.6.1 _ 点群からOctomapをパブリッシュする

Octomapは、octomap_serverを使って変換します。octomap_serverはパブリッシュされた点群データからOctomapを作り出します。ここまでの流れで、すでに点群データをpcd_to_pointcloudがパブリッシュしているので、点群データは、ここから読み込みます。

まずは、下記リストの内容を「octomap.launch」というファイル名で保存します。

<launch>
    <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
        <param name="resolution" value="4" />
        <param name="max_depth" value="16" />
        
        <param name="frame_id" type="string" value="map" />
        
        <param name="occupancy_min_z" value="0.0" />
        <param name="occupancy_max_z" value="100.0" />
        <param name="pointcloud_min_z" value="0.0" />
        <param name="pointcloud_max_z" value="100.0" />

        <param name="sensor_model/max_range" value="100.0" />

        <remap from="cloud_in" to="/cloud_pcd" />
    
    </node>
</launch>

リスト16-1　octomap.launch

このファイルは、roslaunchコマンドを使って、ROSのパッケージを実行する際の起動ファイルです。node要素で、octomap_serverパッケージに含まれるoctomap_server_nodeという名前のノードを起動しています。

param要素は、このパッケージを起動するときに渡すパラメータです。ここでは、次のパラメータを設定しています。

・resolution：Octomapを生成する際に、空間を分割するボクセルの一辺の長さを決めるパラメータです。単位はメートルです

・max_depth：データ内部の木構造の深さをどこまでたどって表示するかを決めるパラメータです。0から16の値をとります

・ frame_id：データを受信するフレームIDを指定します。今回はpcd_to_pointcloudでパブリッシュする際、_frame_id:=/mapという引数を指定してフレームIDとしてmapを指定しているので、それと同じ値を指定します

・ pointcloud_[min|max]_z、occupancy_[min|max]_z：扱うデータの高さ範囲です。pointcloud_[min|max]_zは入力する点群の高さ範囲、occupancy_[min|max]_zは出力するoctomapの高さ範囲を、それぞれ制限します。単位はメートルです

・sensor_model/max_range：入力するデータの範囲を制限します。デフォルト値は-1（無制限）です。単位はメートルです

remapパラメータは設定値の置換をするものです。デフォルトでは、/cloud_inというトピックからデータを受信します。pcd_to_pointcloudでパブリッシュする際、/cloud_pcdというトピック名でパブリッシュしているので、それに合わせるため、このように置換指定します。

上記の内容を保存したら、ターミナルで次のコマンドを実行します。

cd "octomap.launchが保存されているディレクトリ"
roslaunch octomap.launch

コマンドを実行すると、octomap_serverが起動します。そして、pcd_to_pointcloudからパブリッシュされている点群データ（/cloud_pcdトピックのPointCloud2メッセージ）が読み込まれ、Octomapへと変換されます。変換されたデータは、デフォルトでは、/occupied_cells_vis_arrayというトピック名でパブリッシュされます。

なお、この変換は瞬時ではありません。元の点群領域が広域であればあるほど、Octomapの生成には時間を要します。octomap.launch内のsensor_model/max_range が、octomap生成領域の上限を決める値です（単位はメートル）。大きすぎると時間がかかるので、100程度の値から始めることを推奨します。

16.6.2 _ RvizでOctomapをプレビューする

点群を表示対象に加えた手順と同様にして、このようにパブリッシュされたOctomapもRvizに追加して表示できます。

表示内容に加えるには、［Add］ボタンをクリックして、［By topic］タブから［/occupied_cells_vis_array］トピックの［MarkerArray］を選択します。

追加すると、点群とOctomapが重なった状態で表示されます。

図16-13は、sensor_model/max_rangeパラメータを2000などの大きな値にした場合の表示例です。広範囲を対象にしているためOctomap生成完了までに非常に時間がかかります。

Octomapのように、空間が構造物によって占有されているか、または何もない空の空間なのかを表現した環境情報を経路計画アルゴリズムに与えると、障害物・構造物がある場所は避けて、何もない安全な空間のみを通過するルートが求められます。PLATEAUは広大な領域にわたって空間情報を持っているため、3D都市モデルをもとに安全な長距離移動ルートを事前に求めることが可能となると考えられます。

Octomapを使った経路計画機能の例として、OctomapPlannerなどがあります。

16.7 _ ROSでPLATEAUを利用する場合の注意

PLATEAUが持つ3D形状を自己位置推定用の環境地図に使用する場合、PLATEAUに収録されていない障害物（植生、構造物、車両）などが存在すると、センシングデータとの差異が生じて自己位置推定の際のノイズになり、正しく自己位置推定ができない可能性が考えられます。またPLATEAUには構造物、建築物が主に保存されているため、周囲に構造物がない環境などでは、環境地図になるべき3D形状が存在せず、形状のみをリファレンスとする自己位置推定手法のみでは正しく推定ができない可能性があります。

ロボットを自律移動させるためにSLAMなどで環境地図を作ることがよくありますが、現場での作業が必要で、破綻なく広範囲の環境地図データを作るには時間や専門的技術を要します。とはいえ、PLATEAUが持つ3D形状をそのまま環境地図として使えるような環境であれば、正確で広範囲な地図が使える恩恵は非常に大きく、費用や時間を削減できます。

ルートなどに利用する場合は、実際には構造物が存在するが3D都市モデルに構造物が含まれていないため、実際の構造物に近づきすぎたり衝突してしまったりするような経路計画をしてしまうおそれもあります。そのため、リアルタイムに検出したセンサーデータから、元データにはない障害物を避けるルートを再生成する機能を追加で導入したり、到着目標地点を安全な場所にずらしたりする機能が必要です。また自律移動ロボットなどの運用では、実際の環境との差分が経路計画にどのように影響するか注意を払う必要があります。

16.8 _ 事例

ROSを使った事例としては、以下のものがあります。

■ 3D都市モデルとBIMを活用したモビリティ自律運行システム