OpenRTM-aist入門

SUGAR SWEET ROBOTICS CO., LTD.
OpenRTM-aist入門その１
株式会社SUGAR SWEET ROBOTICS 代表取締役
早稲田大学基幹理工学部表現工学科尾形哲也研究室客員次席研究員
芝浦工業大学SIT研究所客員研究員
菅佑樹
1

RTMツールの使い方
• OpenRTM-aistを使うためのツールの使い
方を説明します．
• RTコンポーネントを起動してもシステ
ムの動作は始まらない
• ポートを接続する
• RTCの状態遷移を促す
• RTCのコンフィグレーションを調整
2

インストール (Windows)
• まず，UACをOFFにしましょう．
• OpenRTM-aist バージョン1.1.1 C++言語版 (32bit版)
• http://openrtm.org/openrtm/ja/node/5711
• JDK8・・・Oracle Webサイトからダウンロード．JREはOpenRTMに同梱だが，Java
版を開発する場合等に必要なのでインストールをオススメ(32bit版オススメ)
• Python 2.7・・・ python-2.7.9.msi(32bit)
• PyYAML (2.7対応)・・・PyYAML-3.11.win32-py2.7.exe
• OpenRTM-aist・・・OpenRTM-aist-1.1.1-RELEASE_x86_vc12.msi (32bit)
• GUIツール入りeclipse・・・ eclipse381-openrtp110rc5v20150317-ja-win32.zip (32bit)
• Doxygen
• Doxygen win32 ・・・ doxygen-1.8.9.1-setup.exe
• CMake 3.2
• CMake 3.2 ・・・ cmake-3.2.1-win32-x86.exe
• Visual Studio 2013 Community Edition
• https://www.microsoft.com/ja-jp/dev/products/community.aspx
3

動作確認 (Windows)
• ネームサービスの起動
– 「スタートメニュー」＞「OpenRTM-aist 1.1」＞「tools」＞「Start C++
Naming Service」
• RTCの起動（ConsoleIn, ConsoleOut）
– 「スタートメニュー」＞「OpenRTM-aist 1.1」＞「C++」＞「components」
＞「examples」＞「ConsleInComp.exe」と同じく「ConsoleOutComp.exe」
• GUIツールの起動
– eclipse***.zipを展開
– Eclipseフォルダ内のeclipse.exeを起動
– ワークスペースはデフォルトでOK
• 通常は，/Users/$USER_NAME/workspace
4

動作確認 (Ubuntu)
• 新規ターミナルからNameServer起動
• $ rtm-naming (Ubuntuだと，現在のプロセスをkillして再起動するか聞かれるので，必ずyesして再
起動する．デフォルトで立ち上がっているサービスはRTMに不向き)
• ターミナルからConsoleIn起動
• $ cd /usr/local/share/openrtm-1.1/examples/ (Ubuntuなら/usr/share/openrtm-1.1/examples)
• $ ./ConsoleInComp
• ターミナルからConsoleOut起動
• $ cd /usr/local/share/openrtm-1.1/examples/ (Ubuntuなら/usr/share/openrtm-1.1/examples)
• $ ./ConsoleOutComp
• ターミナルからEclipseを起動
• Downloadsにダウンロードして，そのまま展開してeclipseというディレクトリが出来た，と仮定し
ます．
• $ cd $HOME/Downloads/eclipse
• $ cd Eclipse.app/Contents/MacOS
• $ ./eclipse
• Ubuntuなら解答したディレクトリに移動してeclipseというバイナリを実行するのみ
5

ネームサービスとは
• 実行中のRTCの管理
– OpenRTM-aistを使う場合，RTCを
使ったシステムでは最低１つ必要
– 異なるホストのRTCも登録可能
– 複数のネームサービスを併用可能
6
RTC1
RTC2
RTC3
ネーム
サーバー
ツール
PC2
PC1

ツールの役割
• ツールの種類
• RT System Editor
• Eclipseのプラグイン
• GUI
• rtshell
• コマンドからRTCを制御
• CUI．スクリプト化が可能＝自動化
• 自作ツール
• ツール作成自体も容易
• ツールの役割
• RTC間の接続
• RTCのコンフィグレーションの変更
• RTCの状態の変更
• 実行コンテキストの制御
77
RTC RTC
RTCRTC
CREATED INACTIVE
ACTIVEERROR
DEACTIVATE
ACTIVATE
RESET
RTC
RTC

RT System Editorの使い方
• パースペクティブを「RT
System Editor」に変更
– パースペクティブとはEclipse
の作業画面のレイアウトタイ
プ
– メニュー＞「ウィンドウ」＞
「パースペクティブを開く」
＞「その他」
– 「RT System Editor」を選択
8

• 所望のネームサーバが見つからない場合は追加処理
– ネームサーバとは実行中のRTCの管理を行うサーバ
• localhost (自分自身)
• デフォルトでlocalhostを検索し，ネームサーバーが発見され
れば追加される
• Macでは不具合があるので，ネームサーバーを起動する前に
RTSEを起動した方がいい
• ネームサーバに起動したRTCが登録されていれば成功
• ConsoleInは
• 「/localhost/お使いのPC名.host_cxt/ConsoleIn0.rtc」
という名前でネームサーバーに登録されているはず
• 名前の登録ルールを変更することも簡単
• rtc.conf
9

• System Editorを開いてRTシステムを編集す
る
- File > Open On-Line System Editorもしくはツールバー
上のボタン (右図)
- メインビューに空の「System Diagram」が表示される
• ネームサービスビューからドラッグ＆ドロッ
プ
– 利用するRTCをすべて表示
• ポートとポートをドラッグ＆ドロップで接続
– 表示されるダイアログはOK
10

• すべてのRTCをACTIVATE
• ConsoleInのウィンドウに「Please
Input number」と表示
– 適当な数字を入れると
ConsoleOut側に表示
– RTC間の通信が行われたことが分か
る
• DEACTIVATEする場合は，指令を
送ってからConsoleInに数字を送る
必要がある
– （scanf入力待ちになっている）
ACTIVATE DEACTIVATE
11

まとめ
• ツールの使い方
• ネームサービスが必ず必要
• RT System Editor (RTCの接続，Activate/
Deactivate)
12

OpenRTM-aist入門その２
菅佑樹
13

データポートを使う
• OpenRTM-aistではポートは２種類
• データポート・・・データフロー型
• サービスポート・・・遠隔手続き型
• データポートはちょうど，制御のブロック線
図のようにRTシステムを組むことができる
• ロボット台車の簡易シミュレータを使っ
てRTC開発に慣れよう
14

OpenRTM-aist学習用台車シミュレータ
• Loader.batを実行
– シミュレータ
– 仮想ジョイスティック
– コントローラ
– Java7以降が必要
• すべて接続してACTIVATE
• ジョイスティックで操作
• コントローラGUIで位置や接触センサを確認
シミュレータ仮想ジョイスティック
コントローラGUI
http://ysuga.net/?p=133
15

台車シミュレータの中身
• RT System Editorで表示
仮想ジョイスティック
コントローラ
シミュレータ内のロボット
目標速度
現在位置
接触センサ
ジョイスティック入力
16

実習1．台車に指令を送ってみよう
• 下記のMobileRobotのRTCは移動ロボットシミュレータ内の台車を表している
目標速度「vel」
TimedVelocity2D型
現在位置「pos」
TimedPose2D型
接触センサ「bumper」
TimedBooleanSeq型
17
• 台車に直進と回転の指令を同時に送り，その場でぐるぐる回る動作をさせ
てみよう

RTCプログラミングの流れ
• RTC Builderによるスケルトンコードの生成
• スケルトンコードの特定のイベントハンドラに独自のコードを追加
• on_initialized・・・CREATED->INACTIVE
• on_activated・・・INACTIVE->ACTIVE
• on_deactivated・・・ACTIVE->INACTIVE
• on_execute・・・ACTIVE状態で周期的に呼ばれる
• コンパイルし実行
18

RTC Builder
• パースペクティブをRTC Builderに変更
– Builder Editorで，RTCの骨格（スケルト
ン）コードを作成できる
• Builder Editorを開く
– RTCが持つべき「イベントハンドラ」が実
装されているので，それを編集して自分の
コードを差し込む
19

RTC Builder
• プロジェクト名の入力
– MyController
• 画面下部のタブを切り替え
ながら必要な情報を入力
20

RTC Builder
• 基本タブ
必要情報を入力
21

RTC Builder
• アクティビティタブ
1. onExecuteを選択
2. ONにする
22

RTC Builder
• データポートタブ
• データ型が見つから無い場合はRTM_ROOT環境変数が設定されて無い
• システムの詳細設定や，~/.bashrcなどを確認．必要によっては再起動．
• TimedVelocity2DはTimedVector2Dと間違いやすい！！
出力ポートを追加ポート名を編集
ポートの型を変更
ポートのソースコード上の変数名を入力 23

RTC Builder
• 言語タブ
C++を選択
24
• 基本タブに戻る (下にスクロール)
コード生成

RTC Builder
• MyControllerプロジェクトにソースコードが生
成される
– Builder Editor（真ん中のウィンドウ）は閉じてよい
25

RTCのソースコードを見つける
• Eclipseは通常，自分のホームディレクトリ直下
のworkspaceフォルダにワークスペースを作る
• Winなら C:Usersユーザ名workspace
• Linuxなどなら/home/ユーザ名/workspace
• ワークスペース内に，作成したRTCのソースコー
ドを見つけることができる
26

CMakeによるプロジェクト生成
• CMakeを起動
• CMakeLists.txtを
CMakeにドラッグ
＆ドロップ
27

ROBOTECH2012 RTミドルウエア講習会
CMake
• 出力フォルダパスにbuildを追加
• RTCのフォルダが”…/workspace/MyController”の場合
• ”…/workspace/MyController/build”というフォルダを作る
• Configureを押すとフォルダ作成確認のダイアログが出るのでOK
2012/7/11
28

CMake2.8
• ビルド環境を選択
– Visual Studio 12 2013を選択
– Use default native compilersを選
択
• 赤い表示が出るが恐れず
「Generate」を押す
– Visual Studio用プロジェクトファ
イルが生成される
29

