BTR課題 : fawkes の plugin を作ってみましょう（Interfaces編）

BTR課題 : fawkes の plugin を作ってみましょう．にてplugin作成の勉強をしましたが，今回はそこから一歩踏み込んで他のプラグインとのデータのやり取りをしてみましょう．
ROSで言うところのPublisherとSubscriberの作成みたいなもんです．
プラグイン間のデータのやりとりには，BlackBoardを介します．
そこにデータを書き込むには，データの型の宣言が必要です．
構造体みたいなモノ　と思えば大丈夫です．

ここでは，LRFで取得したデータをファイルに書き込んでみましょうか．
プラグイン名は，単純にLRFLoggerとでもしましょう．

cd ~/fawkes-robotino/src/plugin/
cp -r plugin_template lrf_logger
cd lrf_logger/
mv plugin_template_plugin.cpp lrf_logger_plugin.cpp
mv plugin_template_thread.cpp lrf_logger_thread.cpp
mv plugin_template_thread.h lrf_logger_thread.h

中身のクラス名やヘッダファイル名を修正し，Makefile の中も更新しましょう．

さて，サンプルのプログラムは，Robotino のモーターの値を書き換えるプログラムになっています．
BlackBoard上での名前は，MotorInterfaceの「Robotino」です．
これを前提に読み進めていきましょう．

Makefile

まず，Makefileです．

LIBS_lrf_logger_plugin = m fawkescore fawkesutils fawkesaspects fawkesbaseapp \
                         fawkesblackboard fawkesinterface MotorInterface

ライブラリとして読み込む必要があるので，LIBS_<プラグイン名> にインターフェース名を追加します．

<プラグイン名>_plugin.cpp

<プラグイン名>_plugin.cpp は，プラグインを読み込む仕組みの部分ですので，BlackBoardに関係する部分はありません．

<プラグイン名>_thread.h

<プラグイン名>_thread.h では，MotorInterfaceを手軽に扱うためにnamespaceに入れています（これを入れないと，いちいちfawkes::MotoerInterface を書かないといけなくなる）．

namespace fawkes {
  class MotorInterface;
}

あと，内部的に変数も必要になるので，privateとして変数を宣言（ポインタで扱っています）．

 private:
  fawkes::MotorInterface     *motor_if_;

<プラグイン名>_thread.cpp

さて，プラグインの主たる部分である<プラグイン名>_thread.cppです．
まずは，MotorInterfaceを使うために，ヘッダファイルを読み込んでいます．

#include <interfaces/MotorInterface.h>

初期化のinit関数にて，読み込み用でオープンし，内部変数に代入します（ポインタです）．
ここのBlackBoard名（Robotino）は，読み込みをしているプラグイン（MotorInterfaceの方）を参照して調べましょう．
※read/writeの定義が今ひとつ理解できていませんが，その変数の所有者（プラグイン）がwriteで，それ以外のプラグインはreadでオープンしている感じがします．

void LRFLoggerPluginThread::init() {
        motor_if_ = blackboard->open_for_reading<MotorInterface>("Robotino");
}

終了時の処理はこれと対になっていて，オープンしたから閉じるって感じです．

void LRFLoggerPluginThread::finalize() {
        blackboard->close(motor_if_);
}

で，実際に使う（書き込みをする）のは，msgs_enqueue(msg)の呼び出しです．

void LRFLoggerPluginThread::send_transrot(float vx, float vy, float omega) {
        MotorInterface::TransRotMessage *msg = new MotorInterface::TransRotMessage(
                        vx, vy, omega);
        motor_if_->msgq_enqueue(msg);
}

書き込み形式のmsgは，MotorInterface::TransRotMessage 型で用意していますが，これに関してもMotorInterface側を参照して調べる必要があります．
利用しているプラグイン側はこんな程度ですね．

では，大元のMotorInterface って何じゃラホイってことで，こちらを調べてみましょう．
まず，これを定義しているのは，fawkes/src/libs/interfaces/MotorInterface.xml です．
ちなみに，makeによって，ここからtoluaとhとcppのファイルを自動生成します．
※あと，fawkes/src/libs/の下にはinterfaceとinterfacesの2つのディレクトリがあるため，<タブ>キーでの補間がうまく効きません．気をつけましょう．

MotorInterface.xml

XML形式で書かれた変数の定義ファイルです（構造体的なモノ）．

<interface name="MotorInterface" author="Martin Liebenberg, Tim Niemueller" year="2007">
  <constants>
    <constant type="uint32" name="MOTOR_ENABLED" value="0">
      Motor is enabled and sending drive commands will make the robot move.
    </constant>
...
  </constants>
  <data>
    <comment>This interface is currently prepared best for a holonomic robot.
      It will need modifications or a split to support differential drives.
    </comment>
    <field type="uint32" name="motor_state">
      The current state of the motor.
    </field>
...
  </data>
  <message name="SetMotorState">
    <comment>Sets the current motor state.</comment>
    <field type="uint32" name="motor_state">
      The new motor state to set. Use the MOTOR_* constants.
    </field>
  </message>
...
</interface>

となっており，定数部の記述，データ部の記述，メッセージ部の記述にわかれています．
定数部は，このインターフェースで使う定数を定義しており，以後，名前（name）を使ってこれらの定数を表現できるようになります（たぶん）．
データ部とメッセージ部は，変数の名前とその型を定義しています．
データ部は，単発の変数の情報が並んでいますが，メッセージはそれらの集合（構造体）のようになっています．
また，メッセージ部は，相手にデータを送信するときにつかう部分になっています．
要は，書き込んだデータを「時系列順にちゃんと相手に届ける」必要がある場合がメッセージで，「新しいデータで上書きして最新情報がわかればよい」のがデータ部になっているのかなと勝手に解釈しております（間違っている可能性もあるので，要注意）．

さてさて，LRFのデータを取ってくるにはどうしたらよいでしょうか．
まず，LRFのデータを受け取るプラグインは，fawkes/src/plugins/laser にあります．
fawkes-robotino-2016と2019は，sick_tim55xとurg（HOKUYO）が対応しています．
BTRの場合，a-stepに，rplidarとsweepのプラグインを用意しています（ただし，あまり動作確認していないので要注意）．
※実際のシステムへの組み込みはtk-fawkesを参照して下さい．
LRFから読み取ったデータは，_distances[int_deg]に格納しています．
そして，解像度に応じてLaser360/720/1080としてBlackBoardに書き込んでいます．
インターフェース名は，それぞれ，Laser360Interface/Laser720Interface/Laser1080Interface です．
ちなみに，内部変数的には，以下の様になります．

fawkes::Laser360Interface  *laser360_if;
fawkes::Laser720Interface  *laser720_if;
fawkes::Laser1080Interface *laser1080_if;

これを受け取るプログラムを書けば良いわけです．