はじめに

ROS（Robot Operating System）のプログラミングは、とても簡単です。その一番大きな理由は、ROSはOSと銘打っているけど実はただの通信ライブラリで、規模が小さいので習得が容易なこと。次に挙げられるのは、ツールやパッケージが揃っているので、我々がプログラミングしなければならない部分が小さいこと。あと、ROSの主たる開発言語のC++が順調にバージョン・アップして、とても使いやすくなったこと。

公平を期すために、ROSプログラミングの問題点についても挙げてみましょう。ドキュメントに載っているサンプルがC++の古い規格（C++03）で書かれているので、プログラミングが面倒くさそうに見えてしまうこと。最新であるC++14を使って簡潔に簡単にプログラミングする場合向けのサンプルは、見つからないんですよね。他は、文書、特に日本語の文書が少ないこと1。ただでさえ少ない文書が要素技術単位で細切れになっていて、開発の全体像を掴みづらいとも感じます。

簡単に使えてとても便利なROSなのに、もしこの程度の問題で嫌われてしまったらもったいない。だから、C++14を使用したサンプルを作成してきました。そして本稿で、ROSプログラミングについて、実際の開発に則した形で解説します2。

ロボットを動かしてみる

まずは肩慣らし。プログラムでロボットを動かしてみましょう。

TurtleBot

本稿は、ロボットとしてTurtleBotを使用します。TurtleBotはROSアプリケーションの開発プラットフォームで、iRobot社のCreateやYujin Robot社のKobuki、Microsoft社のKinect、Asux社のXtion PRO LIVEといった市販のハードウェアで構成される、オープン・ソース・ハードウェアのロボットです。

なぜ、TurtleBot？

本稿がTurtleBotを使用する理由は、安くて簡単で情報が豊富なためです。

安い
20万円程度で購入できます。もしiRobot社のルンバの中古品とXbox360から取り外したKinect、ベニヤ板があるなら、公開された設計書に従って自作することも可能です。
簡単
TurtleBotに対応した多数のROSパッケージがあるので、開発範囲を小さくできます。たとえば、ROS WikiのTurtleBotのページの記述通りにコマンド入力すれば、プログラミングせずとも地図作成や自律移動ができます。
情報が豊富
多数の開発者が、安くて簡単なTurtleBotでROSアプリケーションを開発しています。だから、情報も豊富です。たとえばLearn Turtlebot and ROSでは、セットアップからスマートフォンで呼び出し可能なTurtleBotによるコーヒー配膳システムの完成まで、丁寧に解説してくれています。トラブルが発生しても、インターネットを検索すれば解決方法が見つかりますしね。

セットアップ

TurtleBot Installationに従い、TurtleBotに必要なソフトウェアをセットアップしてください。

実機がない場合は、シミュレーターで

TurtleBotを用意できなくても、ご安心ください。ROSに対応した、Gazeboというシミュレーターがあります。Gazebo Bringup Guideに従って、TurtleBotのシミュレーターを起動してみてください。内蔵された物理エンジンが、いい感じに物理演算してくれます。

Gazeboを使用する際は、一点だけ、気をつけてください。Gezeboは初回起動時にオブジェクトのデータをダウンロードするため、画面が表示されるまでかなり時間がかかります。気長に待っていただければ以下の画面が表示され、TurtleBotの実機がなくてもROSを試せるようになります。

Gazebo

ROSの仕組み

プログラミングに入る前に、簡単にROSについて説明させてください。ROSはただの通信ライブラリなのですけれど、どんな通信をしてどのようにロボットを動かすのか、そもそもどうして通信ライブラリがロボットのソフトウェア開発に必要になるのかが分からないと、モヤモヤしますもんね。

ロボットのソフトウェア開発と通信

少し遠回りになりますけど、電子工作相当の低水準の話から。ロボットはセンサーやモーターで構成されているわけですけど、これらはアナログな電気で制御されます。センサーはその測定結果を電圧として渡してきますから、専用の回路でデジタルに変換しなければなりません。DCモーター（普通のモーター）の力の制御も電圧で、でも、電子回路で電圧そのものを制御するのは難しい。だから、PWM（Pulse Width Modulation）制御と呼ばれるスイッチを高速にオン/オフする方式で、DCモーターの力を調節しなければなりません。

で、これがやたらと面倒くさい。だって、高速でスイッチをオン/オフするには、CPUのタイマー割り込みを使った面倒なコードが必要なのですから。なので、例えば電子工作で人気のArduinoでは、analogWrite()というPWM制御用のライブラリを用意し、タイマー割り込みを隠蔽しています。サーボ・モーター（ロボット・アームの関節等で使われる、角度を指定できるモーター）のようにスイッチをオン/オフする周波数が定められている場合向けに、Arduinoは一つのタイマーで複数のサーボ・モーターを制御するServoクラスも提供します。

でも、ライブラリが揃っても、まだ他に問題があるんです。ロボット・プログラムの動作は「センサーの値を読む→読み取った値に合わせてモーター等を制御する」の繰り返しで、それをコードで表現すると以下になります。

Rate loop_rate(30)  // 30Hz（秒に30回）でループできるように、ループ・レートを設定。
while (1) {  // 無限ループ。
  auto accelerator_opening = analogRead(ACCELERATOR_PIN);  // アクセル開度のセンサーから情報を取得。
  analogWrite(MOTOR_PIN, accelerator_opening);  // モーターの速度を設定。
  
  loop_rate.sleep();  // ループ・レートに合わせてスリープする。
}

上のコードは、一定の周期でループが回るようになっています。たとえば秒30コマ撮るビデオ・カメラのデータを使うプログラムなら秒30回処理が動けば十分なので、ループを30Hzに制限します。でも、この周期って、デバイスによって違うんですよ。たとえば、細かい制御が可能なブラシレス・モーター3制御の最小タイミングはモーターが少し回転するたびで、だから、はるかに大きな周波数を設定しなければなりません。でも、センサーやモーターが増えると複数の周波数に対応しなければならなくなって、これを一つのループで表現するとコードが複雑になるのでやりたくない。PWM制御ライブラリのようなタイマー割り込みでプログラミングする方式も、作成コストが高いから避けたい。マルチ・スレッドで複数のループを作る方式は、制御が不正確になってしまうのでダメ。

だから一般に、この複数の周波数問題は、CPUを分けることで解決します。CPUと言ってもPCに入ってるIntel Core i7とかの高級品ではなくて、16MHz動作の8bitでメモリ2KB（単位に注意してください）とかの、マイクロ・コントローラーと呼ばれる安価なやつです。これを、センサーやモーター毎につけて、それぞれのデバイスに適した周波数でループを回して制御すればよい。ロボットを分散型で制御するわけですね。

でも、センサーとマイクロ・コントローラー、モーターとマイクロ・コントローラーのような小さな単位を組み合わせて大きなロボットを作るのは、組み合わせが多すぎるので大変。だから、センサーやモーターのマイクロ・コントローラーを、デジタルなので管理が容易なシリアル通信でつなげて、移動ユニットのような形でコンポーネント化します。このコンポーネントの組み合わなら、ロボットの作成は楽になりそう……なのですけれど、残念ですけどまだ課題があります。

というのも、シリアル通信ではビット列を送受信しますが、そのビット列にどのようなデータを使うのかは利用者の自由なんです。だから、A社の移動ユニットとB社の移動ユニットではシリアル通信する内容が異なっていて、コンポーネントを組み合わせたり交換したりはできません。あと、人工知能が行き先を示すようなハードウェア無関係のソフトウェア・コンポーネントでも、問題が発生します。自由度が高いソフトウェア・コンポーネント同士を自由度が低いシリアルで通信させるのは無意味ですもんね。通信は、ネットワークを使って実現すべきでしょう。

まぁ、ネットワーク通信でも、HTTPでどんな内容のホームページでも送信できるように、自由なデータを送れちゃうんですけどね。でも、多数のソフトウェア階層で実現されていますから、送信内容の規格化は容易です。各種業界団体が専用のXML規格を定めてHTMLで送受信しているように、たとえば移動を表現するデータ構造（X軸とY軸とZ軸の移動量と、X軸とY軸とZ軸の回転量）を定めてあげれば、A社の移動ユニットとB社の移動ユニットが交換可能になります。

でも、先ほど例に出てきたHTTPそのものだとセッション単位で1対1の接続なので、センサーから取得した同じデータを複数回送信することになって効率が悪そう。あと、ブラウザからリクエストが来たらデータを送る方式は、センサーというデータを流し続ける仕組みと合致しない。メッセージがXMLだと、プログラムを書くのが大変そうで嫌だし。構造体のような、もっとプログラミング言語寄りの楽なやり方が欲しい。うん、やっぱり、既成品じゃあダメですね。

というわけで、ロボット専用の通信ライブラリが必要という結論になりました。ROSは、このロボット専用の通信ライブラリなのです。

Publish/Subscribeの、ロボットに適した通信を実現する

さて、そのロボット専門の通信ライブラリであるROSですが、実はその内部では先ほど文句をつけたHTTPも使用しています。でも、外から見えるROSの動作はHTTPとは大きく異なっていて、さすがはロボット専用な感じ。HTTPはブラウザからGETやPOSTというリクエストを受け取ったらサーバーがコンテンツを送る方式ですけれど、ROSでのサーバーはメッセージ流しっぱ、クライアントが必要に応じて読み取るという方式です。いわゆるPublish/Subscribe。センサーに適した、同じデータが複数回流れない効率のよい動作です。

で、このメッセージを流したり受け取ったりするプログラムの単位をROSではノードと呼び、通常はプロセスとして実現します。あと、種々雑多なメッセージが流れると取捨選択が必要になってしまいますから、同じメッセージをトピックとしてまとめます。このノードやトピックは、管理しやすいように階層で管理します。Microsoft社のKinectを管理する/kinectというノードが、取得した3D点群のメッセージを/3dsensor/depth/pointsというトピックに流す感じ。で、3D点群から障害物を検知してハンドル操作する/steeringノードや、3D点群を後で分析できるようにディスクに保存する/pointcloud_to_pcdノードが、先ほどの/3dsensor/depth/pointsというトピックからメッセージを読み込んでそれぞれの処理を実施します。

この、メッセージをトピックに送ることをpublish、トピックからメッセージを受け取ることをsubscribeと呼びます。出版と購読ですね。先ほどは書きそびれましたけど、3D点群から障害物を検知する/steeringノードは、多分、右へ行けとか左に行けというメッセージをpublishします。で、このメッセージを/move_unitノードがsubscribeする。さらに、/steeringノードは/move_unitノードがpublishする機体の情報をsubscribeして、移動が可能かどうかの判断に使っているでしょう。ROSのノードは対等で、HTTPのようなサーバーとかクライアントという区分けは存在しないんです。

ノードのグラフ

サービスで、1対1の同期通信にも対応する

Publish/Subscribeでのメッセージの流れはパブリッシャーからサブスクライバーへの1方向のみで、非同期です。センサーが情報を垂れ流すロボット制御におけるほとんどの処理はPublish/Subscribeが適しているのですけれど、もしメッセージの内容が変わらないならpublishし続けるのは資源の無駄ですし、メッセージを受け取ったかどうかを確認したい場合は非同期だと困ります。

たとえば、ロボットの名前を渡したいとします。名前はいつまでも変わらないですから、名前を何度もpublishするのは無駄です。「あなたの名前は？」「ロボット三等兵です」のようなやりとりの方が、「私の名前はロボット三等兵です」という同じ内容を繰り返し叫び続けるより適切でしょう。

カメラの動作モード設定のような場合も、Publish/Subscribeは使えません。動作モードを切り替えるメッセージをpublishしたけど、/cameraノードはsubscribeしそこねた（RDBMSでトランザクションのようなプログラムを書いている人には信じられないかもしれませんけど、ロボットでは十分にありえる話）。だから、/cameraノードは指定した動作モードとは異なるモードの画像をpublishする。でも、動作モード切り替えを指示したノードは、それを知るすべがない。だから、画像の解析に失敗しちゃう。

というわけで、ROSは、Publish/Subscribe方式に加えてサービスと呼ぶ1対1の同期通信も提供します。残念なことにROSの用語はテキトーで、Publish/Subscribeの場合はトピックとメッセージという用語で、サービスの場合はサービスとサービスという混乱する用語になっています（正しくは、Publish/Subscribeの場合はtopicとmsgで、サービスの場合はserviceとsvcなんですけれど）。だから、わけのわからない説明で申し訳ありませんけど、ノードは、サービスからサービス・リクエストを受け取ってサービス・レスポンスを返すことで1対1の同期通信をするという説明になります。

メッセージやサービスを、簡単に定義/使用可能にする

メッセージは*.msg、サービスは*.svcというファイルで定義します。文法は、struct宣言みたいな感じで簡単です。そして、*.msgや*.svcはビルド時にC++の構造体のコードに変換されます。普通のコードでメッセージやサービスを作れますから、専用のAPIを使わなければならないXML作成より簡単です。トピックにpublishされたメッセージを可視化するツールもあって、XMLと同様に人間が目で内容を確認することもできます。

ここまでをまとましょう。ROSプログラムは、ノードと呼ばれるプログラムの集合になります。ノードは、トピックにメッセージをpublishしたりトピックからメッセージをsubscribeしたりして、非同期通信で処理を進めます。あまり使いませんけど、Publish/Subscribeでは対応できない時のためにサービスという1対1の同期通信もあります。メッセージやサービスは、struct宣言みたいな感じで定義して、我々が書くC++のコードからはstructに見えるようになります。

ほら、「ロボットのソフトウェア開発と通信」で挙げたHTTPとXMLでロボットする場合の問題が、すべて解決されたでしょ？

マルチスレッドとスレッド間通信で、高パフォーマンスも実現する

でも、機能面で優れていても、遅すぎるのでは実用に耐えません。ROSではノードからノードへメッセージがネットワークで送られていくわけで、しかもそのメッセージには画像とか3D点群とかの大きなデータが含まれるみたい。だとしたら、パフォーマンス面に不安があるかも……。

ご安心ください。ROSはパフォーマンスも考慮しています。ROSは、複数のノードをマルチ・スレッドで動作させて、ネットワークではなくメモリでスレッド間通信するNodeletという仕組みも提供しているんです。しかも、コードを修正しなくても、通信相手のノードが別のプロセスや別のコンピューターにあるなら自動でネットワーク通信に切り替えてくれます。これなら、パフォーマンスも安心ですな。

あと、マルチ・スレッドにまつわる面倒事はNodeletが解消してくれますので、マルチ・スレッドであってもプログラミングは難しくありません。本稿では、この速くて簡単なNodeletを使用してプログラミングしていきます。

ついでに、ディストリビューションとバージョンの話を

ROSとROSに対応したパッケージ群を指して、ROSディストリビューションと呼びます。UbuntuやDebianのような、Linuxのディストリビューションと同じ感じ。ディストリビューションの目的は、組み合わせられるパッケージのセットを用意することです。

ROSディストリビューションは、毎年新バージョンがリリースされます。本稿を執筆している2016年3月の最新ディストリビューションの名前は「ROS Jade Turtle」で、2016年5月に出る予定の次のディストリビューションは「ROS Kinetic Kame」です。JからKへと、名前の最初のアルファベットがABC順で進んでいきます。なお、ROSディストリビューションのバージョンは、「ROS Jade」や「ROS Kinetic」のように省略して書くことが多いようです。あと、偶数年にリリースされるROSディストリビューションはLTS（Long Term Support）版で、サポート期間が5年間になります（奇数年リリースのサポート期間は2年間）。2014年リリースの「ROS Indigo Igloo」と2016年リリース予定の「ROS Kinetic Kame」が、LTSになります。

本稿では、ROS Indigoを使用します。最新のROSディストリビューションへの対応が追いついていないパッケージが多いことと、やはりサポートが長いLTSを使いたいですためです。なお、ROS Wikiの日本語のページでは、ROS Indigo Iglooのさらに前のROS Hydro Medusa向けの記事が多く存在します。ROS Wikiの通りにやってもうまく行かないという場合は、その文書がどのROSディストリビューションを対象にしているのかを確認してみてください。

roslaunch

さて、いよいよプログラミング……の前に、既存のROSプログラムを動かしてみましょう。プログラムは完成したけど動かし方が分からないなんて、笑い話にもならないですもんね。ROSのノードは、直接起動、rosrunで起動、roslaunchで起動の3つの方法で起動できます。本稿のおすすめはroslaunchなのですけれど、その理由を理解していただくためにも、順を追って説明していきます。

直接起動する

通常のROSのノードは、実行可能なプログラムです。だから、直接起動できます。ターミナルを3つ開いて、それぞれで以下のコマンドを実行してください。

$ roscore
$ /opt/ros/indigo/lib/turtlesim/turtlesim_node
$ /opt/ros/indigo/lib/turtlesim/turtle_teleop_key

画面は以下のようになって、turtle_teleop_keyを実行したターミナル#3でカーソルキーを押すと、TurtleSimウィンドウの亀が動きます。

TurtleSim

少しだけ解説を。ノード同士が通信するには、相手の情報が必要です。そのために、ROSではMasterというプログラムが動いていて、ノードの情報を集中管理しています。インターネットにおけるDNS（動的な登録ができるので、ダイナミックDNSの方が適切かも）みたいなモノですね。他にも、柔軟なパラメーター管理のために、Parameter Serverというプログラムも動いています。ログ管理も。これらのプログラムを1つずつ動かしていくのは大変なので、ターミナル#1で実行したroscoreというコマンドで一気に起動できるようになっています。あと、ターミナル#2と#3で/opt/ros...のように長々と入力しているのは、ROSのプログラムはディレクトリ分割で管理されていて、プログラムのあるディレクトリ群が多すぎてPATH環境変数に登録できないためです。

ROSのプログラムは、Ctrl-c（キーボードのコントロール・キーを押しながらcを押す）で終了できます。起動したプログラムをすべて終了させて、次に進んでください。

rosrunで起動する

上の直接起動する方式だと、パスを入力するのが面倒です。この問題を解決するために、ROSはrosrunというコマンドを提供します。

$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun turtlesim turtle_teleop_key

これで、少し楽になりました。

roslaunchで起動する

でも、rosrunでもまだまだ面倒くさい。roscoreせずにrosrunするとエラーが表示されますから、roscoreが動いているかどうかは検出できているはず。だったら、何もしなくても必要に応じてroscoreを自動で実行させて欲しい。ターミナルを複数開くのも、コマンドを複数回も入力するのも、終了させるためにCtrl-Cを何度も叩くのも面倒です。

この問題はシェル・スクリプトを書くことで解消できそうなのですけれど、できれば、汎用言語であるシェル・スクリプトではなく、ROSに特化した言語で楽にやりたい。Javaプログラムのビルドを、シェル・スクリプトではなくてAntやMaven（.NET FrameworkならMSBuild、RubyならRake、私が大好きなClojureならLeiningen）でやる方が楽なのと同じ。というわけで、roslaunchを使いましょう。roslaunchは、ROSプログラム起動用のコマンドと、そのコマンドで処理可能な言語（フォーマットはXML）の文法の両方を指します。

ただ、これまでで例として使用してきたturtlesimは、roslaunchのlaunchファイルを提供してくれていないんですよ（roslaunchの文法で書かれたファイルをlaunchファイルと呼びます）。なので、本当はセットアップされたパッケージ対してやってはならないことなのですけれど、launchファイルを追加してみます。

ターミナルで以下のコマンドを実行して、

$ sudo mkdir /opt/ros/indigo/share/turtlesim/launch
$ sudo gedit /opt/ros/indigo/share/turtlesim/launch/teleop.launch

以下の内容のファイルを作成してください4。

<launch>
  <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_1"/>
  <node pkg="turtlesim" type="turtle_teleop_key" name="turtlesim_1_teleop_key"/>
</launch>

roslaunchの場合の起動の仕方は、以下の通り。必要なターミナルは一つだけです。

$ roslaunch turtlesim teleop.launch

TurtleSim

うん、これなら楽ちんですね5。Ctrl-c一回ですべて終了できましたし。

catkin

ついにプログラミング……の前に、パッケージの作成です。ROSアプリケーションは、パッケージと呼ばれる単位で管理されます。我々がこれから作成するROSアプリケーションも、パッケージにしなければなりません。このパッケージのお約束をうまいこと守りながらビルドしてくれるツールとして、catkinというビルド・ツールがあります。

ワーク・スペースの作成

前にも述べましたが、ROSアプリケーションは、複数のノードがメッセージを送り合うことで動作します。で、これから我々が開発するノードが使用するメッセージは、他のパッケージで定義されたものかもしれません。この「他のパッケージ」をうまいこと利用できるようにする手段が「ワーク・スペース」なのですけど、ROSのプログラミングをまだ始めていない状態ではナンノコッチャって感じ。なので、他のケースで思考実験してみましょう。

題材は、C++のヘッダー・ファイルのインクルードにします。あなたのソース・コードは、もちろん#include <stdio>できます。標準ライブラリのヘッダーですもんね。あと、あなたが今日作成したfor_this_project.hも#include "for_this_project.h"できるでしょう。ファイルをインクルード可能なディレクトリに置くでしょうからね。

でも、昔やったプロジェクトのgood_old_days.hだったら？　この場合は、コンパイラにインクルード・ディレクトリを指定するオプション（-I）を付けなければならないでしょう。でもね、追加したいディレクトリがいっぱいあったら？　オプションを何度も書くのは面倒くさいので、どーにかしたい。幸いなことにROSをインストールしたUbuntuはLinuxなのでコマンドがいっぱいあって大抵のことはできます。だから、たとえば、

$ g++ `find ~/my_projects \( -name include -a -type d \) -print | sed "s/^/-I/g" | paste -s` x.cpp

と実行すれば、a_long_time_ago.hがmy_projectsの下のプロジェクト用ディレクトリの下のincludeディレクトリにあるなら#include "a_long_time_ago.h"できます。でもまぁ、include以外のフォルダに置かれていると駄目なんですけど……。

はい、思考実験終わり。この思考実験から得られる結論は、同じルールに従うプロジェクトのディレクトリを、あるディレクトリの下にまとめて格納すると便利ってことです。で、catkinのルールに従うパッケージ用のディレクトリを格納して便利にやってやろうぜというディレクトリが、catkinのワーク・スペースというわけ。

では、その便利なワーク・スペースを作りましょう。ターミナルで以下のコマンドを実行することで、ワーク・スペースを作成できます。

$ source /opt/ros/indigo/setup.bash
$ cd <ワーク・スペースの親ディレクトリ>
$ mkdir -p <ワーク・スペース名>/src
$ cd <ワーク・スペース名>/src
$ catkin_init_workspace
$ cd ..
$ catkin_make
$ echo 'source <ワーク・スペースの親ディレクトリ>/<ワーク・スペース名>/devel/setup.bash' >> ~/.bashrc

最後のechoしている1行では、ターミナルを開いたときにこのワーク・スペース向けの環境設定をするように指示しています（お好きなテキスト・エディタで~/.bashrcを編集してもOK）。これをしておかないとせっかく作成したワークスペースが使われませんので、注意してください。

上のコマンド中の、<ワーク・スペースの親ディレクトリ>は、どこでも構いません。私は~/Documents/Projectsの下に開発プロジェクトのファイルを置くと決めているので、ここにしました。<ワーク・スペース名>もなんでも構わないのですが、一般的にcatkin_wsとすることが多いようで、私もこの名前にしました。というわけで、私の環境では~/Documents/Projects/catkin_wsディレクトリがワーク・スペースになりました。以降の記述では、このワーク・スペースを<catkin_ws>と記述します。頭の中で皆様のワーク・スペースに置き換えてください。

パッケージの作成

ワーク・スペースができたので、パッケージを作成しましょう。ターミナルから以下のコマンドを実行すれば、パッケージを作成できます。

$ cd <catkin_ws>/src
$ catkin_create_pkg <パッケージ名> [<依存するパッケージ名1> [<依存するパッケージ名2>]]

最初にcdする先は、srcディレクトリです（私の環境では~/Documents/Projects/catkin_ws/src）。catkin_create_pkgをワーク・スペース直下で実行しないように、注意してください（まぁ、間違えて実行しても、生成されたディレクトリを消せばOKですけれど）。

2行目のcatkin_create_pkgが、パッケージの雛形生成です。オプションの二番目以降の<依存するパッケージ名>は、そんなの作る前に分かるはずが無いですし、入れると生成される雛形に余計な記述が増えて混乱したりもするので、使うことが確実なC++開発用パッケージであるroscppだけでよいでしょう。

で、さて、必須オプションの<パッケージ名>は、何にしましょうか？　本稿では、ロボットの周囲360°の3D点群や対象物の周囲360°からの3D点群を生成するプログラムを作成します。キーワードは360°ですかね。ただ、プログラミングの世界では180°をπとするラジアンを使うのが一般的なので、2π≒2×3.14＝6.28を名前にしましょう。というわけで、catkin_create_pkg six_point_two_eight roscppしてパッケージを作成してください。

package.xmlとCMakeLists.txtの編集

生成されたパッケージ・ディレクトリをlsしてみると、以下のようになっています。

$ ls -F six_point_two_eight
CMakeLists.txt  include/  package.xml  src/

includeは*.hファイルを格納するディレクトリ、srcは*.cppファイルを格納するディレクトリです。とりあえずこの2つは無視してください。パッケージの情報を表すpackage.xmlファイルと、ビルドを制御するCMakeLists.txtファイルは、今のうちに編集しておきましょう。

package.xml

コメントを削除し、埋めるべきところを埋めると、以下になります。

<?xml version="1.0"?>
<package>
  <name>six_point_two_eight</name>
  <version>0.1.0</version>
  <description>Getting 360 degree PointCloud.</description>
  <maintainer email="rojima1@gmail.com">OJIMA Ryoji</maintainer>
  <license>BSD</license>

  <buildtool_depend>catkin</buildtool_depend>
  
  <build_depend>roscpp</build_depend>
  
  <run_depend>roscpp</run_depend>

  <export>
  </export>
</package>

CMakeLists.txt

自動生成されたゴチャゴチャのファイルから必要な部分だけを残し、C++14でのプログラミング向けの要素を追加します。

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  roscpp
)

# find_package(Boost REQUIRED COMPONENTS system)

# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

# generate_messages(
#   DEPENDENCIES
#   std_msgs  # Or other packages containing msgs
# )

catkin_package(
#  CATKIN_DEPENDS                          # roscppを削除
)

add_compile_options(                       # 追加
  "-std=c++14"                             # 追加
)                                          # 追加

include_directories(
  include                                  # 追加
  ${catkin_INCLUDE_DIRS}
)

add_library(six_point_two_eight
  src/six_point_two_eight.cpp              # ${PROJECT_NAME}/を削除
)

# add_dependencies(six_point_two_eight ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(six_point_two_eight
  ${catkin_LIBRARIES}
)

add_compile_optionsで、C++14モードでコンパイルするように指示しています。あと、catkin_create_pkgはincludeディレクトリを作ったくせに、catkin_create_pkgが生成したCMakeLists.txtのままだと、インクルード対象のディレクトリになりませんでした……。仕方がないですから、include_directoriesで追加しました。

g++ 4.9

最後に、もう一つだけ準備作業を。ROS Indigoの対象プラットフォームであるUbuntu 14.04のg++のバージョンは4.8で、バージョン4.8だとC++11までしかサポートしないんです……。

なので、以下のコマンドを実行して、g++ 4.9をセットアップしておいてください。

$ sudo add-apt-repository ppa:ubuntu-toolchain-r/test
$ sudo apt-get update
$ sudo apt-get install gcc-4.9 g++-4.9
$ sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-4.9 60 --slave /usr/bin/g++ g++ /usr/bin/g++-4.9
$ sudo update-alternatives --config gcc

Nodelet

やりました。ついに、プログラミングです。

*.hに書く？　*.cppに書く？

いつも回りくどくて申し訳ないのですけど、古き良き時代の話から。C++が生まれる前、C言語の時代の物事は単純で、関数の宣言は*.h、関数の実装は*.cに書くと決まっていました。そして、C++が生まれ、クラス宣言を*.hに書くようになったところで、この美しい原則は壊れました。クラスのメンバー関数はC言語の関数よりも小さな場合があり、通常の関数呼び出しだとプログラムが遅くなるからインライン展開が必要で、その場合の実装は*.hに書くことに決まっちゃったんですよ。*.hに、宣言に加えて実装の一部を書くわけです。その後の、ジェネリック・プログラミング（テンプレート）が混乱に拍車をかけました。ジェネリック・プログラミングはコンパイル時にコードを生成する技術で、コード生成には生成元のコードが必要です。コンパイル時には*.hしか参照できませんから、生成元コードは*.hに書くしかない……。というわけで、実装のほとんどが*.hにあるコードが増えてきました。たとえば、線形代数ライブラリのEigenでは、実装のすべてが*.hに入っています。こうして、*.hには、それが宣言であれ実装であれ、*.cppで共有しなければならないものを何でも書くことになりました。

では我々も*.hにコードを書けばよいのかというと、ROSの場合はちょっと微妙です。*.hには複数の*.cppで共有する内容を書くべきなのですけれど、でもROSのノードはROS通信で呼び出されます。つまり、C++の仕組みでの呼び出しは不要なわけ。だからクラス宣言を*.hで共有する必要はないわけで、だったら全部を*.cppに書いても問題ないはず（ROSのサンプルはこの方式が多い）。折衷案として、インライン展開するかどうかはコンパイラの判断に任せることにして、クラス宣言は*.hに、メンバー関数の実装は*.cppに書くという手もあるのですけれど、それではジェネック・プログラミングが使えません。ノード間で共有するユーティリティーではテンプレートを使いたいわけで、その場合だけ*.hに実装があるのではコードの閲覧性が悪くなってしまいます。

と、いろいろ考えても明らかな正解はなさそうだったので、実利だけを考え、本稿はコード量が少なくて閲覧性が高い*.hに実装を書く方式を採用しました。

package.xmlとCMakeLists.txtにnodeletを追加

さて、我々はこれからNodeletを作成するわけですけど、そのNodeletはnodeletパッケージが提供する機能です。でも、我々のsix_point_two_eightパッケージが参照しているのはroscppパッケージだけ。このままではNodeletを作れませんので、nodeletパッケージを参照するようにpackage.xmlとCMakeLists.txtを編集しましょう6。

package.xml

<build_depend>にビルド時に必要なパッケージを、<run_depend>に実行時に必要なパッケージを指定します。nodeletパッケージはビルドでも実行でも必要ですから、両方を記述してください。