MacやLinuxなら
• 以下のコマンドを打つ
• cd $HOME/workspace/MyController # RTCのフォルダに移動
• mkdir build # buildディレクトリ作成
• cd build # buildディレクトリに移動
• cmake ../ # cmakeする
• make #ここでコンパイル
• 古いRTCBを使っている場合，Macの場合は，RTCのヘッダーファイルを書き換える必要がある
• RTCの名前を$ModuleNameとすると・・・
• RTCのディレクトリ/include/$ModuleName/$ModuleName.hを開く
30
#include <rtm/Manager.h>
#include <rtm/DataFlowComponentBase.h>
#include <rtm/CorbaPort.h>
#include <rtm/DataInPort.h>
#include <rtm/DataOutPort.h>
#include <rtm/idl/BasicDataTypeSkel.h>
#include <rtm/idl/ExtendedDataTypesSkel.h>
#include <rtm/idl/InterfaceDataTypesSkel.h>
#include <rtm/idl/BasicDataTypeSkel.h>
#include <rtm/idl/ExtendedDataTypesSkel.h>
#include <rtm/idl/InterfaceDataTypesSkel.h>
#include <rtm/Manager.h>
#include <rtm/DataFlowComponentBase.h>
#include <rtm/CorbaPort.h>
#include <rtm/DataInPort.h>
#include <rtm/DataOutPort.h>

Visual Studio
• build/src/MyController.slnファイルを
ダブルクリック
– Visual Studioが起動する
– 複数のプロジェクトが存在
– MyController・・RTC本体
– MyControllerComp・・・RTCを単体の
アプリケーションとして実行するための
プロジェクト
• MyControllerプロジェクト
– MyController.cppを開く
31

Visual Studio
• MyController::onExecute関数
• RTCがACTIVE状態の場合に周期的に
呼ばれる
• 実行周期は後で設定
• ここに周期的に呼ばれるべき制御ア
ルゴリズム等を記述
32
RTC::ReturnCode_t MyController::onExecute(RTC::UniqueId ec_id)
{
m_velocity.data.vx = 0.05; // バッファに書込む
m_velocity.data.vy = 0;
m_velocity.data.va = 1.0;
m_velocityOut.write(); // データを送信
return RTC::RTC_OK;
}

2次元速度のデータ型
• TimedVelocity2D・・・タイムスタンプ付き2次元平面での
速度および角速度
• tm : Time型・・・タイムスタンプ
• sec : unsigned long型・・・秒
• nsec : unsigned long型・・・ナノ秒
• data : Velocity2D型・・・２次元平面での速度および各
速度
• vx : double型・・・X軸方向速度 (単位m/s)
• vy : double型・・・Y軸方向速度 (単位m/s)
• va : double型・・・各速度 (単位rad/s)
進行方向
X
θ
Y
33

移動ロボットに指令を送る
• 従って，TimedVelocity2D型の変数「vel」
の各メンバにアクセスする際は・・・
– vel.data.vx・・・X軸方向速度 [m/s]
– vel.data.vy・・・Y軸方向速度 [m/s]
– vel.data.va・・・Z軸方向速度 [rad/s]
– データ型がどんな構造体なのか知るには，IDLファ
イルを読む
– 通常は，C:Program Files (x86)OpenRTM-
aist1.1rtmidlにある
– TimedVelocity2Dは，ExtendedDatatype.idlで
定義されている．
X
θ
Y
34
LinuxやMacなら，
/usr/include/openrtm-1.1/rtm/idl
もしくは，
/usr/local/include/openrtm-1.1
/rtm/idl

IDLファイル
• オブジェクト指向言語に変
換可能なインターフェース
定義言語
• C++, Java, Pythonに変換
• 独自のデータ型を定義する
にはIDLを書いてRTC
Builderに読み込ませる
• CORBAの入門書やウェブサ
イトを参考にしてください．
35
/*!
* @struct TimedVelocity2D
* @brief Time-stamped version of Velocity2D.
*/
struct TimedVelocity2D
{
Time tm;
Velocity2D data;
};
/*!
* @struct Velocity2D
* @brief Velocities in 2D cartesian space.
*/
struct Velocity2D
{
/// Velocity along the x axis in metres per second.
double vx;
/// Velocity along the y axis in metres per second.
double vy;
/// Yaw velocity in radians per second.
double va;
};

実行
• ネームサービスの起動確認
• RTCの実行
– MyControllerCompを右クリック
– 「デバッグ→インスタンス作成」で実行
• MobileRobotSim.batを実行するとシミュレー
タのみ起動
• RT System Editorでの接続，ACTIVE化
36

実習2．台車の動きを調整
• コンフィグレーション機能を試す
– コンフィグレーションとは，実行中
のRTCを調整するための機能
• 例：制御ゲインの調整
• コンフィグレーション機能を加え
て実行中にMyControllerの機能を
調整する
進行方向
X
θ
Y
37

RTC Builder
• Eclipseのパースペクティブを
RTC Builderに再変更
• パッケージエクスプローラの
RTC.xmlファイルをダブルク
リックする
– 先ほど作成した情報が記録され
ている
38

RTC Builder
• コンフィグレーションタブ
– velocity_x というコンフィグレーションを追加
39

追加したコンフィグレーション
• velocity_x : double型
– 変数名: velocity_x
– デフォルト値: 0.05
• velocity_theta : double型
– 変数名: velocity_theta
– デフォルト値: 1.0
40

RTC Builder
• 比較ダイアログ
– ここでは「Generated」を選択
– 基本的に「Merge」を選択
• 新しいコードの変更点のみ反映
– Generatedは新しいコード側で上書きされるので，自分の記入したコードが消える
• バックアップファイルがある (ファイル名末尾に日付が入る) ので心配要りま
せん
41

実習2．台車の動きを調整
• コンフィグレーション機能を試す
– コンフィグレーション機能を加えて実行中に
MyControllerの機能を調整する
{
std::cout << "Vx=" << m_velocity_x << std::endl;
std::cout << "Vtheta=" << m_velocity_theta << std::endl;
m_velocity.data.vx = m_velocity_x;
m_velocity.data.va = m_velocity_theta;
m_velocityOut.write();
return RTC::RTC_OK;
}
進行方向
X
θ
Y
42

実行
• RTCの実行
– MobileRobotSim.batを実行するとシミュレータのみ起動
• RT System Editor下部にConfiguration Viewに選択中のRTCの
コンフィグが表示されるので変更して「適用」を選択する（適
用しないと反映されない）
43

実習3．位置の取得
• 現在位置をデータポートから受け取ってコンソールに表示する
TimedVelocity2D型
TimedPose2D型
TimedBooleanSeq型
• TimedPose2D型の入力ポートをMyControllerに追加する

RTC Builder
• 再度，RTCBuilderを開く
• MyController / RTC.xmlを開く (たぶん，開きっぱなし？)
• データポートタブに移動 → TimedPose2D型の入力ポート「pose」を追加
45

2次元位置のデータ型
• TimedPose2D・・・2次元平面での位置および姿勢
• tm : Time型・・・タイムスタンプ
• sec : unsigned long型・・・秒
• nsec : unsigned long型・・・ナノ秒
• data : Pose2D型・・・２次元平面での位置および姿勢
• position : Point2D型・・・２次元平面内での位置
– x : double型・・・X軸方向変異 (単位m)
– y : double型・・・Y軸方向変異 (単位m)
• heading : double型・・・姿勢 (単位rad)
• 従って，C++でTimedPose2D型の変数「pose」の各メンバにアクセスする
際は・・・
– pose.data.position.x・・・X軸方向変位
– pose.data.position.y・・・Y軸方向変位
– pose.data.heading・・・Z軸方向回転
進行方向
X
θ
Y
46

Visual Studio
• 再度，onExecute関数を編集
– 入力ポートはデータが来ているか確認する処理が入る
{
m_velocity.data.vx = m_velocity_x;
m_velocity.data.va = m_velocity_theta;
m_velocityOut.write();
if(m_poseIn.isNew()) { // 入力ポートに入力があるか確認
m_poseIn.read(); // 入力があるならば読み込む
std::cout << "X = " << m_pose.data.position.x << std::endl;
std::cout << "Y = " << m_pose.data.position.y << std::endl;
std::cout << "Z = " << m_pose.data.heading << std::endl;
}
return RTC::RTC_OK;
}
47

実行
• RTCの実行
– MobileRobotSim.batを実行するとシミュレータのみ起
動
• 実行時の周期が速すぎる？
48

rtc.conf
• RTCの実行時の設定ファイル
– ネームサーバのIPアドレス，ポート番号
– 実行周期，実行コンテキストの種類
– RTCの名前付け規則
– ログの有無，ログレベル
– etc…
• rtc.confで実行周期を変更（単位Hz）
– 右クリック→アプリケーション→テキストエディタ
– rtc.confを実行ファイルと同じ場所に置いて，実行ファイルを実行
– Visual C++でデバッグする場合は，プロジェクトファイルと同じディレクトリに置く
（デフォルトでそこがカレントディレクトリになる）
exec_cxt.periodic.rate: 1.0
49

ROBOTECH2012 RTミドルウエア講習会
データポート関連変数の命名法則
• 出力ポートの場合
–「変数名」＝「example」
•m_example
–データポートのデータ
を入れるバッファ
•m_exampleOut
–データポート本体
• 利用方法
• 1. m_example にデータを入
力
• 2. m_exampleOut.write()
2012/7/11
50
• 入力ポートの場合
–「変数名」＝「example」
•m_example
–データポートのデータを入
れるバッファ
•m_exampleIn
–データポート本体
• 利用方法
• 1. m_exampleIn.isNew()で受信確認
• 2. m_exampleIn.read()でデータ取
得
• 2. m_exampleのデータを読み取る

実習4．台車の接触スイッチ
• TimedBooleanSeq 真偽型 (True/False)の
配列
TimedVelocity2D型
TimedPose2D型
TimedBooleanSeq型
配列の0番目要素＝右側スイッチ
配列の1番目要素＝左側スイッチ
51

RTC Builderによるポートの追加
• InPort : TimedBooleanSeq型
– ポート名: bumper
– 変数名： bumper
52

Visual Studio
• **Seq型はdataメンバを配列のように使える
{
・・・省略・・・
if(m_bumperIn.isNew()) {
m_bumperIn.read();
if(m_bumper.data[0] == true) {
std::cout << "Right Bumper Hit!!" << std::endl;
}
if(m_bumper.data[1] == true) {
std::cout << "Left Bumper Hit!!" << std::endl;
}
}
return RTC::RTC_OK;
}
53

みなさんへの課題
• 接触スイッチの反応でロボットの動作を変える
– 左右どちらかに旋回する？
• 位置の値を使って図形を描く
– 正方形，星型などなど
• 仮想ジョイスティックを使った操作
• お使いのロボット関連製品をRTC化する
– onExecute以外の使えるイベントハンドラ例(RTC Builderで使えるように設定)
• onActivated・・・Activate時に一回呼ばれる（初期化用）
• onDeactivated・・・Deactivate時もしくはエラー状態遷移時に呼ばれる（終了処理）
スティック入力「out」
TimedDoubleSeq型
各ボタンの出力値はGUIに記載されている
例： Up(0,1) → 0番目要素が0，1番目要素が1
最初はコンソールにどんなデータが来るか表示して試してみよう
54

