VisionManipulation RTC を作る

いよいよ本体の VisionManipulation RTC の作成に入ります。

MyScheduler RTC からの setModelID の呼び出しを受けたら、VVVRecogTrigger RTC にこのコマンドを飛ばし、結果を recogResult データポートで受け取って表示するRTコンポーネントです。

概念としてはこういう形になります。

RTC Builder

設定する項目だけ列挙します。設定の仕方の詳細は MyScheduler RTC を作るを参照してください。

Eclipse プロジェクト名: VisionManipulation

基本タブ

モジュール名： VisionManipulation
モジュール概要: Vision Manipulation
バージョン: 1.0.0
ベンダ名: AIST
モジュールカテゴリ: VMRG

Output Project: VisionManipulation

アクティビティタブ

onExecute: ON

サービスポートタブ

Scheduler ポート

MyScheduler の Schedulerポートとほぼ同じですが、コマンドを呼ばれる側なので方向が Provided になります。

表示位置 LEFT
scheduler インターフェース
- 方向: Provided
- インスタンス名: scheduler
- 変数名: scheduler
- IDLファイル: /home/asahi/workspace/idl/VisionManipulation.idl
- インターフェース型: SchedulerService
- IDLパス: /home/asahi/workspace/idl/

recogSDL ポート

VVVRecogTrigger RTC を接続するポートです。

表示位置: RIGHT
recogSDL インターフェース
- 方向: Required
- インスタンス名: recogSDL
- 変数名: recogSDL
- IDLファイル: /home/asahi/workspace/idl/VisionManipulation.idl
- インターフェース型: RecognitionServiceSDL
- IDLパス: /home/asahi/workspace/idl/

以下のポートは今回は実装しませんが、今後の拡張のためにあらかじめ定義しておきます。

recogPort ポート

表示位置: RIGHT
recogPort インターフェース
- 方向: Required
- インスタンス名: recogPort
- 変数名: recogPort
- IDLファイル: /home/asahi/workspace/idl/VisionManipulation.idl
- インターフェース型: RecognitionService
- IDLパス: /home/asahi/workspace/idl/

PlanStartPort ポート

表示位置: BOTTOM
PlanStart インターフェース
- 方向: Required
- インスタンス名:
- 変数名: plan_start
- IDLファイル: /home/asahi/workspace/idl/VisionManipulation.idl
- インターフェース型: GraspPlanStart
- IDLパス: /home/asahi/workspace/idl/

言語・環境タブ

言語: Python

データポートタブ

ここでrecogResult ポートを定義します。
前回は設定しなかったタブなので、ここだけ詳しく解説します。

DataPort プロファイルのセクションが、「*ポート名(InPort)」と「「*ポート名(OutPort)」」の二つのリストに分かれています。
このうちのInPort側のAddボタンを押すと、ポート名リストに「dp_name」が加わり、Detail セクションにポート情報が入ります。
ポート名リストのdp_nameをクリックすると名前の変更ができるので、ポート名 recogResult を設定します。

すると、ここで設定したポート名がDetailセクションのポート名として反映します。
さらに、Detail セクションの項目を以下のように設定します。

データ型: RTC::TimedDoubleSeq
変数名: recogResultIn
表示位置: RIGHT

このとき、データ型が選択できないことがあります。
そのときは、RtcBuilder にIDLファイルのパスを設定します。
メニューバーからウィンドウ～設定を選び、左のツリーからRtcBuilderを選択します。
すると「データ型: IDL File Directories」というリストが現れます。
右の「新規」ボタンを押して、場所に「/usr/include/rtm/idl」を追加します。

そうすると、IDLファイルからデータ型を読み込んで、たくさんの候補が選択できるようになります。

これで、倍精度浮動小数点数の配列が流れてくる入力データポート、recogResult が定義できました。

コード生成

BuildViewはこのような形になっています。

コード生成を行ってください。
「'SchedulerService' is not found in IDL」が出るようなら、IDLのinclude文をコメントアウトします。

端末からの設定

VisionManipulation.conf の内容を以下のように修正して、rtc.conf にシンボリックリンクを貼ります。

corba.nameservers: localhost:2809
exec_cxt.periodic.rate: 10

次に、idlcompile.sh を実行します。
エラーが出る場合は、スクリプトのコマンドラインに -I/usr/include を挿入して、やり直してください。

実装

MyScheduler RTC が setModelID を呼び出すと、VisionManipulation では SchedulerService_i::setModelID が呼び出されます。
そこで、VisionManipulation_idl_example.py の該当部分を以下のように修正します。

    # void setModelID(in long ModelID)
    def setModelID(self, ModelID):
        print "setModelID: %s" % ModelID
        
        coord = self.recogSDL_service._ptr().recognize_by_ID(ModelID)
        print "recognize_by_ID: %s" % coord

端末にModelIDを表示し、recogSDL_service を通して recognize_by_ID メソッドを呼んでいます。

その結果はデータポート recogResultIn で受けることになります。
ところで、self.recogSDL_service はまだ定義していません。
これは、呼び出し元からセットしてもらうことにします。

SchedulerService_i クラスに新しいメソッドを追加します。

    def setRecogSDLService(self, recogSDL_service):
        self.recogSDL_service = recogSDL_service

次に、呼び出し元のVisionManipulation.py を開きます。
初期化の時に、SchedulerService_i のインスタンスに、recogSDL_service をセットしてやります。
onInitialize メソッドの最後（return RTC.RTC_OK の前の行）に一行追加してください。

        self._scheduler.setRecogSDLService(self._recogSDL)

そうして、onExecute メソッドでデータポートを監視します。
コードは以下のようになります。

    def onExecute(self, ec_id):
        if self._recogResultIn.isNew():
            print "resultIn:"
            indata = self._recogResultIn.read()
              val = indata.data
              print val
            print "\n"
                   
        time.sleep(0.01)

        return RTC.RTC_OK

今回は、流れてきたデータをそのまま端末に表示しているだけですが、いずれこれを整形して PlanStartPort に渡すなどの処理を追加する必要があるでしょう。

graspPlugin for Choreonoid

Contents

Administration

言語

検索