<?xml version="1.0"?>
<package>
  <!-- 略 -->
  
  <buildtool_depend>catkin</buildtool_depend>
  
  <build_depend>roscpp</build_depend>
  <build_depend>nodelet</build_depend>  <!-- 追加 -->
  
  <run_depend>roscpp</run_depend>
  <run_depend>nodelet</run_depend>      <!-- 追加 -->
  
  <!-- 略 -->
</package>

CMakeLists.txt

find_packageにnodeletを追加するだけです。

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  nodelet  # 追加
  roscpp
)

# 略

make_world_models.h

初めてのROSプログラミングですから、Hello worldをやりましょう。include/six_point_two_eight/make_world_models.hを作成し、以下のコードをタイプしてください。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

namespace six_point_two_eight {
  class MakeWorldModels : public nodelet::Nodelet {
  public:
    void onInit() {
      ROS_INFO_STREAM("Hello, world!");
    }
  };
}

ROS Style Guideでは、ファイル名はunder_scoredとなっています。なので、MakeWorldModelsというクラスのファイル名は、make_world_models.hになりました（本稿のコードの「{」の位置はスタイル・ガイドに準拠していませんけど、個人的に譲れないところなのでご容赦ください。皆様は、スタイル・ガイド準拠でコードを書いてください）。

Nodeletを作る時は、nodelet::Nodeletを継承しなければなりません。あと、初期化処理はonInit()に書きます。今回は、ROSのログ出力マクロであるROS_INFO_STREAMを使用して「Hello, world!」という文字列を出力することにしました（NodeletのドキュメントにはNODELET_INFO_STREAMというNodelet用のログ出力マクロが出てきますが、Nodeletの外側で使用できなくて不便なため、本稿では使用しません）。

あと、MakeWorldModelsというクラス名は「ロボットを動かしてみる」という内容にあっていませんけど、これは、次の章の「対象物の周囲360°からの3D点群を作成する」につなげたいからなので、どうかご容赦ください。

six_point_two_eight.cpp

*.hをインクルードする*.cppも作成しましょう。NodeletのクラスをエクスポートするにはPLUGINLIB_EXPORT_CLASSというマクロを呼び出しておく必要があって、ドキュメントにはそのマクロのヘッダー・ファイル（pluginlib/class_list_macros.h）は*.cppでインクルードしろと書いてあります。*.cppの記述は単純でクラス単位に分割する必要はなさそうですから、全体で一つ、パッケージ名と同じファイル名にしましょう。

というわけで、src/six_point_two_eight.cppに、以下のコードをタイプしてください。

#include <pluginlib/class_list_macros.h>
#include "six_point_two_eight/make_world_models.h"

PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeWorldModels, nodelet::Nodelet)

コードを作成したら、ビルドです。catkinのビルドは、catkin_ws直下で実行します（ワーク・スペース全体のビルドにしないと、パッケージの整合性をとれませんからね。catkin_ws/src/six_point_two_eightでビルドするとエラーになりますので、気をつけてください）。ビルドのコマンドは、catkin_makeです。

$ cd <catkin_ws>
$ catkin_make
Base path: /home/ryo/Documents/Projects/catkin_ws
Source space: /home/ryo/Documents/Projects/catkin_ws/src
Build space: /home/ryo/Documents/Projects/catkin_ws/build
Devel space: /home/ryo/Documents/Projects/catkin_ws/devel
Install space: /home/ryo/Documents/Projects/catkin_ws/install
####
#### Running command: "make cmake_check_build_system" in "/home/ryo/Documents/Projects/catkin_ws/build"
####
-- Using CATKIN_DEVEL_PREFIX: /home/ryo/Documents/Projects/catkin_ws/devel
（後略）

catkin_makeの出力には進捗が％で表示されるのですけれど、それが100%になってエラーが表示されてなければ、ビルド成功です。

six_point_two_eight.xmlと、もう一度package.xml

作成したNodeletには、ROS向けの名前をつけてあげなければなりません。まずは、six_point_two_eight.xmlを作成し、以下のコードをタイプしてください。

<library path="lib/libsix_point_two_eight">
  <class
    name="six_point_two_eight/make_world_models"
    type="six_point_two_eight::MakeWorldModels"
    base_class_type="nodelet::Nodelet"/>
</library>

次に、作成したXMLファイルをROSに認識させるために、package.xmlの<export>に1行追記してください。

<?xml version="1.0"?>
<package>
  <!-- 略 -->
  <export>
    <nodelet plugin="${prefix}/six_point_two_eight.xml" />  <!-- 追加 -->
  </export>
</package>

make_world_models.launch

さて、Nodeletはnodeletパッケージのnodeletコマンドを通じて起動するのですけれど、これが実に面倒くさい。だから、前に述べたroslaunchで起動することにしましょう。launch/make_world_models.launchを作成して、以下をタイプしてください。

<launch>
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node
    pkg="nodelet"
    type="nodelet"
    name="make_world_models"
    args="load six_point_two_eight/make_world_models six_point_two_eight_nodelet_manager"
    output="screen"/>
</launch>

2行目で、NodeletManagerをプロセスとして起動しています。このNodeletManagerの中に、複数のNodeletを起動するわけです（今回は一つだけですけど）。output="screen"と指定しているのは、ログを画面に出力させたいから。これを書いておかないと、何か問題が起きても画面に何も表示されなくて、開発時にかなり混乱します。

3行目が、作成したNodeletの起動です。args属性のloadの次が起動するNodeletの名前で、その次がNodeletManagerの名前です。今回はNodeletManagerに2行目で起動したsix_point_two_eight_nodelet_managerを指定しましたが、ロボットのドライバ等が作成するNodeletManagerを指定することも可能です。たとえば、TurtleBotの深度センサーのドライバがその内部で動作しているcamera/camera_nodelet_managerというNodeletManagerとか。six_point_two_eight_nodelet_managerの代わりにcamera/camera_nodelet_managerを指定すれば、深度センサーからデータを取得する速度が、ネットワーク通信が減る分だけ向上するでしょう。ただし、作成したプログラムの不具合でNodeletManagerが異常終了して深度センサーのドライバまで止まってしまうという危険性もありますから、少なくとも開発時は専用のNodeletManagerの使用を推奨します。

さて、これですべての作業は完了しましたので、我々のNodeletを起動してみましょう。

$ roslaunch six_point_two_eight make_world_models.launch
... logging to /home/ryo/.ros/log/60de7594-df68-11e5-b06d-3b9332081cf6/roslaunch-ryo-T550-17006.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://ryo-T550:33789/

SUMMARY
========

PARAMETERS
 * /rosdistro: indigo
 * /rosversion: 1.11.16

NODES
  /
    make_world_models (nodelet/nodelet)
    six_point_two_eight_nodelet_manager (nodelet/nodelet)

auto-starting new master
process[master]: started with pid [17018]
ROS_MASTER_URI=http://ryo-T550:11311

setting /run_id to 60de7594-df68-11e5-b06d-3b9332081cf6
process[rosout-1]: started with pid [17031]
started core service [/rosout]
process[six_point_two_eight_nodelet_manager-2]: started with pid [17034]
process[make_world_models-3]: started with pid [17044]
[ INFO] [1456807510.442436453]: Loading nodelet /make_world_models of type six_point_two_eight/make_world_models to manager six_point_two_eight_nodelet_manager with the following remappings:
[ INFO] [1456807510.444383031]: waitForService: Service [/six_point_two_eight_nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1456807510.482190090]: Initializing nodelet with 4 worker threads.
[ INFO] [1456807510.486318180]: waitForService: Service [/six_point_two_eight_nodelet_manager/load_nodelet] is now available.
[ INFO] [1456807510.538166303]: Hello, world!

うん、最後の行に、「Hello, world!」が出力されました……。

Publisher

「Hello, world!」と画面に出すだけって、ROS関係ないじゃん！　ごめんなさい、おっしゃる通りです。もう少し高度なこと、TurtleBotを前に進ませる処理を書いてみましょう。

トピックとメッセージ

さて、ロボットに命令を出すにはどうすればよいのか？　前述したように、適切なトピックに適切なメッセージをpublishすればよい。でも、ロボットを移動させる場合は、具体的にどのトピックとメッセージを使えばいいの？

それを調べるツールとして、ROSはrostopicとrosmsgを提供しています。試してみましょう。まずは、ターミナルを開いてTurtleBotのドライバを起動してください。

$ roslaunch turtlebot_bringup minimal.launch
... logging to /home/ryo/.ros/log/b43068d0-df70-11e5-b152-630afb514811/roslaunch-ryo-T550-22396.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://ryo-T550:45177/

SUMMARY
========

PARAMETERS
 * /app_manager/auto_rapp_installation: False

いろいろメッセージが出て、中にはWarningと書かれていたりもしますけど、Errorと表示されない限りは大丈夫です。

もう一つターミナルを開いて、rostopic listしてみてください。

$ rostopic list
ryo@ryo-T550:~/Documents/Projects/catkin_ws$ rostopic list
/capability_server/bonds
/capability_server/events
/cmd_vel_mux/active
/cmd_vel_mux/input/navi
/cmd_vel_mux/input/safety_controller
/cmd_vel_mux/input/teleop
/cmd_vel_mux/parameter_descriptions
/cmd_vel_mux/parameter_updates
/diagnostics
/diagnostics_agg
/diagnostics_toplevel_state
/gateway/force_update
/gateway/gateway_info
/info
/interactions/interactive_clients
/interactions/pairing
/joint_states
/laptop_charge
/mobile_base/commands/controller_info
/mobile_base/commands/digital_output
/mobile_base/commands/external_power
/mobile_base/commands/led1
/mobile_base/commands/led2
/mobile_base/commands/motor_power
/mobile_base/commands/reset_odometry
/mobile_base/commands/sound
/mobile_base/commands/velocity
/mobile_base/controller_info
/mobile_base/debug/raw_control_command
/mobile_base/debug/raw_data_command
/mobile_base/debug/raw_data_stream
/mobile_base/events/bumper
/mobile_base/events/button
/mobile_base/events/cliff
/mobile_base/events/digital_input
/mobile_base/events/power_system
/mobile_base/events/robot_state
/mobile_base/events/wheel_drop
/mobile_base/sensors/bumper_pointcloud
/mobile_base/sensors/core
/mobile_base/sensors/dock_ir
/mobile_base/sensors/imu_data
/mobile_base/sensors/imu_data_raw
/mobile_base/version_info
/mobile_base_nodelet_manager/bond
/odom
/rosout
/rosout_agg
/tf
/tf_static
/turtlebot/incompatible_rapp_list
/turtlebot/rapp_list
/turtlebot/status
/zeroconf/lost_connections
/zeroconf/new_connections

これが、TurtleBotのドライバ起動後にROS上に存在するトピックの一覧です。この中から、適切なトピックを選べばよい……のですけど、TurtleBotのドキュメントを見てもトピックの説明は見つかりませんでした。しょうがないので、rosmsgも活用して、アタリをつけていきます。

$ rostopic info /mobile_base/commands/velocity
Type: geometry_msgs/Twist

Publishers: 
 * /mobile_base_nodelet_manager (http://ryo-T550:58177/)

Subscribers: 
 * /mobile_base_nodelet_manager (http://ryo-T550:58177/)

$ rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

rostopic info <トピック名>で、トピックの情報を表示できます。表示内容で重要なのは1行目のType:で、これで/mobile_base/commands/velocityトピックにはgeometry_msgs/Twistをpublish/subscribeできるということが分かります。で、rosmsg show <メッセージ名>でメッセージの型情報を表示すれば、なんとなく推測ができるというわけ。上の情報から、/mobile_base/commands/velocityトピックがたぶん移動で、geometry_msgs/Twistのlinearが直線移動でangularが回転なんだなぁと推測できます。カン頼みのでたらめな方法ですけど、試して駄目なら別のトピックを調べればいいだけですし、ROSアプリケーションに慣れるとカンが冴えてくるので、それほど大きな問題にはなりません。

別のやり方としては、同じようなことをしているTurtleBot向けROSアプリケーションのコードを調べるという手があります。まずは、ROS.orgのBrowse Softwareでindigoとpackageを選択してテキスト・ボックスに「turtlebot」と入力し、searchボタンをクリックしてください。

Browse Software

検索結果の中から、アプリケーションっぽい感じがするturtlebot_appsをクリック。

Browse Software result

今はソース・コードを見たいので、Source:の後ろのhttps://github.com/turtlebot/turtlebot_apps.gitをクリック。

TurtleBot Apps

移動に関係がありそうなturtlebot_follower、src、follower.cppと開いて、てコードを読む。

TurtleBot Apps

コードを読んでいくと、多分geometry_msgs::Twistが移動用のメッセージなんだろうなぁと推測できます。

follower.cpp

推測したトピックとメッセージに自信がないなら、rostopic pub <トピック名> <メッセージ名> <YAML形式のデータ>を使って、トピックにメッセージをpublishすることもできます。プログラムを書き始める前に、試してみましょう。

$ rostopic pub /mobile_base/commands/velocity geometry_msgs/Twist '{linear: {x: 0.1}}'

TurtleBotが少し前に動くはずです。これで、/mobile_base/commands/velocityトピックとgeometry_msgs/TwistメッセージでTurtleBotを動かせることが分かりました。

座標系と単位

プログラミングに入る前に、ROSの座標系について述べさせてください。先ほどのrostopic pubでlinear.xに0.1を指定すると前に進むってのを理解するには、座標系と単位の知識が必要ですもんね。

ROSの座標系は、X軸が前でY軸が左、Z軸が上です。また、X軸を赤、Y軸を緑、Z軸を青で描きます。だから、linear.xにプラスの数値を入れると前に進むというわけ。回転の場合は、座標軸の正の方向を見た場合の時計回り、負の方向に見ると反時計回りになります。だから、Z軸の回転は、ロボットを上から見ると反時計回り。なので、rostopic pub /mobile_base/commands/velocity geometry_msgs/Twist '{angular: {z: 0.5}}'とすると、少しだけ反時計回りします。

Coordinate System

なお、距離の単位はメートル、時間の単位は秒、角度の単位はラジアン、周波数はHzです。linear.x = 0.1は、秒速0.1mで前進しろ、angular.z = 0.5は秒0.5ラジアンで反時計回りに回転しろという意味になります。

ロボットを前に進ませる

すべての調査が完了し、移動のメッセージはgeometry_msgs/Twistであることが分かったので、プログラムを作成しましょう。

package.xmlとCMakeLists.txt

geometry_msgs/Twistを使うにはpackage.xmlとCMakeLists.txtの修正が必要です。ROSのメッセージ名は<パッケージ名>/<クラス名>に分解できますので、それぞれにgeometry_msgsを追加しましょう。

<?xml version="1.0"?>
<package>
  <!-- 略 -->
  
  <buildtool_depend>catkin</buildtool_depend>
  
  <build_depend>geometry_msgs</build_depend>  <!-- 追加 -->
  <build_depend>nodelet</build_depend>
  <build_depend>roscpp</build_depend>
  
  <run_depend>geometry_msgs</run_depend>      <!-- 追加 -->
  <run_depend>roscpp</run_depend>
  <run_depend>nodelet</run_depend>

  <!-- 略 -->
</package>
cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs  # 追加
  nodelet
  roscpp
)

# find_package(Boost REQUIRED COMPONENTS system)

# 略

include/six_point_two_eight/make_world_models.h

前準備が完了しましたので、プログラミングしましょう。ロボットを前に進ませるコードを追加したmake_world_models.hは、以下の通り。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <thread>

#include <geometry_msgs/Twist.h>

namespace six_point_two_eight {
  // geometry_msgs/Twistメッセージを生成します。
  inline auto createTwistMsg(double linear_x, double angular_z) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear_x;
    msg.angular.z = angular_z;
    
    return msg;
  }
  
  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    std::thread working_thread_;
    
  public:
    void onInit() {
      // Publisherを生成します。
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("mobile_base/commands/velocity", 1);

      // onInitの中でループするとNodeletManagerの処理が止まってしまうので、別スレッドで実行します。
      working_thread_ = std::thread(
        [&]() {
          ros::Rate rate(10);  // 周波数は10Hz（1秒に10回ループします）。
          while (ros::ok()) {  // ROSがシャットダウンされるまで、無限ループします。
            velocity_publisher_.publish(createTwistMsg(0.1, 0.0));  // メッセージを生成して、publishします。
            rate.sleep();  // 適切な周波数になるようにスリープ。
          }
        });
    }
  };
}

geometry_msgs/Twistを使えるようにするために、#include <geometry_msgs/Twist.h>しています。あと、ROSのメッセージには気の利いたコンストラクタが無くて使いづらいので、createTwistMsg()というメッセージ生成用の関数を作りました（TurtleBotは前後進とZ軸の回転しかできないので、linear.xとangular.zのみを設定すれば十分）。これはメンバー関数ではなくて普通の関数なので、inlineを付加しました。そうそう、C++14は、関数の戻り値の型をautoにしておくと型推論してくれてとても便利です（型が無いのとは違います。静的な型チェックが実施されますので、コードに型間違いがあればきちんとコンパイル・エラーになります）。

publishは、ros::Publisherを使用して実施します。ros::Publisherは、ros::NodeHandleクラスのadvertise<メッセージの型>(トピック名, キューのサイズ)メンバー関数で作成できます。このros::NodeHandleクラスってのはROSノードへのハンドルで、ノードに関する操作はこのNodeHandleを通じて実施します（Nodeletはノードを名乗っていますけど、ROSノードを操作するためのメンバー関数は提供しないんですよ……）。で、Nodeletの中でros::NodeHandleを取得するメンバー関数が、上のコードで使用しているgetNodeHandle()なわけです。

getNodeHandle()には、対になるgetMTNodeHandle()というマルチ・スレッド対応バージョンもあるのですけれど、その場合はスレッドの排他制御を自前でプログラミングしなければなりません。getNodeHandle()ならROSからの呼び出しが直列化されますから、排他制御が不要になってとても楽ちんで安心です。あと、getPrivateNodeHandle()とgetMTPrivateNodeHandle()というPrivate付きのバージョンもあるのですけど、Private付きだと名前解決にノード名が追加されてしまいます（mobile_base/commands/velocityが/make_world_models/mobile_base/commands/velocityと解釈されちゃう7）。通常はgetNodeHandle()を使ってください。

あとは、作成したros::Publisherのインスタンスが消えてしまわないよう、メンバー変数のvelocity_publisher_に保持しておきます。

onInit()の後半。別スレッドを起こしてループしているところ。なんでこんな面倒なことをしているかを理解していただくために、先ほどのrostopic echoの時のロボットの動きを思い出してください。あの時、ロボットは少しだけ動いてすぐに止まってしまいましたよね？　これは、ロボットは一定周期で指示を受け取る仕組みになっているためです。動けと言われたので1周期分動いて、次の周期には命令が来ていないから動きを止めちゃったってわけ。というわけで、ロボットを動かし続けるためにはループが必要となります。

で、getNodeHandle()のところで少し述べたように、Nodeletはスレッドの排他制御が不要になる仕組みを用意しています。そのためには、同じリソースを扱うスレッドを並行に動作させない（直列化して順番に実行させる）ことが有効。NodeletでgetNodeHandle()を使うと、異なるノードの処理は並行なのですけど、同じノードの処理については直列で動作するようにになります。ただし、onInit()はどうやら全ノードで直列らしくて、onInit()の中でループすると他のノードの初期化が始まらなくなってしまいます。だから、ループのような終わらない処理を実行する場合は、上のコードのようにstd::threadを使用して別スレッドを作成してあげてください。

最後。std::threadの引数の[&]() { ... }の部分は、C++14のラムダ式です。関数をその場で生成して、std::threadのコンストラクタに渡します。ラムダ式の最初の[]には処理の中で使用するラムダ式の外側の変数を書くのですけど、ここに[&]と書いておくと外側の変数すべてを使用できるようになってとても便利です（ラムダ式の中でvelocity_publisher_を使えるのは、この[&]のおかげです）。

実行する

TurtleBotのノードにメッセージをpublishするプログラムを作成したわけですから、まずは受け取り手のTurtleBotノードを起動します。ターミナルを開いて、以下のコマンドを実行してください（「トピックとメッセージ」のところで起動済みの場合は、コマンドの実行は不要です）。

$ roslaunch turtlebot_bringup minimal.launch

その上で、別のターミナルでmake_world_models.launchをroslaunchしてください。

$ roslaunch six_point_two_eight make_world_models.launch

これで、TurtleBotが前に動き出すはずです。今回作成したプログラムはひたすらまっすぐに進むだけですから、TurtleBotが壁にぶつかる前にCtrl-cして止めてあげてください。

Timer

まっすぐ進むだけなら、ゼンマイ仕掛けのおもちゃだって出来る……。せっかくのROSなのだから、もっと制御したい。というわけで、一定時間が経過したら止まるようにしてみましょう。

ロボットを前に進ませて、停止させる

一定時間毎に処理をさせたい場合は、ros::Timerを使います。今回はこのros::Timerを使用して、ロボットを前に進ませて、一定時間が経過したら停止させましょう。

include/six_point_two_eight/make_world_models.hpp

ros::Timerは簡単に使用できますので、いきなりコードで。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <geometry_msgs/Twist.h>

namespace six_point_two_eight {
  inline auto createTwistMsg(double linear_x, double angular_z) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear_x;
    msg.angular.z = angular_z;
    
    return msg;
  }
  
  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Timer timer_1_;
    ros::Timer timer_2_;
    
  public:
    void onInit() {
      // Publisherを生成します。
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("mobile_base/commands/velocity", 1);

      // ループの代わりにros::Timerを使用します。
      timer_1_ = getNodeHandle().createTimer(
        ros::Duration(0.1),  // 0.1秒に一回＝10Hz。
        [&](const auto& message) {
          velocity_publisher_.publish(createTwistMsg(0.1, 0.0));
        });

      // 5秒経ったら、処理を止めます。
      timer_2_ = getNodeHandle().createTimer(
        ros::Duration(5.0),
        [&](const auto& message) {
          timer_1_.stop();
          timer_2_.stop();
        });
    }
  };
}

ros::Timerは、引数で指定した周期で、引数で指定した処理を呼び出してくれます。ROSでの時間はros::Duration（時刻はros::Time）なので、ros::Timerを作成するcreateTimer()の第一引数はros::Durationです。第二引数は実行する処理で、関数とかクラス・メソッドとかファンクターとか、あとラムダ式が使えます。C++14のラムダ式は、引数の型にもautoが使えてとても便利ですな。autoの前後にconstと&をつけているのは、&だと参照渡しになるのでインスタンスのコピーが不要になって高速化でき、constを付けておけば参照渡しであっても呼び出し側の値が変更されないため値渡しの場合と同じコードを書けるためです。最近のC++のライブラリの引数はconst type& varばっかりで、他の書き方だと特別な意図があるんじゃないかと混乱させちゃうしね。

今回のコードでは、ros::Publisherの知識だけで書いた先ほどのコードのスレッドとループをros::Timerに置き換えて、timer_1_に格納しています。スレッドとループより、この書き方の方が楽ですからね。あと、移動を停止するには移動するようにpublishするタイマーを止めればよいわけですから、5秒後（ros::Duration(5.0)）にtimer_1_.stop()するros::Timerを設定してtimer_2_に格納しています。あと、timer_2_が5秒周期で何回も処理を実施しても無意味ですから、timer_2_のラムダ式の中ではtimer_2_.stop()も実行しておきます。

はい、お疲れさま。これで、TurtleBotが動いて止まる高度なプログラムの完成です！　……まぁ、ゼンマイ仕掛けの安物のおもちゃでも、動いて止まるんだけどな。

ロボットの周囲360°の3D点群を作成する

単純な動きだけじゃツマラナイ！　センサーを使いたい！　了解しました。TurtleBotには深度センサーが付いているので、それを活用して外界データを作成してみましょう。プログラムから、以下の画像のようなデータを作ります。以下の画像のデータはオフィスの私の席の近くの3D点群で、左上で腕をブラブラさせてだらけまくっているのが私になります。もちろん、ただの画像ではなくて、グリグリ動きます。

周囲360°の3D点群

PointCloud2

TurtleBotには、Microsft社のKinectかASUS社のXtion PRO LIVEがついています。これらは深度センサーと呼ばれるデバイスで、外界の3D点群を作成してくれます（Kinectというと骨格のイメージがありますけど、骨格はこの3D点群を解析して作成する、二次成果物です）。

深度センサーを扱うプログラム開発時の、TurtleBotとの接続方法

TurtleBot Installationでは、PCを2台用意して、1台をTurtleBotの上に、残り1台を手元に置くようになっています。でも、深度センサーのデータは大きいので、この方式だとフレームレートが低く（私の環境だと数秒に1コマ！）なってしまうんですよ。本番環境では深度センサーのデータ処理はTurtleBotの上のPCで実行するので問題ありませんが、開発には差し障りが出てしまいます。

だから、今回は手元のPCにTurtleBotをつなぐことにしましょう。長めのUSB延長ケーブルを用意して、手元のPCにTurtleBotをつなげてください。

RViz

まずは、深度センサーのデータがどんなものか、試してみましょう。ROSにはRVizという可視化ツールがあって、それで深度センサーのデータを表示できます。ターミナルを3枚開いて、以下のコマンドを実行してください。

$ roslaunch turtlebot_bringup minimal.launch
$ roslaunch turtlebot_bringup 3dsensor.launch
$ roslaunch turtlebot_rviz_launchers view_model.launch

これで、RVizの画面が表示されるはずです。画面が表示されたら左下の[Add]ボタンを押して、表示されたダイアログの[By display type]タブの[PointCloud2]アイコンを選択し、[OK]ボタンを押してください。RVizの画面左側の[Displays]パネルに[PointCloud2]アイコンが追加されるはずです。なお、ここまでGUI上で実施した作業は、プログラミングでのメッセージ型の指定に相当します。

RVizへのメッセージの追加

ROSではメッセージとトピックの両方が必要なことを前の章でやりました。メッセージに続いて、トピックも設定しましょう。[PointCloud2]アイコンの下の[Topic]ドロップ・ダウン・リストを開いて、「/camera/depth_registered/points」（シミュレーターの場合は「/camera/depth/points」）を選択してください。これで、深度センサーのデータが画面に表示されるはずです。マウスの左ボタンのドラッグで回転、シフト・キーを押しながらの左ドラッグで平行移動、右ドラッグで拡大/縮小できますから、グリグリ回して楽しんでみてください。

RVizでのトピック選択

グリグリ回している途中のキャプチャなので分かりづらいですけれど、3Dのデータができています。

3D点群

なお、実機の場合でトピックを「/camera/depth/points」に変更すると、カメラからの画像を合成していない（色が着いていない）点群が表示されます。このように、トピックやメッセージの内容を調べるためにもRVizは使えますから、いろいろと活用してみてください。

PointCloud2

さて、先ほどRVizのメッセージ型として選択したのは、PointCloud2でした。ポイント・クラウドの日本語訳は点群で、点の集合です（数学の点群と字は同じだけど意味は違う）。RVizの[PointCloud2]アイコン以下の[Style]を「Points」、[Size (Pixels)]を「1」にして点群部分を拡大してみると、点の集合であることが見て分かるでしょう。なお、PointCloud2の「2」は、2次元じゃなくてバージョン2の「2」です。以前のPointCloudメッセージは拡張性が不足していたみたいで、新しくPointCloud2が定義されました。

Wikipediaによれば、点群から面を計算してポリゴンにしたり、NURB（Non-uniform rational B-spline）で曲面を出したりすることもできるようです。うん、点群ってなんだか便利そう。ただ表示するだけでも、けっこう面白いですしね。そうだ、TurtleBotにその場でグルッと回転させて、周囲360°の点群を取得して表示してグリグリしたら楽しいんじゃね？

Subscriber

というわけで、TurtleBotの周囲360°の点群を生成するROSプログラムを作ります。そのために、まずは深度センサーからデータをもらわないと。

いつものpackage.xmlとCMakeLists.txt

PointCloud2メッセージを扱うので、いつものとおりに参照するパッケージを追加しなければなりません。でも、PointCloud2のパッケージって、どうやって調べればよいのでしょうか？　答え。rosmsg show PointCloud2で調べてください。

ryo@ryo-T550:~$ rosmsg show PointCloud2
[sensor_msgs/PointCloud2]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height
uint32 width
sensor_msgs/PointField[] fields
  uint8 INT8=1
  uint8 UINT8=2
  uint8 INT16=3
  uint8 UINT16=4
  uint8 INT32=5
  uint8 UINT32=6
  uint8 FLOAT32=7
  uint8 FLOAT64=8
  string name
  uint32 offset
  uint8 datatype
  uint32 count
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense

というわけで、sensor_msgsを追加しましょう。

package.xml

<?xml version="1.0"?>
<package>
  <!-- 略 -->
  
  <buildtool_depend>catkin</buildtool_depend>
  
  <build_depend>geometry_msgs</build_depend>
  <build_depend>nodelet</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>  <!-- 追加 -->
  
  <run_depend>geometry_msgs</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>nodelet</run_depend>
  <run_depend>sensor_msgs</run_depend>      <!-- 追加 -->

  <!-- 略 -->
</package>

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  nodelet
  roscpp
  sensor_msgs  <!-- 追加 -->
)

# 略

roslaunchで、トピックをリマップ。あと、includeも

先ほどRVizで点群を見た時、TurtleBotの実機とシミュレーターで設定するトピックが異なっていました。実機は/camera/depth_registered/pointsで、シミュレーターでは/camera/depth/pointsでしたもんね。なら、実機でもシミュレーターでも動かしたいなら、トピックだけが異なる同じプログラムを2つ作らなければならない？

そんな保守性ダダ下がりの馬鹿な話はないわけで、roslaunchに少し記述を追加するだけでトピックをリマップできます。

実機用

<launch>
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_world_models" args="load six_point_two_eight/make_world_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="points" to="camera/depth_registered/points"/>  <!-- 追加 -->
  </node>
</launch>

シミュレーター用

<launch>
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_world_models" args="load six_point_two_eight/make_world_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="points" to="camera/depth/points"/>  <!-- 変更 -->
  </node>
</launch>

あれ？　トピックだけが異なっていて他は同じlaunchファイルを2つ作っている？　保守性ダダ下がりな悪い状況のまま？　というわけで、roslaunchのドキュメントに載っている、<arg>タグと<include>タグを使ってみましょう。ついでに、mobile_base/commands/velocityも<remap>します。