まとめ
• ツールの使い方
– RT System Editor (RTCの接続，Activate/Deactivate)
– RTC Builder (RTCのスケルトンコード生成)
– CMake (Visual C++用プロジェクト生成)
• コーディング方法
– データポート入出力
• TimedVelocity2D, TimedPose2D, TimedBooleanSeq
– コンフィグレーション
– rtc.confの設定
55

さらなる発展
• RTCを探してみよう
• http://openrtm.org >プロジェクト > RTコンポーネント
• 独自のデータ型を使ってみよう
• データポートに独自のデータ型を使うことができる
• サービスポートを使ってみよう
• RTCに関数型のインターフェースを追加でき，より柔軟なシステム設計ができ
る
• rtshellを使ってみよう
• コマンドラインから使えるツール．スクリプトを書くことで開発を効率化で
きる
• RTCを接続・コンフィグ・アクティブ化するプログラムを作ってみよう
• システム構築を自動化できる
• 参考ページ： http://ysuga.net
56

OpenRTM-aist入門その3
菅佑樹
57

独自のデータ型を使ってみよう
• 自分独自のデータ型で通信
• データ型がないときに行う
• データを分割して送ると，ひとまとまりのデータなのに別のポー
トになってしまう
• わかりやすいメンバ名を付けられる (ただの配列ではできない)
• 接続できるデータポートを制限できる (DoubleSeqだとどんな
データでも送れるけど，どんなデータも繋がってしまう）
• 設定ファイルを配布しないといけないのが若干手間
• すでにあるデータ型は再定義しないようにしよう (よく調べる
か相談しよう)
58

独自のデータ型を使ってみよう
• Kinectと連携してみよう
• Kinect SDK for Windowsを使ったRTコンポーネ
ント
• KinectだけWindowsで動かして他のRTCは別のシ
ステムで作っても良い
• モーションキャプチャのためのデータ型が見つから
なかったので作った
• 一部のKinect用RTCはTimedDoubleSeqを使ってい
るけど上述の問題で使いにくい
59

すでにあるデータ型
• OpenRTM-aistで用意されているデータ型 ($RTM_ROOT/rtm/idlの中にあるファイル)
• BasicDataType.idl
• プリミティブ型 (Long, Double, Seq)
• ExtendedDataTypes.idl
• ロボット周りの物理 (Point2D/3D, Pose2D/3D, Velocity2D/3D)
• InterfaceDataTypes.idl
• ロボット関連でよく使うデータ (Path2D, OGMap, RangeData, CameraImage)
• 共通インターフェースと呼ばれるデータ型
• 画像系
• マニピュレータ
• 移動系
• 音声対話・コミュニケーション
• その他国際規格
• RoIS (サービスロボット)
60

Kinectのデータ
• 要するに構造体
• Vector4
• ３次元位置
• NuiSkeletonData
• trackingState
• トラッキングの状態
• skeletonPosition[20]
• 各関節の位置
• 関節のインデックスをenumにしておく
• OpenRTM-aistではこの構造体定義をIDL
(Interface Definition Language) で書く
61

Interface Definition Language
• サービスポートの関数やデータ型を宣言するためのファイル形式
(.idlファイル)
– CORBA, RMIやDDSなどの分散処理実現のために使われる (標
準化されている形式)
– Javaに似た形式
– オブジェクト指向
• IDLファイルをコンパイルすると，C++やJava, Pythonのプログラ
ムのスケルトンが生成される
– IDLは中間言語
– C++やJavaなどの実装言語に変換すると，各言語ごとに若干利
用方法が変わるが，関数名などは変わらない
– 今回はC++版を少しだけ解説 62

IDLの書き方
• moduleで名前空間を定義
• 後述のインターフェース名は同じ名前
になりがちなので名前空間で分ける
• structで構造体を定義できる
• 異なるタイプのデータの集まり
• プリミティブデータ型として使える
• long / unsigned long
• short / unsigned short
• char / unsigned char
• float / double
• boolean
• string
63
// 行コメント
// moduleはC++のネームスペースに似た概念
module my_module {
// 構造体を宣言することでプログラムを構造化できる
struct MyData {
long data1;
long data2;
double data3;
};
};

IDLの書き方
• typedefでデータ型の
名称変更
• enumで定数を作る
• sequenceで可変長の
データ配列を作成でき
る
64
module my_module {
// typedef を使ってラベルを変更できる．
typedef long ERROR_CODE;
// enumを使って数にラベルを付けられる
enum MY_STATE {
STATE_RUNNING,
STATE_HALT,
STATE_ERROR
};
enum RETURN_CODE {
RETURN_OK,
RETURN_ERROR
};
struct MyData {
ERROR_CODE errorCode;
My_STATE state;
sequence<double> data;
// sequenceを使うと可変長の配列を使う
};
};

Kinectを使う
• Windows版Kinect SDKを使う
• Kinect for Windows SDK v1.8
• https://www.microsoft.com/en-us/
download/details.aspx?id=40278
• ソースコードは公開済み
• https://github.com/sugarsweetrobotics/
Kinect.git
• 設定できるデータ (入力ポート)
• チルト角度 (targetElevation)
• 取得できるデータ (出力ポート)
• トラッキングした人間の関節の位置
(skeleton)
• 深度情報を持った画像 (depth)
• チルト角度 (currentElevation)
• カメラ画像 (image)
• Kinect自体はマイクロフォンアレイもあるので，
65

skeletonのデータを使う
• 各関節の位置を得られる
• 3次元位置にはPoint3Dなどのデータ型があるが，複数
の人間のトラッキング情報などを統一的に扱うにはデー
タ型が無い
• データ型があるか否かを確認するのが重要
• TimedDoubleSeq (double型の配列) などを安易に
使うと，異なるデータ同士でつなぐことができるよ
うになり，他の人に迷惑
• 独自データ型を使う！！
66

IDLでstructを定義する
module KINECT {
struct スケルトンデータ {
トラッキングした人間の関節位置[20] : Vector4
トラッキングの品質や状態
};

struct スケルトンフレーム {
タイムスタンプやフレーム番号 : Time
スケルトンデータ [ 6 ];
};
};
67
• 単純にKinectSDKでサポートしているstructをそのままIDLにしまし
た
• 上述のgithubサーバーからファイルをダウンロードしてIDLの中身
を見てみよう
• 大まかな構造は下記のようになっている

module KINECT {

enum NUI_SKELETON_TRACKING_STATE {
NUI_SKELETON_NOT_TRACKED,
NUI_SKELETON_POSITION_ONLY,
NUI_SKELETON_TRACKED
};

enum NUI_SKELETON_POSITION_TRACKING_STATE {
NUI_SKELETON_POSITION_NOT_TRACKED,
NUI_SKELETON_POSITION_INFERRED,
NUI_SKELETON_POSITION_TRACKED
};

enum NUI_SKELETON_POSITION_INDEX {
NUI_SKELETON_POSITION_HIP_CENTER,
NUI_SKELETON_POSITION_SPINE,
NUI_SKELETON_POSITION_SHOULDER_CENTER,
NUI_SKELETON_POSITION_HEAD,
NUI_SKELETON_POSITION_SHOULDER_LEFT,
NUI_SKELETON_POSITION_ELBOW_LEFT,
NUI_SKELETON_POSITION_WRIST_LEFT,
NUI_SKELETON_POSITION_HAND_LEFT,
NUI_SKELETON_POSITION_SHOULDER_RIGHT,
NUI_SKELETON_POSITION_ELBOW_RIGHT,
NUI_SKELETON_POSITION_WRIST_RIGHT,
NUI_SKELETON_POSITION_HAND_RIGHT,
NUI_SKELETON_POSITION_HIP_LEFT,
NUI_SKELETON_POSITION_KNEE_LEFT,
NUI_SKELETON_POSITION_ANKLE_LEFT,
NUI_SKELETON_POSITION_FOOT_LEFT,
NUI_SKELETON_POSITION_HIP_RIGHT,
NUI_SKELETON_POSITION_KNEE_RIGHT,
NUI_SKELETON_POSITION_ANKLE_RIGHT,
NUI_SKELETON_POSITION_FOOT_RIGHT,
NUI_SKELETON_POSITION_COUNT
};

68
struct Vector4 {
float v[4];
};
struct NuiSkeletonData {
NUI_SKELETON_TRACKING_STATE trackingState;
long trackingID;
long enrollmentIndex;
long userIndex;
Vector4 position;
Vector4 skeletonPositions[20];
NUI_SKELETON_POSITION_TRACKING_STATE
eSkeletonPositionTrackingState[20];
long qualityFlags;
};

struct NuiSkeletonFrame {
long long liTimeStamp;
long dwFrameNumber;
long dwFlags;
Vector4 vFloorClipPlane;
Vector4 vNormalToGravity;
NuiSkeletonData SkeletonData[ 6 ];
};
struct DepthImage {
long long timestamp;
long width;
long height;
sequence<unsigned short> bits;
double horizontalFieldOfView;
double verticalFieldOfView;
};
struct SoundSourceLocation {
double angle;
double confidence;
};
};

module KINECT {

enum NUI_SKELETON_TRACKING_STATE {
NUI_SKELETON_NOT_TRACKED,
NUI_SKELETON_POSITION_ONLY,
NUI_SKELETON_TRACKED
};

enum NUI_SKELETON_POSITION_TRACKING_STATE {
NUI_SKELETON_POSITION_NOT_TRACKED,
NUI_SKELETON_POSITION_INFERRED,
NUI_SKELETON_POSITION_TRACKED
};

enum NUI_SKELETON_POSITION_INDEX {
NUI_SKELETON_POSITION_HIP_CENTER,
NUI_SKELETON_POSITION_SPINE,
NUI_SKELETON_POSITION_SHOULDER_CENTER,
NUI_SKELETON_POSITION_HEAD,
NUI_SKELETON_POSITION_SHOULDER_LEFT,
NUI_SKELETON_POSITION_ELBOW_LEFT,
NUI_SKELETON_POSITION_WRIST_LEFT,
NUI_SKELETON_POSITION_HAND_LEFT,
NUI_SKELETON_POSITION_SHOULDER_RIGHT,
NUI_SKELETON_POSITION_ELBOW_RIGHT,
NUI_SKELETON_POSITION_WRIST_RIGHT,
NUI_SKELETON_POSITION_HAND_RIGHT,
NUI_SKELETON_POSITION_HIP_LEFT,
NUI_SKELETON_POSITION_KNEE_LEFT,
NUI_SKELETON_POSITION_ANKLE_LEFT,
NUI_SKELETON_POSITION_FOOT_LEFT,
NUI_SKELETON_POSITION_HIP_RIGHT,
NUI_SKELETON_POSITION_KNEE_RIGHT,
NUI_SKELETON_POSITION_ANKLE_RIGHT,
NUI_SKELETON_POSITION_FOOT_RIGHT,
NUI_SKELETON_POSITION_COUNT
};

69
struct Vector4 {
float v[4];
};
struct NuiSkeletonData {
NUI_SKELETON_TRACKING_STATE trackingState;
long trackingID;
long enrollmentIndex;
long userIndex;
Vector4 position;
Vector4 skeletonPositions[20];
NUI_SKELETON_POSITION_TRACKING_STATE
eSkeletonPositionTrackingState[20];
long qualityFlags;
};

struct NuiSkeletonFrame {
long long liTimeStamp;
long dwFrameNumber;
long dwFlags;
Vector4 vFloorClipPlane;
Vector4 vNormalToGravity;
NuiSkeletonData SkeletonData[ 6 ];
};
struct DepthImage {
long long timestamp;
long width;
long height;
sequence<unsigned short> bits;
double horizontalFieldOfView;
double verticalFieldOfView;
};
struct SoundSourceLocation {
double angle;
double confidence;
};
};
各関節のインデックス番号．
NuiSkeletonDataで使う
トラッキング出来ているか否か
トラックした人間一人分のデー
タ．関節部の位置
一回のスキャンで取れるデータ
のまとまり．最大６人

IDLをEclipseに読ませる
• OpenRTP起動
• Window > Preference
• 開いたダイアログからRTCBuilder
を選択
• Newボタンをクリックして，フォ
ルダを指定
• 好きなフォルダを指定できるの
で，例えば「マイドキュメント
/idl」というフォルダを作り，
毎回そこにidlを入れれば良い
• Eclipseを再起動する
70

IDLを登録する
• データポートを作る
時に選択肢が増える
71

ビルド方法
• CMake
– RTCBuilderが古いと失敗する
• Visual Studio
– この中でomniidlというコマンドでIDLをコンパイルするが，Python版
がインストールされていると問題が起こることがある
• PATHの設定によっては，Python版のomniidlが優先して見つかるた
め，C++にコンパイルできない
– → PATHの最初にC++版のomniidl.exeのPATHを設定しなおす
– IDLはビルドできてる (** Not Foundのようなエラーは無い) のにコンパ
イルできない
– → openrtmのメーリングリストに通報
– → IDLコンパイルでidlファイル名.hhというファイルが生成され
る．その中のインターフェースをヘルパークラスがオーバーライド
しているので，返り値の型などを手動で直すとコンパイルできるこ
とがある
72

KinectTest RTC
• KinectTestというRTCを作ってみよう
• Module名がKinectTest
• NuiSkeletonFrame型の入力ポートを持つ
• 頭よりも手の高さが上になったら手を挙げ
ていると判断して表示
73

OnExecute
RTC::ReturnCode_t KinectTest::onExecute(RTC::UniqueId ec_id)
{
if (m_inIn.isNew()) {
m_inIn.read();
if (m_in.dwFrameNumber == 0) {
std::cout << "[KinectTest] No man detected." << std::endl;
}
else {
int index = 0;
double head_z = m_in.SkeletonData[index].skeletonPositions[KINECT::NUI_SKELETON_POSITION_HEAD].v[2];
double right_hand_z = m_in.SkeletonData[index].skeletonPositions[KINECT::NUI_SKELETON_POSITION_HAND_RIGHT].v[2];
double left_hand_z = m_in.SkeletonData[index].skeletonPositions[KINECT::NUI_SKELETON_POSITION_HAND_LEFT].v[2];
if (right_hand_z > head_z) {
std::cout << "[KienctTest] Right Hand Raised." << std::endl;
}
if (left_hand_z > head_z) {
std::cout << "[KienctTest] Left Hand Raised." << std::endl;
}
if (left_hand_z <= head_z && right_hand_z <= head_z) {
std::cout << "[KinectTest] No hands raised....." << std::endl;
}
}
}
return RTC::RTC_OK;
}
74

発展編
• Kinectから受け取ったモーションキャプチャ情報を解析し
て，移動ロボットを操作してみよう
• 右手を右に開いたら右
• 左手を左に開いたら左
• 両手を前に向けたら前進
• RTCはどのようなインターフェースをもっていなければなら
ないか
• 入力： Kinectのフレームデータ
• 出力：移動ロボットへの速度指令
75

まとめ
• 独自のデータ型を定義する方
法について
• まずはOpenRTM-aistで
定義されているデータ型を
検討すること
• TimedDoubleSeqなどは
使わない事！！
• データ型については，UMLの
クラス図を使って情報を交換
するといい
• 他の人と相談するときに使
おう！
76

OpenRTM-aist入門その４
菅佑樹
77

サービスポートの基本
78

データポートだけでは不便
• データが欲しいときだけ処理をして欲しい
• データポートはいつもpush型
• openやclose, resetのようなデータをともなわない通信をし
たい
• Long型で1を送ると・・・みたいなインターフェースはダ
メ．別のLong型とつながっちゃうから
• 複数のデータを同期して受け取りたい
• データポートでもタイムスタンプがあるし不可能ではな
いが・・・
79

サービスポートの概念
• OpenRTM-aistの機能
• RTCに関数呼び出しのインターフェースを付加
– 引数でデータを渡す
– 返り値でデータを受け取る (引数によるデータ受け取りも可能．これなら複数のデー
タを同期して受け取ることができる．)
• データポートと比べて
– データを必要なときだけ要求することができる
– データの送受信の結果が得られる（成否など）
– データの送受信に関わらない処理を実現できる
• reset, initializeなど
– 若干実装が面倒．やっぱりIDLが要る
– 独自のインターフェースが乱立すると再利用性が下がる
– 共通インターフェース（後述）
80

サービスポートの概念
• サービスポートは「インターフェース」を持つ
• インターフェースには極性 (Polarity) がある
• Provider 提供側 (polarity=Provided)
• リソースを提供する．関数が呼ばれ
る側
– Consumer 利用側 (polarity=Required)
• リソースを利用する．関数を呼ぶ側
• 一つのサービスポートが複数の提供・要求イン
ターフェースを持つことができる
• インターフェースのタイプが合えば接続で
き，利用できる．
• ポートをつなげるには全てのインターフェー
スが実装されている必要がある
• １ポート，１インターフェースの方が使い
易い
81
RTC1 RTC2
ProvidedRequired
Service Port

サービスポート作成の流れ
• 1. サービスポートを追加
• サービスポート名を設定
• 2. サービスポートにインターフェースを追加
• 読み込むIDLファイルを選択
• インターフェース型を選択
• インターフェース名*
• インスタンス名
• 極性 (Polarity)
• 注意！
– インターフェース名が違うと繋げるのが面倒になる！
• インターフェース型を定義するファイルを作成 (IDLファイル)
82
RTC
Interface Name : Interface Type
Port Name

Interface Definition Language
• サービスポートの関数やデータ型を宣言するためのファイル
形式 (.idlファイル)
– CORBAやDDSなどの分散処理実現のために使われる
– Javaに似た形式
– オブジェクト指向
• IDLファイルをコンパイルすると，C++やJava, Pythonのプ
ログラムのスケルトンが生成される
– IDLは中間言語
– C++やJavaなどの実装言語に変換すると，各言語ごとに
若干利用方法が変わるが，関数名などは変わらない
– 今回はC++版を少しだけ解説 83

IDLの書き方
• moduleで名前空間を定義
– 後述のインターフェース名は同じ名前になりが
ちなので名前空間で分ける
• interfaceでインターフェース名を定義
– 機能をグループ化
• interface内で関数を定義
• 返り値，引数リスト，Javaとほぼ同じ構文
• 引数にin/outを設定
– in ・・・インターフェース提供側に引数を渡す
(値渡し)
– out ・・・インターフェース提供側が引数にデー
タを設定する (参照渡し)
– inout ・・・インターフェース提供側に引数
データを渡し，提供側がさらにそれを変更する
84
// 行コメント
// moduleはC++のネームスペースに似た概念
module my_module {
// interfaceはJavaのインターフェース，
// C++の仮想クラスに似ている．
// メンバ関数を定義する
interface MyInterface {
// 返り値でデータを受け取るタイプの関数
long getLongData();
// 引数でデータを与えるタイプの関数
void setLongData(in long data);
// 引数に参照渡しでデータを
// 受け取ることもできる
void getDataByArg(out long data);
};
};

IDLの書き方
• structで構造体を定義できる
– 異なるタイプのデータの
集まり
• 引数にも，返り値にも使える
• structでデータ型を定義でき
れば，データポートの独自デー
タ型作成ができる
– 参考: http://ysuga.net/?p=213
85
module my_module {
// 構造体を宣言することでプログラムを構造化できる
struct MyData {
long data1;
long data2;
double data3;
};
// 返り値でデータを受け取るタイプの関数
MyData getMyData();
void setMyData(in MyData data);
// 引数に参照渡しでデータを受け取ることもできる
void getMyDataByArg(out MyData data);
};
};

