LimeDemoのプログラム構造について、概略を説明します。LimeDemoディレクトリをホームディレクトリとし、そこからの相対パスでファイル名を表します。直下のディレクトリは、以下があります。
- bin
docker内で用いられるコマンドが保存されています。ビルド時にdockerイメージにコピーされます。
- doc
GitHubで用いるドキュメントのファイルが保存されています。dockerイメージにはコピーされません。
- project
xacroなどの、定義ファイルが保存されています。docker内の、/projectディレクトリか、/root/turtlebot3_wsディレクトリに分散してコピーされます。
- pytwb_ws
アプリケーションプログラムの保存場所です。現在の構成では、dockerのマウント機能により、docker外の親ディレクトリに変更内容が残るようにしています。
- roscommon
必要に応じて、moveit2等のビルドを行うためのディレクトリです。Dockerのマウント機能により、docker外の親ディレクトリに内容が残るようにすることにより、１回ビルドすれば、dockerを再初期化しても最初のビルド結果が再利用できるようにしています。
〇 実行方式
LimeDemo/LimeSimulDemoは、docker形式で実行されます。実機の場合(LimeDemo)は、Lime本体と、WiFiで接続されたdockerを実行できるPCが必要です。dockerは、Linux PC、もしくは、Windows 上のDocker Desktopによって実行可能です。シミュレーションの場合(LimeSimulDemo)は、単体のPC上で実行が可能です。
LimeSimulDemoでは、シミュレーション用のdockerのコードを実現しています。これをベースに、デモがどのように実行されているか見てゆくことにします。シミュレーションの場合、run_allコマンドによってGazeboが起動し、事前に指定したworld内にLimeが表示されます。また、それを操作するためのRvizが２つ起動します。それにより、Nav2, MoveIt2が内部で起動し、ナビゲーション、および、アーム操作のためのROS通信を受け付けるようになります。これによって、デモを実行するための環境が整います。
デモのアプリケーション本体は、別途Pythonコードとして実行されます。VSCode内部から起動することがお勧めです。以下、この状態を前提とした、アプリケーションの動きと構造を見てゆきます。
アプリケーションのコードは、実行時は、~/pytwb_ws/src/cm1に配置されます。cm1は、パッケージの名前になります。~/pytwb_ws/src/cm1/__main__.pyがメインプログラムになります。ここからros_actorが起動し、コマンドプロンプトが表示されます。その他の大半のコードは、~/pytwb_ws/src/cm1/cm1の下にあります。
コマンドプロンプトから、actor名もしくはBehavior Tree名を入力すると、それが実行されます。
〇 pytwb_wsの構成
アプリケーションを構成する、~/pytwb_ws/src/cm1/cm1から下のファイル構造を見てみます。直下のディレクトリとして、
- lib
共通に用いられるコードを置きます。
- lib/actor
actorのコードを置きます。
- behavior
Behavior Treeで用いられる、Behaviorのコードが定義されています。Behaviorの定義は、クラス定義に’@behavior’を付加することによって行います。Behaviorを定義するクラスには、updateメソッドが必ず含まれている必要があります。Behaviorの定義クラスの多くは、Behaviorと、その機能を実装するactorの対応関係を定義しているだけで、実際の動作は含まれていません。
- trees
XMLで記述されるBehavior Treeのデータが置かれます。コマンドプロンプトから入力するのは、ここに置かれたxxx.xmlファイルの、xxx部分です。
これ以外に、直下のファイルとして、app_main.pyがあります。
〇 ~/pytwb_ws/src/cm1/cm1/lib/actorの構成
~/pytwb_ws/src/cm1/cm1下のファイルのうち、最も重要なコードは、lib/actorにあるactorのコードです。主要なものをあげてみます。
- system.py
ロボットの構成全体を規定するコードが置かれます。そこでは、ロボットの全機能を統合した、Tb3クラスが定義されています。Tb3クラス内では、actorの集合である、’subsystem’をTb3クラス機能に追加しています。追加されるsubsystemは、
-
- Tb3NavigationSystemクラス： 自律移動機能を司るactorの集合
- Tb3ManipulatorSystemクラス： アーム操作を司るactorの集合
- Tb3CameraSystemクラス： カメラ画像の処理を司るactorの集合
です。
Tb3NavigationSystemクラスも同一ファイル内で定義されています。そこでは、Nav2、モータドライバ、odom等に関するROS通信をactorの形式で利用できるように宣言しています。また、ここには書ききれないactorの定義として、別途、ApproachActionクラス中で定義されたactorを追加しています。
同様に、Tb3ManipulatorSystemクラス、Tb3CameraSystemクラスも同一ファイル内で定義され、ROS通信の定義、および、他のactor定義の取り込みを行っています。Tb3ManipulatorSystemクラスは、ManipulatorNetworkクラス、Tb3CameraSystemクラスは、CognitiveNetworkクラスにあるactor定義を取り込んでいます。
また、MapSystemクラスの定義も行っています。これは、SLAM地図を、より利用しやすいベクトル化された地図の形式に変換し、その情報を他のactorに提供する機能を担っています。
システムの起動時には、app_main.pyが、アプリケーションに特化した初期化を行います。Tb３クラス、MapSystemクラスをactorシステムに登録するとともに、すべてのBehavior Treeを、それぞれactorとして登録し、最後にコマンドインタプリタを起動しています。
- approach_action.py
Nav2では実現しにくい、微小な移動の制御を実現するactor群を実現し、ApproachActionクラスとしてまとめています。たとえば、targetted_walkは、ビジュアルフィードバックによって、コーラ缶に正対しながら前進する機能を実現しています。これを用いて、reach_cokeとshift actorが実現されています。shiftは、アームをピッキング位置まで下げた状態で、グリッパとコーラ缶が干渉しないように制御しながら、ピッキングが可能な位置まで前進する機能を持ちます。
- cognitive.py
カメラ画像の処理を行うactor群を実現し、CognitiveNetworkクラスにまとめています。カメラ画像を直接受信して、OpenCVのデータ形式に変換するpic_receiver actor, 画像中からコーラ缶を見つけ出すpic_find actor, 発見したコーラ缶の相対座標を計算するfind_object actor, アームがピッキング位置まで下がった状態でコーラ缶の相対座標を計算するmeasure_center actor, 複数回コーラ缶の位置を計測して確認し、正確な座標を求めるget_found actor, 求めたコーラ缶の座標にtfによる座標変換を施してworld座標系における座標値を求めるobject_loc actor等が定義されています。
- manipulator.py
アーム制御のactor群を実現し、ManipulatorNetworkクラスにまとめています。アームを標準位置まで移動するhome actor, グリッパを開くopen actor, グリッパを閉じるclose actor, アーム位置を調節してコーラ缶に正対させるad actor, fit actor、グリッパを最も低い位置に移動させるpick actor等が定義されています。
〇 ~/pytwb_ws/src/cm1/cm1/lib/treeの構成
ロボットの全体的な動作を決める、Behavior Treeファイルが保存されています。コマンド名として、.xmlをとって使用します。
- bt_pick_place.xml
LimeDemo全体の動作を含みます。内部では、bt_search, bt_catch, bt_carryを呼び出しています。
- bt_search.xml
bt_search を定義しています。地図データから、探索すべき領域を分割し、それぞれの領域に移動しながら、並行してカメラ画像からコーラ缶の検出を行い、コーラ缶を発見した場合は、発見した場所にもどってコーラ缶が見えることを確認します。
- bt_catch.xml
コーラ缶の座標を確認し、接近し、アームを下げ、その状態で再度接近し、グリッパを閉じることによってピッキングを実現しています。
- bt_carry.xml
コーラ缶を持ったまま、定位置まで移動し、パレットとの距離を計測しながらビジュアルフィードバックによってプレース位置まで移動した後、アームを下げ、グリッパを開くことによってプレースを実現しています。