launch/make_world_models.launch（実機用）

<launch>
  <arg name="velocity" default="mobile_base/commands/velocity"/>  <!-- argを宣言 -->
  <arg name="points" default="camera/depth_registered/points"/>   <!-- argを宣言 -->
  
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_world_models" args="load six_point_two_eight/make_world_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="velocity" to="$(arg velocity)"/>  <!-- argを使用 -->
    <remap from="points" to="$(arg points)"/>      <!-- argを使用 -->
  </node>
</launch>

launch/make_world_models_in_gazebo.launch（シミュレーター用）

<launch>
  <include file="$(find six_point_two_eight)/launch/make_world_models.launch">
    <arg name="points" value="camera/depth/points"/>  <!-- argの値を上書きします。 -->
  </include>
</launch>

はい、これで重複がなくなりました。<include>と<arg>って便利ですね。ついでに、turtlebot_bringup minimal.launchとturtlebot_bringup 3dsensor.launchを2回実行しなくて済むように、<include>を使用してこの2つをまとめるlaunchファイルも作成しましょう。

launch/turtlebot_driver.launch（TurtleBot起動用）

<launch>
  <include file="$(find turtlebot_bringup)/launch/minimal.launch" />
  <include file="$(find turtlebot_bringup)/launch/3dsensor.launch" />
</launch>

make_world_models.h

以上で、準備は完了です。プログラミングに入りましょう。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <geometry_msgs/Twist.h>
#include <sensor_msgs/PointCloud2.h>  // 追加。

namespace six_point_two_eight {
  inline auto createTwistMsg(double linear_x, double angular_z) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear_x;
    msg.angular.z = angular_z;
    
    return msg;
  }
  
  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Subscriber points_subscriber_;  // サブスクライバーを保持するメンバー変数。
    ros::Timer timer_1_;
    ros::Timer timer_2_;

  public:
    void onInit() {
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);  // remapに合わせて、トピックを変更。

      // PointCloud2を取得するサブスクライバーを設定します。
      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        "points", 10,  // トピックはremapのfromの値。
        [&](sensor_msgs::PointCloud2ConstPtr message) {  // ラムダ式の展開のされかた次第では（具体的には、中で外側の変数や関数を呼ばない場合は）、const auto&は使えませんでした。
          ROS_INFO_STREAM("Got PointCloud2. Width: " << message->width);  // とりあえず、データを取得したことを表示。
        });

      timer_1_ = getNodeHandle().createTimer(
        ros::Duration(0.1),
        [&](const auto& message) {
          velocity_publisher_.publish(createTwistMsg(0.0, M_PI * 2 / 30));  // 前進ではなく、回転に変更。1周（M_PI * 2）を30秒で回る速度を指示します。
        });
      
      timer_2_ = getNodeHandle().createTimer(
        ros::Duration(45.0),  // 計算上は30秒でよいはずなのですけど、30秒では一周してくれませんでしたので、多めの値で。
        [&](const auto& message) {
          points_subscriber_.shutdown();  // ros::Subscriberの終了は、stop()ではなくてshutdown()です。
          timer_1_.stop();
          timer_2_.stop();
        });
    }
  };
}

subscribeは、ros::Subscriberを使用して実施します。getNodehandle()してros::NodeHandleを取得して、そのメンバー関数のsubscribe<メッセージの型>(トピック名、キューのサイズ、メッセージ取得時の処理)を呼び出して生成してください。引数のトピック名はmake_world_models.launchの<remap>タグのfrom属性に合わせ、キューのサイズは処理中に次のデータがやってきても取りこぼさないよう、多めに10を指定してみました。

処理は、いつも通りにラムダ式で指定します。C++のラムダ式はその内容によって展開のされ方が違う、かつ、しかもROS側で受け取る引数の型のboost::functionは癖が強いようで、試してみたらラムダ式の引数の型をconst auto&にできる場合とできない場合がありました。どうやら、ラムダ式の中でキャプチャした変数や関数を呼び出しているのでファンクター相当品が生成される場合はconst auto&OKで、今回のコードのようにただの無名の関数で表現できる場合は駄目みたいです。なので、今回はメッセージを受け取る処理の引数の型を明示しました。型名は、コンスタントでスマート・ポインターな型のエイリアスで、メッセージの型の後ろにConstPtrという文字列を付加した型になります（ros::NodeHandle.subscribe()のコードを見るとconst sensor_msgs::PointCloud2&を引数に使うバージョンもあるのですけれど、ラムダ式の場合は使えませんでした）。ラムダ式の中に記述する処理は、とりあえず、widthを表示するだけとしています。

さて、これでセンサーのデータを読み込めるようになったわけですけど、それにしても、センサーのデータを「読み込む」処理なのに、データを渡された時に実行する処理を渡す形になっているのは、なぜなのでしょうか？　私は、「読み取りなんだから戻り値でいーじゃん」と感じました。

その答えは多分、ROSは様々な周波数で動作する大きめのユニットを統合するための環境なため。「様々な周波数」なのだから、前の章の最初の頃のTimerを使う前のプログラムのような、ループを組んで「一定の周波数」で動作する中でセンサーのデータを読み取る方式は採用できません。センサーやユニットがデータを送りたいタイミングで、データを取得できないとね。だから、処理を登録する形になっているんだと思います。ROS.orgに載っているプログラムはサンプル・プログラムなので単純な場合が多くて、単純な処理なのでループで実現していてArduinoのコードと見た目そっくりで、だったらArduinoみたいに読み取りでいいじゃんと混乱してしまうのですけれど、実際のアプリケーションを作成すれば、この方式が優れていることが分かると考えます。まぁ、使いづらくて腹がたつ場合も多いんですけど、後述するactionlibとかラムダ式とスマート・ポインターの組み合わせとかで対処できるので、まぁいいかなぁと。

nodelet_topic_tools::NodeletThrottle

データを送る側の都合だけではなく、データを受け取って処理する側の都合も重要でしょう。publish側の深度センサーが全力で送ってくるデータをすべて処理するのは、かなり大変ですもん。私が使っているASUS Xtion PRO LIVEは30fpsだから、30秒かけてTurtleBotを一周させる先ほどのプログラムだと、点群の数は900個になっちゃう。これでは大変すぎるから、深度センサーの周波数を下げたい。

このような時は、nodelet_topic_toolsパッケージのNodeletThrottleが便利です。メッセージの流量制御を、パフォーマンスを落とさずに簡単に実現できます。さっそく使ってみましょう。

package.xmlとCMakeLists.txt

依存するパッケージに、nodelet_topic_toolsを追加してあげてください。これまで何度も見て飽きているでしょうから、コードは省略します。

include/six_point_two_eight/point_cloud_2_throttle.h

nodelet_topic_tools::NodeletThrottleクラスを継承して、throttle()メンバー関数を実装する……などという手間はかかりません。取り扱うメッセージの型を指定するために、以下のコードのようにusingでテンプレート引数を補うだけでOK。なお、C++14（正しくはC++11以降）では型の別名定義にはtypedefとusingの2つの方法があって、usingの方が高機能です。nodelet_topic_toolsのドキュメントではtypedefを使っていますけど、そして、今回の使い方ではどっちを使っても変わらないのですけど、コードを統一するためにusingを使いましょう。

#pragma once

#include <nodelet_topic_tools/nodelet_throttle.h>
#include <sensor_msgs/PointCloud2.h>

namespace six_point_two_eight {
  using PointCloud2Throttle = nodelet_topic_tools::NodeletThrottle<sensor_msgs::PointCloud2>;
}

src/six_point_two_eight.cpp

Nodeletの場合と同様に、エクスポートします。

#include <pluginlib/class_list_macros.h>

#include "six_point_two_eight/make_world_models.h"
#include "six_point_two_eight/point_cloud_2_throttle.h"  // 追加。

PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeWorldModels, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::PointCloud2Throttle, nodelet::Nodelet)  // 追加。

six_point_two_eight.xml

やはりNodeletと同様に、ROSに名前を登録します。

<library path="lib/libsix_point_two_eight">
  <class name="six_point_two_eight/make_world_models" type="six_point_two_eight::MakeWorldModels" base_class_type="nodelet::Nodelet"/>
  <!-- ここから追加。 -->
  <class
    name="six_point_two_eight/point_cloud_2_throttle"
    type="six_point_two_eight::PointCloud2Throttle"
    base_class_type="nodelet::Nodelet"/>
  <!-- ここまで。 --->
</library>

launch/make_world_models.launch

起動もNodeletと一緒です。topic_inから取得したメッセージを、<param name="update_rate">を通じてHzで設定した周波数でtopic_outに流します。

<launch>
  <arg name="velocity" default="mobile_base/commands/velocity"/>
  <arg name="points" default="camera/depth_registered/points"/>
  
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_world_models" args="load six_point_two_eight/make_world_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="velocity" to="$(arg velocity)"/>
    <remap from="points" to="$(arg points)_throttled"/>  <!-- 修正。 -->
  </node>
  <!-- ここから追加。 -->
  <node
    pkg="nodelet"
    type="nodelet"
    name="point_cloud_2_throttle"
    args="load six_point_two_eight/point_cloud_2_throttle six_point_two_eight_nodelet_manager"
    output="screen">
    <remap from="topic_in" to="$(arg points)"/>
    <remap from="topic_out" to="$(arg points)_throttled"/>
    <param name="update_rate" value="0.5"/>  <!-- 0.5Hzは1秒に0.5回なので、2秒に1回になります。 -->
  </node>
  <!-- ここまで。 -->
</launch>

以上。これで、深度センサーからのデータ量を減らすことができました。

PCLを使用して、点群を保存する

とまぁ、いろいろやってPointCloud2を取得できたわけですけれど、取得できただけではなんの意味もありません。でも、rosmsg sensor_msgs/PointCloud2で仕様を見ても、どう使えばよいのかよくわからないし……。

というわけで、PCLと省略して呼ばれるPoint Cloud Libraryを使ってみましょう。

PCLでできること

PCLのドキュメントを開くと、PCLの説明でよく使われる下の絵が表示されます。

PCLの重要なモジュール

filtersモジュールでフィルタ処理して特徴を保ったままで点の数を減らし、featuresモジュールで特徴量を推定して座標位置以外のモノサシを用意して、keypointsモジュールで特徴点を抽出して点群処理の際に注目すべき点を箇所を特定する。registrationモジュールで複数の点群を統合して大きな点群を作成し、kdtreeモジュールやoctreeモジュールで点を構造化する。segmentationモジュールとsample_concensusモジュールで点群を形状に基づいて分割し、surfaceモジュールで面を構築して3次元コンピューター・グラフィックスのライブラリに流す。recognitionモジュールで物体認識して処理対象を見つけてioモジュールで保存したり、visualizationモジュールでスクリーンに表示する……と、とにかく多機能です。

PCLを使う場合の、コードの書き方

プログラマーとしてPCLについて知って置かなければならない最も重要なことは、「C++のライブラリであること」と「バージョン1.7.2ではC++14（C++11も）に対応していないこと」です。

ROSからPCLを扱うための便利機能が詰まったpcl_rosパッケージを使っても、ROSの通信機能経由でPCLの機能を使えるわけではありません。ROSそのものはC++とPythonとCommon Lispとその他多数の言語に対応しているのですけど、PCLを使うノードの開発ではC++一択になります。

C++14に対応していないというのは、C++14の機能を使用して書かれていないという意味ではなく、PCLを使うコードをC++14モードでコンパイルすると、コンパイルはできるけど実行時に異常終了しちゃうということです（異常終了するかどうかは使う機能次第で、普通に使える機能もありました。PCL1.8でC++11でも大丈夫になるみたいなことが書いてあったのですけど、ごめんなさい、未確認です）。

だから、PCLを使用する部分は通常のコードとは分離し、しかもC++03で書かなければなりません。以下に、その具体的な方法を示しましょう。処理内容は、PCLを使用した点群の保存です。

package.xmlとCMakeLists.txt

package.xmlとCMakeLists.txtに、pcl_rosパッケージを追加してください。

include/six_point_two_eight/point_cloud_utilities.h

コードの分離を確実にするために、C++14とPCLを完全に排除した*.hを作ります。ここには関数宣言だけを記述しましょう。

#ifndef SIX_POINT_TWO_EIGHT_POINT_CLOUD_UTILITIES_H
#define SIX_POINT_TWO_EIGHT_POINT_CLOUD_UTILITIES_H

#include <sensor_msgs/PointCloud2.h>

namespace six_point_two_eight {
  sensor_msgs::PointCloud2Ptr savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
}

#endif

src/point_cloud_utiliites.cpp

PCLのioモジュールを使えば点群の保存なんて簡単……ではありますけど、残念なことにそれなりの量のコードを書かなければなりません。PCLって機能はスゴイんですけど、プログラミング・スタイルが少し独特なんですよ。入力も出力も引数でやる方式なので、1行の関数呼び出しのためだけに変数を定義したりreturnを別途書いたりしなければなりません。

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include "six_point_two_eight/point_cloud_utilities.h"

// 点群の型の整理。
typedef pcl::PointXYZRGB Point;  // 本稿では、色付きの点を使用します。あと、C++03なので、usingではなくtypedefを使用します。
typedef pcl::PointCloud<Point> PointCloud;

// 引数で指定した点群のファイル・パスを取得します。パスは、/tmp/six_point_two_eight_<点群の時刻>.pcdとします。
std::string filePathString(PointCloud::ConstPtr point_cloud) {
  std::stringstream file_path_stringstream;
  file_path_stringstream << "/tmp/six_point_two_eight_" << std::setfill('0') << std::setw(19) << pcl_conversions::fromPCL(point_cloud->header).stamp.toNSec() << ".pcd";

  return file_path_stringstream.str();
}

// 点群を保存します。
PointCloud::Ptr savePointCloudFile(PointCloud::ConstPtr point_cloud) {
  std::string file_path_string = filePathString(point_cloud);  // C++03なので、autoは使えません。

  pcl::io::savePCDFileBinary(file_path_string, *point_cloud);
  ROS_INFO_STREAM("PointCloud is saved to " << file_path_string);

  //  続けて別の関数を呼び出せるように、点群を返します。
  return PointCloud::Ptr(new PointCloud(*point_cloud));  // boost::shared_ptr<const X>からboost::shared_ptr<X>には変換できないので、新たにインスタンスを生成します。
}

// ROSの点群を、PCLの点群に変換します。
PointCloud::Ptr fromROSMsg(sensor_msgs::PointCloud2ConstPtr points) {
  PointCloud::Ptr converted_point_cloud(new PointCloud());
  pcl::fromROSMsg(*points, *converted_point_cloud);
    
  return converted_point_cloud;
}

// PCLの点群を、ROSの点群に変換します。
sensor_msgs::PointCloud2Ptr toROSMsg(PointCloud::ConstPtr point_cloud) {
  sensor_msgs::PointCloud2Ptr converted_points(new sensor_msgs::PointCloud2());
  pcl::toROSMsg(*point_cloud, *converted_points);
  
  return converted_points;
}

// 機能へのインターフェースを提供します。
sensor_msgs::PointCloud2Ptr six_point_two_eight::savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points) {
  return toROSMsg(savePointCloudFile(fromROSMsg(points)));
}

include/six_point_two_eight/make_world_models.h

ロボット制御側の（C++14の）コードは以下の通り。point_cloud_utilitiesがPCLを隠蔽してくれますから、こちらは簡単です。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <geometry_msgs/Twist.h>
#include <sensor_msgs/PointCloud2.h>

#include "six_point_two_eight/point_cloud_utilities.h"  // 追加。

namespace six_point_two_eight {
  inline auto createTwistMsg(double linear_x, double angular_z) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear_x;
    msg.angular.z = angular_z;
    
    return msg;
  }
  
  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Subscriber points_subscriber_;
    ros::Timer timer_1_;
    ros::Timer timer_2_;

  public:
    void onInit() {
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);

      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        "points", 10,
        [&](sensor_msgs::PointCloud2ConstPtr message) {
          savePointCloud2File(message);  // 点群を保存します。
        });

      timer_1_ = getNodeHandle().createTimer(
        ros::Duration(0.1),
        [&](const auto& message) {
          velocity_publisher_.publish(createTwistMsg(0.0, M_PI * 2 / 30));
        });
      
      timer_2_ = getNodeHandle().createTimer(
        ros::Duration(45.0),
        [&](const auto& message) {
          points_subscriber_.shutdown();
          timer_1_.stop();
          timer_2_.stop();
        });
    }
  };
}

CMakeLists.txt

C++03モードでのビルドのために、CMakeLists.txtの変更も必要です。今回のように特定のファイルにだけコンパイル・オプションを設定したい場合は、set_cource_files_properties()が使えます。以下のように、PCLに対応するために「-std=c++03 -Wno-deprecated」を設定してください。-std=c++03に加えて-Wno-deprecatedを指定しているのは、PCLのvisualizationモジュールが使用しているパッケージが推奨されていない古い機能を使っているためです。ビルドのたびに警告が表示されて、しかも我々のコード以外が原因なので対応不可能だと、ストレスがたまるだけですもんね。

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  nodelet
  nodelet_topic_tools
  pcl_ros
  roscpp
  sensor_msgs
)

# 略

catkin_package(
#  CATKIN_DEPENDS
)

add_compile_options(
  "-std=c++14"
)

# PCL1.7.2はC++14に対応していません。あと、visualizationがdeprecatedな機能を使用しています。コンパイル・オプションを指定して対応します。
set_source_files_properties(src/point_cloud_utilities.cpp  # 追加
  PROPERTIES COMPILE_FLAGS "-std=c++03 -Wno-deprecated"    # 追加
)                                                          # 追加

# 略

実行……してみたら、まだまだ機能が足りない

保存した点群は、pcl_viewerというコマンドで見ることができます。プログラムを実行し、Ctrl-cで止め、pcl_viewerで見てみましょう。

$ roslaunch six_point_two_eight turtlebot_driver.launch
$ catkin_make && rm -f /tmp/six_point_two_eight_*.pcd && roslaunch six_point_two_eight make_world_models.launch
# 処理が完了したら、Ctrl-cでプログラムを止める。

$ pcl_viewer /tmp/six_point_two_eight_*.pcd
# 対象が画面に映るよう、rキーを押してカメラ・アングルをリセットする。
# 使い方は、hキーを押すとターミナルに表示される。
# マウスでグリグリしてみる。
# 飽きたら、qキーを押して終了させる。

ターミナル#2の1行目。コマンドを&&でつなぐと、「前のコマンドが成功したら後ろのコマンドを実行する」という指示になります。上のコマンドは、ビルドして、前の実行で生成された点群のファイルを消して、make_world_modelsを実行する（ただし、途中で失敗したら処理を止めろ）の意味になるわけ。この形で起動すれば、ビルド忘れや過去の実行結果の混入を避けられます。

で、その肝心のpcl_viewerで実行結果を見ると、なんだか意味不明です。

make_world_models（その1）

このような結果になった理由は、深度センサーからの相対座標の点群を、同じ座標系に重ねちゃったから。以下のように1枚だけを表示すれば、普通に点群を見られます。

$ pcl_viewer `ls -s /tmp/six_point_two_eight_*.pcd | head -n 1`

TurtleBotの姿勢に合わせて、適切に点群を移動/回転させてあげる処理が必要なようです。

オドメトリー

少し話は変わりますけど、車やバイクにはオドメーターというのがあって、累計走行距離が分かるようになっています。あと、スピード・メーターには速度が表示されていますけど、速度は移動距離÷時間で、やっぱり距離が関係しています。この距離は、どうやって測っているのでしょうか？

車やバイクによっていくつかバリエーションはあるのですけれど、トランスミッション等の回転速度からタイヤの回転数を推測し、カタログ上のタイヤの外周のサイズをかけて移動距離を計算しています。同じことはロボットでも可能です。モーター等のセンサーから、タイヤが何回転したかは推測できます。タイヤのサイズも設計書から分かる。だから、どれだけ移動したかが計算できるはず。TurtleBotのように左右にタイヤが付いている移動方式だと、左右のタイヤの回転数の差から、現在どちらを向いているのかも計算できるはず。

このような、モーター等のセンサーを使用して推定した位置情報を、オドメトリーと呼びます（車のオドメーターとは違って、距離ではなく位置と向きが分かります）。TurtleBotでは、/odomトピックでオドメトリーを取得できます。

さっそく、試してみましょう。

$ roslaunch six_point_two_eight turtlebot_driver.launch
$ roslaunch turtlebot_teleop keyboard_teleop.launch
$ rostopic echo /odom
header: 
  seq: 4443
  stamp: 
    secs: 1457333175
    nsecs: 603418136
  frame_id: odom
child_frame_id: base_footprint
pose: 
  pose: 
    position: 
      x: 0.20837154614
      y: -0.108868545114
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.305446027598
      w: 0.952209390956
  covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0,
 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
 0.0, 0.0, 0.0]
---

ターミナル#2で適当にキーを押してTurtleBotを移動させると、ターミナル#3のpose.poseのpositionとorientationの値が変わっていきます。ROSの座標系はX軸が前でY軸が左でZ軸が上、回転は上から（Z軸のプラス側からマイナス側を）見て反時計回りという知識を活用すると、/odomは起動時の状態からの相対座標/回転となっていることが分かります。

PCLを使用して、座標変換してフィルタリングしてダウンサンプリングする

でも、よっしゃー/odomトピックからオドメトリーを取得して点群を移動/回転させてやるぜーってのは、ごめんなさい、あまりうまくいきません。以下、その理由と対策をやって、続けて点群のダウンサンプリングとフィルタリングをやります。

TF

ROSのメッセージは大抵headerを含んでいて、headerのstampはそのメッセージが作成された時刻になっています。時刻が異なる複数のメッセージを使う処理、たとえば5秒前に取得したオドメトリーで3秒前に取得した点群を変換するような処理は、無意味です。だって、オドメトリーを受け取った後に移動したかもしれないんですから。

でも、もし1秒前のオドメトリーも持っているなら、どうでしょうか？　等速度運動をしていると仮定すれば、3秒前の位置は、5秒前の位置と1秒前の位置の中間だと推定することができます。ただし、そのためには過去の状態を複数保持して、間を補完する計算処理が必要です。メモリを食いつぶすわけにはいきませんから、ある程度古くなったメッセージを捨てる処理も必要でしょう。でも、これを/odomトピックのサブスクライバーの中で作るのは大変です。深度センサーがロボット・アームの先に付いているような場合は、さらに面倒になります。肩のサーボ・モーターがこの角度で肘のサーボ・モーターがあの角度だから、深度センサーはこの位置でこんな向きのはずだという計算は、とてもとても面倒くさそうですから。

だから、ROSのパッケージのTFを使いましょう。TFは、上に挙げた面倒な計算処理をすべて実施してくれます。

TFの解説と使い方は、「TF,TF2　完全理解」にわかりやすくまとまっていて、すべての日本人ROSプログラマーはこのスライドは絶対に見ておくべきなんですけれど、本稿の範囲だとpcl_rosパッケージのPCLとTFを取り持ってくれる機能の範囲の知識で大丈夫かも……。この機能を使って、さくっとプログラミングしちゃいましょう。

package.xmlとCMakeLists.txt

TFを使用するために、package.xmlとCMakeLists.txtに、tfパッケージとtf_conversionsパッケージを追加してください。

include/six_point_two_eight/point_cloud_utilities.h

関数宣言の段階で、pcl_rosの知識だけでは駄目で、TFの知識が必要でした……。まずは、TFのフレームの話をさせてください。というのも、座標変換前の点群を調べてみたら、深度センサーの位置を原点にして、前がZ軸で右がX軸、下がY軸になっていたんですよ。

深度センサーの座標系

これはROSの座標系とは反転が必要なレベル（いわゆる右手系と左手系のレベル）で異なっているわけで、オドメトリーがどうのこうのの前に、まずはROSの座標系に変換しなければなりません。幸いなことにこの変換は、四元数と呼ばれる複素数を拡張した数体系を使えば、簡単に実施できるみたいです（x = -0.5、y = 0.5、z = -0.5、w = 0.5の四元数で変換できた）。次に問題になるのは、TurtleBotの原点と深度センサーの原点が異なっていること。これは、ベクトルの引き算で変換できます。でも、こんな変換をいちいちプログラムしていくのは大変すぎます。なので、ロボットの構成要素と構成要素を、構成要素間の座標系の変換でつなぐことにしましょう。camera_rgb_optical_frame→（変換）→camera_rgb_frame→（変換）→base_linkみたいな感じ。この方式だとプログラム作成ではなくてデータ作成になりますので、とても楽です。

ただ、base_linkには深度センサー以外の構成要素もつながっていて、例えばwheel_right_link（右の車輪）なんてのもあります。この点も考慮すれば、構成要素の関係はリストではなくて木構造が適切かなぁと。で、さらに考えてみると、オドメトリーとロボット本体の関係も、この木構造で表現できそうです。変換が毎回同じか、移動にともなって変わっていくかの違いだけですもんね。

というわけで、TFではこの構成要素をフレームと呼んで木構造にして、座標変換でつなげます。TureltBotのドライバーを動かした状態だと、odomフレーム（名前は一緒ですしどちらもオドメトリーを表現していますけど、odomトピックとは別物）が根で、camera_rgb_optical_frameフレームやwheel_right_linkフレームを葉に持つ木構造が作られています。

TFを使用して座標変換する際には、このフレームを指示します。どのフレームの座標系からどのフレームの座標系に変換するかを、指定するわけ。今回の場合、点群はcamera_rgb_optical_frameの座標系であることが分かっている（というか、ROSではheader.frame_idにメッセージのフレームが設定されている）ので、変換先のフレームだけ指定すればよいでしょう。なので、二番目の引数としてtarget_frameを渡しています。

// 略

namespace six_point_two_eight {
  sensor_msgs::PointCloud2Ptr savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr transformPointCloud2(  // 追加。
    sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double x_adjust = 0.0);  // 追加。
}

// 略

残りの引数のx_adjustは、プログラムを作成して試してみたら奥行きに少しズレがあったから追加してみました。こんな引数があってはならないのですけれど、今回だけはご容赦ください。

src/point_cloud_utilities.cpp

関数の実装は、なんだかとても長いコードになってしまいました……。その理由は、1つのグローバル変数と3つの処理が必要だったから。以下、それぞれについて述べます。

グローバル変数。transform_listener_のところ。TFが機能するには、流れてくる姿勢情報（親フレームからどのように座標変換したか）を蓄えておかなければなりません。その役割を担っているのがtf::TransformListenerです。常に待機しておかなければならないわけで、だから一般にグローバル変数にします。

処理その1。奥行きの補正です。補正しないバージョンのプログラムでデータを調べてみたら、なんだか少しだけずれていたので追加しました。C++14なら拡張forが使えるのですけど、今はC++03なのでBOOST_FOREACHを使用しました。

処理その2。TFは5秒前の姿勢情報と3秒前の姿勢情報から、4秒前の姿勢を推測できます。しかし、3秒前の姿勢情報の後の姿勢情報を受け取っていない場合は、2秒前の姿勢を推測できません。TF関連でよくある失敗として、「現在の姿勢を教えてくれ」と聞いて毎回もれなくエラーになるというのがあるらしい。TFはデータの間を補間する機能しかなくて、未来のデータなんて存在しないのだから、エラーになって当たり前なんですな。本稿のコードの場合は、点群の時刻が新しすぎて、点群の時刻以降のロボットの姿勢情報をまだ受け取っていないのでエラーになる危険性があります。それでは困るので、点群の時刻よりもあとの姿勢情報を受信するまで待つように指示しています。これがwaitForTransform()メンバー関数を呼び出しているところ。で、「待っても姿勢情報が来なかった」場合と、「姿勢情報を貯めこむTransformListenerに最初の姿勢情報が届く前の点群がとれちゃったので待ってもどうにもならない」場合は、エラーになります。だから、if文で戻り値をチェックしてエラー・ハンドリングしました。

waitForTransform

処理その3。やっと本番の座標変換。ここでは普通は失敗しないのですけど、コードに統一性を持たせるためにエラー・ハンドリングしてみました。

#include <boost/foreach.hpp>  // 追加。
#include <pcl/filters/filter.h>  // 追加。
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>  // 追加。
#include <tf/transform_listener.h>  // 追加。

#include "six_point_two_eight/point_cloud_utilities.h"

typedef pcl::PointXYZRGB Point;
typedef pcl::PointCloud<Point> PointCloud;

static tf::TransformListener transform_listener_;  // 追加。

// 略

// 点群をtarget_frameの座標系に変換します。その際に、x_adjustで指定された値で、奥行きも補正します。
PointCloud::Ptr transformPointCloud(
  PointCloud::ConstPtr point_cloud, const std::string& target_frame, double x_adjust) 
{
  // 奥行きがずれているようだったので、補正します。
  PointCloud::Ptr x_adjusted_point_cloud(new PointCloud(*point_cloud));
  BOOST_FOREACH(Point& point, *x_adjusted_point_cloud) {
    point.z += x_adjust;  // 深度センサーの生データは、実は座標系が異なります。右がX軸で下がY軸、前がZ軸なんです。
  }

  // TFは姿勢情報と姿勢情報の間しか補完できない（最新の姿勢情報よりも後の時刻で問い合わせるとエラーになる）ので、点群よりも後の姿勢情報が来るまで待ちます。
  std_msgs::Header header = pcl_conversions::fromPCL(point_cloud->header);
  if (!transform_listener_.waitForTransform(target_frame, header.frame_id, header.stamp, ros::Duration(1.0))) {
    ROS_WARN_STREAM("Can't wait...");
    throw std::exception();
  }
  
  // TFを使用して、座標変換します。
  PointCloud::Ptr transformed_point_cloud(new PointCloud());
  if (!pcl_ros::transformPointCloud(
    target_frame, *x_adjusted_point_cloud, *transformed_point_cloud, transform_listener_)) 
  {
    ROS_WARN("Transform failed...");
    throw std::exception();
  }

  // 座標変換した点群を返します。
  return transformed_point_cloud;
}

// 略

sensor_msgs::PointCloud2Ptr six_point_two_eight::transformPointCloud2(
  sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double x_adjust) 
{
  return toROSMsg(transformPointCloud(fromROSMsg(points), target_frame, x_adjust));
}

include/six_point_two_eight/make_world_models.h

C++14のロボット制御側は、いつもの通りに簡単です。point_cloud_utilities.cppのエラー・ハンドリングで例外を使用しましたので、try/catchを追加しました。

// 略