IDLの書き方
• typedefでデータ型
の名称変更
• enumで定数を作る
• sequenceで可変長の
データを作成できる
86
module my_module {
enum MY_STATE {
STATE_RUNNING,
STATE_HALT,
STATE_ERROR
};
enum RETURN_CODE {
RETURN_OK,
RETURN_ERROR
};
struct MyData {
My_STATE state;
};
// 返り値で処理の成否を返すことができる
RETURN_CODE setMyData(in MyData data);
RETURN_CODE getMyDataByArg(out MyData data);
};
};

RTC Builder
• 「サービスポート」タブ選択
• 「Add Port」でポート追加
• 「ポート名」を設定
• 「Add Interface」でインターフェース追
加
• 「インターフェース名」「方向」「インス
タンス名」「変数名」を設定
• 「IDLファイル」の「Browse」ボタンで
IDLを読み込む
• 「インターフェース型」を選択
• 「IDLパス」は「IDLファイルがあった
フォルダを選択」
87

ビルド方法
• CMake
– RTCBuilderが古いと失敗する
• Visual Studio
– この中でomniidlというコマンドでIDLをコンパイルするが，Python版がインストール
されていると問題が起こることがある
• PATHの設定によっては，Python版のomniidlが優先して見つかるため，C++に
コンパイルできない
– → PATHの最初にC++版のomniidl.exeのPATHを設定しなおす
– IDLはビルドできてる (** Not Foundのようなエラーは無い) のにコンパイルできない
• サービスポートを簡易に実行するためのヘルパークラスが生成されるが，独自の
IDLパーサーを使っているため問題が多い
– → 最初は構造体などをあまり使わずに
– → openrtmのメーリングリストに通報
– → IDLコンパイルでidlファイル名.hhというファイルが生成される．その中
のインターフェースをヘルパークラスがオーバーライドしているので，返り
値の型などを手動で直すとコンパイルできることがある
88

