[openrtm-users 02418] 自分のプログラムを無理矢理RTC化する方法
kumagai ＠ tjcc.tohoku-gakuin.ac.jp
kumagai ＠ tjcc.tohoku-gakuin.ac.jp
2012年 2月 1日 (水) 20:51:33 JST
東北学院大学の熊谷でございます。
最近、研究室の卒研がらみで(まずはRTCHokuyoAISTを使うために)、OpenRTM-
aistをいじっています。
その過程で、これまで研究室で独自に動いていたプログラムにRTCのインター
フェイスを半ば強引につけて、他のRTCの情報をもらえるようにしてみました。
と、twitterでつぶやいたところ、まとめてMLにというお勧めを頂いたので
以下、お知らせします。
推奨される方法だとは決して思いませんが、「とりあえずRTC化したい、
どんな汚い手を使っても」というときには、手軽だとおもいます。
環境：
・Windows XP 32bit, Win7pro 64bit
・OpenRTM-aist 1.1 (C++:RC3, Python:RC1)
・VisualStudio 2008 (C++)
====================================================================
１：背景
====================================================================
当方では、PC(たいていWindows)とマイコン(H8やdsPIC)を組み合わせた制御系を
組むことが良くあります。
具体的には、ハードに近いローカルな制御(モータの電流速度位置制御など)、
センサ信号処理(AD, フィルタ、簡単な加工)をマイコンで実装する場合、
マイコン側にあらかたの自律性を持たせた上で実験のためのデータの吸い
出しやパラメータ・指令値の変更を行うなどです。
大抵は、マイコン側が一定間隔で通信を送り、それに答える形でPC側が
計算値を送る形になり、それなりにリアルタイムな応答をするようにして
います。
シリアル通信部分はこういうときの頭痛の種ですが、そこには１０年近く
自前のマルチスレッドな通信ライブラリを使ってきていました。
RTC化するには、この部分をどうにか結合させないとならないため、着手が
遅れていましたが、卒論の締め切り間際の時期ともなり(笑)、無茶をする
ことになりました。
====================================================================
２：方針
====================================================================
・とにかくRTCと既存の制御プログラム類をつなぐ
・RTC的なかっこよさは求めない(当然max_instance=1)
・RTCのマネージャをnon-blockで呼び出すことで、RTC側を
バックグラウンド(別スレッド)で動かし、既存のプログラムを
フォアグランド(メインスレッド)で動かし、その間を
volatileしたグローバル変数でつなぐ。
====================================================================
３：上流RTC側の準備
====================================================================
無茶をする覚悟で、なるべくcookedな、小さなデータを送ることにしました。
具体的には
・RTCHokuyoAISTからのレンジデータから、正面の壁面の角度を求める
・同じく、近いところにある障害物らしい塊の座標列を求める
（おのおの目的が違う別のRTC）
というRTCをつくり、サンプルののTkLRFViewer.pyを改造して演算結果を
オーバーレイできるようにしたものと併せて、３個のRTC群を組にしました。
３個で、モニタしつつ、上記の結果を出力します。
====================================================================
４：RTC部分をとりあえず作る
====================================================================
rtctempl.bat:
rtc-template.py -bcxx^
--module-name=RTCSL --module-desc="RTC SerialLoop Bridge template"^
--module-version=0.1 --module-vendor=RDE --module-category=experiment^
--module-comp-type=DataFlowComponent --module-act-type=SPORADIC^
--module-max-inst=1^
--inport=In:TimedFloatSeq^
--outport=Out:TimedFloatSeq
を作って、実行して、ひな形をつくりました。
今回必要だったのは、TimedFloatSeq(floatの配列)の入力でしたが、
今後のため、出力も作りました。
※補足、安藤さんより
OpenRTM-aist 1.1RC3のrtc-template.pyは問題があるそうです：
http://www.openrtm.org/redmine/issues/2209
slntool.pyを
http://openrtm.org/svnroot/OpenRTM-aist/trunk/OpenRTM-aist/build/slntool.py
に置き換える必要があるそうです。
＃私は無理矢理slntool.pyをいじって動くようにしました
※/補足
※【わすれちゃいけないcopyprops.bat】＜見落として悩んだ
※※できれば、rtc-template.pyで面倒を見て頂いた方が...
これをコンパイルして、activateやポート接続ができることを確認しました。
====================================================================
５：RTCのonExecuteの実装、マネージャのnon-blockingテスト
====================================================================
RTCSL.hに追加：＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝
extern volatile float RTCSLIn[100];
extern volatile int RTCSLInNum,RTCSLInUpdate;
extern volatile float RTCSLOut[100];
extern volatile int RTCSLOutNum,RTCSLOutUpdate;
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
(コメント解除)
RTCSL.cppに追加：＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝
volatile float RTCSLIn[100];
volatile int RTCSLInNum=0,RTCSLInUpdate=0;
volatile float RTCSLOut[100];
volatile int RTCSLOutNum=0,RTCSLOutUpdate=0;
RTC::ReturnCode_t RTCSL::onExecute(RTC::UniqueId ec_id)
{
int i;
if(m_InIn.isNew()) // 動作検証済み
{
while(m_InIn.isNew())
m_InIn.read();
RTCSLInNum=m_In.data.length();
if(RTCSLInNum>100) RTCSLInNum=100;
for(i=0;i<RTCSLInNum;i++)
RTCSLIn[i]=m_In.data[i];
RTCSLInUpdate=1;
}
if(RTCSLOutUpdate) // たぶん動くと思う
{
m_Out.data.length(RTCSLOutNum);
for(i=0;i<RTCSLOutNum;i++)
m_Out.data[i]=RTCSLOut[i];
RTCSLOutUpdate=0;
}
return RTC::RTC_OK;
}
RTCSLComp.cppのmain関数の後半を修正：＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝
// run the manager in blocking mode
// runManager(false) is the default.
//manager->runManager();
// If you want to run the manager in non-blocking mode, do like this
manager->runManager(true); // こっちにする
while(1) // Ctrl-Cで止めるしかない無限ループ
{ // しかもsleepなしだから無駄にCPUパワーを食う＜あくまでテスト
int i;
if(RTCSLInUpdate)
{
for(i=0;i<RTCSLInNum;i++)
printf("% 10.3lf ",RTCSLIn[i]);
puts("");
RTCSLInUpdate=0;
}
}
これを実行すると、InにつないだTimedFloatSeqにデータがくるたびに
画面になにか表示されます。たぶん。
※補足
・onExecuteではRTCが受信したら、RTCSLIn[]配列に、入力値を
コピーしてRTCSLInUpdateを１にする
・メインスレッドではRTCSLInUpdateを監視して、０でなくなったら
値を表示して、０にもどす。
・マルチスレッドなのにロックしていないから、ひどい問題がおきる
可能性を持つので注意。せめて、ダブルバッファするとか、
リングバッファするとか、まじめにロックするかが必要。
（でも、今回は手抜きに徹するので気にしない）
※蛇足
何も考えずにポート名をInとOutにしたら、妙な変数が(汗
====================================================================
６：自分のプログラムに結合
====================================================================
RTCSL.h、RTCSL.cpp、RTCSLComp.cppを自分のプログラムのプロジェクトに
コピーし、VisualStudioのプロジェクトに組み込みます。
int main(int argc, char** argv)
を
int RTCmain(int argc, char** argv)
に名前変更。
RTCSLComp.cppのmain関数の後半を修正：＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝
// run the manager in blocking mode
// runManager(false) is the default.
//manager->runManager();
for(int i=0;i<100;i++)
RTCSLIn[i]=0;
// If you want to run the manager in non-blocking mode, do like this
manager->runManager(true);
/* while(1)
{
：
}*/
return 0;
}
・変数の初期化
・デバッグ用のwhile(1)をコメント化
→RTCmainを呼ぶと、RTCを動くようにして、マネージャを起動して、
（＝RTCとしての機能を初期化して）そのままreturnする。
自分のプログラム：＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝
#include "RTCSL.h" 追加
int main(int argc, char* argv[])
のなかで
int RTCmain(int argc, char** argv)
を適当なタイミングで呼ぶようにする。
RTCSLInを適当に使うようにプログラムをつくる。
※必要ならupdateをチェック
※私のプログラムでは主たる方は独自にタイミングをとっているので、
※RTCSLInNum(＝RTCのinportに届いたseqのデータ数)が必要数
※あるだけをチェックしてそのまま使いました
これで、そのままビルドするといろいろ足りないのでエラーがでます。
====================================================================
７：コンパイルできるようにする
====================================================================
・必要ファイルのコピー
user_config.vsprof と rtm_config.vsprof もコピーしてくる
・VisualStudioのプロジェクトのプロパティに不足するものを追加する
・「構成プロパティ→全般→継承プロジェクトプロパティシート」に
４，５で使っていたテンプレをつくったときのプロジェクトから
内容をコピーする。
※私のところでは $(SolutionDir)rtm_config.vsprops;(続く)
※ $(SolutionDir)user_config.vsprops
・「Ｃ＋＋→プリプロセッサ→プリプロセッサの定義」に
テンプレプロジェクトの内容を「追加」する。
※すでにいくつか入っていたため
※私が追加したもの：
※USE_stub_in_nt_dll;WIN32;_DEBUG;_CONSOLE;__WIN32__;__x86__;
※_WIN32_WINNT=0x0400;__NT__;__OSVERSION__=4;
※_CRT_SECURE_NO_DEPRECATE
・「リンカ→入力→追加の依存ファイル」に
テンプレプロジェクトの内容を追加する
※$(rtm_libd)を追加
これで、ビルドが通って、実行できて、RTSystemEditorでアクティベート
できて、ポートの接続ができて、データが流し込みできるようになりました。
====================================================================
８：まとめ
====================================================================
大まかに手順をまとめますと
・テンプレートとなるRTCをつくる
・マネージャをnon-blockingで動かす
・テンプレのファイルを自分のプロジェクトに追加する
・テンプレのプロジェクトから設定をコピー、追加する
です。
冒頭にもいいましたように、かなり「汚い」実装です。あくまで、
「とりあえず試したい」という場合に、覚悟して使う手だと思いますが、
はまらなければ１時間もあれば、既存のプログラムにRTCのインターフェイスが
つくと思います。
ただ、行儀よく終了しないため、上流のRTCを巻き添えにするケースが
たびたび確認されています（気づくと赤くなってる）
おそらく、正しくdeactivateしたりするコードなどを用意した方がいいと
思いますが、まあ、実験用ですし、と割り切って...
＃という臨時コードがいつまでも残り続ける危険性があるのは
＃重々自覚しています(笑)
では。
openrtm-users メーリングリストの案内