namespace six_point_two_eight {
  // 略
  
  class MakeWorldModels : public nodelet::Nodelet {
    // 略
    
    void onInit() {
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);

      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        "points", 10,
        [&](sensor_msgs::PointCloud2ConstPtr message) {
          try {                                 // 追加。
            savePointCloud2File(
              transformPointCloud2(             // 追加。
                message,
                "odom",                         // 追加。
                -0.05));                        // 追加。
            
          } catch (const std::exception& ex) {  // 追加。
            ROS_WARN_STREAM(ex.what());         // 追加。
          }                                     // 追加。
        });

      timer_1_ = getNodeHandle().createTimer(
        ros::Duration(0.1),
        [&](const auto& message) {
          velocity_publisher_.publish(createTwistMsg(0.0, M_PI * 2 / 30));
        });
      
      timer_2_ = getNodeHandle().createTimer(
        ros::Duration(45.0),
        [&](const auto& message) {
          points_subscriber_.shutdown();
          timer_1_.stop();
          timer_2_.stop();
        });
    }
  };
}

試してみた

作成したプログラムを、動かしてみました。結果は以下の通り。正しく座標変換できているけど、まだダメ。

make_world_models（その2）

だって、遠くにある点の精度が悪すぎますもんね。センサーの有効範囲外なので、遠くにある点の精度向上は望めません。近くの点だけになるようにフィルタリングする処理が必要みたいです。

フィルタリング

C++14なら、boostと組み合わせてラムダ式を引数に与えて1行で簡単にフィルタリングできる……のですけど、C++03で書いている今は駄目。PCLに点群の点をフィルタリングする機能がありましたので、それで我慢しましょう。

PCLのフィルタリングのチュートリアルの中の、Removing outliers using a Conditional or RadiusOutlier removalを読むと、フィールドと定義済み演算子と比較対象の値を渡すという簡単なフィルターが使われています。このやり方だと直方体の切り抜きしかできなくて、直方体だとセンサーからの距離が一定にならないので駄目。他によいのがあるかもしれないとリファレンスを見ても、うん、見つかりませんね……。というわけで、pcl::ConditionRemovalクラスと独自作成のpcl::Conditionのサブ・クラスで実装しました。

include/six_point_two_eight/point_cloud_utilities.h

センサーからの距離が一定の値以下の点を集めたいわけですから、球の中にある点だけを抽出すればよい。球を表現するには中心と半径が必要。というわけで、点群と中心と半径を引数に撮るgetShperePointCloud()を宣言しました。

// 略

#include <geometry_msgs/Point.h>  // 追加。
#include <sensor_msgs/PointCloud2.h>

namespace six_point_two_eight {
  sensor_msgs::PointCloud2Ptr getSpherePointCloud2(  // 追加。
    sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Point& center, double radius);  // 追加。
  sensor_msgs::PointCloud2Ptr savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr transformPointCloud2(
    sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double z_adjust = 0.0);
}

// 略

src/point_cloud_utilities.cpp

フィルタリング条件は、pcl::ConditionBaseを継承して作成するみたいです（ラムダ式なんて贅沢は言いません。せめてファンクターにして欲しかった……）。getSpherePointCloud()関数は、作成したクラスを使うだけです。

#include <boost/foreach.hpp>
#include <pcl/filters/conditional_removal.h>  // 追加。#include <pcl/filters/filter.h>は削除。
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <tf/transform_listener.h>

// 略

// 目的に合致するpcl::Conditionがなかったので、新規に作成しました。
template <typename PointT>
class SphereCondition : public pcl::ConditionBase<PointT> {
private:
  Eigen::Vector3f center_;
  double radius_;

public:
  SphereCondition(const Eigen::Vector3f& center, double radius)
    : center_(center), radius_(radius)
  {
    ;
  }

  virtual bool evaluate(const PointT& point) const {
    // 中心からの距離がradius_以下ならtrueを返す。
    return (point.getVector3fMap() - center_).norm() <= radius_;  // Eigenでは、ベクトル演算ができます。norm()は、原点からの距離を返します。
  }
};

// pcl::ConditionalRemovalを使用して、球の内側の点のみを抽出します。
PointCloud::Ptr getSpherePointCloud(PointCloud::ConstPtr point_cloud, const Eigen::Vector3f& center, double radius) {
  // コンディションを生成。
  SphereCondition<Point>::Ptr condition(new SphereCondition<Point>(center, radius));

  // pcl::ConditionRemovalを生成。
  pcl::ConditionalRemoval<Point> conditional_removal(condition);

  // 入力用の点群を設定。
  conditional_removal.setInputCloud(point_cloud);
  
  // 出力用の点群を設定。
  PointCloud::Ptr sphere_point_cloud(new PointCloud());
  conditional_removal.filter(*sphere_point_cloud);

  // 球の内側の点のみの点群をリターン。
  return sphere_point_cloud;
}

// 略

sensor_msgs::PointCloud2Ptr six_point_two_eight::getSpherePointCloud2(
  sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Point& center, double radius) 
{
  return toROSMsg(getSpherePointCloud(fromROSMsg(points), Eigen::Vector3f(center.x, center.y, center.z), radius));
}

サブスクライバーとラムダ式とスマート・ポインター

作成したgetSpherePointCloud2()関数を呼びだすには点群とオドメトリーの両方のデータが必要なのですけど、でも、その2つは異なるラムダ式の引数です。さて、どうしましょうか？

ROSのサンプル・プログラムでは、メンバー変数に格納することで対応しているようです。以下みたいな感じ。

class Foo : public nodelet::Nodelet {
private:
  ros::Subscriber odom_subscriber_;
  ros::Subscriber points_subscriber_;

  // オドメトリーを共有するためのメンバー変数。
  nav_msgs::Odometry odom_;
  
  void odomCallback(nav_msgs::OdometryConstPtr message) {
    odom_ = *message;
  }
  
  void pointsCallback(sensor_msgs::PointCloud2ConstPtr message) {
    // messageとodom_を使う処理。
  }

public:
  void onInit() {
    odom_subscriber_   = getNodeHandle().subscribe("odom",   1, &Foo::odomCallback,   this);
    points_subscriber_ = getNodeHandle().subscribe("points", 1, &Foo::pointsCallback, this);
  }
};

でも、この方式って、状態遷移があると条件分岐が増えてしまうので、できれば避けたい。例えば、オドメトリーを使用して目標地点まで移動したあとに深度センサーのデータを取得する処理だと、以下のようにif文だらけになってしまいます。

class Foo : public nodelet::Nodelet {
private:
  ros::Subscriber odom_subscriber_;
  ros::Subscriber points_subscriber_;

  // オドメトリーを共有するためのメンバー変数。
  nav_msgs::Odometry odom_;
  
  // 移動先を示すメンバー変数。
  geometry_msgs::Pose goal_pose_;

  // 移動が完了したかどうかを示すメンバー変数。
  bool is_reached_goal_;
  
  void odomCallback(nav_msgs::OdometryConstPtr message) {
    if (!is_reached_goal_) {
      if (!isReached(message, goal_pose_)) {
        // goal_pose_に近づくように、ロボットを移動させる。
        
      } else {
        is_reached_goal_ = true;
        odom_ = *message;
      }
    }
  }
  
  void pointsCallback(sensor_msgs::PointCloud2ConstPtr message) {
    if (is_reached_goal_) {
      // messageとodom_を使う処理。
    }
  }

public:
  void onInit() {
    odom_subscriber_   = getNodeHandle().subscribe("odom",   1, &Foo::odomCallback,   this);
    points_subscriber_ = getNodeHandle().subscribe("points", 1, &Foo::pointsCallback, this);
    
    goal_pose_ = // 目標地点。
    is_reached_goal_ = false;
  }
};

この種の問題は、デザイン・パターンのStateパターンで解決できます。でも、Stateパターンって、クラスがいっぱい必要で書くのが面倒なんですよね……。だから、C++14な我々はラムダ式で解決することにしましょう。

class Foo : public nodelet::Nodelet {
private:
  ros::Subscriber odom_subscriber_;
  ros::Subscriber points_subscriber_;

  void takePhoto(const nav_msgs::Odometry& odom) {
    points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
      "points", 1,
      [&, odom](sensor_msgs::PointCloud2ConstPtr message) {
        // messageとodomを使う処理。
      });
  }

  void moveTo(const geometry_msgs::Pose& goal_pose) {
    odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
      "odom", 1,
      [&, goal_pose](nav_msgs::OdometryConstPtr message) {
        if (!isReached(message, goal_pose)) {
          // goal_poseに近づくように、ロボットを移動させる。
          
        } else {
          this->takePhoto(*message);
          odom_subscriber_.shutdown();
        }
      });
  }

public:
  void onInit() {
    moveTo(/* 目標地点 */);
  }
};

ほら、簡単になったでしょ？

ついでですから、一番最初に書いたオドメトリーを共有する場合のコードもラムダ式版にしてみましょう。C++はラムダ式が参照していても関数を抜けると自動変数を破棄してしまうので、スマート・ポインターを使って寿命を管理しています。

class Foo : public nodelet::Nodelet {
private:
  ros::Subscriber odom_subscriber_;
  ros::Subscriber points_subscriber_;

public:
  void onInit() {
    // 共有したい情報を格納するstruct。
    struct State {
      nav_msgs::Odometry odom;
    };
    
    // 寿命管理のために、スマート・ポインターを使用します。
    std::shared_ptr<State> state(new State());
  
    odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
      "odom", 1,
      [&, state](nav_msgs::OdometryConstPtr message) {
        state->odom = *message;
      });
      
    points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
      "points", 1,
      [&, state](sensor_msgs::PointCloud2ConstPtr message) {
        // messageとstate->odomを使う処理。
      });
  }
};

メンバー変数版とは違って、状態が増えてもメンバー関数を増やすことで対応できます。Stateパターンにおけるクラス相当のモノを、ラムダ式なら関数で作れるんです。

以上、方針が決まりましたから、プログラミングに入りましょう。

package.xmlとCMakeLists.txt

nav_msgs/Odometryメッセージを使いますので、package.xmlとCMakeLists.txtに、nav_msgsパッケージを追加してください。

include/six_point_two_eight/make_world_models.h

ロボット制御側のコードはこんな感じ。今回の処理はすべてをodomのサブスクライバーのラムダ式の中に書く方式が適切なのですけれど、この後に本稿で作る処理にはスマート・ポインターでステート共有できる処理はなさそうで、それだとスマート・ポインター方式を使うサンプルがどこにもなくなってしまう……。ので、敢えてスマート・ポインター方式にしてみました。

あと、深度センサーのカメラは起動直後は安定しないみたいなので、3秒待つ処理も付け加えてみました。ラムダ式を使うと、いろいろ簡単で捗りますな。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>  // 追加。
#include <sensor_msgs/PointCloud2.h>

#include "six_point_two_eight/point_cloud_utilities.h"

namespace six_point_two_eight {
  // 略。

  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Subscriber odom_subscriber_;  // 追加。
    ros::Subscriber points_subscriber_;
    ros::Timer timer_1_;
    ros::Timer timer_2_;

  public:
    void onInit() {
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);

      // ラムダ式間で共有するステート。C++のラムダ式は寿命管理をしないので、スマート・ポインターで寿命管理します。
      struct State {
        geometry_msgs::Point position;
      };
      std::shared_ptr<State> state(new State());

      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&, state](const auto& message) {
          state->position = message->pose.pose.position;  // 現在位置をステートに保存します。
        });

      auto starting_time = ros::Time::now();  // 開始時刻。
      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        "points", 10,
        [&, state, starting_time](const auto& message) {  // キャプチャーにstateとstarting_timeを追加。
          // 起動直後は動作が安定しないので、3秒間は何もしません。
          if (message->header.stamp < starting_time + ros::Duration(3.0)) {
            return;
          }
          
          try {
            savePointCloud2File(
              getSpherePointCloud2(  // 追加。
                transformPointCloud2(
                  message,
                  "odom",
                  -0.05),
                state->position,     // TurtleBotの位置から
                1.5));               // 1.5m以内の点群を取得します。
            
          } catch (const std::exception& ex) {
            ROS_WARN_STREAM(ex.what());
          }
        });

      // 略
    }
  };
}

試してみた

試してみると、以下のキャプチャのような感じ。うまくフィルタリングできています。でも、点の密度が濃すぎて少し重たく感じます。

make_world_models（その3）

PCLのダウンサンプリングの機能を活用して、点の数を減らしてみましょう。

ダウンサンプリング

PCLにはいろいろなダウンサンプリングの機能がありますけど、今回使用するのはpcl::VoxelGridクラスです。指定した密度になるように点を間引いてくれます。

include/six_point_two_eight/point_cloud_utilities.h

pcl::VoxelGridクラスは「leaf_sizeで指定した空間あたりの点を1つ」にするものなので、2番目の引数としてleaf_sizeを追加しました。

// 略

namespace six_point_two_eight {
  sensor_msgs::PointCloud2Ptr downsamplePointCloud2(sensor_msgs::PointCloud2ConstPtr points, double leaf_size);  // 追加。
  sensor_msgs::PointCloud2Ptr getSpherePointCloud2(sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Point& center, double radius);
  sensor_msgs::PointCloud2Ptr savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr transformPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double z_adjust = 0.0);
}

// 略

src/point_cloud_utilities.cpp

PCLの少し面倒なコーディング・スタイルにも慣れてきました。ダウンサンプリングの機能を一から作るのに比べれば圧倒的に楽なのですから、このくらいのコードは余裕です。

#include <boost/foreach.hpp>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/voxel_grid.h>  // 追加。
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <tf/transform_listener.h>

// 略

// pcl::VoxelGridを使用して、ダウンサンプリングします。
PointCloud::Ptr downsamplePointCloud(PointCloud::ConstPtr point_cloud, double leaf_size) {
  pcl::VoxelGrid<Point> voxel_grid;
  voxel_grid.setInputCloud(point_cloud);
  voxel_grid.setLeafSize(leaf_size, leaf_size, leaf_size);

  PointCloud::Ptr downsampled_point_cloud(new PointCloud());
  voxel_grid.filter(*downsampled_point_cloud);

  return downsampled_point_cloud;
}

// 略

sensor_msgs::PointCloud2Ptr six_point_two_eight::downsamplePointCloud2(sensor_msgs::PointCloud2ConstPtr points, double leaf_size) {
  return toROSMsg(downsamplePointCloud(fromROSMsg(points), leaf_size));
}

// 略

include/six_point_two_eight/make_world_models.h

ロボットを制御する側は、例によって簡単です。ダウンサンプリングした後に他の処理を実行するほうが点の数が少なくて実行効率がよくなりますから、一番内側で呼び出しました。

// 略

namespace six_point_two_eight {
  // 略
  
  class MakeWorldModels : public nodelet::Nodelet {
  // 略

  public:
    void onInit() {
      // 略
      
      auto starting_time = ros::Time::now();
      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        "points", 10,
        [&, state, starting_time](const auto& message) {
          if (message->header.stamp < starting_time + ros::Duration(3.0)) {
            return;
          }
          
          try {
            savePointCloud2File(
              getSpherePointCloud2(
                transformPointCloud2(
                  downsamplePointCloud2(  // 追加。
                    message,
                    0.005),               // 0.005m（5mm）あたり、点が1つ。
                  "odom",
                  -0.05),
                state->position,
                1.5));
            
          } catch (const std::exception& ex) {
            ROS_WARN_STREAM(ex.what());
          }
        });

      // 略
    }
  };
}

完成！

ついにプログラム完成です。点の数が減りました。ちなみに、下のキャプチャの左側は机のキャビネットで、右側は椅子です。

make_world_models（その4）

はい、お疲れさまでした。実は作成した点群を拡大するとズレがあって本章の最初に載せたキャプチャのレベルに達していないのですけど、今は気がつかなかったことにして8、対象物の周囲360°からの点群の作成に入りましょう。

対象物の周囲360°からの3D点群を作成する

もっとロボットをダイナミックに動かしたい。だいたい、その場でグルッと回る方式だと、ドーナッツみたいなデータになって真ん中に穴が開いてしまうのでちょっと寂しい。というわけで、対象物の周囲を回りながら深度センサーで点群を採取して、3Dモデルを作るプログラムをやりましょう。以下の画像のような感じ。私が通勤で使っている、サラリーマンとは思えないカバンの3D点群です。

対象物の周囲360°からの3D点群

とりあえず、目的地まで移動させてみる

千里の道も一歩から。とりあえず、狙った場所まで移動するプログラムを書いてみましょう9。

共有する関数はどうしよう？

移動するためにはgeometry_msgs/Twistメッセージを作成しなければならず、その作成用のcreateTwistMsg()関数を前の章ではmake_world_models.hに書きました。このままでは、同じ関数を2回プログラミングすることになってしまいますから、共通の別ファイルに移しましょう。include/six_point_two_eight/utilities.hを作成し、createTwistMsg()関数を移します。

include/six_point_two_eight/utilities.h

単純にコピー＆ペーストしただけです。

#pragma once

#include <geometry_msgs/Twist.h>

namespace six_point_two_eight {
  inline auto createTwistMsg(double linear_x, double angular_z) {
    geometry_msgs::Twist msg;
    msg.linear.x = linear_x;
    msg.angular.z = angular_z;
    
    return msg;
  }
}

include/six_point_two_eight/make_world_models.h

ただ削除して、#includeを調節しただけです。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>

#include "six_point_two_eight/utilities.h"
#include "six_point_two_eight/point_cloud_utilities.h"

namespace six_point_two_eight {
  class MakeWorldModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Subscriber odom_subscriber_;
    ros::Subscriber points_subscriber_;
    ros::Timer timer_1_;
    ros::Timer timer_2_;

    // 略

9時の方向に0.5m進んでみる

移動プログラムでは、/odomトピックの値に合わせた、適切なgeometry_msgs/Twistメッセージを作成していけばよいはず。目的地までの距離が遠かったり角度のズレが大きい場合は少し速く動いたり回転したりして、近くなったらゆっくりにする。目的地と/odomが一致したら移動終了。ただし、一致と言ってもピッタリにはできないだろうから、少し誤差を認めよう。目的地は、三角関数で計算できるはずだな。

と、この程度を考えたところで、プログラムを組んでみました。結果を先に述べておくと、プログラムが複雑になってしまうので、もっと工夫しないと駄目な感じです。

include/six_point_two_eight/utilities.h

位置を表現するためのgeometry_msgs/Pointメッセージ作成関数と、角度を正規化する関数を追加しました。

#pragma once

#include <cmath>

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>

namespace six_point_two_eight {
  inline auto createTwistMsg(double linear_x, double angular_z) {
    // 略
  }

  // geometry_msgs::Pointを作成します。
  inline auto createPointMsg(double x, double y) {
    geometry_msgs::Point msg;
    msg.x = x;
    msg.y = y;

    return msg;
  }

  // -π〜πになるように、角度を正規化します。
  inline auto normalizeAngle(double angle) {
    return std::atan2(std::sin(angle), std::cos(angle));  // -1000とかの場合に備えるための再帰やループは面倒だったので、ライブラリ任せで実現しました。
  }
}

include/six_point_two_eight/make_target_model.h

新規に作成しました。速度調節用のtunedLinearX()メンバー関数とtunedAngularZ()メンバー関数、移動をするためのmoveTo()メンバー関数を2つ（片方は作業用）、あとはいつものonInit()メンバー関数を作成しました。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>

#include <nav_msgs/Odometry.h>

#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class MakeTargetModels : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Subscriber odom_subscriber_;

    // 適切な前進速度を返します。
    auto tunedLinearX(double distance, double angle) const {
      if (distance < 0.05) {  // 目的地への距離が0.05mになったら、前進をやめる。
        return 0.0;
      }
      if (std::fabs(angle) > M_PI / 180 * 45) {  // 目的地への角度が±45°を超える場合は、前進しない（で回転する）。
        return 0.0;
      }
      
      return std::min(std::fabs(distance) * 0.5, 0.1);  // 0.1m/secか距離*0.5/secの小さい方を返す。
    }
    
    // 適切な回転速度を返します。
    auto tunedAngularZ(double angle) const {
      if (std::fabs(angle) < M_PI / 180 * 1) {  // 目的地への角度が±1°になったら、回転をやめる。
        return 0.0;
      }
      
      return std::min(std::fabs(angle) * 0.5, M_PI / 180 * 10) * (angle / std::fabs(angle));  // 10°/secか角度*0.5/secの小さい方を返す。マイナスの場合があることに注意。
    }

    // 目的地まで移動します（作業用）。
    auto moveTo(const geometry_msgs::Point& goal_position, nav_msgs::OdometryConstPtr odometry) {
      // 目的地と現在位置の差を計算します。
      auto difference = createPointMsg(
        goal_position.x - odometry->pose.pose.position.x, goal_position.y - odometry->pose.pose.position.y);

      // 距離と角度を計算します。単純な計算なので、TFやEigenを使わずに自前でやります。
      auto distance = std::sqrt(std::pow(difference.x, 2) + std::pow(difference.y, 2));
      auto angle = 
        normalizeAngle(std::atan2(difference.y, difference.x) - tf::getYaw(odometry->pose.pose.orientation));

      // 移動速度を計算します。
      auto linear_x = this->tunedLinearX(distance, angle);
      auto angular_z = this->tunedAngularZ(angle);

      // ロボットを移動させます。
      velocity_publisher_.publish(createTwistMsg(linear_x, angular_z));

      // 移動したかどうかを返します。
      return linear_x != 0.0 || angular_z != 0.0;
    }

    // 目的地まで移動します。
    auto moveTo(geometry_msgs::Point goal_position) {
      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&, goal_position](const auto& message) {
          if (this->moveTo(goal_position, message)) {  // 移動した場合は、それで今回分の処理は終了なので、
            return;                                    // そのままリターンします。
          }
          
          ROS_INFO_STREAM("Reached!");  // 移動しない＝到着なので、ログ出力して、
          odom_subscriber_.shutdown();  // サブスクライバーをシャットダウンします。
        });
    }
    
  public:
    void onInit() {
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);

      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&](const auto& message) {
          auto position = message->pose.pose.position;
          auto yaw = tf::getYaw(message->pose.pose.orientation);  // TFの関数を使用して、ヨーを取得します。

          this->moveTo(
            createPointMsg(
              position.x + std::cos(yaw + M_PI / 2) * 0.5, position.y + std::sin(yaw + M_PI / 2) * 0.5));  // 9時の方向に0.5m移動。
        });
    }
  };
}

このプログラムを拡張して、対象物の周囲360°からの点群を作らせるためには、移動の後に対象物が正面に来るように回転し、回転が終わったら深度センサーから点群をとり、また移動に戻るという処理を繰り返さなければなりません。コード中のROS_INFO_STREAM("Reached!")している部分で次の処理のメンバー関数を呼ばなければならないわけで、つまり関数の中から関数を呼び出して延々と処理がつながることになって、それっていわゆるJavaScriptのコールバック地獄じゃん……。なんとかしなければなりません。

src/six_point_two_eight.cpp

でも、なんとかする前に、まずはプログラムを動かしてテストしないとね。そのためにはコンパイルしなければなりませんので、src/six_point_two_eight.cppを修正しました。

#include <pluginlib/class_list_macros.h>

#include "six_point_two_eight/make_target_models.h"  // 追加。
#include "six_point_two_eight/make_world_models.h"
#include "six_point_two_eight/point_cloud_2_throttle.h"

PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeTargetModels, nodelet::Nodelet)  // 追加。
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeWorldModels, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::PointCloud2Throttle, nodelet::Nodelet)

six_point_two_eight.xml

続けて、プログラムを起動できるようにsix_point_two_eight.xmlを修正しました。

<library path="lib/libsix_point_two_eight">
  <class
    name="six_point_two_eight/make_target_models"
    type="six_point_two_eight::MakeTargetModels"
    base_class_type="nodelet::Nodelet"/>  <!-- 追加 -->
  <class name="six_point_two_eight/make_world_models" type="six_point_two_eight::MakeWorldModels" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/point_cloud_2_throttle" type="six_point_two_eight::PointCloud2Throttle" base_class_type="nodelet::Nodelet"/>
</library>

launch/make_target_models.launch

起動を簡単にするために、launchファイルも作成しました。

<launch>
  <arg name="odom" default="odom"/>
  <arg name="velocity" default="mobile_base/commands/velocity"/>
  
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_target_models" args="load six_point_two_eight/make_target_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="odom" to="$(arg odom)"/>
    <remap from="velocity" to="$(arg velocity)"/>
  </node>
</launch>

launch/make_target_models_in_gazebo.launch

シミュレーター用のlaunchファイルも。

<launch>
  <include file="$(find six_point_two_eight)/launch/make_target_models.launch"/>
</launch>

actionlib

さて、先ほどのコールバック地獄まっしぐらなプログラムを、ここで何とかしましょう。サービスを使えば解決できそうな気がしますけど、サービスは柔軟性が低いのでできれば使いたくない。幸いなことに、ROSにはactionlibという、中断することが可能な、長い時間をかけて実行するサーバー・プログラムを作成するためのパッケージがあって、これを使うとコールバック地獄をキレイに解決できるんです。

地図作成と自律移動のパッケージを動かして、ROSの可能性を感じてみる

actionlibはとりあえず脇に置いて、一時的に本稿から離れてBuild a map with SLAMとAutonomously navigate in a known mapを開き、指示に従ってみてください（もっと細かい情報が必要なら、Learn TurtleBot and ROSも参照してください）。

これらのページの指示に従ってコマンドを入力していくと、TurtleBotに地図を作成させ、作成した地図を活用して自律走行させることができます。しかも、必要な作業はパッケージの起動だけ。プログラミングは無しです。

本稿ではROSを通信ライブラリとして扱っていますけど、実は、ROSの凄さは2,000を超えるROS対応パッケージ群にあります。しかも、それらのパッケージはメッセージとトピックという抽象的なレベルでつながりますから、様々な異なるロボットで使用可能です。TurtleBot専用に見える先ほどのページでも、実は汎用のパッケージを使用しています。ページ中の呼び出しに使用しているturtlebot_navigationパッケージは、汎用のgmappingパッケージ10とnavigationパッケージを呼び出すためのlaunchファイルが詰まったパッケージ11なんです。

時間があるときに、ROS.orgのBrowse Softwareを見てみてください。私のお気に入りはhector_slamとnavigation。これらのパッケージの紹介ビデオを見れば、自律移動するロボットを簡単に作れることが分かって、ROSがいかに便利かを感じ取れるはずです。あと、MoveIt!もスゴイ。ロボット・アームがグリグリして、なんでもできちゃいそうです。

actionlib::SimpleActionClient

先ほどのTurtleBotの自律移動のチュートリアルでは移動先をRVizのGUIで指定していましたけれど、一味違う我々は、プログラムから指定してみましょう。turtlebot_navigationパッケージが使用しているnavigationパッケージのドキュメントのSending Goals to the Navigation Stackに、移動先をプログラムから指定する方法が書いてありました。このドキュメントの途中にはactionlibを使うと書いてありますから、navigationに指示を出すプログラムを組むことはactionlibの調査につながるはず。

とってもお得な話ですから、さっそく試してみましょう。

package.xmlとCMakeLists.txt

「Sending Goals to the Navigation Stack」に必要だと書いてあるmove_base_msgsパッケージとactionlibパッケージを、依存するパッケージに追加してください。

launch/make_target_models.launch

使用するトピックが変更になりますので、launchファイルを変更します。

<launch>
  <arg name="move_base" default="move_base"/>  <!-- 追加。他のargは削除しました。 -->
  
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_target_models" args="load six_point_two_eight/make_target_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="move_base" to="$(arg move_base)"/>  <!-- 追加。他のremapは削除しました。 -->
  </node>
</launch>

include/six_point_two_eight/utilities.h

actionlibは、通常のROSのメッセージ通信を活用して、長時間処理を実現します。だから、プログラムから使用するのは普通のメッセージ。どのようなメッセージを使うのかを確認したければ、ターミナルでrosmsg show move_base_msgs/MoveBaseまで入力して[Tab]キーを押してみてください。MoveBaseを実現するのに必要なメッセージが一覧表示されます。

$ rosmsg show move_base_msgs/MoveBase
move_base_msgs/MoveBaseAction          move_base_msgs/MoveBaseActionResult    move_base_msgs/MoveBaseResult
move_base_msgs/MoveBaseActionFeedback  move_base_msgs/MoveBaseFeedback
move_base_msgs/MoveBaseActionGoal      move_base_msgs/MoveBaseGoal

この中で我々が中身を知らなければならないのは、move_base_msgs/MoveBaseGoalメッセージとmove_base_msgs/MoveBaseResultメッセージとmove_base_msgs/MovebaseFeedbackメッセージ。ゴール（何をするかの指示）と結果と途中経過のフィードバックですね。他のメッセージは動作を制御するためのもので、ライブラリがうまいこと隠蔽してくれます。

utilities.hでは、とりあえず移動先を指定するためのMoveBaseGoalメッセージを作成する関数を追加しました。MoveBaseGoalメッセージを構成するgeometry_msgs/PoseStampedとgeometry_msgs/Pose、geometry_msgs/Quaternionを生成する関数も、合わせて作成しておきます。

#pragma once

#include <cmath>
#include <tf/transform_datatypes.h>  // 追加。

#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseGoal.h>  // 追加。<geometry_msgs/Pose.h>は削除。

namespace six_point_two_eight {
  // 略。

  // move_base_msgs/MoveBaseGoalに必要なgeometry_msgs/PoseStampedに必要なgeometry_msgs/Poseに必要なgeometry_msgs/Quaternionを作成します。
  inline auto createQuaternionMsg(double yaw) {
    return tf::createQuaternionMsgFromYaw(yaw);
  }
  
  // move_base_msgs/MoveBaseGoalに必要なgeometry_msgs/PoseStampedに必要なgeometry_msgs/Poseを作成します。
  inline auto createPoseMsg(double x, double y, double yaw) {
    geometry_msgs::Pose msg;
    msg.position = createPointMsg(x, y);
    msg.orientation = createQuaternionMsg(yaw);

    return msg;
  }
  
  // move_base_msgs/MoveBaseGoalに必要なgeometry_msgs/PoseStampedを作成します。
  inline auto createPoseStampedMsg(
    const std::string& frame_id, const ros::Time& stamp, double x, double y, double yaw) 
  {
    geometry_msgs::PoseStamped msg;
    msg.header.frame_id = frame_id;
    msg.header.stamp = stamp;
    msg.pose = createPoseMsg(x, y, yaw);

    return msg;
  }