コーディング方法例 (提供側)
89
my_module::RETURN_CODE MyInterfaceSVC_impl::setMyData(const my_module::MyData& data)
{
my_module::RETURN_CODE result;
if (data.errorCode == 0) {
result = my_module::RETURN_OK;
} else {
result = my_module::RETURN_ERROR;
}
for(int i = 0;i < data.data.length();i++) {
std::cout << "Data " << i << " is " << data.data[i] << std::endl;
}
return result;
}
my_module::RETURN_CODE MyInterfaceSVC_impl::getMyDataByArg(my_module::MyData_out data)
{
my_module::RETURN_CODE result = my_module::RETURN_OK;
my_module::MyData_var myData = new my_module::MyData;
myData->state = my_module::STATE_RUNNING;
myData->errorCode = 0;
myData->data.length(4);
myData->data[0] = 0.1;
data = myData._retn();
return result;
}
module my_module {
enum MY_STATE {
STATE_RUNNING,
STATE_HALT,
STATE_ERROR
};
enum RETURN_CODE {
RETURN_OK,
RETURN_ERROR
};
struct MyData {
My_STATE state;
};
};
};
シンプルな関数呼び出しになる

out型引数は_var型を使う
引数がないときは簡単．参照渡しが難しいので解説．

コーディング方法例 (要求側)
90
my_module::MyData data;
data.errorCode = 0;
data.state = my_module::STATE_HALT;
data.data.length(3);
data.data[0] = 1.2;
data.data[1] = 1.4;
data.data[2] = 1.8;
if(m_variableName->setMyData(data) != my_module::RETURN_OK) {
std::cout << "[YourModule] setMyData failed." << std::endl;
}
my_module::MyData_var yourData = new my_module::MyData();
if(m_variableName->getMyDataByArg(yourData) != my_module::RETURN_OK) {
std::cout << "[YourModule] getMyData failed." << std::endl;
}

std::cout << "[YourModule] getMyData succeeded." << std::endl;
std::cout << " errorCode : " << yourData->errorCode << std::endl;
std::cout << " state : " << yourData->state << std::endl;
std::cout << " data : [";
for(int i = 0;i < yourData->data.length();i++) {
std::cout << yourData->data[i] << ", ";
}
std::cout << "]" << std::endl;
module my_module {
enum MY_STATE {
STATE_RUNNING,
STATE_HALT,
STATE_ERROR
};
enum RETURN_CODE {
RETURN_OK,
RETURN_ERROR
};
struct MyData {
My_STATE state;
};
};
};
シンプルな関数呼び出しになる

out型引数は_var型を使う
引数がないときは簡単．参照渡しが難しいので解説．

サービスポートの接続
• インターフェース名が同じであれ
ば，問題なく接続できる．
• インターフェース名が違う場合は，
RT System Editorで「接続でき
るインターフェースがない」と言
われるが，詳細ダイアログで接続
するインターフェースを追加・選
択すると接続できるようになる
• インターフェース名を再利用
したいRTCと同じにするのが
大事
91

実践編
マニピュレーター共通インターフェース
92

共通インターフェース
• RTCを定義しただけではロボット開発は便利にならない
• インターフェースの共通化
• 同じタイプのロボットならば入れ替えが効くようにす
る
• できるだけ多くの機能をカバー．「実装してない」を許
す
• ロボットアーム，移動ロボット，音声コミュニケーショ
ン，カメラ
• 関連するツールや知能モジュールが再利用しやすくなる！
93

マニピュレーター
共通インターフェース
• 2013年に改定 (埼玉大学琴坂研究室による)
• ２つのレベルに分割
• 共通レベル (ManipulatorCommonInterface_Common)
• 各関節レベルでの動作，位置獲得
• エラー情報取得・復帰
• 中位レベル (ManipulatorCommonInterface_Middle)
• 各関節レベルでの動作
• 手先位置での動作
• 手先移動範囲の指定や，ツールおよび台座のオフセット
• ハンドの開閉
• 共通するデータ型をManipulatorCommonInterface_DataTypesに規定
94

/*
Manipulator Common Interface (Data type defenition)
- This IDL is used as service port on RTC
- This command specification is provided by Intelligent RT Software
Project of JARA.
rev. 20140120
*/
#ifndef MANIPULATORCOMMONINTERFACE_DATATYPES_IDL
#define MANIPULATORCOMMONINTERFACE_DATATYPES_IDL
#include "BasicDataType.idl"
module JARA_ARM{
typedef sequence<double> DoubleSeq;
typedef sequence<double> JointPos;
struct LimitValue {
double upper;
double lower;
};
struct RETURN_ID {
long id;
string comment;
};
const long OK = 0;
const long NG = -1;
const long STATUS_ERR = -2;
const long VALUE_ERR = -3;
const long NOT_SV_ON_ERR = -4;
const long FULL_MOTION_QUEUE_ERR = -5;
const long NOT_IMPLEMENTED = -6;
struct TimedJointPos {
RTC::Time tm;
JointPos pos;
};
typedef unsigned long ULONG;
};
#endif // MANIPULATORCOMMONINTERFACE_DATATYPES_IDL
95
ManipulatorCommonInterface_Dat
aTypes.idl
RETURN_IDでデータを返す

/*
Manipulator Common Interface (Common Commands)
Project of JARA.
rev. 20140120
*/
#ifndef MANIPULATORCOMMONINTERFACE_COMMON_IDL
#define MANIPULATORCOMMONINTERFACE_COMMON_IDL
#include "ManipulatorCommonInterface_DataTypes.idl"
module JARA_ARM{
enum AlarmType {
FAULT,
WARNING,
UNKNOWN
};
struct Alarm {
unsigned long code;
AlarmType type;
string description;
};
typedef sequence<Alarm> AlarmSeq;
typedef sequence<LimitValue> LimitSeq;
struct ManipInfo {
string manufactur;
string type;
ULONG axisNum;
ULONG cmdCycle;
boolean isGripper;
};

96
~Common.idl
マニピュレーター情報構造体

const ULONG CONST_BINARY_00000001 = 0x01; //isServoOn
const ULONG CONST_BINARY_00000010 = 0x02; //isMoving
const ULONG CONST_BINARY_00000100 = 0x04; //isAlarmed
const ULONG CONST_BINARY_00001000 = 0x08; //isBufferFull
interface ManipulatorCommonInterface_Common {
RETURN_ID clearAlarms();
RETURN_ID getActiveAlarm(out AlarmSeq alarms);
RETURN_ID getFeedbackPosJoint(out JointPos pos);
RETURN_ID getManipInfo(out ManipInfo mInfo);
RETURN_ID getSoftLimitJoint(out LimitSeq softLimit);
RETURN_ID getState(out ULONG state);
RETURN_ID servoOFF();
RETURN_ID servoON();
RETURN_ID setSoftLimitJoint(in LimitSeq softLimit);
};
};
#endif // MANIPULATORCOMMONINTERFACE_COMMON_IDL
97
サーボON/OFF
関節のリミット設定

/*
Manipulator Common Interface (Middle Level Commands)
Project of JARA.
rev. 20140120
*/
#ifndef MANIPULATORCOMMONINTERFACE_MIDDLE_IDL
#define MANIPULATORCOMMONINTERFACE_MIDDLE_IDL
#include "ManipulatorCommonInterface_DataTypes.idl"
module JARA_ARM{
typedef double HgMatrix [3][4];
struct CarPosWithElbow {
HgMatrix carPos;
double elbow;
ULONG structFlag;
};
struct CartesianSpeed {
double translation;
double rotation;
};
interface ManipulatorCommonInterface_Middle {
RETURN_ID closeGripper();
RETURN_ID getBaseOffset(out HgMatrix offset);
RETURN_ID getFeedbackPosCartesian(out CarPosWithElbow pos);
RETURN_ID getMaxSpeedCartesian(out CartesianSpeed speed);
RETURN_ID getMaxSpeedJoint(out DoubleSeq speed);
RETURN_ID getMinAccelTimeCartesian(out double aclTime);
RETURN_ID getMinAccelTimeJoint(out double aclTime);
RETURN_ID getSoftLimitCartesian(out LimitValue xLimit,
out LimitValue yLimit, out LimitValue zLimit );
RETURN_ID moveGripper(in ULONG angleRatio);
RETURN_ID moveLinearCartesianAbs(in CarPosWithElbow carPoint);
RETURN_ID moveLinearCartesianRel(in CarPosWithElbow carPoint);
RETURN_ID movePTPCartesianAbs(in CarPosWithElbow carPoint);
RETURN_ID movePTPCartesianRel(in CarPosWithElbow carPoint);
RETURN_ID movePTPJointAbs(in JointPos jointPoints);
RETURN_ID movePTPJointRel(in JointPos jointPoints);
98
Middle.idl
デカルト座標系での位置・姿勢
を表すための構造体 (変換行列)
グリッパ開閉
手先位置取得
movePTPCartesianAbs
(手先の絶対位置座標指定移動)
movePTPCartesianRel
(手先の相対位置座標指定移動)

RETURN_ID openGripper();
RETURN_ID pause();
RETURN_ID resume();
RETURN_ID stop();
RETURN_ID setAccelTimeCartesian(in double aclTime);
RETURN_ID setAccelTimeJoint(in double aclTime);
RETURN_ID setBaseOffset(in HgMatrix offset);
RETURN_ID setControlPointOffset(in HgMatrix offset);
RETURN_ID setMaxSpeedCartesian(in CartesianSpeed speed);
RETURN_ID setMaxSpeedJoint(in DoubleSeq speed);
RETURN_ID setMinAccelTimeCartesian(in double aclTime);
RETURN_ID setMinAccelTimeJoint(in double aclTime);
RETURN_ID setSoftLimitCartesian(in LimitValue xLimit,
in LimitValue yLimit, in LimitValue zLimit);
RETURN_ID setSpeedCartesian(in ULONG spdRatio);
RETURN_ID setSpeedJoint(in ULONG spdRatio);
RETURN_ID moveCircularCartesianAbs(in CarPosWithElbow carPointR,
in CarPosWithElbow carPointT);
RETURN_ID moveCircularCartesianRel(in CarPosWithElbow carPointR,
in CarPosWithElbow carPointT);
RETURN_ID setHome(in JointPos jointPoint);
RETURN_ID getHome(out JointPos jointPoint);
RETURN_ID goHome();
};
};
#endif // MANIPULATORCOMMONINTERFACE_MIDDLE_IDL
99
setSpeedCartesian
(直行座標系における手先速度)

