ステップ2ではTankロボットの関節の状態をJointState型のROSトピックとしてPublishし外部に出力する機能を実装しました。ステップ3ではこのトピックをSubscribeして利用する側のプログラムを作成します。具体的には、シミュレーションとは別にもうひとつChoreonoidを起動し、そこで現在の関節の状態を可視化するビュアー的な機能を実現します。このような機能はロボットのモニタリングや遠隔操作にも応用できるものです。本ステップではロボットの状態の可視化用としてもうひとつのChoreonoidを起動して使用します。準備として、プロジェクトの雛形を作りましょう。言ってもこれは非常に簡単で、シミュレーションに使用しているのと同じTankモデルをひとつ読み込んだプロジェクトを作成すればOKです。シミュレーションを行うわけではないので、シミュレータアイテムは当然必要ないですし、今回はTankの関節の状態を表示することが目的なので、環境モデルも必要ありません。それらを取りまとめるためのワールドアイテムも必要ありません。従って雛形となるプロジェクトのアイテムツリーはシンプルに以下のようになります。Tankプロジェクト無しでChoreonoidノードを起動してBodyアイテムの読み込みを行ってもよいですし、ステップ2までのプロジェクトファイルをコピーして、そこからTankモデル以外のアイテムを削除して作成してもよいです。このプロジェクトの画面は以下のようになります。ここでTankモデルはこのように少し大きめに表示されるようズームを調整しておくとよいでしょう。ディレクトリに保存しておくことにします。状態可視化用コントローラの導入¶プロジェクトの雛形を作成しましたが、もちろんこれだけではTankロボットの状態を可視化することはできません。これを行うためには、JointStateトピックをSubscribeし、それを基に可視化用のTankモデルの状態を更新する処理をどこかに導入する必要があります。この処理についてはいろいろな実装方法が考えられますが、本チュートリアルではシミュレーションでも使用しているシンプルコントローラをこの処理にも用いることとします。シンプルコントローラは柔軟な設計となっており、必ずしもシミュレーション中のロボットの制御だけではなく、Bodyアイテムとして読み込まれて表示されているだけのモデルについても処理対象とすることが可能となっています。ここではまず状態可視化用コントローラのソースコードを提示しますので、それをビルドしてプロジェクトに導入し、関節状態の可視化をまず動かしてみましょう。それによってこのコントローラの機能をまず把握してから、実装内容の解説を行うことで、実装内容をより理解しやすくなるのではないかと思います。状態可視化用コントローラのソースコードは以下になります。いうファイル名でsrcディレクトリに作成してください。srcディレクトリのCMakeLists.txtに以下のビルド処理を記述します。これはCMakeの「インポートライブラリ」と呼ばれるもので、あたかもこのライブラリが同じCMakeプロジェクト内でビルドされているかのように使うことができるものです。ここではChoreonoidのBodyプラグインに対応するCnoidBodyPluginライブラリを指定していて、これによって該当ライブラリへのリンクや、その他ビルドに必要な(インクルードディレクトリやコンパイルオプション等の)各種設定もなされることになります。このようなChoreonoidが提供するライブラリのインポートライブラリは、find_packageでchoreonoidを指定すると使用できるようになります。いう形式で定義されます。ここで指定しているBodyプラグインのライブラリによって、Bodyプラグインやその依存先のライブラリで定義されているクラスや関数を利用することが可能となります。本コントローラでは可視化を行うためにChoreonoidのGUIと関連するBodyItemクラスやcallLater関数を使用しており、この記述が必要となっています。Choreonoid::CnoidBodyPlugin)という記述になりますが、CnoidBaseはCnoidBodyPluginが依存しているので、後者のみの記述で前者も取り込まれることになります。buildでビルドを行いましょう。ビルドに成功すると"RttJointStateSubscriber.so"というファイル名でシンプルコントローラのバイナリファイルが生成されます。プロジェクトへの導入¶ビルドしたコントローラをプロジェクトに導入してプロジェクトを完成させましょう。しておくとよいでしょう。アイテムツリーは以下のようになります。上書き保存してください。本サンプルでは以下の2つのChoreonoidノードを起動します。(本ステップで作成したstep3.cnoid)ここで注意が必要な点として、ノード名の重複を避ける必要があります。2つのChoreonoidノードを起動する際に、どちらも同じ名前で起動すると、ROS通信において両者を区別できなくなってしまいます。従って、どちらかのノードについてはノード名をデフォルトとは異なるものにする必要があります。これを踏まえた上で、2つのChoreonoidノードを起動してみましょう。シミュレーション用のChoreonoidノードとその関連ノードを、ステップ2用のLaunchファイルで起動します。次に別の端末から、可視化用のChoreonoidノードをrosrunコマンドで直接起動してみましょう。この書き方は、ROSノードを起動する際の標準のオプションとなります。ここまで実行すると、デスクトップ画面には以下のように2つのChoreonoidのウィンドウが表示されているかと思います。listを実行すると、以下のように表示されます。可視化用のChoreonodiノードに対応します。このように2つのChoreonoidノードが表示されていればOKです。ゲームパッドでTankロボットを動かしてみましょう。砲身を動かすと、その動きにあわせて可視化用Choreonoid上のTankモデルの関節角度も変化するかと思います。Tankロボットを動かして砲身を壁にぶつけたりすると、その分砲身の関節も反力で多少動きますが、その動きも可視化用Choreonoid上で表示されるかと思います。そのような挙動になっていれば、本プロジェクトはうまく動いています。先程は可視化用のChoreonoidノードをrosrunコマンドで直接起動しましたが、この操作もLaunchファイルにまとめて、本サンプルの実行で使用する全てのノードを一度に起動できるようにしましょう。いうファイルをlaunchディレクトリに作成します。/></launch>最後のnodeタグの記述が可視化用Choreonoidノードに対応しています。ここでname="choreonoid2"という記述により、このノードの名前を変更しています。このようにLaunchファイルではノード名の変更を簡潔に記述することができます。step3.launchとして起動することで、本サンプルを実行できます。step2.launchに含まれていた関節軌道表示用のrqt_plotノードについては、本launchファイルでは含めないようにしています。ここまで作業を進めると、本チュートリアルのパッケージは以下のファイル構成になります。RttJointStateSubscriber.cppソースコードの解説¶コントローラRttJointStateSubscriberのソースコードを解説します。本コントローラでも、ステップ1のRttTankControllerと同様に、roscppのSubscriberクラスを用いてトピックのSubscribeを行っています。従ってroscppにおけるSubscriberのコーディングという点では両者は本質的に同じです。RttTankControllerはTankロボットの制御を行うものでしたが、RttJointStateSubscriberでは制御は行わずにモデルの状態を直接更新するというもので、Choreonoid上での利用方法が異なっており、その点でコードにも違いが出てきます。Choreonoidではシミュレーション対象のロボットの制御だけでなく、今回のような可視化に使うこともできるという意識で読んでいただけると、以下の解説がより有益なものになるのではないかと思います。今回のソースでは新たに2つのヘッダを導入しています。<cnoid/BodyItem>でBodyItemクラスを使えるようにしています。BodyItemはBodyプラグインで定義されているクラスで、ロボットのモデルに対応するBodyオブジェクトをChoreonoidのGUI上で操作できるようにするためのものです。通常コントローラは特定のGUIには依存しないように実装されますが、今回はGUI上のモデルを直接更新することがそもそもの目的なので、BodyItemを使用するようにしています。<cnoid/LazyCaller>によって、callLaterという関数を使えるようにしています。この関数については後ほど解説しますが、これもGUIと関連するもので、ChoreonoidのBaseモジュールで定義されているものです。これらのクラスや関数を使用するためには、対応するライブラリを追加でリンクする必要があります。具体的にはlibCnoidBaseやlibCnoidBodyPluginといったライブラリです。リンクするライブラリを追加していたのはこのためです。subscriber;については、ステップ1のRttTankControllerで使用しているものと同じです。ROSのノードハンドルとSubscriberに対応するもので、これらのオブジェクトによってSubscribeの処理を行います。bodyItem;は上述のBodyItemに対応するポインタ変数です。BodyItemのポインタ型は通常BodyItem*となりますが、BodyItemPtrはこのスマートポインタ版で、この型を用いることで指しているオブジェクトを維持するようになります。無いとは思いますが、通信をしている最中に可視化用Choreonoidノード上でTankのアイテムを削除すると、タイミングによっては削除後にモデルの更新処理が呼ばれてしまうこともあり得るため、念の為スマートポインタ版を使っています。今回のコントローラでは初期化処理をSimpleControllerのvirtual関数のひとつであるconfigure関数で行っています。...}configure関数はコントローラがプロジェクトに導入されて対象のBodyアイテムと関連付けられる際に呼ばれます。シミュレーション中のロボットの制御を行う際にはinitialize関数で初期化を行いますが、今回はそうではなく、Bodyアイテムとして読み込まれるモデルの状態を直接更新します。そのための初期化はconfigure関数で実現することができます。static_cast<BodyItem*>(config->bodyItem());によって、状態更新の対象となるBodyアイテムを取得しています。この部分はstatic_castを用いたややトリッキーな記述となっていますが、これは元々シンプルコントローラがGUIに依存しないクラスとして定義されているためです。そのようなシンプルコントローラであっても、今回の例のようにGUIと連携できると便利なことがあります。そこで例外的ではありますがSimpleControllerConfigのbodyItem関数によってBodyItemオブジェクトも取得できるようになっています。直接は依存していないライブラリに含まれる型をそのまま返すことはできませんので、この関数はベースとなるReference型のポインタを返すようになっており、利用側ではそれをBodyItem型にキャストする必要があります。ややこしいですが、シンプルコントローラからBodyItemを利用する場合はとりあえずこのように書くことになります。ros::NodeHandle(bodyItem->name()));これまでのステップと同様にROSノードのハンドルを生成しています。次にSubscriberを生成します。なり、ステップ2のRttJointStatePublisherがPublishするトピックの名前と一致しています。トピック名を対象Bodyアイテムの名前を基に生成しているのは、他のモデルでも適用できるようにするためです。第二引数についてはキューサイズで、ステップ1と同様に、最新の情報を得られればよいものとして、1に設定しています。第三引数もステップ1と同様にメンバ関数の形式でコールバック関数を指定しています。そのコールバック関数は以下のように実装しています。});}新しいJointStateがSubscribeされる度にこの関数が呼ばれます。ここの処理がステップ1のRttTankControllerとは特に異なる部分です。RttTankControllerでは制御を行うcontrol関数が定期的に呼ばれるので、コールバック関数ではデータ交換用の変数を更新するだけでした。本コントローラは制御とは関係なくモデルを更新するものなので、そのための処理をここから実行する必要があります。更新処理をこの関数内で直接行ってはいけません。言うと、コールバックが呼ばれるスレッドは通常のスレッド(メインスレッド)とは異なるからです。Subscribeというのは受信用ポートへの入力によってトリガーされる非同期処理で、そのための専用のスレッドで処理されます。一方で可視化用モデルはGUIを稼働しているメインスレッド内で管理されています。この場合Subscribe用のスレッドからメインスレッドのオブジェクトに直接アクセスすることはできません。どのようにすればよいかと言うと、メインスレッド内ではGUIを稼働するためのイベントループというものがあり、そこにイベントを投じることで、別スレッドからメインスレッドへの処理の転送を実現できます。これを行う関数がcallLaterで、この関数はどのスレッドからも実行することができ、引数として与えた関数はイベントループを経由してメインスレッド上で実行されます。SubscribeされたJointState型のデータは、callLaterに与えたラムダ式でキャプチャしていて、この際データが(キャプチャ後の)別変数にコピーされています。このコピー処理により、JointStateデータに関しては排他制御を行う必要はありません。注釈シミュレーション対象のロボットの制御を行う場合、制御対象のロボットモデルに同じ方法でアクセスすることはできません。シミュレーションにおける物理計算もまた別スレッドで処理されており、そこで使用するBodyオブジェクトはシミュレーション初期化時にメインスレッドからコピーされたものだからです。これはメインスレッドで管理されているBodyオブジェクトとは異なりますので、逆にメインスレッドから処理することはできないのです。介してBodyオブジェクトにアクセスするようになっています。モデルの状態更新¶メインスレッドから実行されるモデル状態更新の処理をみてみましょう。この処理を実装しているのが以下の関数になります。...}まず更新対象のBodyオブジェクトを取得しています。bodyItem->body();ちなみにこのBodyオブジェクトはconfigure関数のconfigからconfig->body()として得られるものと同じです。state.position;としてJointState型のデータが有する名前の配列と関節変位の配列について参照を定義しています。これは単純にこれ以下のコードを簡潔に記述するためです。size);このコードで状態更新の対象となる関節の数を決定しています。関節の数は同じモデルであれば決まっていて、Tankモデルの場合は砲身を動かすヨー、ピッチの2軸になります。ここでは2という数で固定することも可能なのですが、各データのサイズが想定外のときもクラッシュしないように、各データサイズの最小値の範囲内でアクセスするようにしています。実際のところ、ROSトピックとして得られるデータの中には何が入っているか分かりません。Publishする側に不具合があるかもしれませんし、使用しているモデルが完全に同じではなかったり、そもそもSubscribeする対象を間違えてしまっている場合もあり得ます。ROSのように複数のコンポーネントをネットワーク通信で接続して構築するシステムにおいては、この点を考慮してなるべくロバストなプログラムとなっていることが望ましいかと思います。}}上記コードで決定した関節数nの分だけループを回して、各関節の関節角度の現在値をモデルにセットしています。ここで関節の名前も同じであることをチェックしていますが、これもプログラムをなるべくロバストにするための措置です。受信したデータが想定とは異なるモデルを対象としたものである場合、関節角をそのまま適用してもあまり意味はなく、あり得ない姿勢になることも多いためです。bodyItem->notifyKinematicStateChange(true);モデルの状態が更新されたことをChoreonoidのGUIに通知します。これを行うことで、シーン描画を含むChoreonoid上の各種GUIコンポーネントがモデルの更新を反映するようになります。configure関数でBodyItemのオブジェクトを取得したのは、この通知を行うためです。引数のtrueにより、通知前に順運動学計算も適用されます。これはbody->calcForwardKinematics();bodyItem->notifyKinematicStateChange();としているのと同じです。終了処理¶コントローラの終了処理はunconfigure関数で行います。ros::Subscriber();}こちらもSimpleControllerのvirtual関数で、コントローラが対象モデルやプロジェクト全体から切り離されるときに呼ばれます。configure関数で行った初期化処理に対応する終了処理は、通常この関数内に記述することになります。コントローラの終了後はもうトピックをSubscribeする必要はないので、subscriberをクリアし、関連するオブジェクトのポインタもクリアしています。このような後始末の処理もきちんと実装しておくことが望ましいです。JointStateトピックのSubscribeによるTankの。