  // move_base_msgs/MoveBaseGoalを作成します。
  inline auto createMoveBaseGoalMsg(
    const std::string& frame_id, const ros::Time& stamp, double x, double y, double yaw) 
  {
    move_base_msgs::MoveBaseGoal msg;
    msg.target_pose = createPoseStampedMsg(frame_id, stamp, x, y, yaw);

    return msg;
  }

  // 略。
}

include/six_point_two_eight/make_target_models.h

MoveBaseActionを呼び出す形で、six_point_two_eight::MakeTargetModelsクラスを全面的に書きなおしました。actionlibなし版と比較できるようにお題は同じ、9時の方向に0.5m進みます。

#pragma once

#include <actionlib/client/simple_action_client.h>  // 追加。
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <thread>  // 追加。

#include <move_base_msgs/MoveBaseAction.h>  // 追加。

#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class MakeTargetModels : public nodelet::Nodelet {
  private:
    std::thread working_thread_;

    auto moveTo(const move_base_msgs::MoveBaseGoal& goal) {
      // actionlibのクライアントを作成します。
      actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> action_client("move_base", true);

      // actionlibのサーバーの起動を待ちます。
      if (!action_client.waitForServer(ros::Duration(60.0))) {
        ROS_ERROR_STREAM("Timeout occured when waiting action server.");
        throw std::exception();
      }

      // actionlibのサーバーにゴールを送信します。
      action_client.sendGoal(goal);

      // actionlibのサーバーの処理が完了するのを待ちます。
      if (!action_client.waitForResult(ros::Duration(60.0))) {
        ROS_ERROR_STREAM("Timeout occured when waiting result.");
        throw std::exception();
      }

      // ステータスを調べます。
      if (action_client.getState() != actionlib::SimpleClientGoalState::StateEnum::SUCCEEDED) {
        ROS_ERROR_STREAM(action_client.getState().toString());
        throw std::exception();
      }

      // 結果を返します。
      return *action_client.getResult();
    }
    
  public:
    void onInit() {
      // onInitの中でループするとNodeletManagerの処理が止まってしまうので、別スレッドで実行します。
      working_thread_ = std::thread(
        [&]() {
          // base_linkの現在の座標系で、X=0、Y=0.5、向きはそのままになるように移動させる（9時の方向に0.5mで方向は変えない）。
          moveTo(createMoveBaseGoalMsg("base_link", ros::Time::now(), 0.0, 0.5, 0.0));

          // 移動が終わったら実行する処理を、ここに書くことができます！
          ROS_INFO_STREAM("Reached!");
        });
    }
  };
}

上のコードのonInit()に注目してください。移動が完了した後の処理（ROS_INFO_STREAM()）が、moveTo()メンバー関数の呼び出しの後に書かれています。これで、コールバック地獄が発生しない、見通しがよいコードになりました。actionlib便利すぎです。

あと、移動先をどのフレームの座標系で示すかを指示できるので、三角関数が不要になりました。やっぱり便利すぎ。

actionlib::SimpleActionClient呼び出しを、ライブラリ化する

でも、先ほどのコードのactionlibのサーバーを呼び出す部分は、書くのが面倒くさい……。ライブラリ化してしまいましょう。

include/six_point_two_eight/utilities.h

本稿の要件の場合、actionlibのフィードバックは不要です。同期処理にできるわけで、だからファンクターで実装できるでしょう。やってみます。

#pragma once

#include <actionlib/client/simple_action_client.h>  // 追加。
#include <cmath>
#include <tf/transform_datatypes.h>

#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseAction.h>  // 追加。<move_base_msgs/MoveBaseGoal.h>の#includeは削除。

namespace six_point_two_eight {
  // 本稿用の例外。
  class SixPointTwoEightException : public std::exception {
  private:
    std::string what_;

  public:
    SixPointTwoEightException(const std::string& what)
      : what_(what)
    {
      ;
    }

    const char* what() const noexcept {
      return what_.c_str();
    }
  };
  
  // 略。

  // actionlib呼び出し用ファンクター。
  template <typename Action>
  struct CallAction {
    // Goalの型。
    using Goal = typename Action::_action_goal_type::_goal_type;
    
    // Resultの型。
    using Result = typename Action::_action_result_type::_result_type;
    
    // Nodeletの初期化はonInit()なので、メンバー変数の初期化をやりづらい。なので、トピックはoperator()の引数にしました。
    // ただし、毎回引数にトピックを書くと保守性が低くなってしまうので、onInit()でトピックをstd::bindできるようにします。以下は、バインド後の変数のための型です。
    using TopicBinded = std::function<Result(const Goal&)>;

    // 何度もstd::bindを書くのは面倒なのでヘルパー用関数を用意します。この関数をonInit()の中で呼んでください。
    static auto bindTopic(const std::string& topic) {
      return std::bind(CallAction<Action>(), topic, std::placeholders::_1);
    }

    // 例外メッセージを生成します。
    auto exceptionWhat(const std::string& message, const std::string& topic) const {
      std::stringstream what;
      what << message << " topic = " << topic << ".";
      
      return what.str();
    }

    // actionlibを呼び出します。
    auto operator()(const std::string& topic, const Goal& goal) {
      actionlib::SimpleActionClient<Action> action_client(topic, true);
      if (!action_client.waitForServer(ros::Duration(60.0))) {
        throw SixPointTwoEightException(exceptionWhat("Timeout occured when waiting action server.", topic));
      }
      
      action_client.sendGoal(goal);
      if (!action_client.waitForResult(ros::Duration(60.0))) {
        throw SixPointTwoEightException(exceptionWhat("Timeout occured when waiting result.", topic));
      }
      
      if (action_client.getState() != actionlib::SimpleClientGoalState::StateEnum::SUCCEEDED) {
        throw SixPointTwoEightException(exceptionWhat(action_client.getState().toString(), topic));
      }
      
      return *action_client.getResult();
    }
  };

  // 使用時にメンバー変数宣言とbindTopic()で2回クラス名を書くのは大変なので、usingしておきます。
  using CallMoveBaseAction = CallAction<move_base_msgs::MoveBaseAction>;
}

TopicBindedとbindTopic()のあたりが複雑になっているのは、Nodeletの初期化がonInit()で実施されるためです。メンバー変数の初期化をコンストラクターの初期化リストに書けない（初期化リストに書くと、初期化のコードが分散するので保守性が落ちてしまう）ので、トピックはoperator()の引数になります。でも呼び出しのたびにトピックを引数に書くのは面倒だし保守性が下がってしまうので、onInit()の中でトピックをstd::bindしておきたい。このstd::bindした結果を格納するメンバー変数の型がTopicBindedで、std::bind呼び出しを少しだけ楽にするのがbindTopic()メンバー関数です。具体的な使い方は、次のmake_target_models.hでやります。

include/six_point_two_eight/make_target_models.h

作成したCallActionを使用する形に、MakeTargetModelsを書き直しましょう。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <thread>

// <actionlibclient/simple_action_client.h>と<move_base_msgs/MoveBaseGoal.h>の#includeは削除。

#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class MakeTargetModels : public nodelet::Nodelet {
  private:
    // actionlibは、CallActionを経由して呼び出します。
    CallMoveBaseAction::TopicBinded move_base_;
    
    std::thread working_thread_;
    
  public:
    void onInit() {
      // トピックをバインドします。
      move_base_ = CallMoveBaseAction::bindTopic("move_base");
      
      working_thread_ = std::thread(
        [&]() {
          // actionlibを呼び出します。
          move_base_(createMoveBaseGoalMsg("base_link", ros::Time::now(), 0.0, 0.5, 0.0));
          
          ROS_INFO_STREAM("Finished!");
        });
    }
  };
}

びっくりしちゃうくらい簡単になりました。actionlibは本当に便利ですな。

actionlib::SimpleActionServer

でも、対象物の周囲360°からの点群作成のための道具としては、navigationパッケージはちょっとオーバースペックな気がします。acitonlibの勉強がてら、同じ処理（ただし、地図を参照して最短経路を探したり障害物を避けたりしない。指定された座標にただ進むだけの低機能版）をするNodeletを作ってみましょう。

include/six_point_two_eight/move_base_server.h

actionlibのサーバーは、actionlib::SimpleActionServerクラスを使用して作成します。実際の処理をする関数を引数に渡して、処理が終了したらsetSucceeded()メンバー関数を呼ぶだけ。使い方そのものは簡単です。

ただし、このSimpleActionServerクラス、初期化はコンストラクタでやることになっていて、しかもoperator=が無効になっているんですよ。このプログラミング・スタイルそのものは、良いプログラミング・スタイルではあります。不完全なインスタンスは危険ですから、初期化をコンストラクタで完全にやっておくのは良いことでしょう。そのクラスをメンバー変数に持つ場合は、コンストラクタの初期化リストで初期化できます。初期化リストでメンバー変数を初期化するのは、良いスタイルだとされていますしね。変数への再代入を禁止しておけば状態遷移がなくなって、関数型言語のコードみたいに単純になるでしょう。と、良いこと尽くめなプログラミング・スタイルなのですけれど、残念なことにNodeletと相性が悪いんですよ……。

Nodeletの場合、SimpleActionServerをコンストラクタの初期化リストで初期化するのは駄目です。NodeletのonInit()がまだ呼ばれていませんからね。operator=が無効になっているので、onInit()の中での初期化も不可能です。

class MoveBaseServer : public nodelet::Nodelet {
private:
  actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> move_base_action_server_;
  
public:
  MoveBaseServer()
    : move_base_action_server_(getNodeHandle(), "move_base", false)  // NG。Nodeletの初期化がなされていないのに、getNodeHandle()を呼んでいる。
  {
    ;
  }
  
  void onInit() {
    velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);
    
    // 以下はコンパイル・エラーになります。
    // move_base_action_server_ = actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction>(getNodeHandle(), "move_base", false);
    
    ...
  }
};

困った時のGoogle先生で検索したら、別クラス作る方式のコードが見つかりました。以下のような感じ。

class MoveBaseServerImpl {
private:
  actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> move_base_action_server_;

public:
  MoveBaseServerImpl(ros::NodeHandle* node_handle)
    : move_base_action_server_(*node_handle, "move_base", false)
  {
    // move_base_action_server_を使う処理。
  }
};

class MoveBaseServer {
private:
  boost::optional<MoveBaseServerImpl> impl_;
  
public:
  void onInit() {
    impl_ = boost::in_place(&getNodeHandle());
  }
};

ただ、このコードだと、ちょっと不格好ですね……。処理が分散していて、保守性が低そうです。

でも、このコードはよいヒントになります。このコードで使われているBoostのIn-Place Factoryなら、初期化を遅らせることが可能です。だったら、わざわざ間にクラスを挟まなくてもよいはず。というわけで、以下のように、In-Place Factoryを使ってコードを書きました。

移動処理のほとんどは、以前作成したactionlibを使わないバージョンからのコピー＆ペーストです。回転する処理と、/odom以外のフレームが指定された時に備えるためにTFで座標変換する処理を付け加えた程度です。

#pragma once

#include <actionlib/server/simple_action_server.h>
#include <boost/utility/in_place_factory.hpp>
#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <move_base_msgs/MoveBaseAction.h>

#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class MoveBaseServer : public nodelet::Nodelet {
  private:
    ros::Publisher velocity_publisher_;
    ros::Subscriber odom_subscriber_;

    // MoveBaseActionのサーバーです。boostのIn-Place Factoryを使用して、初期化を遅らせます。
    boost::optional<actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction>> move_base_action_server_;
    
    auto tunedLinearX(double distance, double angle) const {  // 前に作成したものと同じ。
      if (distance < 0.05) {
        return 0.0;
      }
      if (std::fabs(angle) > M_PI / 180 * 45) {
        return 0.0;
      }
      
      return std::min(std::fabs(distance) * 0.5, 0.1);
    }
    
    auto tunedAngularZ(double angle) const {  // 以前作成したものと同じ。
      if (std::fabs(angle) < M_PI / 180 * 1) {
        return 0.0;
      }
      
      return std::min(std::fabs(angle) * 0.5, M_PI / 180 * 10) * (angle / std::fabs(angle));
    }

    // ゴールの向きと同じなるように回転します（作業用）。
    auto turnTo(const geometry_msgs::Pose& goal_pose, nav_msgs::OdometryConstPtr odometry) {
      // ゴールの向きと現在の向きの差を計算します。
      auto angular_z = this->tunedAngularZ(
        normalizeAngle(tf::getYaw(goal_pose.orientation) - tf::getYaw(odometry->pose.pose.orientation)));

      // ロボットを回転させます。
      velocity_publisher_.publish(createTwistMsg(0, angular_z));

      // 回転したかどうかを返しhます。
      return angular_z != 0.0;
    }

    // ゴールの向きと同じなるように回転します。
    auto turnTo(const geometry_msgs::Pose& goal_pose) {
      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&, goal_pose](const auto& message) {
          if (this->turnTo(goal_pose, message)) {  // 回転した場合は、それで今回分の処理は終了なので、
            return;                                // そのままリターンします。
          }

          move_base_action_server_->setSucceeded(move_base_msgs::MoveBaseResult());  // 回転しない＝終了なので、MoveBaseActionを終了させます。
          odom_subscriber_.shutdown();  // サブスクライバーをシャットダウンします。
        });
    }
    
    auto moveTo(const geometry_msgs::Pose& goal_pose, nav_msgs::OdometryConstPtr odometry) {  // 以前作成したものから、1つ目の引数を変えただけ。
      auto difference = createPointMsg(
        goal_pose.position.x - odometry->pose.pose.position.x, goal_pose.position.y - odometry->pose.pose.position.y);
      
      auto distance = std::sqrt(std::pow(difference.x, 2) + std::pow(difference.y, 2));
      auto angle =
        normalizeAngle(std::atan2(difference.y, difference.x) - tf::getYaw(odometry->pose.pose.orientation));
      
      auto linear_x = this->tunedLinearX(distance, angle);
      auto angular_z = this->tunedAngularZ(angle);
      
      velocity_publisher_.publish(createTwistMsg(linear_x, angular_z));
      
      return linear_x != 0.0 || angular_z != 0.0;
    }

    auto moveTo(const geometry_msgs::Pose& goal_pose) {
      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&, goal_pose](const auto& message) {
          if (this->moveTo(goal_pose, message)) {
            return;
          }

          this->turnTo(goal_pose);  // 撮影ポイントまで移動した後は、対象物が正面に来るように回転します。
        });
    }

  public:
    void onInit() {
      velocity_publisher_ = getNodeHandle().advertise<geometry_msgs::Twist>("velocity", 1);

      // boostのIn-Place Factoryを使用して、サーバーを初期化します。
      move_base_action_server_ = boost::in_place(getNodeHandle(), "move_base", false);
      
      // Goalが送られてきた場合の処理を登録します。
      move_base_action_server_->registerGoalCallback(
        [&]() {
          // acceptNewGoal()でゴールを取得します。
          auto goal = move_base_action_server_->acceptNewGoal();

          // TFで座標変換します。
          geometry_msgs::PoseStamped goal_pose_stamped;
          transform_listener_.transformPose("odom", goal->target_pose, goal_pose_stamped);

          // 実際の処理を担当するのメンバー関数を呼び出します。
          moveTo(goal_pose_stamped.pose);
        });

      // サーバーを開始します。
      move_base_action_server_->start();
    }
  };
}

src/six_point_two_eight.cpp

move_base_server.hがコンパイルされるよう、*.cppを修正します。

#include <pluginlib/class_list_macros.h>

#include "six_point_two_eight/make_target_models.h"
#include "six_point_two_eight/make_world_models.h"
#include "six_point_two_eight/move_base_server.h"  // 追加。
#include "six_point_two_eight/point_cloud_2_throttle.h"

PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeTargetModels, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeWorldModels, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MoveBaseServer, nodelet::Nodelet)  // 追加。
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::PointCloud2Throttle, nodelet::Nodelet)
six_point_two_eight.xml

作成したsix_point_two_eight::MoveBaseServerをROSから扱えるように、名前を付けます。

<library path="lib/libsix_point_two_eight">
  <class name="six_point_two_eight/make_target_models" type="six_point_two_eight::MakeTargetModels" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/make_world_models" type="six_point_two_eight::MakeWorldModels" base_class_type="nodelet::Nodelet"/>
  <!-- ここから追加。 -->
  <class
    name="six_point_two_eight/move_base_server"
    type="six_point_two_eight::MoveBaseServer"
    base_class_type="nodelet::Nodelet"/>
  <!-- ここまで。 -->
  <class name="six_point_two_eight/point_cloud_2_throttle" type="six_point_two_eight::PointCloud2Throttle" base_class_type="nodelet::Nodelet"/>
</library>

include/six_point_two_eight/make_target_models.h

ついでですから、対象物の周囲360°を回るように、make_target_models.hを修正してしまいましょう。対象物は正面にあるものとし、その対象物の周囲の撮影ポイントを回ります。処理を単純にするために、/odomを使用して絶対位置で計算しました。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <nav_msgs/Odometry.h>  // 追加。<thread>の#includeは削除。

#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class MakeTargetModels : public nodelet::Nodelet {
  private:
    static constexpr double radius = 1.25;  // 対象物の周囲を回る円の半径。
    static const int step_size = 16;        // 1周回る間に撮影する回数。

    ros::Subscriber odom_subscriber_;
    CallMoveBaseAction::TopicBinded move_base_;
    
  public:
    void onInit() {
      move_base_ = CallMoveBaseAction::bindTopic("move_base");

      // 相対座標だとプログラムが複雑になるので、/odomを使用して絶対座標で位置決めをします。
      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&](const auto& message) {
          // 自機の絶対位置。
          auto position = message->pose.pose.position;
          auto yaw = tf::getYaw(message->pose.pose.orientation);

          // ターゲットの絶対位置を計算します。
          auto target_position = createPointMsg(position.x + std::cos(yaw) * radius, position.y + std::sin(yaw) * radius);
          auto target_yaw = normalizeAngle(yaw + M_PI);  // ターゲットから見た、自機の方向。

          // ターゲットの周囲を回ります。
          for (auto i = 0; i < step_size; ++i) {
            ROS_INFO_STREAM("Step " << (i + 1) << "/" << step_size);

            // 次の撮影位置に移動します。
            target_yaw = normalizeAngle(target_yaw + M_PI * 2 / step_size);
            move_base_(
              createMoveBaseGoalMsg(
                "odom",
                ros::Time::now(),
                target_position.x + std::cos(target_yaw) * radius,
                target_position.y + std::sin(target_yaw) * radius,
                normalizeAngle(target_yaw + M_PI)));
          }

          ROS_INFO_STREAM("Finished!");
          odom_subscriber_.shutdown();
        });
    }
  };
}

actionlibのおかげで、移動→回転→移動……をループで表現出来ました。やっぱりactionlibは便利ですね。

launch/make_target_models.launch

最後。動作確認をするために、起動用のlaunchファイルを修正しましょう。、

<launch>
  <arg name="move_base" default="move_base"/>
  <arg name="odom" default="odom"/>  <!-- 追加 -->
  <arg name="velocity" default="mobile_base/commands/velocity"/>  <!-- 追加 -->
  
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_target_models" args="load six_point_two_eight/make_target_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="move_base" to="$(arg move_base)"/>
  </node>
  <!-- ここから追加。 -->
  <node
    pkg="nodelet"
    type="nodelet"
    name="move_base_server"
    args="load six_point_two_eight/move_base_server six_point_two_eight_nodelet_manager"
    output="screen">
    <remap from="odom" to="$(arg odom)"/>
    <remap from="velocity" to="$(arg velocity)"/>
  </node>
  <!-- ここまで。 -->
</launch>

で、実際に動かしてみると、移動が遅くて完了までかなり長い時間がかかります……。これは、オドメトリーの誤差をできるだけ減らすためです。navigationパッケージで速く動けているのは、あれは深度センサーの情報を元に現在位置を補正し続けているからなんですよ。今回はそんな高度な処理は作成していませんので、申し訳ないですけど、遅いのは我慢してください。

*.actionの定義

点群を取得する処理も、actionlib化してしまいましょう。既存のアクションを再利用できない処理なので、新しいアクションの定義方法を学ぶこともできますしね。

action/GetPointCloud2.action

アクションは、actionディレクトリの下に、*.actionとして定義してください。ファイル名がそのままアクション名になります。*.actionの中は、---で3つに区切られます。区切り方は、以下のような感じ。

Goalのメッセージ定義
---
Resultのメッセージ定義
---
Feedbackのメッセージ定義

で、メッセージ定義は、型 変数名の0行以上の繰り返し。何もいらない（move_base_msgs/MoveBaseResultのような）場合は、0行になります。今回は、どのトピックからデータを取得するのかを渡して、PointCloud2を返す感じ。途中経過は使わないので、フィードバックは無しで。具体的なGetPointCloud2.actionの中身は、以下になります。

string target_topic
---
sensor_msgs/PointCloud2 points
---

package.xmlとCMakeLists.xml

メッセージ定義の場合は、package.xmlの<build_depend>にmessage_generation、<run_depend>にmessage_runtimeを指定するという面倒なルールがあるのですけれど、アクション定義の場合はどちらもactionlib_msgsパッケージで大丈夫です。actionlib_msgsをpackage.xmlの<build_depends>と<run_depends>に追加してください。

CMakeLists.xmlはもう少し複雑です。というのも、*.actionをコンパイルするように指示しなければならないんです。あと、*.ationのコンパイルが終了してC++のコードが生成されるまで、*.cppのビルドを待つようにも指示します。

cmake_minimum_required(VERSION 2.8.3)
project(six_point_two_eight)

find_package(catkin REQUIRED COMPONENTS
  actionlib
  actionlib_msgs
  geometry_msgs
  move_base_msgs
  nav_msgs
  nodelet
  nodelet_topic_tools
  pcl_ros
  roscpp
  sensor_msgs
  tf
  tf_conversions
)

# find_package(Boost REQUIRED COMPONENTS system)

# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

# *.actionを登録します。
add_action_files(FILES
  GetPointCloud2.action
)

# メッセージを生成するように指示します。add_xxx_filesに加えて、generate_massagesも必要です。
generate_messages(DEPENDENCIES  # 追加。
  actionlib_msgs                # 追加。\*.actionが参照するメッセージを含むパッケージを書きます。
  sensor_msgs                   # 追加。\*.actionが参照するメッセージを含むパッケージを書きます。
)                               # 追加。

catkin_package(CATKIN_DEPENDS
  actionlib_msgs  # 追加。ここには、catkinの実行に必要なパッケージを設定します。
)

add_compile_options(
  "-std=c++14"
)

set_source_files_properties(src/point_cloud_utilities.cpp
  PROPERTIES COMPILE_FLAGS "-std=c++03 -Wno-deprecated"
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_library(six_point_two_eight
  src/six_point_two_eight.cpp
  src/point_cloud_utilities.cpp
  src/utilities.cpp
)

# メッセージの生成が終わるまで（*.hが生成されるまで）、*.cppのビルドを待つように指示します。
add_dependencies(six_point_two_eight ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(six_point_two_eight
  ${catkin_LIBRARIES}
)

ここまで書いたらターミナルでcatkin_makeしてみてください。<catkin_ws>/devel/include/six_point_two_eightディレクトリに以下のファイルができていればOKです。

$ ls devel/include/six_point_two_eight
GetPointCloud2ActionFeedback.h  GetPointCloud2Action.h        GetPointCloud2Feedback.h   GetPointCloud2Result.h
GetPointCloud2ActionGoal.h      GetPointCloud2ActionResult.h  GetPointCloud2Goal.h

include/six_point_two_eight/utilities.h

いつものutilities.hに、メッセージ生成関数とCallActionの別名定義を追加しておきます。

six_point_two_eightパッケージの中で定義したメッセージのの名前空間は、six_point_two_eightになります。あと、includeファイルはsix_point_two_eight/メッセージ名.h。一般には、我々がmove_base_msgs/MoveBaseActionを再利用したようにメッセージだけが再利用される可能性がありますから、*_msgsという名前のパッケージを作ってそこにメッセージを定義すべきです。でもまぁ、今回のメッセージは我々しか利用しないでしょうから、ごめんなさい、パッケージ分割はなしで。

#pragma once

#include <actionlib/client/simple_action_client.h>
#include <cmath>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>

#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <six_point_two_eight/GetPointCloud2Action.h>  // 追加。

namespace six_point_two_eight {
  // 略。
  
  // GetPointCloud2Goalメッセージを生成します。
  inline auto createGetPointCloud2GoalMsg(const std::string& target_topic) {
    six_point_two_eight::GetPointCloud2Goal msg;
    msg.target_topic = target_topic;

    return msg;
  }

  // GetPointCloud2Resultメッセージを生成します。
  inline auto createGetPointCloud2ResultMsg(sensor_msgs::PointCloud2ConstPtr points) {
    six_point_two_eight::GetPointCloud2Result msg;
    msg.points = *points;

    return msg;
  }

  // 利便性のために、別名を定義しておきます。
  using CallGetPointCloud2Action = CallAction<six_point_two_eight::GetPointCloud2Action>;

  // 略。
}

include/six_point_two_eight/get_point_cloud_server.h

GetPointCloud2アクションを担当するNodeletを作ります。これまでに学習した知識（と作成したコードのコピー＆ペーストと少しの修正）で、サクッと作りました。

#pragma once

#include <actionlib/server/simple_action_server.h>
#include <boost/utility/in_place_factory.hpp>
#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <six_point_two_eight/GetPointCloud2Action.h>

#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class GetPointCloud2Server : public nodelet::Nodelet {
  private:
    ros::Subscriber points_subscriber_;
    boost::optional<actionlib::SimpleActionServer<six_point_two_eight::GetPointCloud2Action>> get_point_cloud_2_action_server_;
    
    auto getPointCloud2(const std::string& target_topic) {
      auto points_subscribing_time = ros::Time::now();
      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        target_topic, 1,
        [&, points_subscribing_time](const auto& message) {
          // 起動直後は動作が安定しないので、3秒間は何もしません。
          if (message->header.stamp < points_subscribing_time + ros::Duration(3.0)) {
            return;
          }
          
          get_point_cloud_2_action_server_->setSucceeded(createGetPointCloud2ResultMsg(message));
          points_subscriber_.shutdown();
        });
    }

  public:
    void onInit() {
      get_point_cloud_2_action_server_ = boost::in_place(getNodeHandle(), "get_point_cloud_2", false);
      get_point_cloud_2_action_server_->registerGoalCallback(
        [&]() {
          getPointCloud2(get_point_cloud_2_action_server_->acceptNewGoal()->target_topic);
        });
      get_point_cloud_2_action_server_->start();
    }
  };
}

src/six_point_two_eight.cpp

move_base_server.hの時と同じ、単純作業です。

#include <pluginlib/class_list_macros.h>

#include "six_point_two_eight/get_point_cloud_2_server.h"  // 追加。
#include "six_point_two_eight/make_target_models.h"
#include "six_point_two_eight/make_world_models.h"
#include "six_point_two_eight/move_base_server.h"
#include "six_point_two_eight/point_cloud_2_throttle.h"

PLUGINLIB_EXPORT_CLASS(six_point_two_eight::GetPointCloud2Server, nodelet::Nodelet)  // 追加。
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeTargetModels, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeWorldModels, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MoveBaseServer, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::PointCloud2Throttle, nodelet::Nodelet)

six_point_two_eight.xml

単純作業が続いて申し訳ありませんが、Nodeletに名前を付ける単純作業を実施してください。

<library path="lib/libsix_point_two_eight">
  <!-- ここから追加。 -->
  <class
    name="six_point_two_eight/get_point_cloud_2_server"
    type="six_point_two_eight::GetPointCloud2Server"
    base_class_type="nodelet::Nodelet"/>
  <!-- ここまで。 -->
  <class name="six_point_two_eight/make_target_models" type="six_point_two_eight::MakeTargetModels" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/make_world_models" type="six_point_two_eight::MakeWorldModels" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/move_base_server" type="six_point_two_eight::MoveBaseServer" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/point_cloud_2_throttle" type="six_point_two_eight::PointCloud2Throttle" base_class_type="nodelet::Nodelet"/>
</library>

include/six_point_two_eight/make_target_models.h

作成したactionlibを呼び出すように、make_world_models.hも修正しましょう。

#pragma once

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>  // 追加。

#include "six_point_two_eight/point_cloud_utilities.h"  // 追加。
#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class MakeTargetModels : public nodelet::Nodelet {
  private:
    static constexpr double radius = 1.25;
    static const int step_size = 16;

    ros::Subscriber odom_subscriber_;
    CallGetPointCloud2Action::TopicBinded get_point_cloud_2_;  // 追加。
    CallMoveBaseAction::TopicBinded move_base_;

  public:
    void onInit() {
      get_point_cloud_2_ = CallGetPointCloud2Action::bindTopic("get_point_cloud_2");  // 追加。
      move_base_ = CallMoveBaseAction::bindTopic("move_base");

      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&](const auto& message) {
          auto position = message->pose.pose.position;
          auto yaw = tf::getYaw(message->pose.pose.orientation);

          auto target_position = createPointMsg(position.x + std::cos(yaw) * radius, position.y + std::sin(yaw) * radius);
          auto target_yaw = normalizeAngle(yaw + M_PI);

          // ターゲットの周囲を、深度センサーからデータを取得しながら回ります。
          for (auto i = 0; i < step_size; ++i) {
            ROS_INFO_STREAM("Step " << (i + 1) << "/" << step_size);

            // ここから追加。
            // 深度センサーからデータを取得します。
            while (ros::ok()) {
              try {
                savePointCloud2File(
                  getSpherePointCloud2(
                    transformPointCloud2(
                      sensor_msgs::PointCloud2ConstPtr(  // PointCloud2をPointCloud2ConstPtrに変換します。
                        new sensor_msgs::PointCloud2(
                          get_point_cloud_2_(createGetPointCloud2GoalMsg("points")).points)),  // アクションから点群を取得。
                      "odom"),
                    target_position,
                    radius * 0.5));
                break;
              
              } catch (const std::exception& ex) {
                ROS_WARN_STREAM(ex.what());
              }
            }
            // ここまで。
            
            target_yaw = normalizeAngle(target_yaw + M_PI * 2 / step_size);
            
            move_base_(
              createMoveBaseGoalMsg(
                "odom",
                ros::Time::now(),
                target_position.x + std::cos(target_yaw) * radius,
                target_position.y + std::sin(target_yaw) * radius,
                normalizeAngle(target_yaw + M_PI)));
          }

          ROS_INFO_STREAM("Finished!");
          odom_subscriber_.shutdown();
        });
    }
  };
}