マニピュレータ共通インタフェース対応ロボット
• 埼玉大学琴坂研究室で開発され
ている
• http://design.mech.saitama-u.ac.jp/
research/rtm_industrial/rtm_industrial.html
• 三菱電機，DENSOウェーブ，
ヤマハ，VSTONEの教育用
ロボット
• 旧版共通インターフェース対応
• 前川製作所 OROCHI
• カワダロボティクス
Nextage Open
100
埼玉大学琴坂研究室

VSTONEアカデミックスカラロボット
• 垂直多関節型ロボットアー
ム
• 回転3軸＋手先上下1軸
＋ハンド開閉
• 共通インターフェースを
使えば手先位置指示可能
• カメラなどの環境認識セ
ンサを組み合わせて使う
101
https://www.vstone.co.jp/robotshop

VS_ASR_RTC
• https://github.com/sugarsweetrobotics/VS_ASR_RTC.git
• 琴坂研で開発したものをフォークして修正
• CP2110ソフトウェアが必要
• http://jp.silabs.com/products/interface/Pages/
CP2110EK.aspx
• 現状ではWinのみ対応
• 動作させるときは，CP2110ソフトウェア付属の
SILABHIDtoUART.dllおよびSILABHIDDevice.dllが必要
• C:SilabsMCUCP2110LibraryWindowsx86
102

共通インターフェーステスト用RTC
• これを作ってみる
• https://github.com/sugarsweetrobotics/ManipulatorCommonTest
• 先ほどのVS_ASR_RTCをダウンロードし，idlフォルダ内のidlファイルをコピーして，サービスポートを実装
する
• コンポーネント名： ManipulatorCommonTest （自由だけど）
• サービスポート：manipCommon
• インターフェース
• インターフェース名：JARA_ARM_ManipulatorCommonInterface_Common
• Required
• インスタンス名・変数名： manipCommon
• idl ： ManipulatorCommonInterface_Common.idl
• タイプ：JARA_ARM::ManipulatorCommonInterface_Common
• サービスポート：manipMiddle
• インターフェース
• インターフェース名：JARA_ARM_ManipulatorCommonInterface_Middle
• Required
• インスタンス名・変数名： manipMiddle
• idl ： ManipulatorCommonInterface_Middle.idl
• タイプ： JARA_ARM::ManipulatorCommonInterface_Middle
103

まとめ
• サービスポートについて紹介
• サービスポートの作成方法について紹介
– IDLについて紹介
– C++版のサービスポートの実装方法につ
いて紹介
104

OpenRTM-aist入門その６
105
菅佑樹

便利なツール群を紹介
• rtshell
• RTno
• V-REPシミュレータのRTC対応
106

rtshell
• コマンドラインからRTCを操作
• http://www.openrtm.org/openrtm/ja/node/
869
• pip install rtshellでインストール
• OpenRTM-aist Python版も必要
• RT System Editor相当の機能がある
• 保存するxmlファイルはRT System Editorと共通
107

rtshell練習
• ConsoleInCompとConsoleOutCompを起動
• rtcwd localhost
• rtls （これでlocalhost以下のツリーが見える）
• rtcwd %HOST_NAME%.host_cxt
• rtls （きっとこれでRTCが二つ見える）
• rtcon ConsoleIn0.rtc:out ConsoleOut0.rtc:in
• rtact ConsoleIn0.rtc
• rtact ConsoleOut0.rtc
• rtcryo localhost -o MySystem.xml (システムファイルを保存)
108

rtshell練習
• ConsoleInCompとConsoleOutCompを起動
• rtresurrect MySystem.xml (システムファイルを読み込んで接続・
コンフィグ)
• rtstart MySystem.xml (システムファイルの参加RTCをアクティ
ブ化)
• rtstop MySystem.xml (インアクティブ化)
• ConsoleInはscanfを待ってるので1などを入力しないと進まな
い
• rtteardown MySystem.xml
109

rtshellの便利機能その他
• rtlog : 出力データポートのデータをログ．
入力データポートにログを再生
• rtprint ：出力データポートのデータをコ
ンソールに表示
• rtinject ：入力ポートにデータを投げ込む
• rtconf ：RTCのコンフィグ変更
110

RTno (あーるてぃーの)
• Arduino + RT = RTno
• Arduinoを簡単にRTコンポーネントにするライブラリ
• 自作の回路やデバイスをRTコンポーネントにできる
• http://ysuga.net/?p=124
111

V-REPシミュレータ
• RTコンポーネントとの通信に対応した動
力学シミュレータ V-REP
• 使い方はこちら
• http://ogata-lab.jp/?p=1264
112

OpenHRI
• 音声コミュニケーション用のRTC群
• http://openhri.readthedocs.org/en/
latest/index-ja.html
• http://openrtp.sakura.ne.jp/openhri/
• Juliusを使った音声認識
• OpenJTalkを使った発話
113

その他
• RTM on Android
• Androidアプリから直接RTCと通信
• Choreonoid
• ロボットの振り付け
• 動力学シミュレータ機能も
114

OpenRTM-aist入門その５
115
菅佑樹

RTCをうまく使う
• システムを分割する指針
• RTCの粒度をどうするか
• 階層的アプローチを紹介
• RTCのインターフェース (データポート，サービ
スポート，コンフィグレーション) をどうするか
• 設計指針について持論を紹介
• 実際にどう使っているか
• 移動ロボットフレームワークを紹介
116

ロボットミドルウェアの
典型的ユースケース
• 複数のロボット製品を統合してタスクを行う
• 例：危険物除去ロボット
• A: サーボコントローラ
• B: センサー
• C: ロボットアーム統合
• D: ロボットハンド統合
• E: 運動学モジュール
• F: ロボット台車
• G: システム統合
• H: 学習アルゴリズムモジュール
117

RTCの粒度決定方法
• 階層的アプローチ
118
ロボット統合
コントローラロボット要素
アプリ
アルゴリズム

階層的アプローチ
• ロボット要素層（再利用性高い）
• ロボットのハードウェア単位 (モータ，センサ，マイク等)
• ロボット統合層（ロボット単位での再利用を見込める）
• 複数のロボット要素をまとめて一つの要素のようにする層
• 単位や座標系の変換等の統合
• コントローラ層（ロボットの再利用に付随して再利用が可能）
• 運動学や起動追従制御など，ロボットハードウェア構成に依存が大きい
アルゴリズム
• アルゴリズム層（再利用性が高い）
• 認識・分類，軌道計画等，ハードウェアへの依存が少ないアルゴリズム
• アプリケーション層（ドメインに依存．再利用性は低い）
• ドメインに依存したアプリケーション．GUIなど．
119

インターフェースをどうするか
• ロボット要素層
• センサやアクチュエータはデータポートが中心
• ハードウェアの設定値をコンフィグレーションに
• 初期化はonActivateで，周期処理をonExecute
• ロボット統合層（ロボット単位での再利用を見込める）
• 要素からのデータを統合して，組み立てたロボットに依存する座標変換などを行う
• データポート中心．TimedVelocityやTimedPoseなどの物理量を意識
• 変換をするのみであればコールバックを使うと良い
• コントローラ層（ロボットの再利用に付随して再利用が可能）
• サービスポートが中心．マニピュレータの共通インターフェースなどを参考に
• アルゴリズム層（再利用性が高い）
• サービスポートが中心．計算負荷が高く，要求とレスポンスの対応関係が明確
• アプリケーション層（ドメインに依存．再利用性は低い）
• アルゴリズムやコントローラを利用するためのサービスポート（要求側）が中心
120

RTM対応移動ロボット
ナビゲーションフレームワーク
• 台車型移動ロボットのナビゲーションが目的
– すべてオープンソース
• マップ作成
– SLAMのみ
• 地図を用いたナビゲーション
– レーザーレンジセンサーを用いた位置推定
– 軌道計画
– 軌道追従
• 機能ごとにモジュール化しており，インターフェースを共通化して，入替えが可能
– 例：独自の移動ロボットを適用
– 例：独自のプランニング・パスフォロワーを適用
• 基本操作が可能なGUIを準備
121

RTM対応移動ロボット
MRPT版
• 環境
– C++版
• OpenRTM-aist 1.1.0 C++ VC2010版
• MRPT 1.0.2 for Visual Studio 2010版
• Visual C++ 2010
• その他，OpenRTM-aist開発に必要なツール
– Java版
• OpenRTM-aist 1.1.0 RC1
122

フレームワークの特徴
• 上位のインターフェースはすべてサービスポート
• 処理の成功・失敗を返り値で受け取ることがで
きる
• サービスロボットの上位処理はデータフロー的
ではなく，イベント的
• MobileRobot.idlでインターフェースを定義済み
– これをインポートすればコンシューマ側は簡
単に作成できる
123

MobileRobot.idl
• 移動ロボットナビゲーションに関わるインターフェースやデータ型
を定義
• OpenRTM-aist同梱のExtendedDataTypes.idlや
InterfaceDataTypes.idlを再利用
• 今回のように独自IDLからincludeする場合は，自分のIDLフォ
ルダにこれらのIDLをコピーしておく必要がある
• 統一されたインターフェース設計思想
– 値渡しでデータを渡し，参照渡しでデータを受け取る．返り値
で処理の成否を受け取る
124