途中が少しごちゃごちゃしているのは、point_cloud_utilities.hの関数はsensor_msgs::PointCloud2ConstPtrを引数にとる形で定義しているのにsix_point_two_eight::GetPointCloud2Resultから受け取れるデータはsensor_msgs::PointCloud2（ConstPtrがつかない）なためです。だから、型変換しなければなりませんでした。点群に含まれる点の数が少ないため、今回はダウンサンプリングはしていません。あと、point_cloud_utilities.hの関数を呼び出した時に例外が発生した場合に点群を再採取できるように、whileループで囲んでいます。

launch/make_target_models.launch

launchファイルも修正します。せっかくNodeletを作っても、起動させなければ無意味ですもんね。

<launch>
  <arg name="get_point_cloud_2" default="get_point_cloud_2"/>
  <arg name="move_base" default="move_base"/>
  <arg name="odom" default="odom"/>
  <arg name="points" default="camera/depth_registered/points"/>  <!-- 追加 -->
  <arg name="velocity" default="mobile_base/commands/velocity"/>
  
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="make_target_models" args="load six_point_two_eight/make_target_models six_point_two_eight_nodelet_manager" output="screen">
    <remap from="get_point_cloud_2" to="$(arg get_point_cloud_2)"/>
    <remap from="move_base" to="$(arg move_base)"/>
  </node>
  <!-- ここから追加。 -->
  <node
    pkg="nodelet"
    type="nodelet"
    name="get_point_cloud_2_server"
    args="load six_point_two_eight/get_point_cloud_2_server six_point_two_eight_nodelet_manager"
    output="screen">
    <remap from="points" to="$(arg points)"/>
  </node>
  <!-- ここまで。 -->
  <node pkg="nodelet" type="nodelet" name="move_base_server" args="load six_point_two_eight/move_base_server six_point_two_eight_nodelet_manager" output="screen">
    <remap from="odom" to="$(arg odom)"/>
    <remap from="velocity" to="$(arg velocity)"/>
  </node>
</launch>

launch/make_target_models_in_gazebo.launch

今回の処理は点群を扱いますから、トピックが実機と異なっているシミュレーター用のlaunchファイルも、修正します。

<launch>
  <include file="$(find six_point_two_eight)/launch/make_target_models.launch">
    <arg name="points" value="camera/depth/points"/>
  </include>
</launch>

PCLを使用して、床を除去する

知識と作成済みコードが増えたら、なんだか大抵の作業が機械的にできるようになっちゃってつまんない。少し新しいことをやりたいな……。というわけで、少し高度な点群処理をやってみましょう。床の除去です。

床を除去？

今回のプログラムでは床を除去する意味はあまりないのですけれど、ロボット・アームを使う場合等では、床の除去はとても重要です。ロボット・アームでテーブルの上にあるモノを掴むとき、もしテーブル板を除去できるならば、どこにどんなモノがあるのかを簡単に判定できるようになりますから。

PCLは当然この重要な機能を提供します。チュートリアルのPlane model segmentationを見ると、平面を検出する方法が紹介されています。点群を形状でセグメンテーション分割する方法の解説ですね。このセグメンテーション機能で、床の除去をやってみましょう12。

セグメンテーション

ただ、PCLが提供する機能は、点群の中で一番大きな平面の検出だけなんですよ。平らな壁も、平面として識別されてしまいます。対象物に大きな平面がある場合も危険です。幸いなことに平面を識別する機能では見つかった平面の式の係数（平面の方程式ax + by + cz + d = 0の、aとb、c、d）を返してくれます。aとbが小さくてcが大きいなら水平面だと考えられますから、水平な床と垂直な壁は区別できそう。dが小さければ位置が低いでしょうから、対象物の平面ではないことも判断できそう。以上で、ロジックが決まりました。

include/six_point_two_eight/point_cloud_utilities.h

作成する関数の名前は、removeFloorFromPointCloud2とします。

#ifndef SIX_POINT_TWO_EIGHT_POINT_CLOUD_UTILITIES_H
#define SIX_POINT_TWO_EIGHT_POINT_CLOUD_UTILITIES_H

#include <geometry_msgs/Point.h>
#include <sensor_msgs/PointCloud2.h>

namespace six_point_two_eight {
  sensor_msgs::PointCloud2Ptr downsamplePointCloud2(sensor_msgs::PointCloud2ConstPtr points, double leaf_size);
  sensor_msgs::PointCloud2Ptr getSpherePointCloud2(sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Point& center, double radius);
  sensor_msgs::PointCloud2Ptr removeFloorFromPointCloud2(sensor_msgs::PointCloud2ConstPtr points);  // 追加。
  sensor_msgs::PointCloud2Ptr savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr transformPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double z_adjust = 0.0);
}

#endif

src/point_cloud_utilities.cpp

床を削除するコードは、こんな感じ。床ではない平面が検出された場合は次の平面を探さなければなりませんので、再帰呼出ししています。平面の数はそれほど多くないでしょうから、再帰呼出しでもスタックが不足して問題になることはないでしょう。また、点を削除するときにインデックスが変わってしまわないように、setKeepOrganized()にtrueを設定できるようにもしてあります。

あと、ごめんなさい。以前のコードはグローバルな名前空間を汚していて嫌だっとので、今回の修正で、実装部分の関数をpoint_cloud_utilities名前空間で囲んでみました。

#include <boost/foreach.hpp>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/extract_indices.h>  // 追加。
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>  // 追加。
#include <pcl/sample_consensus/model_types.h>  // 追加。
#include <pcl/segmentation/sac_segmentation.h>  // 追加。
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <tf/transform_listener.h>

#include "six_point_two_eight/point_cloud_utilities.h"

namespace point_cloud_utilities {  // グローバルな名前空間を汚さないように、名前空間で囲みました。
  // 略。

  // 点群からインデックス群で示される点を除去します。
  PointCloud::Ptr removeIndicesFromPointCloud(PointCloud::ConstPtr point_cloud, pcl::PointIndices::ConstPtr point_indices, bool keep_organized = false) {
    // インデックス群抽出のpcl::ExtractIndicesを生成します。
    pcl::ExtractIndices<Point> extract_indices;
    extract_indices.setInputCloud(point_cloud);
    extract_indices.setIndices(point_indices);
    extract_indices.setKeepOrganized(keep_organized);  // 点を削除するか、NaNとして保持しておくかどうか。
    extract_indices.setNegative(true);  // trueにすると、インデックス群以外を残します。falseにするとインデックス群を残す（切り抜き）になります。デフォルトはfalse。

    // インデックス群を除去します。
    PointCloud::Ptr indices_removed_point_cloud(new PointCloud());
    extract_indices.filter(*indices_removed_point_cloud);

    return indices_removed_point_cloud;
  }

  // 点群の中の、床だと思われる点のインデックス群を取得します。
  pcl::PointIndices::Ptr getFloorPointIndices(PointCloud::ConstPtr point_cloud) {
    // 点群に点がなくなったらどうしようもないので、終了します。
    if (point_cloud->size() == 0) {
      return pcl::PointIndices::Ptr();
    }
    
    // 平面検出用のpcl::SACSegmentationを生成します。
    pcl::SACSegmentation<Point> segmentation;
    segmentation.setOptimizeCoefficients(true);
    segmentation.setModelType(pcl::SACMODEL_PLANE);
    segmentation.setMethodType(pcl::SAC_RANSAC);
    segmentation.setDistanceThreshold(0.025);  // 平面の誤差。
    segmentation.setInputCloud(point_cloud);

    // 平面を検出します。
    pcl::ModelCoefficients::Ptr floor_model_coefficients(new pcl::ModelCoefficients());
    pcl::PointIndices::Ptr floor_point_indices(new pcl::PointIndices());
    segmentation.segment(*floor_point_indices, *floor_model_coefficients);

    // これ以上平面を検出できない場合はどうしようもないので、終了します。
    if (floor_point_indices->indices.size() == 0) {
      ROS_WARN_STREAM("Can't find plane...");
      return pcl::PointIndices::Ptr();
    }

    // 検出した平面が床ではないと思われる場合の処理。
    if (std::abs(floor_model_coefficients->values[0]) > 0.2 ||
        std::abs(floor_model_coefficients->values[1]) > 0.2 ||
        floor_model_coefficients->values[2] < 0.9 ||
        std::abs(floor_model_coefficients->values[3] > 0.2))
    {
      ROS_WARN_STREAM(
        "Can't find floor... Coefficients are " <<
        floor_model_coefficients->values[0] <<
        ", " <<
        floor_model_coefficients->values[1] <<
        ", " <<
        floor_model_coefficients->values[2] <<
        " and " <<
        floor_model_coefficients->values[3]);

      // 床ではない平面を除去して、再帰呼出しします。
      return getFloorPointIndices(removeIndicesFromPointCloud(point_cloud, floor_point_indices, true));
    }

    // 床と思われる平面を返します。
    return floor_point_indices;
  }

  // 点群から床を除去します。
  PointCloud::Ptr removeFloorFromPointCloud(PointCloud::ConstPtr point_cloud) {
    // 床のインデックス群を取得します。
    pcl::PointIndices::Ptr floor_point_indices = getFloorPointIndices(point_cloud);
    if (!floor_point_indices) {
      ROS_WARN_STREAM("Can't find floor...");
      throw std::exception();
    }

    // 床のインデックスの点を除去します。
    return removeIndicesFromPointCloud(point_cloud, floor_point_indices);
  }

  // 略。
}

// 処理関数を名前空間で囲んだので、以下の関数群の実装に、point_cloud_utiliites::を付けました。

sensor_msgs::PointCloud2Ptr six_point_two_eight::downsamplePointCloud2(sensor_msgs::PointCloud2ConstPtr points, double leaf_size) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::downsamplePointCloud(point_cloud_utilities::fromROSMsg(points), leaf_size));
}

sensor_msgs::PointCloud2Ptr six_point_two_eight::getSpherePointCloud2(sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Point& center, double radius) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::getSpherePointCloud(point_cloud_utilities::fromROSMsg(points), Eigen::Vector3f(center.x, center.y, center.z), radius));
}

// ここから追加。
sensor_msgs::PointCloud2Ptr six_point_two_eight::removeFloorFromPointCloud2(
  sensor_msgs::PointCloud2ConstPtr points) 
{
  return 
    point_cloud_utilities::toROSMsg(
      point_cloud_utilities::removeFloorFromPointCloud(
        point_cloud_utilities::fromROSMsg(
          points)));
}
// ここまで。

sensor_msgs::PointCloud2Ptr six_point_two_eight::savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::savePointCloudFile(point_cloud_utilities::fromROSMsg(points)));
}

sensor_msgs::PointCloud2Ptr six_point_two_eight::transformPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double x_adjust) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::transformPointCloud(point_cloud_utilities::fromROSMsg(points), target_frame, x_adjust));
}

include/six_point_two_eight/make_target_models.h

作成したremoveFloorFromPointCloud2()関数を呼び出すように、呼び出し側のmake_target_models.hを修正します。

// 略。

namespace six_point_two_eight {
  class MakeTargetModels : public nodelet::Nodelet {
  private:
    // 略。
    
  public:
    void onInit() {
      // 略。
      
      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&](const auto& message) {
          auto position = message->pose.pose.position;
          auto yaw = tf::getYaw(message->pose.pose.orientation);

          auto target_position = createPointMsg(position.x + std::cos(yaw) * radius, position.y + std::sin(yaw) * radius);
          auto target_yaw = normalizeAngle(yaw + M_PI);

          for (auto i = 0; i < step_size; ++i) {
            ROS_INFO_STREAM("Step " << (i + 1) << "/" << step_size);

            while (ros::ok()) {
              try {
                savePointCloud2File(
                  removeFloorFromPointCloud2(  // 追加。
                    getSpherePointCloud2(
                      transformPointCloud2(
                        sensor_msgs::PointCloud2ConstPtr(
                          new sensor_msgs::PointCloud2(
                            get_point_cloud_2_(createGetPointCloud2GoalMsg("points")).points)),
                        "odom"),
                      target_position,
                      radius * 0.5)));
                break;
              
              } catch (const std::exception& ex) {
                ROS_WARN_STREAM(ex.what());
              }
            }
            
            target_yaw = normalizeAngle(target_yaw + M_PI * 2 / step_size);
            
            move_base_(
              createMoveBaseGoalMsg(
                "odom",
                ros::Time::now(),
                target_position.x + std::cos(target_yaw) * radius,
                target_position.y + std::sin(target_yaw) * radius,
                normalizeAngle(target_yaw + M_PI)));
          }

          ROS_INFO_STREAM("Finished!");
          odom_subscriber_.shutdown();
        });
    }
  };
}

センサーの誤差

さて、ついに完成したプログラムを動かしてみると……ガッタガタじゃん！　分身の術みたいになってる。カバンのショルダー・ストラップが2本に分裂しちゃってるよ！

make_target_models

このようにガタガタの点群になってしまった理由は、タイヤがスリップしたりセンサーがモーターの回転数を数え間違えたりして、オドメトリーに誤差が出るためです。ビジネス・アプリケーションでRDBMSでトランザクションな世界に慣れているととてもびっくりするんですけど、センサーからのデータを疑わずにプログラムを作成しては駄目なんです。だから、どうにかして誤差を抑えこまなければなりません。でも、ねぇ、どうやって？

3D点群を統合する

誤差を抑えこむために、高度な数学を駆使する技巧の限りを尽くしたプログラムを作成する……のは大変そうですから、PCLにどうにかしてもらいましょう。PCLのドキュメントを見ると、RegistrationのチュートリアルのInteractive Iterative Closest Pointのやり方で出来そうです。このドキュメントの最後にある位置と角度がずれた点群を重ねあわせていくGIFアニメーションを見ると、うん、簡単にできそう。さっそく、やってみましょう。

Boost Range

点群を統合するには、その前に点群のファイルを読み込む処理を作らないとなりません。処理経過を見るためのビューアーも表示したい。make_world_modelsでは360°以上回っていますから、無駄を省くために360°を超えるデータを捨てる処理も書きたい。なので、とりあえずPCLによる点群の統合部分はダミーにして、処理全体をうまく流す呼び出し部分を作ってしまいましょう。

beginとendって、もうやめた方がよいと思う

まずは、ファイルの読み込み部分から考えてみます。/tmpにあるファイルの一覧の取得処理は、boost::filesystem::directory_iteratorを使えば作れます。six_point_two_eightのデータ・ファイルかどうかはファイル名で分かりますから、判定する関数は書けるでしょう。条件が一致するものを集めるのはstd::copy_ifで可能。boost::filesystem::directory_iteratorが指し示すboost::filesystem::directory_entryを文字列に変換する処理は、ラムダ式で書けちゃうくらいに簡単。コレクションの要素の変換は、たしかstd::transformでできたなぁ。

と考えて、コードを書いてみたら、以下に示すやたらと面倒なコードになっちゃった……。

#pragma once

#include <boost/filesystem.hpp>
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <thread>

namespace six_point_two_eight {
  class RegisterModels : public nodelet::Nodelet {
  private:
    std::thread working_thread_;

    // six_point_two_eightのファイルの場合かどうか判定します。
    auto isSixPointTwoEightFile(const boost::filesystem::directory_entry& directory_entry) const {
      auto path = directory_entry.path();
      auto file_prefix = std::string("six_point_two_eight_");

      return path.filename().string().substr(0, file_prefix.size()) == file_prefix && path.extension().string() == ".pcd";
    }
    
  public:
    void onInit() {
      working_thread_ = std::thread(
        [&]() {
          // six_point_two_eightのデータ・ファイル（型はdirectory_entry）の集合を作成します。
          std::vector<boost::filesystem::directory_entry> six_point_two_eight_directory_entries;
          std::copy_if(
            boost::filesystem::directory_iterator("/tmp"), boost::filesystem::directory_iterator(),
            std::back_inserter(six_point_two_eight_directory_entries),
            std::bind(&RegisterModels::isSixPointTwoEightFile, this, std::placeholders::_1));

          // six_point_two_eightのデータ・ファイル（型はstring）の集合を作成します。
          std::vector<std::string> six_point_two_eight_path_strings;
          std::transform(
            six_point_two_eight_directory_entries.begin(), six_point_two_eight_directory_entries.end(),
            std::back_inserter(six_point_two_eight_path_strings),
            [](const auto& six_point_two_eight_directory_entry) { return six_point_two_eight_directory_entry.path().string(); });

          // この後に、ファイル読み込み等の処理を似た感じで作る。
        });
    }
  };
}

何か作業をするたびにコレクションに値を入れ直している点が、上のコードの致命的な欠陥だと考えます。何か処理をする際には、必ず変数宣言という前処理をしなければならないんですもんね。でも、そうしないと、次の関数の引数のbeginとendを用意できなくなっちゃうし……。C++以外のほとんどの言語では、イテレーターが開始と終了をパックして管理するのでcopy_ifの戻り値をtransformの引数に回すことができるのになぁ。たとえばPythonなら、こんな感じで書けるのに……。

x = map(lambda filename: "/tmp/" + filename, 
        (filter(lambda filename: filename.startswith("six_point_two_eight_") and filename.endswith(".pcd"), 
                os.listdir("/tmp"))))

Boost Rangeに乗り換えよう

C++がbeginとend方式を採用したのは、最も軽量な順次アクセス手段であるポインターをイテレーターとして扱えるようにするため。その理屈は分かるけれども、高速なCPUで高度なコレクションを扱うときに面倒なコードを強制されるのは、勘弁して欲しい。

このように思う人は多かったみたいで、BoostにはRangeというライブラリが含まれています。Rangeは、範囲を持つイテレーター。他の言語での、ごく普通のイテレーター相当ですな。このRangeを使うと、以下のコードのように簡潔に記述できます。名前空間があるので少しゴチャゴチャしているけど、それを除けばPython版とあまり変わりません。

auto x =
  boost::adaptors::transform(
    boost::adaptors::filter(
      boost::make_iterator_range(boost::filesystem::directory_iterator(boost::filesystem::path("/tmp")), {}),
      std::bind(&RegisterModels::isSixPointTwoEightFile, this, std::placeholders::_1)),
    [](const auto& directory_entry) { return directory_entry.path().string(); });

でもなぁ、処理結果を引数にするコードって、先に処理される関数が後に記述されるので分かりづらいんだよなぁ。AしてBするのに、B(A())と書くんだもん。と、このように考える人も多かったみたいで、Rangeはパイプライン記法と呼ばれる機能も提供しています。パイプライン記法を使って先ほどのコードを書き換えた結果は、以下の通り。

auto xxx =
  boost::make_iterator_range(boost::filesystem::directory_iterator(boost::filesystem::path("/tmp")), {}) |
  boost::adaptors::filtered(std::bind(&RegisterModels::isSixPointTwoEightFile, this, std::placeholders::_1)) |
  boost::adaptors::transformed([](const auto& directory_entry) { return directory_entry.path().string(); });

美しい！　beginとendはもうやめて、Rangeに乗り換えることにしましょう。

データ・ファイルを読み込んで、ダミーの点群レジストレーション処理を呼び出して、保存する

Boost Rangeを活用して、点群のレジストレーション以外の処理を先に作成してしまいましょう。

include/six_point_two_eight/point_cloud_utilities.h

作成する処理の中で少し難しいのは、360°回転した以降のデータを捨てるところでしょう。この処理を実現するには、360°回転したところの点群かどうかを判断しなければなりませんからね。幸いなことにオドメトリーの誤差はそれほど大きくないので、2つの点群がどれだけ重なっているかで判断できそうです。そして、各点群の大きさに差はありませんから、PCLのAPIを眺めていた時に見つけたcompute3DCentroid()で重心を求めて、その距離を測るだけでよいでしょう。というわけで、getCentroidOfPointCloud2()関数を宣言しました。あと、データ・ファイルを削除するためのremovePointcloud2File()関数と、もちろん、loadPointCloud2File()も。

点群を画面に表示する方法を学ぶために、PCLのチュートリアルのPCLVisualizerを見たところ、点群はIDで管理されて更新/削除されるらしい。あと、画面上の点群を区別するに色分けが必要ということで、std::string idとint rgbを引数に取るshowPointCloud2()関数を宣言しました。また、どうやらPCLVisualizerは別スレッドを起動して独自に動いてくれるわけではなくて、画面を更新させるにはspinOnce()メンバー関数の呼び出しが必要みたい。だから、spinVisualizer()関数も宣言します。

最後に何もしないダミーのregisterPointCloud2()関数を作成して、point_cloud_utilities.hの更新は完了です。

#ifndef SIX_POINT_TWO_EIGHT_POINT_CLOUD_UTILITIES_H
#define SIX_POINT_TWO_EIGHT_POINT_CLOUD_UTILITIES_H

#include <geometry_msgs/Point.h>
#include <sensor_msgs/PointCloud2.h>

namespace six_point_two_eight {
  sensor_msgs::PointCloud2Ptr downsamplePointCloud2(sensor_msgs::PointCloud2ConstPtr points, double leaf_size);
  geometry_msgs::Point getCentroidOfPointCloud2(sensor_msgs::PointCloud2ConstPtr points);  // 追加。
  sensor_msgs::PointCloud2Ptr getSpherePointCloud2(sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Point& centroid, double radius);
  sensor_msgs::PointCloud2Ptr loadPointCloud2File(const std::string& file_path); // 追加。
  inline sensor_msgs::PointCloud2Ptr registerPointCloud2(  // 追加。
    sensor_msgs::PointCloud2ConstPtr source_points, sensor_msgs::PointCloud2ConstPtr target_points) 
  {
    return sensor_msgs::PointCloud2Ptr(new sensor_msgs::PointCloud2(*source_points));  // とりあえず、何もしないダミー実装で。
  }
  sensor_msgs::PointCloud2Ptr removeFloorFromPointCloud2(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr removePointCloud2File(sensor_msgs::PointCloud2ConstPtr points); // 追加。
  sensor_msgs::PointCloud2Ptr savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr showPointCloud2(  // 追加。
    sensor_msgs::PointCloud2ConstPtr points, const std::string& id, int rgb);
  void spinVisualizer();  // 追加。
  sensor_msgs::PointCloud2Ptr transformPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double z_adjust = 0.0);
}

#endif

src/point_cloud_utilities.cpp

PCLのAPIリファレンスを見ながら、淡々と処理を実装します。APIが揃っていますから、ただ呼び出すだけ。

#include <boost/foreach.hpp>
#include <pcl/common/centroid.h>  // 追加。
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <tf/transform_listener.h>

#include "six_point_two_eight/point_cloud_utilities.h"

namespace point_cloud_utilities {
  typedef pcl::PointXYZRGB Point;
  typedef pcl::PointCloud<Point> PointCloud;

  tf::TransformListener transform_listener_;
  pcl::visualization::PCLVisualizer::Ptr visualizer_;  // 追加。

  // 略。

  // 点群の重心を取得します。
  Eigen::Vector4f getCentroidOfPointCloud(PointCloud::ConstPtr point_cloud) {
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*point_cloud, centroid);
    
    return centroid;
  }
 
  // 文字列をunsigned longに変換します。
  unsigned long strtoul(const std::string& string) {
    char* e = 0;
    return std::strtoul(string.c_str(), &e, 10);
  }
  
  // 点群のデータ・ファイル（*.pcd）を読み込みます。
  PointCloud::Ptr loadPointCloudFile(const std::string& file_path) {
    PointCloud::Ptr loaded_point_cloud(new PointCloud());
    pcl::io::loadPCDFile(file_path, *loaded_point_cloud);

    // ファイルにはROSヘッダーに相当する情報がないので、独自に設定します。
    std_msgs::Header header;
    header.stamp = ros::Time().fromNSec(point_cloud_utilities::strtoul(file_path.substr(file_path.size() - 4 - 19, 19)));
    loaded_point_cloud->header = pcl_conversions::toPCL(header);
    
    return loaded_point_cloud;
  }

  std::string filePathString(PointCloud::ConstPtr point_cloud) {
    std::stringstream file_path_stringstream;
    file_path_stringstream << "/tmp/six_point_two_eight_" << std::setfill('0') << std::setw(19) << pcl_conversions::fromPCL(point_cloud->header).stamp.toNSec() << ".pcd";

    return file_path_stringstream.str();
  }

  // 略。

  // 点群のデータ・ファイルを削除します。
  PointCloud::Ptr removePointCloudFile(PointCloud::ConstPtr point_cloud) {
    std::remove(filePathString(point_cloud).c_str());

    return PointCloud::Ptr(new PointCloud(*point_cloud));
  }

  // 点群を表示します。
  PointCloud::Ptr showPointCloud(PointCloud::ConstPtr point_cloud, const std::string& id, int rgb) {
    // PCLVisualizerのインスタンスが生成されていない場合は、生成します。
    if (!visualizer_) {
      visualizer_.reset(new pcl::visualization::PCLVisualizer("Viewer"));
    }

    // 点群の表示色を作成します。
    pcl::visualization::PointCloudColorHandlerCustom<Point> point_cloud_color(point_cloud, (rgb >> 16) & 0x00ff, (rgb >> 8) & 0x00ff, rgb & 0x00ff);
    
    // 点群を表示します。
    if (!visualizer_->updatePointCloud(point_cloud, point_cloud_color, id)) {  // 点群の追加（updatePointCloud()の戻り値が偽）の場合は、
      visualizer_->addPointCloud(point_cloud, point_cloud_color, id);          // 色と表示方法を設定します。
      visualizer_->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, id);
    }
    
    return PointCloud::Ptr(new PointCloud(*point_cloud));
  }

  // PCLVisualizerを再描画します。
  void spinVisualizer() {
    visualizer_->spinOnce(100);
  }
  
  // 略。
}

// 略。

geometry_msgs::Point six_point_two_eight::getCentroidOfPointCloud2(sensor_msgs::PointCloud2ConstPtr points) {
  Eigen::Vector4f centroid = point_cloud_utilities::getCentroidOfPointCloud(point_cloud_utilities::fromROSMsg(points));

  // Eigen::Vector4fをgeometry_msgs::Pointに変換します。
  geometry_msgs::Point point;
  point.x = centroid.x();
  point.y = centroid.y();
  point.z = centroid.z();

  return point;
}

// 略。

sensor_msgs::PointCloud2Ptr six_point_two_eight::loadPointCloud2File(const std::string& file_path) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::loadPointCloudFile(file_path));
}

// 略。

sensor_msgs::PointCloud2Ptr six_point_two_eight::removePointCloud2File(sensor_msgs::PointCloud2ConstPtr points) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::removePointCloudFile(point_cloud_utilities::fromROSMsg(points)));
}

// 略。

sensor_msgs::PointCloud2Ptr six_point_two_eight::showPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& id, int rgb) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::showPointCloud(point_cloud_utilities::fromROSMsg(points), id, rgb));
}

void six_point_two_eight::spinVisualizer() {
  point_cloud_utilities::spinVisualizer();
}

sensor_msgs::PointCloud2Ptr six_point_two_eight::transformPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double x_adjust) {
  return point_cloud_utilities::toROSMsg(point_cloud_utilities::transformPointCloud(point_cloud_utilities::fromROSMsg(points), target_frame, x_adjust));
}

include/six_point_two_eight/register_models.h

メインの処理です。ちょっと複雑なので、今回はコメントをいっぱい入れてみました。onInit()の前半ではBoost Rangeを使って快調にプログラミングできたのですけれど、二周目の点群のインデックスの取得や点群のレジストレーションでは、Boost Rangeでキレイに処理を書けませんでした……。なので、メンバー関数として抽出して隠しています。

二周目の点群のインデックス取得処理（getNextRoundStartingIndex()）では、上で述べたように点群の重心間の距離で判断しています。一番距離が近い点群の「インデックス」を取得するのですけれど、インデックス化に必要なUbuntu 14.04のBoost 1.54のboost::adaptors::indexedは使いづらかった（1.60になると使いやすそう）ので、forループで実現しました。あと、1番目と2番目の点群の重心の距離は近いでしょうから、処理の対象は点群の集合の後ろ半分としています。

点群の表示(shorPointCloud2())では、点群のIDと色が常に同じになるように、点群のタイムスタンプを使用しました。

点群のレジストレーション処理（registerPointCloud2s()）では、最初の点群に2番目の点群をレジストレーションした結果に3番目の点群をレジストレーションした結果に4番目の……と繰り返して処理が続きます。これは、関数型言語でreduceとかfoldと呼ばれる処理パターンですね。C++の場合は名前が変わって、accumulateになります。accumulateに渡すラムダ式の中では、どこまで処理が進んだかを我々にレポートさせるためにROS_INFO_STREAMでログ出力して、レジストレーションして、表示して、レジストレーション済み点群の集合に追加して、次のターンのためにレジストレーション済み点群の集合をリターンしています。

あと、make_world_modelsの結果の統合では二周目の点群を削除、make_target_modelsなら何もしないように分岐しなければなりません。これは、パラメーターで実現することにしましょう。Nodeletでパラメーターを取得するには、getPrivateNodeHandle().getParam("パラメーター名", 変数)とします。

#pragma once

#include <boost/filesystem.hpp>
#include <boost/range.hpp>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/numeric.hpp>
#include <limits>
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <thread>

#include <sensor_msgs/PointCloud2.h>

#include "six_point_two_eight/point_cloud_utilities.h"
#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class RegisterModels : public nodelet::Nodelet {
  private:
    bool cut_next_round_;
    std::thread working_thread_;

    // six_point_two_eightのデータ・ファイルかどうかを判定します。
    auto isSixPointTwoEightFile(const boost::filesystem::directory_entry& directory_entry) const {
      auto path = directory_entry.path();
      auto file_prefix = std::string("six_point_two_eight_");

      return 
        path.filename().string().substr(0, file_prefix.size()) == file_prefix && 
        path.extension().string() == ".pcd";
    }

    // 二周目の点群のインデックスを計算します。
    // 最初の点群の重心に最も近い重心を持つ点群を一周目の最後とみなして、処理を実施します。
    auto getNextRoundStartingIndex(const std::vector<sensor_msgs::PointCloud2ConstPtr>& point_clouds) const {
      auto next_round_starting_index = point_clouds.size();

      // Ubuntu14.04のBoostのバージョンは1.54で、1.54のboost::adaptors::indexedはとても使いづらいです。
      // なので、forループで実装しました。
      
      // これまでで最小の重心間の距離。
      auto min_distance = std::numeric_limits<double>::max();
      for (auto i = point_clouds.size() / 2; i < point_clouds.size(); ++i) {
        // 重心を計算します。
        auto centroid_1 = getCentroidOfPointCloud2(point_clouds[0]);
        auto centroid_2 = getCentroidOfPointCloud2(point_clouds[i]);

        // 重心間の距離を計算します。
        auto distance = 
          std::sqrt(std::pow(centroid_1.x - centroid_2.x, 2) + std::pow(centroid_1.y - centroid_2.y, 2));  // 上下には動かないので、zは無視します。
          
        // もし、重心間の距離がこれまでの点群よりも小さいなら、そこを一周目の終わり（仮）とします。
        if (distance < min_distance) {
          min_distance = distance;
          next_round_starting_index = i + 1;  // 計算対象は二周目の最初なので、+1します。
        }
      }

      return next_round_starting_index;
    }

    // 点群を表示します。
    auto showPointCloud2(sensor_msgs::PointCloud2ConstPtr point_cloud) const {
      std::vector<int> colors{0x00ff0000, 0x00ffff00, 0x0000ffff, 0x000000ff, 0x00ff00ff};

      std::stringstream id;
      id << point_cloud->header.stamp;

      return 
        six_point_two_eight::showPointCloud2(
          point_cloud, 
          id.str(), 
          colors[(point_cloud->header.stamp.toNSec() / 1000) & colors.size()]);
    }

    // 点群の集合を表示します。
    auto showPointCloud2s(const std::vector<sensor_msgs::PointCloud2ConstPtr>& point_clouds) {
      boost::for_each(point_clouds, std::bind(&RegisterModels::showPointCloud2, this, std::placeholders::_1));
    }

    // 点群の集合をレジストレーションします。
    auto registerPointCloud2s(const std::vector<sensor_msgs::PointCloud2ConstPtr>& point_clouds) const {
      // Ubuntu14.04のBoostのバージョンは1.54で、1.54のboost::adaptors::indexedが使いづらいので、変数管理します。
      int index = 1;
      
      return
        boost::accumulate(
          point_clouds | boost::adaptors::sliced(1, boost::distance(point_clouds)),  // レジストレーションするのは、2番目の点群から。
          std::vector<sensor_msgs::PointCloud2ConstPtr>{point_clouds.front()},       // 1番目の点群は、レジストレーション不要。
          [&](auto& registered_point_clouds, const auto& point_cloud) {
            ROS_INFO_STREAM("Register " << index++ << "/" << (point_clouds.size() - 1) << ".");
            
            registered_point_clouds.push_back(
              this->showPointCloud2(
                registerPointCloud2(point_cloud, registered_point_clouds.back())));  // レジストレーション済みの最後の点群に、レジストレーションします。

            spinVisualizer();
            
            return registered_point_clouds;
          });
    }
    
  public:
    void onInit() {
      // パラメーターを取得します。
      getPrivateNodeHandle().getParam("cut_next_round", cut_next_round_);
      
      working_thread_ = std::thread(
        [&]() {
          // boost::sort()はRandom Access Rangeを要求するので、std::vectorに変換します。
          std::vector<std::string> six_point_two_eight_file_paths;
          boost::copy(
            // ファイル一覧を作成して、
            boost::make_iterator_range(boost::filesystem::directory_iterator(boost::filesystem::path("/tmp")), {}) |
            // six_point_two_eightのファイルだけを抽出して、
            boost::adaptors::filtered(std::bind(&RegisterModels::isSixPointTwoEightFile, this, std::placeholders::_1)) |
            // 文字列に変換します。
            boost::adaptors::transformed([](const auto& directory_entry) { return directory_entry.path().string(); }),
            std::back_inserter(six_point_two_eight_file_paths));
          
          // ファイル名（時刻がファイル名に含まれているので、時刻順になる）でソートします。
          boost::sort(six_point_two_eight_file_paths);
          
          // 後の処理で複数回使用したいので、std::vectorに変換しておきます。
          std::vector<sensor_msgs::PointCloud2ConstPtr> point_clouds;
          boost::copy(
            // ファイルを読み込みます。
            six_point_two_eight_file_paths | boost::adaptors::transformed(loadPointCloud2File),
            std::back_inserter(point_clouds));
          
          // make_world_modelsのデータの場合は、データを一周分に制限します。
          if (cut_next_round_) {
            auto next_round_starting_index = getNextRoundStartingIndex(point_clouds);

            // 一周目のデータを抽出します。
            std::vector<sensor_msgs::PointCloud2ConstPtr> this_round_point_clouds;
            boost::copy(
              point_clouds | boost::adaptors::sliced(0, next_round_starting_index),
              std::back_inserter(this_round_point_clouds));

            // 無関係なファイルが残っていると混乱するので、削除します。
            boost::for_each(
              point_clouds | boost::adaptors::sliced(next_round_starting_index, boost::distance(point_clouds)),
              removePointCloud2File);

            // 一周目のデータのみを、レジストレーションの対象に設定。
            point_clouds = this_round_point_clouds;
          }

          // とりあえず、表示します。
          this->showPointCloud2s(point_clouds);
          spinVisualizer();

          // 点群をレジストレーションします。
          std::vector<sensor_msgs::PointCloud2ConstPtr> registered_point_clouds;
          boost::copy(
            // レジストレーションした結果を、
            this->registerPointCloud2s(point_clouds) |
            // 保存します。
            boost::adaptors::transformed(savePointCloud2File),
            std::back_inserter(registered_point_clouds));
          
          // 結果を表示します。
          this->showPointCloud2s(registered_point_clouds);
          while (ros::ok()) {
            spinVisualizer();
          }
        });
    }
  };
}

src/six_point_two_eight.cpp

register_models.hがコンパイルされるように#includeして、six_point_two_eight::RegisterModelsをエクスポートしましょう。

#include <pluginlib/class_list_macros.h>

#include "six_point_two_eight/get_point_cloud_2_server.h"
#include "six_point_two_eight/make_target_models.h"
#include "six_point_two_eight/make_world_models.h"
#include "six_point_two_eight/move_base_server.h"
#include "six_point_two_eight/point_cloud_2_throttle.h"
#include "six_point_two_eight/register_models.h"  // 追加。

PLUGINLIB_EXPORT_CLASS(six_point_two_eight::GetPointCloud2Server, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeTargetModels, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MakeWorldModels, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::MoveBaseServer, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::PointCloud2Throttle, nodelet::Nodelet)
PLUGINLIB_EXPORT_CLASS(six_point_two_eight::RegisterModels, nodelet::Nodelet)  // 追加。

six_point_two_eight.xml

いつものように、Nodeletに名前を付けてください。本稿でsix_point_two_eight.xmlを編集するのもこれで最後、単純作業はもうありませんのでご安心を。

<library path="lib/libsix_point_two_eight">
  <class name="six_point_two_eight/get_point_cloud_2_server" type="six_point_two_eight::GetPointCloud2Server" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/make_target_models" type="six_point_two_eight::MakeTargetModels" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/make_world_models" type="six_point_two_eight::MakeWorldModels" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/move_base_server" type="six_point_two_eight::MoveBaseServer" base_class_type="nodelet::Nodelet"/>
  <class name="six_point_two_eight/point_cloud_2_throttle" type="six_point_two_eight::PointCloud2Throttle" base_class_type="nodelet::Nodelet"/>
  // 追加。
  <class
    name="six_point_two_eight/register_models"
    type="six_point_two_eight::RegisterModels"
    base_class_type="nodelet::Nodelet"/>
</library>

launch/register_models.launch

起動用のlaunchファイルを作成します。パラメーターの値は、register_models.launchを<include>するregister_world_models.launchとregister_target_models.launchに設定させるようにしましょう。

<launch>
  <arg name="cut_next_round"/>
  
  <node pkg="nodelet" type="nodelet" name="six_point_two_eight_nodelet_manager" args="manager" output="screen"/>
  <node pkg="nodelet" type="nodelet" name="register_models" args="load six_point_two_eight/register_models six_point_two_eight_nodelet_manager" output="screen">
    <param name="cut_next_round" value="$(arg cut_next_round)"/>
  </node>
</launch>

launch/register_world_models.launch

make_world_modelsの結果の統合用のlaunchファイルでは、cut_next_roundをtrueに。

<launch>
  <include file="$(find six_point_two_eight)/launch/register_models.launch">
    <arg name="cut_next_round" value="true"/>
  </include>
</launch>

launch/register_target_models.launch

そして、make_world_modelsの結果の統合用のlaunchファイルでは、cut_next_roundをfalseに設定します。

<launch>
  <include file="$(find six_point_two_eight)/launch/register_models.launch">
    <arg name="cut_next_round" value="false"/>
  </include>
</launch>

PCLを使用して、点群をレジストレーションする

というわけで、PCLを使った点群のレジストレーションをやってみましょう。

CUIは便利

最初に断っておきますけど、ここから先で迷走して、大量の試行錯誤をします。make_world_modelsでデータ作成→register_world_modelsでデータ統合をやる方式だと、実施する回数が多いのでかなり大変でしょう。だから、make_world_modelsやmake_target_modelsの結果を適当なディレクトリに退避しておいてください。以下のような感じ。

$ rm -f /tmp/six_point_two_eight_*.pcd && roslaunch six_point_two_eight make_world_models.launch
$ mkdir -p ~/Desktop/world/in && cp /tmp/six_point_two_eight_*.pcd ~/Desktop/world/in
$ rm -f /tmp/six_point_two_eight_*.pcd && roslaunch six_point_two_eight make_target_models.launch
$ mkdir -p ~/Desktop/target/in && cp /tmp/six_point_two_eight_*.pcd ~/Desktop/target/in

register_modelsのコードを修正した後は、以下のコマンドでテストしてください。

$ catkin_make && rm -f /tmp/six_point_two_eight_*.pcd && cp ~/Desktop/world/in/six_point_two_eight_*.pcd /tmp && roslaunch six_point_two_eight register_world_models.launch
$ catkin_make && rm -f /tmp/six_point_two_eight_*.pcd && cp ~/Desktop/target/in/six_point_two_eight_*.pcd /tmp && roslaunch six_point_two_eight register_target_models.launch

入力が面倒だというなら、もちろんシェル・スクリプトを作成しても構いません。うん、CUIって便利ですね。

チュートリアルに従ってみる

PCLのチュートリアルのHow to use iterative closest pointを参考に、プログラムを組んでみましょう。

include/six_point_two_eight/point_cloud_utilities.h

ダミー実装を削除します。

// 略。

namespace six_point_two_eight {
  sensor_msgs::PointCloud2Ptr downsamplePointCloud2(sensor_msgs::PointCloud2ConstPtr points, double leaf_size);
  geometry_msgs::Point getCentroidOfPointCloud2(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr getSpherePointCloud2(sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Point& centroid, double radius);
  sensor_msgs::PointCloud2Ptr loadPointCloud2File(const std::string& file_path);
  sensor_msgs::PointCloud2Ptr registerPointCloud2(  // 修正。
    sensor_msgs::PointCloud2ConstPtr source_points, sensor_msgs::PointCloud2ConstPtr target_points);
  sensor_msgs::PointCloud2Ptr removeFloorFromPointCloud2(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr removePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr showPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& id, int rgb);
  void spinVisualizer();
  sensor_msgs::PointCloud2Ptr transformPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double z_adjust = 0.0);
}

src/point_cloud_utilities.cpp

チュートリアルを参考に、実装を書きます。チュートリアルで使用しているpcl::IterativceClosestPoint::setInputCloud()の使用は推奨されていないみたいなので、setInputSource()に変えました。

#include <boost/foreach.hpp>
#include <pcl/common/centroid.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>  // 追加。
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <tf/transform_listener.h>

#include "six_point_two_eight/point_cloud_utilities.h"

namespace point_cloud_utilities {
  // 略。
  
  PointCloud::Ptr registerPointCloud(PointCloud::ConstPtr source_point_cloud, PointCloud::ConstPtr target_point_cloud) {
    pcl::IterativeClosestPoint<Point, Point> icp;
    icp.setInputSource(source_point_cloud);
    icp.setInputTarget(target_point_cloud);

    PointCloud::Ptr registered_point_cloud(new PointCloud());
    icp.align(*registered_point_cloud);

    if (!icp.hasConverged()) {
      ROS_WARN_STREAM("Can't register...");
      throw std::exception();
    }
    
    return registered_point_cloud;
  }

  // 略。
}

// 略。

sensor_msgs::PointCloud2Ptr six_point_two_eight::registerPointCloud2(
  sensor_msgs::PointCloud2ConstPtr source_points, sensor_msgs::PointCloud2ConstPtr target_points) 
{
  return
    point_cloud_utilities::toROSMsg(
      point_cloud_utilities::registerPointCloud(
        point_cloud_utilities::fromROSMsg(source_points),
        point_cloud_utilities::fromROSMsg(target_points)));
}

// 略。

実行してみると、全然駄目……

プログラムが出来上がったので試してみると、これがもう、全然駄目……。

チュートリアルのコードのコピペでレジストレーション

チュートリアルのコードのコピペでレジストレーション

register_world_modelsなんか、処理する前の方がまともな有り様。どうしましょ？

残りのチュートリアルを読んでみる

とりあえず、残りのチュートリアルを読んでみましょう。

How to incrementally register pair of clouds

法線を使って処理しているみたい。あと、pcl::IterativeClosestPointNonLinearってのを使っています。で、コード中で呼び出しているsetTransformationEplilon()とsetMaxCorrespondenceDistance()とsetPointRepresentation()って何？

Interactive Iterative Closest Point

先ほど作成した失敗コードと同じやり方。でも、ドキュメント上では、ピッタリとレジストレーションできています。でも、このGIFアニメの点群って、少し移動と回転をさせた同じ点群なのでは？　我々が今苦労している、異なる点群のレジストレーションとは条件が違いそう……。

How to use Normal Distribution Transform

pcl::IterativeClosestPointクラスじゃなくて、pcl::NormalDistributionsTransformクラスってのを使っています。レジストレーションのアルゴリズムって、複数あるみたいですね。Normal Distribution Transformアルゴリズムは、点群が大きい時に有効だと書いてある……。

試してみる

ここまでで、適当にコードを真似して試してみました。結果は、良くはなるけれど、それでもまだ誤差が大きくて駄目。register_world_modelsでは、一周しても最初の場所に戻りません。register_target_modelsでは、途中であらぬ方向にずれていきます。

……アルゴリズムとパラメーターについて真面目に調べないと駄目みたいですね。

リファレンスを読んでみる

そう、リファレンスを読めば、アルゴリズムとパラメーターを理解できるはず。読んでみました。

pcl::IterativeClosestPointのリファレンス

pcl::IterativeClosestPointのリファレンスを読んでみると、Iterative Closest Pointアルゴリズムの基本実装だって書いてあります。応用があるってことかなぁ。継承関係の図を見ると、pcl::IterativeClosestPointNonLinearとpcl::IterativeClosestPointWithNormalsとpcl::JointIterativeClosestPointの3つの派生クラスが見つかりました。

あと、3つの終了条件があると書いてあります。

  1. 繰り返しの回数（setMaximumIterations()で設定）
  2. 一回前の座標変換と今回の座標変換の間の差（setTransformationEpsilon()で設定）
  3. ユークリッド座標系での差の合計（setEuclideanFitnessEpsilon()で設定）

でも、メンバー関数のドキュメントを見ても詳細は載っておらず、単位が何なのかすら分かりません……。1はともかくとして、2と3は何をどんなふうに指定するんでしょうか？　唯一単位が推測できる繰り返しの回数を、1万と10万にしてみたけど実行時間は変わりません。一体どういうこと？

リファレンス中にサンプル・コードもありました。その中でsetMaxCorrespondenceDistance()で何か設定していて、設定した値よりも離れた点の対応は無視されると書いてあるけど、無視するってのはどういう意味なのでしょうか？

pcl::IterativeClosestPointNonLinearのリファレンス

Levenberg-Marquard法を取り入れたと書いてあります。それ以外はpcl::IterativeClosestPointと同じ。Levenberg-Marquard法ってのを少し調べてみると、局所解に陥るのをうまいこと防ぎながら、それなりに速く答えを探してくれるアルゴリズムみたい。試してみると、pcl::IterativeClosestPointでは中途半端にしかレジストレーションしない場合にも、pcl::IterativeClosestPointNonlinearだとさらに探索してくれました。遅いですけどね。このあたりで、統合に時間がかかるのは諦めることにしました。

pcl::IterativeClosestPointWithNomalのリファレンス

解説が1行しか書いてなくて、なんだか分かりませんでした……。名前からして、法線を使うんでしょう。法線を使うバージョンはチュートリアルのところでも試してみたけど、あまりうまくいきませんでした。ごちゃごちゃしていて面があまりないデータだからかなぁと。だから、このクラスは無視で。

pcl::JointIterativeClosestPointのリファレンス

同じ座標変換を共有する複数のフレーム向け（multiple frames which share the same transform）って書いてあるけど、意味がわかりません……。続いて"This is particularly useful when solving for camera extrinsics using multiple observations"って書いてあるけど、私にはどんな場合に有効なのか分かりません……。

pcl::NormalDistributionsTransformのリファレンス

論文を読むように書いてありました……。でも論文はダウンロード出来ませんでした。将来、暇でどうしようもないときにもう一度試すことにしましょう。

試してみる

pcl::IterativeClosestPointとpcl::IterativeClosestPointNonLinearを試してみました。pcl::IterativeClosestPointNonLinearの方が、大きな間違いをしないようです。というわけで、今回はpcl::InteractiveClosestPointNonLinearを採用。

パラメーターもいろいろな値を試してみました。setMaxCorrespondenceDistance()を0.05のような小さな値にすると精度が高まるのですけど、「Not enough correspondences found. Relax your threshold parameters」と表示されてエラーになってしまう場合もありました。setMaximumIterations()とsetTransformationEpsilon()とsetEuclideanFitnessEpsilon()については、結果が変わる場合と変わらない場合があって、もうなんだか分かりません。

……ドキュメントの品質が低いことだけが分かりました。具体的なコードがどこかにないかなぁ。

テスト・コードを読んでみる

PCLはオープン・ソースなので、ソース・コードのダウンロードが可能です。適当なディレクトリでgit clone http://github.com/PointCloudLibrary/pcl.gitして、テスト・コードを読んでみましょう。

test_fpcs_ia.cpp、test_kfpcs_ia.cpp

これまでに見つけた以外にも、pcl::registration::FPCSInitialAlignmentクラスとpcl::registration::KFPCSInitialAlignmentクラスでもレジストレーションできることが分かりました。ただ、すでに手を広げすぎているので、これらのクラスは次の機会に調査することにしましょう。

test_registration.cpp

コードを読んだら、pcl::JointIterativeClosestPointクラスの用途が分かりました。ソースとターゲットの両方に複数の点群を設定し、ソース群からターゲット群への座標変換を計算するためのクラスみたいです。やっぱりコードがあると、いろいろ明確になるなぁ。今回の処理には無関係ですけど。

あと、pcl::registration::CorresondenceRejectorクラスのサブ・クラスを使うと、対応する点だとみなさない条件を付加できることも分かりました。ただ、ドキュメントでCorresondenceRejectorのサブ・クラスを調べた限りでは、今回の処理に役立ちそうなのは見つかりませんでした。残念だけどないものはしょうがない。

setMaxCorrespondenceDistance()の使い方も、分かりました。対応する点とみなす距離のしきい値なんですね。値を小さくすると遠くの点に引っ張られた不正なレジストレーションをする危険性が減って精度が上がり、でも値が小さすぎると対応する点が見つからないのでレジストレーション出来なくてエラーになるというわけ。

試してみる

setMaxCorrespondenceDistance()については、試した限りでは推測通りの動きをしました。大きめの値でレジストレーションしたあとに、小さめの値でレジストレーションすれば良さそうです。

テスト熱中症の方たちの言うとおり、テスト・コードはドキュメントなんですね。テスト・コードはとても有用でした。でも、setMaximumIterations()とsetTransformationEpsilon()とsetEuclideanFitnessEpsilon()については、まだ分かりません。動きが分からないと、どうにもならなそうです。

ログを出力してみる

PCLはエラーの場合しかログを出力してくれない、ROSのドキュメントに従ってログ・レベルを変更してもログが増えないなと思っていたら、PCLのログ・レベルはROSとは別に設定しなければならないことが分かりました。

src/point_cloud_utilities.cpp

align()のところの、ログ・レベルを変更しました。PCLのログ・レベルの設定は、pcl::console::setVerbosityLevel()関数で実施します。

PointCloud::Ptr registerPointCloud(PointCloud::ConstPtr source_point_cloud, PointCloud::ConstPtr target_point_cloud) {
  // 実験で精度が高かった、IterativeClosestPointNonLinearを使用します。
  pcl::IterativeClosestPointNonLinear<Point, Point> icp;
  icp.setInputSource(source_point_cloud);
  icp.setInputTarget(target_point_cloud);

  // 遠くの点に引きづられないように、近くの点同士で対応付けるようにします。
  icp.setMaxCorrespondenceDistance(0.1);
    
  // デバッグのために、ログ・レベルを設定します。
  pcl::console::VERBOSITY_LEVEL verbosity_level = pcl::console::getVerbosityLevel();
  pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
    
  PointCloud::Ptr registered_point_cloud(new PointCloud());
  icp.align(*registered_point_cloud);

  // ログ・レベルを元に戻します。
  pcl::console::setVerbosityLevel(verbosity_level);
    
  if (!icp.hasConverged()) {
    ROS_WARN_STREAM("Can't register...");
    throw std::exception();
  }
    
  return registered_point_cloud;
}

実行して、ログを調べましょう。