IDLファイル
125
#include "ExtendedDataTypes.idl"
#include "InterfaceDataTypes.idl"
module RTC {
/*!
* @struct OCMap
* @brief OccupancyGridMap Data
*/
struct OGMap
{
/// Time stamp.
Time tm;
/// OccupancyGridMap Configuration
OGMapConfig config;
/// OccupancyGridMap Data
OGMapTile map;
};
/*!
* @struct OGMapConfig
* @brief Configuration of a occupancy-grip map.
*/
struct OGMapConfig
{
/// Scale on the x axis (metres per cell).
double xScale;
/// Scale on the y axis (metres per cell).
double yScale;
/// Number of cells along the x axis.
unsigned long width;
/// Number of cells along the y axis.
unsigned long height;
/// Pose of the cell at (0, 0) in the real world.
Pose2D origin;
};
/*!
* @typedef OGMapCells
*/
typedef sequence<octet> OGMapCells;
/*!
* @struct OGMapTile
* @brief A tile from an occupancy-grid map.
*/
struct OGMapTile
{
/// X coordinate of the (0, 0) cell of this tile in the whole map.
unsigned long column;
/// Y coordinate of the (0, 0) cell of this tile in the whole map.
unsigned long row;
/// Number of cells along the x axis in this tile;
unsigned long width;
/// Number of cells along the y axis in this tile;
unsigned long height;
/// Tile cells in (row, column) order.
OGMapCells cells;
};
OGMapはマップデータ

OGMapConfig型や

OGMapTile型は

InterfaceDataTypes.idl

で定義されている
MobileRobot.idl
InterfaceDataTypes.idl

IDLファイル
126
enum RETURN_VALUE {
// COMMON
RETVAL_OK,
RETVAL_INVALID_PARAMETER,
RETVAL_EMPTY_MAP,
RETVAL_INVALID_PRECONDITION,
RETVAL_NOT_IMPL,
RETVAL_UNKNOWN_ERROR,
RETVAL_NOT_FOUND,
RETVAL_ODOMETRY_INVALID_VALUE,
RETVAL_ODOMETRY_TIME_OUT,
RETVAL_RANGE_INVALID_VALUE,
RETVAL_RANGE_TIME_OUT,
RETVAL_EMERGENCY_STOP,
RETVAL_OUTOF_RANGE
};
enum MAPPER_STATE {
MAPPER_STOPPED,
MAPPER_MAPPING,
MAPPER_SUSPEND,
MAPPER_ERROR,
MAPPER_UNKNOWN
};
enum FOLLOWER_STATE {
FOLLOWER_STOPPED,
FOLLOWER_FOLLOWING,
FOLLOWER_SUSPEND,
FOLLOWER_ERROR,
FOLLOWER_UNKNOWN
};
enumで返り値の基本値 (RETURN_VALUE) や，

MapperやFollowerの状態み関する変数が定義されている
MobileRobot.idl

IDLファイル
127
/*!
* @interface OGMapper
* @brief Occupancy Grid Map Builder Service Interface
*/
interface OGMapper {

/// Initialize Current Build Map Data
RETURN_VALUE initializeMap(in OGMapConfig config, in Pose2D initialPose);
RETURN_VALUE startMapping();
RETURN_VALUE stopMapping();
RETURN_VALUE suspendMapping();
RETURN_VALUE resumeMapping();
RETURN_VALUE getState(out MAPPER_STATE state);
/// Request Current Build Map Data
RETURN_VALUE requestCurrentBuiltMap(out OGMap map);
};
OGMapperはマップ作成用インターフェース

startMappingでマップ作成開始

stopMappingでマップ作成終了

requestCurrentBuiltMapで現在のマップ要求
/*!
* @interface OGMapServer
* @brief Occupancy Grid Map Service Interface
*/
interface OGMapServer {

};
OGMapServerはマップを単純に配信するための

インターフェース

requestCurrentBuiltMapでマップを取得

IDLファイル
128
struct PathPlanParameter {
//Environmental Map
OGMap map;
/// Location of the goal.
Pose2D targetPose;
/// Location of Robot.
Pose2D currentPose;
/// How far away from the waypoint is considered success (radius in metres).
double distanceTolerance;
/// How much off the target heading is considered success (in radians).
double headingTolerance;
/// Target time to arrive at the waypoint by.
Time timeLimit;
/// Maximum sped to travel at while heading to the waypoint.
Velocity2D maxSpeed;
};

interface PathPlanner {
/// Plan Path from PathPlanParater.
RETURN_VALUE planPath(in PathPlanParameter param, out Path2D outPath);
};
interface PathFollower {
RETURN_VALUE followPath(in Path2D path);

RETURN_VALUE getState(out FOLLOWER_STATE state);

RETURN_VALUE followPathNonBlock(in Path2D path);
};
PathPlannerは軌道計画のためのインターフェース

planPathで軌道計画をする
PathPlanParameterは，PathPlannerに

渡すためのパラメータ構造体
PathFollowerは軌道追従のためのインターフェース

IDLファイル
129
/*!
* @struct Waypoint2D
* @brief A waypoint in 2D space, including constraints.
*/
struct Waypoint2D
{
/// Location of the waypoint.
Pose2D target;
/// How far away from the waypoint is considered success (radius in metres).
/// How much off the target heading is considered success (in radians).
Time timeLimit;
};
/*!
* @typedef Waypoint2DList
*/
typedef sequence<Waypoint2D> Waypoint2DList;
/*!
* @struct Path2D
* @brief A time-stamped path in 2D space.
*/
struct Path2D
{
/// Time stamp.
Time tm;
/// The sequence of waypoints that make up the path.
Waypoint2DList waypoints;
};
Path2DはWaypoint2Dの配列

Waypointは，目標位置姿勢，

距離・角度許容差，

タイムリミットや最大速度が入る

IDLファイル全文 (MobileRobot.idl)
130
#include "ExtendedDataTypes.idl"
#include "InterfaceDataTypes.idl"
module RTC {
/*!
* @struct OCMap
* @brief OccupancyGridMap Data
*/
struct OGMap
{
/// Time stamp.
Time tm;
/// OccupancyGridMap Configuration
OGMapConfig config;
/// OccupancyGridMap Data
OGMapTile map;
};
enum RETURN_VALUE {
// COMMON
RETVAL_OK,
RETVAL_INVALID_PARAMETER,
RETVAL_EMPTY_MAP,
RETVAL_INVALID_PRECONDITION,
RETVAL_NOT_IMPL,
RETVAL_UNKNOWN_ERROR,
RETVAL_NOT_FOUND,
RETVAL_ODOMETRY_INVALID_VALUE,
RETVAL_ODOMETRY_TIME_OUT,
RETVAL_RANGE_INVALID_VALUE,
RETVAL_RANGE_TIME_OUT,
RETVAL_EMERGENCY_STOP,
RETVAL_OUTOF_RANGE
};
enum MAPPER_STATE {
MAPPER_STOPPED,
MAPPER_MAPPING,
MAPPER_SUSPEND,
MAPPER_ERROR,
MAPPER_UNKNOWN
};
enum FOLLOWER_STATE {
FOLLOWER_STOPPED,
FOLLOWER_FOLLOWING,
FOLLOWER_SUSPEND,
FOLLOWER_ERROR,
FOLLOWER_UNKNOWN
};
/*!
* @interface OGMapper
* @brief Occupancy Grid Map Builder Service Interface
*/
interface OGMapper {

/// Initialize Current Build Map Data
RETURN_VALUE initializeMap(in OGMapConfig config,
in Pose2D initialPose);
RETURN_VALUE startMapping();
RETURN_VALUE stopMapping();
RETURN_VALUE suspendMapping();
RETURN_VALUE resumeMapping();
RETURN_VALUE getState(out MAPPER_STATE state);
};
/*!
* @interface OGMapServer
* @brief Occupancy Grid Map Service Interface
*/
interface OGMapServer {

};

//Environmental Map
OGMap map;
Pose2D targetPose;
Pose2D currentPose;
/// How far away from the waypoint is
/// considered success (radius in metres).
/// How much off the target heading
/// is considered success (in radians).
Time timeLimit;
/// Maximum sped to travel
/// at while heading to the waypoint.
};

//Environmental Map
OGMap map;
Pose2D targetPose;
Pose2D currentPose;
/// How far away from the waypoint is considered
/// success (radius in metres).
/// How much off the target heading is considered
/// success (in radians).
Time timeLimit;
};

interface PathPlanner {
/// Plan Path from PathPlanParater.
RETURN_VALUE planPath(in PathPlanParameter param,
out Path2D outPath);
};
interface PathFollower {
RETURN_VALUE followPath(in Path2D path);

RETURN_VALUE getState(out FOLLOWER_STATE state);

RETURN_VALUE followPathNonBlock(in Path2D path);
};
};
#endif

マップの作成
• ロボットに北陽URG等のレーザースキャナを搭載
• ロボットの移動情報 (オドメトリ) とスキャン情報か
らマップを作成
• ロボットの移動はジョイスティックなどで指令を送る
131

マップ作成用フレームワーク
(基本的な使い方)
132

全RTC起動し図のように接続
Mapper_ALL.batで起動
133

コンフィグレーションの設定
• UrgRTCのCOMポートをコンフィグレーション
(port_name) で指定
• geometry_x, y, zで，センサの位置＝ロボットの座
標系 (車軸中心) からのオフセットを指定 (単位[m])
134

全RTC起動をアクティブ化
135

GUIの利用
• Start Mappingボタンでマッ
ピング開始
• NavigationManagerの
targetVelocityポートを接続
していれば，右図のGUIジョ
イスティックが起動
• ジョイスティック (黄色い円)
を上に引っ張ると，ロボット
が前進
• 別のジョイスティックRTCを
使うことも可能
136
GUIジョイスティックStartボタン

GUIの利用
• Save Mapでマップを保存
• マップデータは二つのファ
イルで保存される
– ****.png : グリッド
マップのデータ
– ****.yaml : マップの原
点やピクセル/長さの
比の情報
137
Saveボタン

(操作とRTシステムとの関係)
138

Start Mapping時
GUI
SLAM
マッピング開始命令
GUIからSLAMモジュールにマッピング開始を指⽰
139
*OGMapperインターフェース

Mapping中
スキャンデータ
SLAM
オドメトリ
レンジスキャナのデータとロボットのオドメトリからマップを作成
140
ロボット
レンジセンサ

Mapping中の表示
SLAM
推定自己位置
ロボットの⾃⼰位置とスキャンデータをデータポートから取得
サービスポートで現在のマップデータを取得．重ねて表⽰
141
現在のマップデータを

要求し取得
レンジセンサ

GUIジョイスティックでの操作
GUI
SLAM
目標速度指令
targetVelocityをGUIから指⽰
142
ロボット

Save Map時
SLAM
サービスポートで現在のマップデータを取得．保存．
143
現在のマップデータを

要求し取得
GUI

(各RTCの解説)
144

北陽電機 URG
• 主なコンフィグレーション
– スキャナのロボット座
標系での位置，オフセッ
ト [m]
• geometry_x
• geometry_y
• geometry_z
– COMポート番号 (COM3
とか)
• port_name
145
• レーザーセンサ URGのRTC
• 出⼒ポート
• range : RangeData
• レーザースキャナ出⼒

YPSpur対応RTC
– 最大加速度 (activate時に
更新) [m/s^2]
• max_acc
• max_rot_acc
– 最大速度 (activate時に更
新) [m/s]
• max_vel
• max_rot_vel
146
• YPSpur対応RTコンポーネント
• ⼊⼒ポート
• targetVelocity : TimedVelocity2D
• ⽬標速度
• poseUpdate : TimedPose2D
• 現在位置の更新
• 出⼒ポート
• currentVelocity : TimedVelocity2D
• 現在速度
• currentPose : TimedPose2D
• 推定⾃⼰位置 (オドメトリ）

Mapper_MRPT
• SLAMモジュール (MRPTを利用)
• 入力ポート
– range : RangeData
• LiDAR入力
– odometry : TimedPose2D
• オドメトリ入力
• 出力ポート
– estimatedPose : TimedPose2D
• スキャンマッチ後の推定自己位置
• サービスポート
– gridMapper : RTC::OGMapper
(Provided)
• マッピング開始・終了，マップ取
得などの操作
147
• x_max, x_min : X軸⽅向のマップ広さ初期値[m]
• y_max, y_min : Y軸⽅向のマップ広さ初期値
[m]
• resolution : グリッドの解像度 [m]

NavigationManager
• ナビゲーション用GUI
• 入力ポート
– currentPose : TimedPose2D
• ロボットの自己位置 (表示用)
• LiDAR入力 (表示用)
– camera : CameraImage
• カメラ画像表示
• 出力ポート
– targetVelocity : TimedVelocity2D
• GUIジョイスティックでの操作時に接続
– mapperService : RTC::OGMapper
(Required)
• SLAMモジュールを接続．マッピングの
開始・終了・マップ取得
148

ナビゲーション
• ロボットに北陽URG等のレーザースキャナを搭載
• ロボットの移動情報 (オドメトリ) とスキャン情報
をマップとマッチングして自己位置を高精度に推定
• マップ情報から軌道計画を自動化
• 計画した軌道への自動的な追従
149

ナビゲーション用フレームワーク
(基本的な使い方)
150

全RTCを起動し図のように接続
Navi_ALL.batで起動
151

コンフィグレーションの設定
• UrgRTCのCOMポートをコン
フィグレーション
(port_name) で指定
• geometry_x, y, zで，ロボッ
トの座標系 (車軸中心) からの
オフセットを指定 (単位[m])
• MapServerのマップファイル
のファイル名を設定
– ****.yamlを設定すること
152

全RTCをActivate
153

GUIの利用
• MapServerにマップが読み込まれていれ
ば，マップが表示され，ロボットの位置と
レンジセンサのデータが表示される
• マップ上の目標位置をクリックするとゴー
ルを指定できる
154

GUIの利用
• マップ上のゴールを指定すると，ゴー
ルの姿勢を選択するダイアログが表
示される
• 赤い円内をクリックすると，ゴール
姿勢 (マップ座標系での姿勢) を指
定できるので，OKする
• マップ上に指定した姿勢でのゴール
が出現する (円の中心が位置，直線
の伸びている方向がロボットのX方
向)
155

GUIの利用
• Plan Pathボタンでパスプランニング
• Followボタンでロボットが追従開始
156

(操作とRTシステムとの関係)
157

Activate時 (自己位置推定)
まず，MapServerにマップを要求して取得
オドメトリとスキャンデータからマップ内での⾃⼰位置を推定
158
ロボット
レンジセンサ
オドメトリマップデータを要求
*OGMapServerインターフェース

Activate時 (表示)
GUIはまず，MapServerにマップを要求して取得
推定⾃⼰位置とスキャンデータをマップに重ねて表⽰
159
⾃⼰位置推定
レンジセンサ
推定自己位置
マップデータを要求
GUI

ゴール設定時
GUI内で処理は完結．クリックした位置を
表⽰したマップ上の位置に変換して記録
160
⾃⼰位置推定マップデータを要求
GUI

Plan Path時
ロボットの位置をスタート，指定されたゴール，MapServerから
取得したマップ，パスフォローの許容誤差等のパラメータを⽤い
てPathPlannerにプランニングを要求し，Pathを取得・表⽰
161
⾃⼰位置推定RTC
レンジセンサ
推定自己位置
GUI
軌道計画
*PathPlannerインターフェース

Follow時
⽬標軌道を引数として，軌道追従要求をPathFollowerに要求．成功・失敗を返
り値として得る
ロボットの⾃⼰位置と軌道を照らし合わせながら，ロボットの⽬標速度を送信
162
⾃⼰位置推定RTC
推定自己位置
GUI
軌道追従要求
ロボットへの速度指令
軌道追従RTC
*PathFollowerインターフェース

(各RTCの説明)
163

北陽電機 URG
– スキャナのロボット座
標系での位置，オフセッ
ト
• geometry_x
• geometry_y
• geometry_z
– COMポート番号
• port_name
164
• レーザーセンサ URGのRTC
• 出⼒ポート
• range : RangeData
• レーザースキャナ出⼒

YPSpur対応RTC
– 最大加速度 (activate
時に更新)
• max_acc
• max_rot_acc
– 最大速度 (activate時
に更新)
• max_vel
• max_rot_vel
165
• YPSpur対応RTコンポーネント
• ⼊⼒ポート
• targetVelocity : TimedVelocity2D
• ⽬標速度
• poseUpdate : TimedPose2D
• 現在位置の更新
• 出⼒ポート
• currentVelocity : TimedVelocity2D
• 現在速度
• currentPose : TimedPose2D
• 推定⾃⼰位置 (オドメトリ）

NavigationManager
• ナビゲーション用GUI
• 入力ポート
• ロボットの自己位置 (表示用)
• LiDAR入力 (表示用)
– camera : CameraImage
• カメラ画像表示
• 出力ポート
– targetVelocity : TimedVelocity2D
• GUIジョイスティックでの操作時に接続
– mapServer : RTC::OGMapServer
• マップ配信モジュールを接続．マップ取得
– pathPlanner : RTC::PathPlanner
• 軌道計画モジュールを接続
– pathFollower : RTC::PathFollower
• 軌道追従モジュールを接続 166

MapServer
• マップデータを管理・出力
– mapServer :
MapServer
• マップデータの出力
167
• ﬁlename
• マップデータのファイル名．Yamlファイル
を指定すること．
• Yamlファイルとpngファイルは同じフォル
ダに配置すること
• 実⾏ディレクトリからの相対パスか，絶対
パスで指定．
• よくわからなければ
C:mapmy_may.yamlなどが無難

Localization_MRPT
• モンテカルロ法による自己位置推定
• 入力ポート
• LiDAR入力
– odometry : TimedPose2D
• オドメトリ
• 出力ポート
– estimatedPose : TimedPose2D
• 推定自己位置
– mapServer : MapServer
• マップデータの取得
168
• Dieter Fox，KLD-sampling: Adaptive particle
ﬁlters. In Advances in Neural Information
Processing Systems 14，2002

PathPlanner_MRPT
• パスプランニング
(MRPT使用)
– pathPlanner :
PathPlanner
• パスプランニング
169
• 円柱型ロボット(と仮定した場合)の半径
• robotRadius
• 軌道追従の位置・姿勢許容差
• pathDistanceTolerance
• pathHeadingTolerance
• 最終ゴール地点の位置・姿勢許容差
• goalDistanceTolerance
• pathHeadingTolerance

SimplePathFollower
• Path2D型の経路への追従
• 入力ポート
• 現在位置
• 出力ポート
– velocity : TimedVelocity2D
• 目標速度
– pathFollower : PathFollower
• パス追従命令取得
170
• distanceToTranslationGain
• Pathとの位置のズレと並進移動速度のゲイン
• distanceToRotationGain
• Pathとの位置のズレと回転移動速度のゲイン
• directionToTranslationGain
• Pathとの向きのズレと並進移動速度のゲイン
• directionToRotationGain
• Pathとの向きのズレと回転移動速度のゲイン
• approachDistanceGain
• 最後の終着点との距離と速度のゲイン
• approachDirectionGain
• 最後の終着点の⽅向と回転速度のゲイン

SimplePathFollowerアルゴリズム
• 最近傍のパスを選ぶ (右図では1-2間)
• 着目したパスとの有効距離をΔX，パ
スとロボットの向きの差分をΔHとす
る
• ゲインから速度を計算する
– Vx = K1 * ΔX + K2 * ΔH
– Va = K3 * ΔX + K4 * ΔH
• 近傍点のdistanceTolerannceよりもΔX
が大きい場合は，OUT_OF_RANGEエ
ラーを返してFollow終了
• ΔHが近傍点のheadingToleranceより
も大きい場合は，Vxをゼロにする
171
1
0
3
2
4
ΔX
ΔH
• コンフィグレーションとの対応
• distanceToTranslationGain = K1
• Pathとの位置のズレと並進移動速度のゲイン
• directionToTranslationGain = K2
• Pathとの向きのズレと並進移動速度のゲイン
• distanceToRotationGain = K3
• Pathとの位置のズレと回転移動速度のゲイン
• directionToRotationGain = K4
• Pathとの向きのズレと回転移動速度のゲイン

SimplePathFollowerアルゴリズム
• 最近傍パスがゴールを含む場合は
ゴールアプローチモードになる
• まずゴール点まで移動
– ゴールとロボットとの距離を
ΔX
– ゴール・ロボット間の直線と
ロボットの向きとの差分をΔH
• Vx = K5 * ΔX
• Va = K6 * ΔH
– ゴール点のdistanceTolerance
内で停止
• ゴール点の姿勢までその場で回転
し，headingToleranceに収まれば
ゴール到達
172
m
l n
ΔX
ΔH
• 主なコンフィグレーションとの対応
• approachDistanceGain = K5
• 最後の終着点との距離と速度のゲイン
• approachDirectionGain = K6
• 最後の終着点の⽅向と回転速度のゲイン

応用
• フレームワークのカスタマイズ例
• 独自RTCによるフレームワークの利用
173

ナビゲーション機能の
カスタマイズ
174

RTCごとの入れ替え
• インターフェースが共通であれば，入れ替
えは容易
175

ロボットRTCの入替え
独⾃ロボットへのナビゲーション機能の実装
176
TimedVelocity2D⼊⼒，TimedPose2D出⼒

軌道計画RTCの改良
軌道計画アルゴリズムを改良
RRT-CONNECTなどの実装
177
PathPlannerサービスポート (Provided) を実装

OpenRTM-aist入門

Recomendados

Recomendados

Mais conteúdo relacionado

Mais procurados

Mais procurados (20)

Destaque

Destaque (20)

Semelhante a OpenRTM-aist入門

Semelhante a OpenRTM-aist入門 (20)

Mais de Yuki Suga

Mais de Yuki Suga (7)

OpenRTM-aist入門