[ INFO] [1458613763.391897758]: Register 1/18.
[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code 1, having a residual norm of 2.97591. 
Final solution: [0.002378 0.001121 -0.001424 -0.000064 -0.000951 -0.002097]
[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 1 out of 10.
[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 0.999989 rotation (cosine) and 0.000009 translation.
[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: 179769313486231570814527423731704356798070567525844996598917476803157260780028538760589558632766878171540458953514382464234321326889464182768467546703537516986049910576551282076245490090389328944075868508455133942304583236903222948165808559332123348274797826204144723168738177180919299881250404026184124858368.000000 / 0.000269.
[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code 1, having a residual norm of 3.00172. 
Final solution: [0.001883 0.000129 -0.001032 -0.000765 -0.000606 -0.001603]
[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 2 out of 10.
[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 0.999993 rotation (cosine) and 0.000005 translation.
[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: 0.000269 / 0.000270.
[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code 1, having a residual norm of 3.04932. 
Final solution: [0.001608 0.001652 -0.002038 -0.000862 -0.001092 -0.002114]

（略）

[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 7 out of 10.
[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 0.999990 rotation (cosine) and 0.000006 translation.
[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: 0.000305 / 0.000314.
[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code 1, having a residual norm of 3.39458. 
Final solution: [0.000741 0.002286 -0.000316 -0.000582 -0.000212 -0.002052]
[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 8 out of 10.
[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 0.999991 rotation (cosine) and 0.000006 translation.
[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: 0.000314 / 0.000331.
[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code 1, having a residual norm of 3.44345. 
Final solution: [0.000557 0.002939 0.000277 -0.000344 0.000082 -0.002317]
[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 9 out of 10.
[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 0.999989 rotation (cosine) and 0.000009 translation.
[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: 0.000331 / 0.000339.
[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code 1, having a residual norm of 3.50634. 
Final solution: [0.000586 -0.000040 -0.000448 -0.001298 -0.000152 -0.000817]
[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 10 out of 10.
Transformation is:
    0.999138    0.040513    -0.009068   0.012273
    -0.040389   0.999094    0.013426    0.017471
    0.009603    -0.013048   0.999869    -0.008465
    0.000000    0.000000    0.000000    1.000000

Iterationが10回で終わってしまっています。ログ中のCurrent transformationは、繰り返しあたりの座標変換でしょう。Previous / Current MSE for correspondences distancesというのは、一つ前と今回の繰り返しでのレジストレーション後のズレでしょう。このログから、10回目でもまだ座標変換をしていて、ズレが減るどころか増えていることが分かります。setMaxIterationCounts()が必要でしょう。

setMaxIterationCounts()

コードに、setMaxIterationCounts()を追加しました。繰り返し回数は、少し多めに500回にします。

PointCloud::Ptr registerPointCloud(PointCloud::ConstPtr source_point_cloud, PointCloud::ConstPtr target_point_cloud) {
  pcl::IterativeClosestPointNonLinear<Point, Point> icp;
  icp.setInputSource(source_point_cloud);
  icp.setInputTarget(target_point_cloud);

  icp.setMaxCorrespondenceDistance(0.1);  // 遠くの点に引きづられないように、近くの点同士を対応付けるようにします。
  
  // 繰り返しの回数を設定します。
  icp.setMaximumIterations(500);
    
  pcl::console::VERBOSITY_LEVEL verbosity_level = pcl::console::getVerbosityLevel();  // デバッグのために、ログ・レベルを設定します。
  pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
    
  PointCloud::Ptr registered_point_cloud(new PointCloud());
  icp.align(*registered_point_cloud);

  pcl::console::setVerbosityLevel(verbosity_level);  // ログ・レベルを元に戻します。
    
  if (!icp.hasConverged()) {
    ROS_WARN_STREAM("Can't register...");
    throw std::exception();
  }
    
  return registered_point_cloud;
}

結果は、どうでしょうか？

[ INFO] [1458614620.310489286]: Register 1/18.
[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code 1, having a residual norm of 2.97591. 
Final solution: [0.002378 0.001121 -0.001424 -0.000064 -0.000951 -0.002097]
[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 1 out of 500.
[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 0.999989 rotation (cosine) and 0.000009 translation.
[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: 179769313486231570814527423731704356798070567525844996598917476803157260780028538760589558632766878171540458953514382464234321326889464182768467546703537516986049910576551282076245490090389328944075868508455133942304583236903222948165808559332123348274797826204144723168738177180919299881250404026184124858368.000000 / 0.000269.
[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code 1, having a residual norm of 3.00172. 
Final solution: [0.001883 0.000129 -0.001032 -0.000765 -0.000606 -0.001603]
[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 2 out of 500.
[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 0.999993 rotation (cosine) and 0.000005 translation.
[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: 0.000269 / 0.000270.
[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code 1, having a residual norm of 3.04932. 
Final solution: [0.001608 0.001652 -0.002038 -0.000862 -0.001092 -0.002114]

（略）

[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 32 out of 500.
[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 1.000000 rotation (cosine) and 0.000000 translation.
[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: 0.000438 / 0.000439.
[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code 1, having a residual norm of 3.97762. 
Final solution: [0.000040 -0.000006 -0.000010 -0.000036 -0.000011 -0.000091]
[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 33 out of 500.
[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 1.000000 rotation (cosine) and 0.000000 translation.
[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: 0.000439 / 0.000440.
[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code 1, having a residual norm of 3.98374. 
Final solution: [0.000033 0.000006 -0.000072 -0.000048 -0.000017 -0.000095]
[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 34 out of 500.
[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 1.000000 rotation (cosine) and 0.000000 translation.
[pcl::DefaultConvergenceCriteria::hasConverged] Previous / Current MSE for correspondences distances is: 0.000440 / 0.000441.
[pcl::registration::TransformationEstimationLM::estimateRigidTransformation] LM solver finished with exit code 1, having a residual norm of 3.98739. 
Final solution: [0.000000 0.000000 0.000000 0.000000 0.000000 0.000000]
[pcl::DefaultConvergenceCriteria::hasConverged] Iteration 35 out of 500.
[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 1.000000 rotation (cosine) and 0.000000 translation.
Transformation is:
    0.998080    0.060976    -0.010927   0.015213
    -0.060726   0.997914    0.021906    0.024033
    0.012240    -0.021201   0.999700    -0.010746
    0.000000    0.000000    0.000000    1.000000

時間はかかりますけど、Iterationの回数が増え、精度が高くなっています。私のデータ（make_world_models）の場合、繰り返し回数は1つ目の点群で35、2つ目の点群で31、以後、27、32、32、57、45、45、42、54……となりました。Iterationはこの程度の値で収束するので、だから、リファレンスを読んだ時に試した、1万とか10万とかの設定は無意味だったんですね。

さて、このコードでの結果を見ると、まだまだズレが大きすぎます。

make_world_modelsのデータの場合、一周すると床の位置がずれちゃっています。

setMaximumIterationsしてレジストレーション

make_target_modelsのデータの場合、途中でカバンが横に倒れてしまっています。

setMaximumIterationsしてレジストレーション

まだ駄目ですね。

setTransformationEpsilon()

だから、終了条件の2番目、setTransformationEpsilon()を設定してみましょう。この値はCurrent transformationの値だと思われます。ログではすでに1.000000や0.000000になっていますので、桁を1つ増やしました。

PointCloud::Ptr registerPointCloud(PointCloud::ConstPtr source_point_cloud, PointCloud::ConstPtr target_point_cloud) {
  pcl::IterativeClosestPointNonLinear<Point, Point> icp;
  icp.setInputSource(source_point_cloud);
  icp.setInputTarget(target_point_cloud);

  icp.setMaxCorrespondenceDistance(0.1);
  icp.setMaximumIterations(500);

  // 処理を終了する、座標変換のしきい値を設定します。
  icp.setTransformationEpsilon(1.0e-8);
    
  pcl::console::VERBOSITY_LEVEL verbosity_level = pcl::console::getVerbosityLevel();  // デバッグのために、ログ・レベルを設定します。
  pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
    
  PointCloud::Ptr registered_point_cloud(new PointCloud());
  icp.align(*registered_point_cloud);

  pcl::console::setVerbosityLevel(verbosity_level);  // ログ・レベルを元に戻します。
    
  if (!icp.hasConverged()) {
    ROS_WARN_STREAM("Can't register...");
    throw std::exception();
  }
    
  return registered_point_cloud;
}

make_world_modelsのデータでIterationの回数を見ると、29、26、29、27、28、54、45、40, 38、54……となりました。設定する前は35、31、27、32、32、57、45、45、42、54でしたので、あれ、減ってしまっています。

ただ、目で見て分かる範囲では、結果に違いはなさそうです。

setTransformationEpsilonレジストレーション

setTransformationEpsilonレジストレーション

もう一桁増やして1.0e-9にしても、やっぱり変わりません。

setTransformationEpsilonレジストレーション

というわけで、setTransformationEpsilon()する値は、1.0e-8に決定しましょう。

さて、この先はどうしましょうか？　setEuclideanFitnessEpsilon()の値はログのPrevious / Current MSE for correspondences distancesだと思うのですけど、この値は点群ごとにバラバラなので、しきい値を決めるのは無理のようです。もう、精度向上のために打てる手がありません……。

誤差を分散させる

深度センサーとして使用しているASUS Xtion PRO LIVEのデータに誤差があるのか、PCLのレジストレーションの誤差があるのかは分かりませんけど、精度を向上させることで問題を解消するのは難しいのではないでしょうか？　そもそも、もしどこまでも精度を向上させられるなら、レジストレーションなんかやらないでオドメトリーの精度を向上させればよいわけですしね。

というわけで、誤差の存在を認めて、その誤差を分散させる方法を考えてみましょう。

四元数は、補間が可能

先ほどお見せしたPCLのログには、どのように座標変換したかが4×4の行列として含まれていました。

[pcl::DefaultConvergenceCriteria::hasConverged] Current transformation gave 1.000000 rotation (cosine) and 0.000000 translation.
Transformation is:
    0.998080    0.060976    -0.010927   0.015213
    -0.060726   0.997914    0.021906    0.024033
    0.012240    -0.021201   0.999700    -0.010746
    0.000000    0.000000    0.000000    1.000000

この行列は、getFinalTransformation()メンバー関数で取得できます。戻り値の型はEigen::Matrix4fで、少し工夫すれば座標変換を表現するROSのgeometry_msgs/Transformに変換できます。geometry_msgs/Transformは、その内部にgeometry_msgs/Vector3 translationとgeometry_msgs/Quaternion rotationを持ちます。Quaternionというのは、TFのところで少しだけ触れた四元数ですな。この四元数は、別の四元数との間の補間が可能という素晴らしい特徴を持っています。

3Dグラフィックスでは、四元数を任意の軸を中心にどれだけ回転したかと解釈します。4元数の最初の3つ、xとyとzが回転軸を表現し、残り1つのwが回転量を表現するわけ。で、この4つの数は、4次元の球の表面の座標を表していると考えることもできるらしい（どうしてそうなるのかは、私以外の人に聞いてください）。そして、球の表面上の点から別の点への経路は、3次元の場合は地球上を飛行機で旅するのと同じに計算できます。これは、四次元の球でもかわらないみたい。で、経路なのだから、A地点からB地点への経路を3/4進んだところとかも計算できます。つまり、2つの四元数の間の補間が可能なわけ。

補間ができるという特徴を今回の処理活用してみましょう。最後の点群と最初の点群をレジストレーションした際のrotationを、四元数の補間を使って、点群の数で分割します。このrotationを点群に適用すれば、回転に関する誤差を分散できます。もう一方のtranslationはごく普通のベクトルですから、線形補間できるでしょう。

誤差を均等に分散する

やることが決まりましたので、プログラミングに入ります。

include/six_point_two_eight/point_cloud_utilities.h

registerPointCloud2()関数の戻り値を、geometry_msgs::Transformに変更します。そうしないと、解消の対象の誤差（座標変換）を取得できませんからね。あと、setMaxCorrespondenceDistance()に設定する値を小さくしながらレジストレーションできるようにするために、max_correspondence_distanceという引数も追加しておきます。

上の変更でregisterPointCloud2()は点群の座標変換をしないことにりましたから、geometry_msgs::Transformを引数にとるバージョンのtransformPointCloud2()関数も追加しておきます。これなら、transformPointCloud2(point_cloud, registerPointCloud2(point_cloud, ...))でこれまでと同じ処理も実施できます。座標変換を余計に1回実施する分だけパフォーマンスが落ちますけど、レジストレーションが遅いので無視できるでしょう。

#ifndef SIX_POINT_TWO_EIGHT_POINT_CLOUD_UTILITIES_H
#define SIX_POINT_TWO_EIGHT_POINT_CLOUD_UTILITIES_H

#include <geometry_msgs/Point.h>
#include <geometry_msgs/Transform.h>  // 追加。
#include <sensor_msgs/PointCloud2.h>

namespace six_point_two_eight {
  sensor_msgs::PointCloud2Ptr downsamplePointCloud2(sensor_msgs::PointCloud2ConstPtr points, double leaf_size);
  geometry_msgs::Point getCentroidOfPointCloud2(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr getSpherePointCloud2(sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Point& centroid, double radius);
  sensor_msgs::PointCloud2Ptr loadPointCloud2File(const std::string& file_path);
  geometry_msgs::Transform registerPointCloud2(  // 変更。戻り値の型をgeometry_msgs::Transformに、max_correspondence_distanceを追加。
    sensor_msgs::PointCloud2ConstPtr source_points, sensor_msgs::PointCloud2ConstPtr target_points, double max_correspondence_distance);
  sensor_msgs::PointCloud2Ptr removeFloorFromPointCloud2(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr removePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr savePointCloud2File(sensor_msgs::PointCloud2ConstPtr points);
  sensor_msgs::PointCloud2Ptr showPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& id, int rgb);
  void spinVisualizer();
  sensor_msgs::PointCloud2Ptr transformPointCloud2(  // 追加。
    sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Transform& transform);
  sensor_msgs::PointCloud2Ptr transformPointCloud2(sensor_msgs::PointCloud2ConstPtr points, const std::string& target_frame, double z_adjust = 0.0);
}

#endif

src/point_cloud_utilities.cpp

transformPointCloud()関数は、pcl::transformPointCloud()関数を呼び出すだけです。registerPointCloud()関数の戻り値の変更は、returnの後をicp.getFinalTransformation()に変更したところ。

今回大変だったのは、geometry_msgs::TransformとEigen::Matrix4fの変換という一番簡単そうな処理（fromROSMsg関数とtoROSMsg関数）でした……。この2つの型を変換する関数は存在しなくて、TFの型を挟まなければ変換できなかったためです13。

transformPointCloud2()関数とregisterPointCloud2()関数は、ただのインターフェースです。これまでと同じ形で作成しました。

#include <boost/foreach.hpp>
#include <pcl/common/centroid.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>  // 追加。

#include "six_point_two_eight/point_cloud_utilities.h"

namespace point_cloud_utilities {
  // 略。
  
  // 点群を座標変換します。
  PointCloud::Ptr transformPointCloud(PointCloud::ConstPtr point_cloud, Eigen::Matrix4f transform) {
    PointCloud::Ptr transformed_point_cloud(new PointCloud());
    pcl::transformPointCloud(*point_cloud, *transformed_point_cloud, transform);

    return transformed_point_cloud;
  }

  // 略。

  // 点群をレジストレーションします。
  // 戻り値をEigen::Matrix4fに変更し、引数にmax_correspondence_distanceを追加しました。
  Eigen::Matrix4f registerPointCloud(PointCloud::ConstPtr source_point_cloud, PointCloud::ConstPtr target_point_cloud, double max_correspondence_distance) {
    pcl::IterativeClosestPointNonLinear<Point, Point> icp;
    icp.setInputSource(source_point_cloud);
    icp.setInputTarget(target_point_cloud);
    
    // 点の対応付けをする距離のしきい値を、引数で指定された値に設定します。
    icp.setMaxCorrespondenceDistance(max_correspondence_distance);
    
    icp.setMaximumIterations(500);
    icp.setTransformationEpsilon(1.0e-9);
    
    // pcl::console::VERBOSITY_LEVEL verbosity_level = pcl::console::getVerbosityLevel();  // デバッグのために、ログ・レベルを設定します。
    // pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
    
    PointCloud::Ptr registered_point_cloud(new PointCloud());
    icp.align(*registered_point_cloud);

    // pcl::console::setVerbosityLevel(verbosity_level);  // ログ・レベルを元に戻します。
    
    if (!icp.hasConverged()) {
      ROS_WARN_STREAM("Can't register...");
      throw std::exception();
    }
    
    return icp.getFinalTransformation();  // 戻り値を変更。
  }

  // 略。

  // ROSのメッセージのgeometry_msgs::Transformを、PCLが使用するEigen::Matrix4fに変換します。
  Eigen::Matrix4f fromROSMsg(const geometry_msgs::Transform& transform) {
    // TF→Eigenの変換ライブラリしか無いので、とりあえずTFの型に変換します。
    tf::Transform tf_transform;
    tf::transformMsgToTF(transform, tf_transform);

    Eigen::Matrix4f converted_transform;
    pcl_ros::transformAsMatrix(tf_transform, converted_transform);

    return converted_transform;
  }

  // PCLが使用するEigen::Matrix4fを、ROSのメッセージのgeometry_msgs::Transformに変換します。
  geometry_msgs::Transform toROSMsg(const Eigen::Matrix4f& transform) {
    // TF→Eigenの変換ライブラリしかないので、とりあえずTFの型に変換します。
    tf::Transform tf_transform;
    tf::transformEigenToTF(Eigen::Affine3d(transform.cast<double>()), tf_transform);

    geometry_msgs::Transform converted_transform;
    tf::transformTFToMsg(tf_transform, converted_transform);

    return converted_transform;
  }
}

// 略。

// 点群をレジストレーションします。
// 戻り値をgeometry_msgs::Transformに変更し、引数にmax_correspondence_distanceを追加しました。
geometry_msgs::Transform six_point_two_eight::registerPointCloud2(
  sensor_msgs::PointCloud2ConstPtr source_points, sensor_msgs::PointCloud2ConstPtr target_points, double max_correspondence_distance) 
{
  return
    point_cloud_utilities::toROSMsg(
      point_cloud_utilities::registerPointCloud(
        point_cloud_utilities::fromROSMsg(source_points),
        point_cloud_utilities::fromROSMsg(target_points),
        max_correspondence_distance));
}

// 略。

// 点群を座標変換します。変換の内容は引数で指示します。
sensor_msgs::PointCloud2Ptr six_point_two_eight::transformPointCloud2(
  sensor_msgs::PointCloud2ConstPtr points, const geometry_msgs::Transform& transform) 
{
  return 
    point_cloud_utilities::toROSMsg(
      point_cloud_utilities::transformPointCloud(
        point_cloud_utilities::fromROSMsg(points),
        point_cloud_utilities::fromROSMsg(transform)));
}

// 略。

include/six_point_two_eight/register_models.h

呼び出し側です。誤差の分散とレジストレーションの制御構造は同じ（どちらもboost::accumulateで実現できる）なので、共通部分をprocessPointCloud2s()メンバー関数として抽出しました。引数のstd::functionで、誤差の分散やレジストレーション固有の処理を渡します。

誤差の分散を実施するadjustPointCloud2s()メンバー関数では、四元数やベクトルを補完して誤差を分散させています。ROSのメッセージにはデータの格納以外の機能はなく、補間の計算式をすべて自力でプログラミングするのは大変だったので、TFの型に変換してTF任せで計算をしています。具体的な処理としては、最後の点群と最初の点群のレジストレーションで解消しなければならない誤差（座標変換）の量を計算し、それをprocessPointCloud2s()に渡す引数のラムダ式の中で補間しています。四元数を補間する基準点は、最初の点群に対する座標変換は不要ですから、getIdentity()で取得できる何も変換しない四元数を使用しています。

誤差の分散とレジストレーションの両方を実施する、registerAndAdjustPointCloud2s()メンバー関数も追加しました。max_correspondence_distanceの値を大→小と変更しながら、誤差の変換とレジストレーションを交互に呼び出す形で実装しています。繰り返し回数とmax_correspondence_distanceの値は、何回かプログラムを実行して結果を見ながら調整しました。

#pragma once

#include <boost/filesystem.hpp>
#include <boost/range.hpp>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/numeric.hpp>
#include <limits>
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <thread>

#include <sensor_msgs/PointCloud2.h>

#include "six_point_two_eight/point_cloud_utilities.h"
#include "six_point_two_eight/utilities.h"

namespace six_point_two_eight {
  class RegisterModels : public nodelet::Nodelet {
  private:
    // 略。
    
    // 点群の集合に対して、引数で指示される処理を実施します。registerPointCloud2とadjustPointCloud2の共通部分をメンバー関数として抽出しました。
    auto processPointCloud2s(
      const std::vector<sensor_msgs::PointCloud2ConstPtr>& point_clouds,
      const std::string& caption,
      std::function<void(std::vector<sensor_msgs::PointCloud2ConstPtr>*, const sensor_msgs::PointCloud2ConstPtr&, int)> op) const
    {
      int index = 1;  // Ubuntu14.04のBoostのバージョンは1.54で、1.54のboost::adaptors::indexedが使いづらいので、変数管理します。
      
      return
        boost::accumulate(
          point_clouds | boost::adaptors::sliced(1, boost::distance(point_clouds)),  // 処理するのは、2番目の点群から。
          std::vector<sensor_msgs::PointCloud2ConstPtr>{point_clouds.front()},       // 1番目の点群の処理は不要。
          [&](auto& processed_point_clouds, const auto& point_cloud) {
            ROS_INFO_STREAM(caption << " " << index++ << "/" << (point_clouds.size() - 1) << ".");
            
            op(&processed_point_clouds, point_cloud, index);

            this->showPointCloud2(processed_point_clouds.back());
            spinVisualizer();
            
            return processed_point_clouds;
          });
    }

    // 誤差を分散させます。
    auto adjustPointCloud2s(const std::vector<sensor_msgs::PointCloud2ConstPtr>& point_clouds, double max_correspondence_distance) const {
      // 最終的に補正しなければならない誤差を計算します。
      ROS_INFO_STREAM("Calculating transform for adjusting.");
      auto adjusting_tf_transform = transformMsgToTF(registerPointCloud2(point_clouds.back(), point_clouds.front(), max_correspondence_distance));
      
      return
        processPointCloud2s(
          point_clouds, "Adjust",
          [&](auto* adjusted_point_clouds, const auto& point_cloud, const auto& index) {
            adjusted_point_clouds->push_back(
              transformPointCloud2(  // 誤差を補完して、座標変換します。
                point_cloud,
                transformTFToMsg(
                  tf::Transform(
                    tf::Quaternion::getIdentity().slerp(  // 四元数を補間して、解消すべき誤差を計算します。
                      adjusting_tf_transform.getRotation(),
                      static_cast<double>(index) / static_cast<double>(point_clouds.size() - 1)),
                    adjusting_tf_transform.getOrigin() * index / (point_clouds.size() - 1)))));
          });
    }
    
    // 点群の集合をレジストレーションします。
    auto registerPointCloud2s(const std::vector<sensor_msgs::PointCloud2ConstPtr>& point_clouds, double max_correspondence_distance) const {
      return
        processPointCloud2s(
          point_clouds, "Register",
          [&](auto* registered_point_clouds, const auto& point_cloud, const auto& index) {
            registered_point_clouds->push_back(
              transformPointCloud2(  // registerPointCloud2が返すTransformで座標変換します。
                point_cloud,
                registerPointCloud2(point_cloud, registered_point_clouds->back(), max_correspondence_distance)));
          });
    }

    // 誤差の分散とレジストレーションを実施します。
    auto registerAndAdjustPointCloud2s(const std::vector<sensor_msgs::PointCloud2ConstPtr>& point_clouds) const {
      // max_correspondence_distanceの値を大→小へと変化させながら、誤差の分散とレジストレーションを実施します。
    
      return
        adjustPointCloud2s(          // 5. レジストレーションの誤差を分散。
          registerPointCloud2s(      // 4. 細かくレジストレーション。
            adjustPointCloud2s(      // 3. レジストレーションの誤差を分散。
              registerPointCloud2s(  // 2. 粗くレジストレーション
                adjustPointCloud2s(  // 1. オドメトリーの誤差を分散。
                  point_clouds,
                  0.1),
                0.1),
              0.1),
            0.05),
          0.05);
    }
    
  public:
    void onInit() {
      getPrivateNodeHandle().getParam("cut_next_round", cut_next_round_);
      
      working_thread_ = std::thread(
        [&]() {
          // 略。
          
          this->showPointCloud2s(point_clouds);
          spinVisualizer();

          // 誤差の修正とレジストレーションを実施し、結果を保存します。
          std::vector<sensor_msgs::PointCloud2ConstPtr> registered_and_adjusted_point_clouds;
          boost::copy(
            this->registerAndAdjustPointCloud2s(point_clouds) |
            boost::adaptors::transformed(savePointCloud2File),
            std::back_inserter(registered_and_adjusted_point_clouds));

          this->showPointCloud2s(registered_and_adjusted_point_clouds);
          while (ros::ok()) {
            spinVisualizer();
          }
        });
    }
  };
}

include/six_point_two_eight/utilities.h

プログラムを実行する前に、少し回転について考えてみましょう。回転は、原点を中心に実施する処理ですよね。先ほど作成した誤差の分散では回転を補間しながら適用していますけど、原点から遠く離れた位置にある点群でも正しく動くのかどうか分かりません……。年のため、データ採取時に点群が原点の回りに集まるようにしておきましょう。

点群の座標変換はpoint_cloud_utilities.hのtransformPointCloud2()関数として宣言済みです。あとは、引数のgeometry_msgs/Transformメッセージを生成する関数の作成だけで、点群を原点の回りに移動させられます。作成しましょう。

あと、一つ前のregister_models.hのadjustPointCloud2s()関数でさりげなく使っていた（のにコードを載せ忘れた）transformMsgToTF()とtransformTFToMsg()は、こんな感じです。

ragma once

#include <actionlib/client/simple_action_client.h>
#include <cmath>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>

#include <geometry_msgs/Transform.h>  // 追加。
#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <six_point_two_eight/GetPointCloud2Action.h>

namespace six_point_two_eight {
  // 略。
  
  // geometry_msgs/Vector3fメッセージを生成します。
  inline auto createVector3Msg(double x, double y) {
    geometry_msgs::Vector3 msg;
    msg.x = x;
    msg.y = y;

    return msg;
  }

  // 略。

  // geometry_msgs/Transformメッセージを生成します。
  inline auto createTransformMsg(double x, double y, double yaw) {
    geometry_msgs::Transform msg;
    msg.translation = createVector3Msg(x, y);
    msg.rotation = createQuaternionMsg(yaw);

    return msg;
  }
  
  // 略。

  // geometry_msgs/TransformメッセージをTF::Transformに変換します。
  inline auto transformMsgToTF(const geometry_msgs::Transform& transform) {
    tf::Transform converted_transform;
    tf::transformMsgToTF(transform, converted_transform);

    return converted_transform;
  }

  // TF::Transformをgeometry_msgs/Transformメッセージに変換します。
  inline auto transformTFToMsg(const tf::Transform& transform) {
    geometry_msgs::Transform converted_transform;
    tf::transformTFToMsg(transform, converted_transform);

    return converted_transform;
  }
}

include/make_world_models.h

点群が原点の回りに配置されるように、make_world_models.hも修正します。

// 略。

namespace six_point_two_eight {
  class MakeWorldModels : public nodelet::Nodelet {
  private:
    // 略。

  public:
    void onInit() {
      // 略。
      
      auto points_subscribing_time = ros::Time::now();
      points_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>(
        "points", 10,
        [&, state, points_subscribing_time](const auto& message) {
          // 起動直後は動作が安定しないので、3秒間は何もしません。
          if (message->header.stamp < points_subscribing_time + ros::Duration(3.0)) {
            return;
          }
          
          try {
            savePointCloud2File(
              transformPointCloud2(  // 追加。誤差補正での回転処理をやりやすくするために、原点が中心になるように補正します。
                getSpherePointCloud2(
                  transformPointCloud2(
                    downsamplePointCloud2(
                      message,
                      0.005),
                    "odom",
                    -0.05),
                  state->position,
                  1.5),
                createTransformMsg(-state->position.x, -state->position.y, 0)));  // 追加。
            
          } catch (const std::exception& ex) {
            ROS_WARN_STREAM(ex.what());
          }
        });

      // 略。
    }
  };
}

include/make_target_models.h

make_target_modelsも、同様に修正します。

// 略。

namespace six_point_two_eight {
  class MakeTargetModels : public nodelet::Nodelet {
  private:
    // 略。
    
  public:
    void onInit() {
      // 略。
      
      odom_subscriber_ = getNodeHandle().subscribe<nav_msgs::Odometry>(
        "odom", 1,
        [&](const auto& message) {
          // 略。
          
          for (auto i = 0; i < step_size; ++i) {
            ROS_INFO_STREAM("Step " << (i + 1) << "/" << step_size);

            while (ros::ok()) {
              try {
                savePointCloud2File(
                  removeFloorFromPointCloud2(
                    transformPointCloud2(  // 追加。誤差補正での回転処理をやりやすくするために、原点が中心になるように補正します。
                      getSpherePointCloud2(
                        transformPointCloud2(
                          sensor_msgs::PointCloud2ConstPtr(
                            new sensor_msgs::PointCloud2(
                              get_point_cloud_2_(createGetPointCloud2GoalMsg("points")).points)),
                          "odom"),
                        target_position,
                        radius * 0.5),
                      createTransformMsg(-target_position.x, -target_position.y, 0))));  // 追加。
                
                break;
              
              } catch (const std::exception& ex) {
                ROS_WARN_STREAM(ex.what());
              }
            }

            // 略。
        });
    }
  };
}

完成！！！

おお、うまくいきました！　これでプログラムは完成です！

誤差を分散してレジストレーション

誤差を分散してレジストレーション

前にも載せましたけど、pcl_viewerで色付きで見るとこんな感じ。グリグリ動かすと私のお腹がぽっこりしているのが分かったりして、結構面白いです。

周囲360°の3D点群

対象物の周囲360°からの3D点群

最終版のソースコードと私の環境でのデータを、http://github.com/tail-island/six_point_two_eightに置きました。お時間があるときにでも、ご覧になってください。

おわりに。ビジネス・アプリケーション・プログラマーにとっての、ロボットのソフトウェア開発

これまで長々とROSプログラミングについて書いておきながら申し訳ないのですけど、実は、私の本業はビジネス・アプリケーションのプログラマーです。普段は関数型言語でWebアプリケーションを作ったりしています。まぁ、私がロボットの素人であることは、「3D点群を統合する」の章での迷走っぷりから皆様も気がついていたでしょうけど。

ロボットのソフトウェア開発は簡単

本稿で私の恥ずかしい迷走劇をご覧頂いたのは、専門家「以外」がROSプログラミングに必要な情報を集める方法をご紹介したかったためです。チュートリアルをやって、リファレンスを見て、テスト・コードを読んで、実際に動かしてみれば、その道の専門家でなくともライブラリは使えます（もしこれで足りなければ、他人が作成したコードを読む）。

で、このような面倒な手順を紹介すべきだと考えた理由は、ロボット工学の専門家「以外」もロボットのソフトウェアを開発できる時代になったと考えるためです。実際に本稿で、点群の素人の私がIterative Closest PointとLM法を活用するロボットのソフトウェアを作成できたわけですしね（私がやったのはPCLの呼び出しだけですけど、ソフトウェアを外から見るだけならバレないはず）。本稿の途中で紹介したgmappingとnavigationを使えば、「自律移動する○○」という、最近良く見る商品のソフトウェアをすぐに開発できます。

ROSの豊富なパッケージを使えば、プログラマーなら誰でもロボットのソフトウェアを開発できるんです。ロボットのソフトウェア開発は、簡単なんですよ。

ロボットのソフトウェア開発は難しい

でも、ビジネス・アプリケーションでRDBMSでトランザクションでエラーをはじいたクリーン・データでビジネス・ロジックを書くことに慣れた私だと、誤差を解消する手段が分からなくて途方に暮れることも多いです。そもそも、誤差なんて概念は、私の頭のどこにも無いわけですし。

本稿で使用したPCLや途中でご紹介したgmappingやnavigationのような、誤差を解消してくれるパッケージを使える分野なら、問題はありません。でも、パッケージが無かったり、パッケージの限界を超えるような場合は、自分でどうにかしなければなりません。今回のように簡単な計算で誤差をごまかせるのは稀で、専門知識が必要になるケースがほとんどでしょう。パッケージを使うのは簡単ですけど、パッケージを作るのはやっぱり難しい。

その意味では、ロボットのソフトウェア開発は難しいとも言えます。特に、私のような誤差に馴染みがない、伝統的なビジネス・アプリケーションのプログラマーにとっては。

ロボットのSIなんて、どうでしょうか？

Wikipediaによれば、System Integrationは「複数のベンダから汎用のパッケージ・ソフトウェアやハードウェアなどの完成品を購入して、1つのシステムとして矛盾なく、効果が出るように組み立て、統合する」ことなんだそうです14。ROSを使ったロボットのソフトウェア開発なら、まさにこのSIができると思うんですよ。お客様の業務と、パッケージの特徴と限界の知識を活用し、効果が出る統合を実現すればよいわけですから。

ロボットのSIでも、ビジネス・アプリケーション開発で培ったお客様の業務に関する知識、これまで多種多様なソフトウェアやハードウェアを評価してきた選別眼、システム全体を運用可能にするノウハウを、大きく活用できると私は考えています。

皆様なら、パッケージの作成も

本稿で作成したプログラム、とくに3D点群を統合する部分は、残念ながら実用性は低いです。遅いですし、点群の統合に失敗することもあります。

遅いという問題の特効薬は、新しいアルゴリズムです。私の場合、これは専門家が研究して論文（とコード）にまとめてくれるのを待つしかありません。ただ、今回使用した範囲でのPCLの動きを見ると、CPUが一つしか使われていませんし、GPUプログラミングもなされていないようです。並列化の余地は大きいでしょう。プログラミング・テクニックで実行効率を向上させられる範囲が、それなりにあるはずです。

精度の問題でも、やっぱり新しいアルゴリズムが欲しい。でも、これも私のレベルだと専門家待ち。ただ、TurtleBotはZ軸での回転しかできなくて、回転の軸が少なければ解空間が小さくなるので、TurtleBot用に少し特化することで精度を向上できるかもしれません。計算量が減りますから、パフォーマンスが上がる可能性もあるでしょう。

あと、私がROSのメッセージを使うときに感じているのは、大昔のO/Rマッピング・ツールみたいだなぁということです。ROSのメッセージは、専用言語でデータ構造を定義してコードを生成させていた古い時代の、プログラミング言語の中でエンティティ・オブジェクトを定義可能になる前のO/Rマッピング・ツールと同じやり方なんですよ。メッセージを明示的に投げるのも格好悪い。Webアプリケーションで使われているリアクティブ・プログラミングなら、全自動で変更が伝播するのにね。

私のようなビジネス・アプリケーション・プログラマー、ロボット工学の素人でも、嬉しいことにROSに貢献することができるかもしれません！　こんなダラダラした文章をここまで読む根性がある皆様なら、余裕ですよ。もし皆様がロボット工学をほんの少しでもやっているなれば（もしくは今から始めれば）、パッケージ作りまくりですよ。

  1. 2015年にROS勉強記録で有名な小倉崇さんによる「ROSではじめるロボットプログラミング」が出版され、ROSそのものやPythonでのプログラミングの日本語文書は充実しました。他にも、「詳説ROSロボットプログラミング -導入からSLAM、Gazebo、MoveItまで-」のように、自律移動やロボット・アームの操作までの広い全体像を解説する無料の書籍もあります。でも、C++14を用いたプログラミングについては、2016年3月現在ではまだ不十分に思えます。↩

  2. 本稿では、プログラミング部分のみを述べます。セットアップやROSのパッケージ群については、ROS Wikiや「ROSではじめるロボットプログラミング」を参照してください。↩

  3. モーターは電磁石と永久磁石が引き合ったり反発したりして回っていて、電磁石の極を入れ替える（+/-を入れ替える）ことで回転を継続させています。回転で機械的に+/-を切り替えるのが普通のモーターで、電子回路で+/-を切り替えるのがブラシレス・モーターになります。↩

  4. nameの値は、ユニークな値なら何でも構いません。typeと同じ値を使う場合が多いようです。↩

  5. turtiesimにはdraw_square等のノードもあり、キーボードで操作したり自律移動させたりして楽しめます。なので、ごめんなさい。無条件にturtle_teleop_keyを起動してしまう今回のlaunchファイルは、楽ちんではありますけど正しくはありません。turtlesim_nodeの起動とturtle_teleop_keyの起動は分けておくのが、turtlesimの使い方的に正しい形でしょう。↩

  6. 実は、package.xmlとCMakeLists.txtを修正しなくてもビルドできちゃう場合も多いです。でも、package.xmlに依存するパッケージが正しく入っていないと他の環境でどのパッケージが必要なのか分からなくてビルドできなくなっちゃいますし、CMakeLists.txtのfind_package()はパッケージ情報を環境変数に設定してくれてそれを使ってビルドを制御するかもしれませんから、面倒でも登録するようにしてください。本稿でも、真面目に追加していきます。↩

  7. getPrivateNodeHandle().advertise<geometry_msgs::Twist>("/mobile_base/commands/velocity", 1)（トピック名を「/」で始める）にすれば問題ないように思えるかもしれませんが、ロボットが2台あって/turtlebot_1/mobile_base...と/turtlebot_2/mobile_base...に分けているような場合に問題が発生します。トピック名はノードが所属している名前空間からの相対パスで解釈されますので、/turtlebot_1/make_world_modelsと/turtlebot_2/make_world_modelsとしてノードを起動し、「/」から始まらないトピック名にしておけば、同じプログラムで2台のTurtleBotを扱えて便利なんですよ。↩

  8. 点群のズレ問題は最後の章で解消しますので、ご安心ください。↩

  9. turtlebot_actionsパッケージの説明を読むとまさにこの移動を実現してくれそうなのですけれど、試した限りでは精度が低くてためでした。↩

  10. 実は、gmappingパッケージもOpenSlamのラッパーなのですけど。↩

  11. 深度センサーのデータをLiDARのデータに偽装するプログラム等も含まれていますので、launchファイル「だけ」ではありませんけど。↩

  12. まぁ、床なら、平面の検出ではなくて、z軸の値で検出できるんですけどね……。↩

  13. 多言語対応するためにはしょうがないのかもしれませんけど、ROSのメッセージを表現する型は新規に作成しなければならないしメソッドも定義できません。だから、既存の型を使うことが出来ませんし、内容によってはメッセージとは別の新たな型を定義しなければなりません。結果として、似た情報を表す複数の型が存在することになって、相互変換がとにかく面倒です。もしC++で定義したクラスをメッセージにできるなら、こんな面倒はなくなるのに……。↩

  14. ShitaukeをIntegrationしてソフトウェアを一から構築する日本固有のSIは、今は忘れてください。↩