More Related Content Similar to OpenRTM-aist入門
Similar to OpenRTM-aist入門 (20) OpenRTM-aist入門1. SUGAR SWEET ROBOTICS CO., LTD.
OpenRTM-aist入門 その1
株式会社SUGAR SWEET ROBOTICS 代表取締役
早稲田大学基幹理工学部表現工学科 尾形哲也研究室 客員次席研究員
芝浦工業大学SIT研究所 客員研究員
菅 佑樹
1
2. SUGAR SWEET ROBOTICS CO., LTD.
RTMツールの使い方
• OpenRTM-aistを使うためのツールの使い
方を説明します.
• RTコンポーネントを起動してもシステ
ムの動作は始まらない
• ポートを接続する
• RTCの状態遷移を促す
• RTCのコンフィグレーションを調整
2
3. SUGAR SWEET ROBOTICS CO., LTD.
インストール (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
4. SUGAR SWEET ROBOTICS CO., LTD.
動作確認 (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
5. SUGAR SWEET ROBOTICS CO., LTD.
動作確認 (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
6. SUGAR SWEET ROBOTICS CO., LTD.
ネームサービスとは
• 実行中のRTCの管理
– OpenRTM-aistを使う場合,RTCを
使ったシステムでは最低1つ必要
– 異なるホストのRTCも登録可能
– 複数のネームサービスを併用可能
6
RTC1
RTC2
RTC3
ネーム
サーバー
ツール
PC2
PC1
7. SUGAR SWEET ROBOTICS CO., LTD.
ツールの役割
• ツールの種類
• RT System Editor
• Eclipseのプラグイン
• GUI
• rtshell
• コマンドからRTCを制御
• CUI.スクリプト化が可能=自動化
• 自作ツール
• ツール作成自体も容易
• ツールの役割
• RTC間の接続
• RTCのコンフィグレーションの変更
• RTCの状態の変更
• 実行コンテキストの制御
77
RTC RTC
RTCRTC
CREATED INACTIVE
ACTIVEERROR
DEACTIVATE
ACTIVATE
RESET
RTC
RTC
8. SUGAR SWEET ROBOTICS CO., LTD.
RT System Editorの使い方
• パースペクティブを「RT
System Editor」に変更
– パースペクティブとはEclipse
の作業画面のレイアウトタイ
プ
– メニュー>「ウィンドウ」>
「パースペクティブを開く」
>「その他」
– 「RT System Editor」を選択
8
9. SUGAR SWEET ROBOTICS CO., LTD.
RT System Editorの使い方
• 所望のネームサーバが見つからない場合は追加処理
– ネームサーバとは実行中のRTCの管理を行うサーバ
• localhost (自分自身)
• デフォルトでlocalhostを検索し,ネームサーバーが発見され
れば追加される
• Macでは不具合があるので,ネームサーバーを起動する前に
RTSEを起動した方がいい
• ネームサーバに起動したRTCが登録されていれば成功
• ConsoleInは
• 「/localhost/お使いのPC名.host_cxt/ConsoleIn0.rtc」
という名前でネームサーバーに登録されているはず
• 名前の登録ルールを変更することも簡単
• rtc.conf
9
10. SUGAR SWEET ROBOTICS CO., LTD.
RT System Editorの使い方
• System Editorを開いてRTシステムを編集す
る
- File > Open On-Line System Editorもしくはツールバー
上のボタン (右図)
- メインビューに空の「System Diagram」が表示される
• ネームサービスビューからドラッグ&ドロッ
プ
– 利用するRTCをすべて表示
• ポートとポートをドラッグ&ドロップで接続
– 表示されるダイアログはOK
10
11. SUGAR SWEET ROBOTICS CO., LTD.
RT System Editorの使い方
• すべてのRTCをACTIVATE
• ConsoleInのウィンドウに「Please
Input number」と表示
– 適当な数字を入れると
ConsoleOut側に表示
– RTC間の通信が行われたことが分か
る
• DEACTIVATEする場合は,指令を
送ってからConsoleInに数字を送る
必要がある
– (scanf入力待ちになっている)
ACTIVATE DEACTIVATE
11
12. SUGAR SWEET ROBOTICS CO., LTD.
まとめ
• ツールの使い方
• ネームサービスが必ず必要
• RT System Editor (RTCの接続,Activate/
Deactivate)
12
13. SUGAR SWEET ROBOTICS CO., LTD.
OpenRTM-aist入門 その2
株式会社SUGAR SWEET ROBOTICS 代表取締役
早稲田大学基幹理工学部表現工学科 尾形哲也研究室 客員次席研究員
芝浦工業大学SIT研究所 客員研究員
菅 佑樹
13
14. SUGAR SWEET ROBOTICS CO., LTD.
データポートを使う
• OpenRTM-aistではポートは2種類
• データポート・・・データフロー型
• サービスポート・・・遠隔手続き型
• データポートはちょうど,制御のブロック線
図のようにRTシステムを組むことができる
• ロボット台車の簡易シミュレータを使っ
てRTC開発に慣れよう
14
15. SUGAR SWEET ROBOTICS CO., LTD.
OpenRTM-aist学習用台車シミュレータ
• Loader.batを実行
– シミュレータ
– 仮想ジョイスティック
– コントローラ
– Java7以降が必要
• すべて接続してACTIVATE
• ジョイスティックで操作
• コントローラGUIで位置や接触センサを確認
シミュレータ 仮想ジョイスティック
コントローラGUI
http://ysuga.net/?p=133
15
16. SUGAR SWEET ROBOTICS CO., LTD.
台車シミュレータの中身
• RT System Editorで表示
仮想ジョイスティック
コントローラ
シミュレータ内のロボット
目標速度
現在位置
接触センサ
ジョイスティック入力
16
17. SUGAR SWEET ROBOTICS CO., LTD.
実習1.台車に指令を送ってみよう
• 下記のMobileRobotのRTCは移動ロボットシミュレータ内の台車を表している
目標速度 「vel」
TimedVelocity2D型
現在位置 「pos」
TimedPose2D型
接触センサ 「bumper」
TimedBooleanSeq型
17
• 台車に直進と回転の指令を同時に送り,その場でぐるぐる回る動作をさせ
てみよう
18. SUGAR SWEET ROBOTICS CO., LTD.
RTCプログラミングの流れ
• RTC Builderによるスケルトンコードの生成
• スケルトンコードの特定のイベントハンドラに独自のコードを追加
• on_initialized・・・CREATED->INACTIVE
• on_activated・・・INACTIVE->ACTIVE
• on_deactivated・・・ACTIVE->INACTIVE
• on_execute・・・ACTIVE状態で周期的に呼ばれる
• コンパイルし実行
18
19. SUGAR SWEET ROBOTICS CO., LTD.
RTC Builder
• パースペクティブをRTC Builderに変更
– Builder Editorで,RTCの骨格(スケルト
ン)コードを作成できる
• Builder Editorを開く
– RTCが持つべき「イベントハンドラ」が実
装されているので,それを編集して自分の
コードを差し込む
19
20. SUGAR SWEET ROBOTICS CO., LTD.
RTC Builder
• プロジェクト名の入力
– MyController
• 画面下部のタブを切り替え
ながら必要な情報を入力
20
23. SUGAR SWEET ROBOTICS CO., LTD.
RTC Builder
• データポートタブ
• データ型が見つから無い場合はRTM_ROOT環境変数が設定されて無い
• システムの詳細設定や,~/.bashrcなどを確認.必要によっては再起動.
• TimedVelocity2DはTimedVector2Dと間違いやすい!!
出力ポートを追加ポート名を編集
ポートの型を変更
ポートのソースコード上の変数名を入力 23
25. SUGAR SWEET ROBOTICS CO., LTD.
RTC Builder
• MyControllerプロジェクトにソースコードが生
成される
– Builder Editor(真ん中のウィンドウ)は閉じてよい
25
26. SUGAR SWEET ROBOTICS CO., LTD.
RTCのソースコードを見つける
• Eclipseは通常,自分のホームディレクトリ直下
のworkspaceフォルダにワークスペースを作る
• Winなら C:Usersユーザ名workspace
• Linuxなどなら/home/ユーザ名/workspace
• ワークスペース内に,作成したRTCのソースコー
ドを見つけることができる
26
27. SUGAR SWEET ROBOTICS CO., LTD.
CMakeによるプロジェクト生成
• CMakeを起動
• CMakeLists.txtを
CMakeにドラッグ
&ドロップ
27
28. SUGAR SWEET ROBOTICS CO., LTD.
ROBOTECH2012 RTミドルウエア講習会
CMake
• 出力フォルダパスにbuildを追加
• RTCのフォルダが”…/workspace/MyController”の場合
• ”…/workspace/MyController/build”というフォルダを作る
• Configureを押すとフォルダ作成確認のダイアログが出るのでOK
2012/7/11
28
29. SUGAR SWEET ROBOTICS CO., LTD.
CMake2.8
• ビルド環境を選択
– Visual Studio 12 2013を選択
– Use default native compilersを選
択
• 赤い表示が出るが恐れず
「Generate」を押す
– Visual Studio用プロジェクトファ
イルが生成される
29
30. SUGAR SWEET ROBOTICS CO., LTD.
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>
31. SUGAR SWEET ROBOTICS CO., LTD.
Visual Studio
• build/src/MyController.slnファイルを
ダブルクリック
– Visual Studioが起動する
– 複数のプロジェクトが存在
– MyController・・RTC本体
– MyControllerComp・・・RTCを単体の
アプリケーションとして実行するための
プロジェクト
• MyControllerプロジェクト
– MyController.cppを開く
31
32. SUGAR SWEET ROBOTICS CO., LTD.
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;
}
33. SUGAR SWEET ROBOTICS CO., LTD.
2次元速度のデータ型
• TimedVelocity2D・・・タイムスタンプ付き2次元平面での
速度および角速度
• tm : Time型・・・タイムスタンプ
• sec : unsigned long型・・・秒
• nsec : unsigned long型・・・ナノ秒
• data : Velocity2D型・・・2次元平面での速度および各
速度
• vx : double型・・・X軸方向速度 (単位m/s)
• vy : double型・・・Y軸方向速度 (単位m/s)
• va : double型・・・各速度 (単位rad/s)
進行方向
X
θ
Y
33
34. SUGAR SWEET ROBOTICS CO., LTD.
移動ロボットに指令を送る
• 従って,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
35. SUGAR SWEET ROBOTICS CO., LTD.
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;
};
36. SUGAR SWEET ROBOTICS CO., LTD.
実行
• ネームサービスの起動確認
• RTCの実行
– MyControllerCompを右クリック
– 「デバッグ→インスタンス作成」で実行
• MobileRobotSim.batを実行するとシミュレー
タのみ起動
• RT System Editorでの接続,ACTIVE化
36
37. SUGAR SWEET ROBOTICS CO., LTD.
実習2.台車の動きを調整
• コンフィグレーション機能を試す
– コンフィグレーションとは,実行中
のRTCを調整するための機能
• 例:制御ゲインの調整
• コンフィグレーション機能を加え
て実行中にMyControllerの機能を
調整する
進行方向
X
θ
Y
37
38. SUGAR SWEET ROBOTICS CO., LTD.
RTC Builder
• Eclipseのパースペクティブを
RTC Builderに再変更
• パッケージエクスプローラの
RTC.xmlファイルをダブルク
リックする
– 先ほど作成した情報が記録され
ている
38
40. SUGAR SWEET ROBOTICS CO., LTD.
追加したコンフィグレーション
• velocity_x : double型
– 変数名: velocity_x
– デフォルト値: 0.05
• velocity_theta : double型
– 変数名: velocity_theta
– デフォルト値: 1.0
40
41. SUGAR SWEET ROBOTICS CO., LTD.
RTC Builder
• 比較ダイアログ
– ここでは「Generated」を選択
– 基本的に「Merge」を選択
• 新しいコードの変更点のみ反映
– Generatedは新しいコード側で上書きされるので,自分の記入したコードが消える
• バックアップファイルがある (ファイル名末尾に日付が入る) ので心配要りま
せん
41
42. SUGAR SWEET ROBOTICS CO., LTD.
実習2.台車の動きを調整
• コンフィグレーション機能を試す
– コンフィグレーション機能を加えて実行中に
MyControllerの機能を調整する
RTC::ReturnCode_t MyController::onExecute(RTC::UniqueId ec_id)
{
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.vy = 0;
m_velocity.data.va = m_velocity_theta;
m_velocityOut.write();
return RTC::RTC_OK;
}
進行方向
X
θ
Y
42
43. SUGAR SWEET ROBOTICS CO., LTD.
実行
• ネームサービスの起動確認
• RTCの実行
– MobileRobotSim.batを実行するとシミュレータのみ起動
• RT System Editorでの接続,ACTIVE化
• RT System Editor下部にConfiguration Viewに選択中のRTCの
コンフィグが表示されるので変更して「適用」を選択する(適
用しないと反映されない)
43
44. SUGAR SWEET ROBOTICS CO., LTD.
実習3.位置の取得
• 現在位置をデータポートから受け取ってコンソールに表示する
目標速度 「vel」
TimedVelocity2D型
現在位置 「pos」
TimedPose2D型
接触センサ 「bumper」
TimedBooleanSeq型
• TimedPose2D型の入力ポートをMyControllerに追加する
45. SUGAR SWEET ROBOTICS CO., LTD.
RTC Builder
• 再度,RTCBuilderを開く
• MyController / RTC.xmlを開く (たぶん,開きっぱなし?)
• データポートタブに移動 → TimedPose2D型の入力ポート「pose」を追加
45
46. SUGAR SWEET ROBOTICS CO., LTD.
2次元位置のデータ型
• TimedPose2D・・・2次元平面での位置および姿勢
• tm : Time型・・・タイムスタンプ
• sec : unsigned long型・・・秒
• nsec : unsigned long型・・・ナノ秒
• data : Pose2D型・・・2次元平面での位置および姿勢
• position : Point2D型・・・2次元平面内での位置
– 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
47. SUGAR SWEET ROBOTICS CO., LTD.
Visual Studio
• 再度,onExecute関数を編集
– 入力ポートはデータが来ているか確認する処理が入る
RTC::ReturnCode_t MyController::onExecute(RTC::UniqueId ec_id)
{
m_velocity.data.vx = m_velocity_x;
m_velocity.data.vy = 0;
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
48. SUGAR SWEET ROBOTICS CO., LTD.
実行
• ネームサービスの起動確認
• RTCの実行
– MobileRobotSim.batを実行するとシミュレータのみ起
動
• RT System Editorでの接続,ACTIVE化
• 実行時の周期が速すぎる?
48
49. SUGAR SWEET ROBOTICS CO., LTD.
rtc.conf
• RTCの実行時の設定ファイル
– ネームサーバのIPアドレス,ポート番号
– 実行周期,実行コンテキストの種類
– RTCの名前付け規則
– ログの有無,ログレベル
– etc…
• rtc.confで実行周期を変更(単位Hz)
– 右クリック→アプリケーション→テキストエディタ
– rtc.confを実行ファイルと同じ場所に置いて,実行ファイルを実行
– Visual C++でデバッグする場合は,プロジェクトファイルと同じディレクトリに置く
(デフォルトでそこがカレントディレクトリになる)
exec_cxt.periodic.rate: 1.0
49
50. SUGAR SWEET ROBOTICS CO., LTD.
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のデータを読み取る
51. SUGAR SWEET ROBOTICS CO., LTD.
実習4.台車の接触スイッチ
• TimedBooleanSeq 真偽型 (True/False)の
配列
目標速度 「vel」
TimedVelocity2D型
現在位置 「pos」
TimedPose2D型
接触センサ 「bumper」
TimedBooleanSeq型
配列の0番目要素=右側スイッチ
配列の1番目要素=左側スイッチ
51
52. SUGAR SWEET ROBOTICS CO., LTD.
RTC Builderによるポートの追加
• InPort : TimedBooleanSeq型
– ポート名: bumper
– 変数名: bumper
52
53. SUGAR SWEET ROBOTICS CO., LTD.
Visual Studio
• **Seq型はdataメンバを配列のように使える
RTC::ReturnCode_t MyController::onExecute(RTC::UniqueId ec_id)
{
・・・省略・・・
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
54. SUGAR SWEET ROBOTICS CO., LTD.
みなさんへの課題
• 接触スイッチの反応でロボットの動作を変える
– 左右どちらかに旋回する?
• 位置の値を使って図形を描く
– 正方形,星型などなど
• 仮想ジョイスティックを使った操作
• お使いのロボット関連製品をRTC化する
– onExecute以外の使えるイベントハンドラ例(RTC Builderで使えるように設定)
• onActivated・・・Activate時に一回呼ばれる(初期化用)
• onDeactivated・・・Deactivate時もしくはエラー状態遷移時に呼ばれる(終了処理)
スティック入力「out」
TimedDoubleSeq型
各ボタンの出力値はGUIに記載されている
例: Up(0,1) → 0番目要素が0,1番目要素が1
最初はコンソールにどんなデータが来るか表示して試してみよう
54
55. SUGAR SWEET ROBOTICS CO., LTD.
まとめ
• ツールの使い方
– RT System Editor (RTCの接続,Activate/Deactivate)
– RTC Builder (RTCのスケルトンコード生成)
– CMake (Visual C++用プロジェクト生成)
• コーディング方法
– データポート入出力
• TimedVelocity2D, TimedPose2D, TimedBooleanSeq
– コンフィグレーション
– rtc.confの設定
55
56. SUGAR SWEET ROBOTICS CO., LTD.
さらなる発展
• RTCを探してみよう
• http://openrtm.org >プロジェクト > RTコンポーネント
• 独自のデータ型を使ってみよう
• データポートに独自のデータ型を使うことができる
• サービスポートを使ってみよう
• RTCに関数型のインターフェースを追加でき,より柔軟なシステム設計ができ
る
• rtshellを使ってみよう
• コマンドラインから使えるツール.スクリプトを書くことで開発を効率化で
きる
• RTCを接続・コンフィグ・アクティブ化するプログラムを作ってみよう
• システム構築を自動化できる
• 参考ページ: http://ysuga.net
56
57. SUGAR SWEET ROBOTICS CO., LTD.
OpenRTM-aist入門 その3
株式会社SUGAR SWEET ROBOTICS 代表取締役
早稲田大学基幹理工学部表現工学科 尾形哲也研究室 客員次席研究員
芝浦工業大学SIT研究所 客員研究員
菅 佑樹
57
58. SUGAR SWEET ROBOTICS CO., LTD.
独自のデータ型を使ってみよう
• 自分独自のデータ型で通信
• データ型がないときに行う
• データを分割して送ると,ひとまとまりのデータなのに別のポー
トになってしまう
• わかりやすいメンバ名を付けられる (ただの配列ではできない)
• 接続できるデータポートを制限できる (DoubleSeqだとどんな
データでも送れるけど,どんなデータも繋がってしまう)
• 設定ファイルを配布しないといけないのが若干手間
• すでにあるデータ型は再定義しないようにしよう (よく調べる
か相談しよう)
58
59. SUGAR SWEET ROBOTICS CO., LTD.
独自のデータ型を使ってみよう
• Kinectと連携してみよう
• Kinect SDK for Windowsを使ったRTコンポーネ
ント
• KinectだけWindowsで動かして他のRTCは別のシ
ステムで作っても良い
• モーションキャプチャのためのデータ型が見つから
なかったので作った
• 一部のKinect用RTCはTimedDoubleSeqを使ってい
るけど上述の問題で使いにくい
59
60. SUGAR SWEET ROBOTICS CO., LTD.
すでにあるデータ型
• 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
61. SUGAR SWEET ROBOTICS CO., LTD.
Kinectのデータ
• 要するに構造体
• Vector4
• 3次元位置
• NuiSkeletonData
• trackingState
• トラッキングの状態
• skeletonPosition[20]
• 各関節の位置
• 関節のインデックスをenumにしておく
• OpenRTM-aistではこの構造体定義をIDL
(Interface Definition Language) で書く
61
62. SUGAR SWEET ROBOTICS CO., LTD.
Interface Definition Language
• サービスポートの関数やデータ型を宣言するためのファイル形式
(.idlファイル)
– CORBA, RMIやDDSなどの分散処理実現のために使われる (標
準化されている形式)
– Javaに似た形式
– オブジェクト指向
• IDLファイルをコンパイルすると,C++やJava, Pythonのプログラ
ムのスケルトンが生成される
– IDLは中間言語
– C++やJavaなどの実装言語に変換すると,各言語ごとに若干利
用方法が変わるが,関数名などは変わらない
– 今回はC++版を少しだけ解説 62
63. SUGAR SWEET ROBOTICS CO., LTD.
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;
};
};
64. SUGAR SWEET ROBOTICS CO., LTD.
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を使うと可変長の配列を使う
};
};
65. SUGAR SWEET ROBOTICS CO., LTD.
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
66. SUGAR SWEET ROBOTICS CO., LTD.
skeletonのデータを使う
• 各関節の位置を得られる
• 3次元位置にはPoint3Dなどのデータ型があるが,複数
の人間のトラッキング情報などを統一的に扱うにはデー
タ型が無い
• データ型があるか否かを確認するのが重要
• TimedDoubleSeq (double型の配列) などを安易に
使うと,異なるデータ同士でつなぐことができるよ
うになり,他の人に迷惑
• 独自データ型を使う!!
66
67. SUGAR SWEET ROBOTICS CO., LTD.
IDLでstructを定義する
module KINECT {
struct スケルトンデータ {
トラッキングした人間の関節位置[20] : Vector4
トラッキングの品質や状態
};
struct スケルトンフレーム {
タイムスタンプやフレーム番号 : Time
スケルトンデータ [ 6 ];
};
};
67
• 単純にKinectSDKでサポートしているstructをそのままIDLにしまし
た
• 上述のgithubサーバーからファイルをダウンロードしてIDLの中身
を見てみよう
• 大まかな構造は下記のようになっている
68. SUGAR SWEET ROBOTICS CO., LTD.
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;
};
};
69. SUGAR SWEET ROBOTICS CO., LTD.
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で使う
トラッキング出来ているか否か
トラックした人間一人分のデー
タ.関節部の位置
一回のスキャンで取れるデータ
のまとまり.最大6人
70. SUGAR SWEET ROBOTICS CO., LTD.
IDLをEclipseに読ませる
• OpenRTP起動
• Window > Preference
• 開いたダイアログからRTCBuilder
を選択
• Newボタンをクリックして,フォ
ルダを指定
• 好きなフォルダを指定できるの
で,例えば「マイドキュメント
/idl」というフォルダを作り,
毎回そこにidlを入れれば良い
• Eclipseを再起動する
70
72. SUGAR SWEET ROBOTICS CO., LTD.
ビルド方法
• CMake
– RTCBuilderが古いと失敗する
• Visual Studio
– この中でomniidlというコマンドでIDLをコンパイルするが,Python版
がインストールされていると問題が起こることがある
• PATHの設定によっては,Python版のomniidlが優先して見つかるた
め,C++にコンパイルできない
– → PATHの最初にC++版のomniidl.exeのPATHを設定しなおす
– IDLはビルドできてる (** Not Foundのようなエラーは無い) のにコンパ
イルできない
– → openrtmのメーリングリストに通報
– → IDLコンパイルでidlファイル名.hhというファイルが生成され
る.その中のインターフェースをヘルパークラスがオーバーライド
しているので,返り値の型などを手動で直すとコンパイルできるこ
とがある
72
73. SUGAR SWEET ROBOTICS CO., LTD.
KinectTest RTC
• KinectTestというRTCを作ってみよう
• Module名がKinectTest
• NuiSkeletonFrame型の入力ポートを持つ
• 頭よりも手の高さが上になったら手を挙げ
ていると判断して表示
73
74. SUGAR SWEET ROBOTICS CO., LTD.
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
75. SUGAR SWEET ROBOTICS CO., LTD.
発展編
• Kinectから受け取ったモーションキャプチャ情報を解析し
て,移動ロボットを操作してみよう
• 右手を右に開いたら右
• 左手を左に開いたら左
• 両手を前に向けたら前進
• RTCはどのようなインターフェースをもっていなければなら
ないか
• 入力: Kinectのフレームデータ
• 出力: 移動ロボットへの速度指令
75
76. SUGAR SWEET ROBOTICS CO., LTD.
まとめ
• 独自のデータ型を定義する方
法について
• まずはOpenRTM-aistで
定義されているデータ型を
検討すること
• TimedDoubleSeqなどは
使わない事!!
• データ型については,UMLの
クラス図を使って情報を交換
するといい
• 他の人と相談するときに使
おう!
76
77. SUGAR SWEET ROBOTICS CO., LTD.
OpenRTM-aist入門 その4
株式会社SUGAR SWEET ROBOTICS 代表取締役
早稲田大学基幹理工学部表現工学科 尾形哲也研究室 客員次席研究員
芝浦工業大学SIT研究所 客員研究員
菅 佑樹
77
79. SUGAR SWEET ROBOTICS CO., LTD.
データポートだけでは不便
• データが欲しいときだけ処理をして欲しい
• データポートはいつもpush型
• openやclose, resetのようなデータをともなわない通信をし
たい
• Long型で1を送ると・・・みたいなインターフェースはダ
メ.別のLong型とつながっちゃうから
• 複数のデータを同期して受け取りたい
• データポートでもタイムスタンプがあるし不可能ではな
いが・・・
79
80. SUGAR SWEET ROBOTICS CO., LTD.
サービスポートの概念
• OpenRTM-aistの機能
• RTCに関数呼び出しのインターフェースを付加
– 引数でデータを渡す
– 返り値でデータを受け取る (引数によるデータ受け取りも可能.これなら複数のデー
タを同期して受け取ることができる.)
• データポートと比べて
– データを必要なときだけ要求することができる
– データの送受信の結果が得られる(成否など)
– データの送受信に関わらない処理を実現できる
• reset, initializeなど
– 若干実装が面倒.やっぱりIDLが要る
– 独自のインターフェースが乱立すると再利用性が下がる
– 共通インターフェース(後述)
80
81. SUGAR SWEET ROBOTICS CO., LTD.
サービスポートの概念
• サービスポートは「インターフェース」を持つ
• インターフェースには極性 (Polarity) がある
• Provider 提供側 (polarity=Provided)
• リソースを提供する.関数が呼ばれ
る側
– Consumer 利用側 (polarity=Required)
• リソースを利用する.関数を呼ぶ側
• 一つのサービスポートが複数の提供・要求イン
ターフェースを持つことができる
• インターフェースのタイプが合えば接続で
き,利用できる.
• ポートをつなげるには全てのインターフェー
スが実装されている必要がある
• 1ポート,1インターフェースの方が使い
易い
81
RTC1 RTC2
ProvidedRequired
Service Port
82. SUGAR SWEET ROBOTICS CO., LTD.
サービスポート作成の流れ
• 1. サービスポートを追加
• サービスポート名を設定
• 2. サービスポートにインターフェースを追加
• 読み込むIDLファイルを選択
• インターフェース型を選択
• インターフェース名*
• インスタンス名
• 極性 (Polarity)
• 注意!
– インターフェース名が違うと繋げるのが面倒になる!
• インターフェース型を定義するファイルを作成 (IDLファイル)
82
RTC
Interface Name : Interface Type
Port Name
83. SUGAR SWEET ROBOTICS CO., LTD.
Interface Definition Language
• サービスポートの関数やデータ型を宣言するためのファイル
形式 (.idlファイル)
– CORBAやDDSなどの分散処理実現のために使われる
– Javaに似た形式
– オブジェクト指向
• IDLファイルをコンパイルすると,C++やJava, Pythonのプ
ログラムのスケルトンが生成される
– IDLは中間言語
– C++やJavaなどの実装言語に変換すると,各言語ごとに
若干利用方法が変わるが,関数名などは変わらない
– 今回はC++版を少しだけ解説 83
84. SUGAR SWEET ROBOTICS CO., LTD.
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);
};
};
85. SUGAR SWEET ROBOTICS CO., LTD.
IDLの書き方
• structで構造体を定義できる
– 異なるタイプのデータの
集まり
• 引数にも,返り値にも使える
• structでデータ型を定義でき
れば,データポートの独自デー
タ型作成ができる
– 参考: http://ysuga.net/?p=213
85
module my_module {
// 構造体を宣言することでプログラムを構造化できる
struct MyData {
long data1;
long data2;
double data3;
};
interface MyInterface {
// 返り値でデータを受け取るタイプの関数
MyData getMyData();
// 引数でデータを与えるタイプの関数
void setMyData(in MyData data);
// 引数に参照渡しでデータを受け取ることもできる
void getMyDataByArg(out MyData data);
};
};
86. SUGAR SWEET ROBOTICS CO., LTD.
IDLの書き方
• typedefでデータ型
の名称変更
• enumで定数を作る
• sequenceで可変長の
データを作成できる
86
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を使うと可変長の配列を使う
};
interface MyInterface {
// 引数でデータを与えるタイプの関数
// 返り値で処理の成否を返すことができる
RETURN_CODE setMyData(in MyData data);
// 引数に参照渡しでデータを受け取ることもできる
// 返り値で処理の成否を返すことができる
RETURN_CODE getMyDataByArg(out MyData data);
};
};
87. SUGAR SWEET ROBOTICS CO., LTD.
RTC Builder
• 「サービスポート」タブ選択
• 「Add Port」でポート追加
• 「ポート名」を設定
• 「Add Interface」でインターフェース追
加
• 「インターフェース名」「方向」「インス
タンス名」「変数名」を設定
• 「IDLファイル」の「Browse」ボタンで
IDLを読み込む
• 「インターフェース型」を選択
• 「IDLパス」は「IDLファイルがあった
フォルダを選択」
87
88. SUGAR SWEET ROBOTICS CO., LTD.
ビルド方法
• 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. SUGAR SWEET ROBOTICS CO., LTD.
コーディング方法例 (提供側)
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;
myData->data[1] = 0.3;
myData->data[2] = 0.5;
myData->data[3] = 0.7;
data = myData._retn();
return result;
}
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を使うと可変長の配列を使う
};
interface MyInterface {
// 引数でデータを与えるタイプの関数
// 返り値で処理の成否を返すことができる
RETURN_CODE setMyData(in MyData data);
// 引数に参照渡しでデータを受け取ることもできる
// 返り値で処理の成否を返すことができる
RETURN_CODE getMyDataByArg(out MyData data);
};
};
シンプルな関数呼び出しになる
out型引数は_var型を使う
引数がないときは簡単.参照渡しが難しいので解説.
90. SUGAR SWEET ROBOTICS CO., LTD.
コーディング方法例 (要求側)
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 {
// 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を使うと可変長の配列を使う
};
interface MyInterface {
// 引数でデータを与えるタイプの関数
// 返り値で処理の成否を返すことができる
RETURN_CODE setMyData(in MyData data);
// 引数に参照渡しでデータを受け取ることもできる
// 返り値で処理の成否を返すことができる
RETURN_CODE getMyDataByArg(out MyData data);
};
};
シンプルな関数呼び出しになる
out型引数は_var型を使う
引数がないときは簡単.参照渡しが難しいので解説.
91. SUGAR SWEET ROBOTICS CO., LTD.
サービスポートの接続
• インターフェース名が同じであれ
ば,問題なく接続できる.
• インターフェース名が違う場合は,
RT System Editorで「接続でき
るインターフェースがない」と言
われるが,詳細ダイアログで接続
するインターフェースを追加・選
択すると接続できるようになる
• インターフェース名を再利用
したいRTCと同じにするのが
大事
91
93. SUGAR SWEET ROBOTICS CO., LTD.
共通インターフェース
• RTCを定義しただけではロボット開発は便利にならない
• インターフェースの共通化
• 同じタイプのロボットならば入れ替えが効くようにす
る
• できるだけ多くの機能をカバー.「実装してない」を許
す
• ロボットアーム,移動ロボット,音声コミュニケーショ
ン,カメラ
• 関連するツールや知能モジュールが再利用しやすくなる!
93
94. SUGAR SWEET ROBOTICS CO., LTD.
マニピュレーター
共通インターフェース
• 2013年に改定 (埼玉大学 琴坂研究室による)
• 2つのレベルに分割
• 共通レベル (ManipulatorCommonInterface_Common)
• 各関節レベルでの動作,位置獲得
• エラー情報取得・復帰
• 中位レベル (ManipulatorCommonInterface_Middle)
• 各関節レベルでの動作
• 手先位置での動作
• 手先移動範囲の指定や,ツールおよび台座のオフセット
• ハンドの開閉
• 共通するデータ型をManipulatorCommonInterface_DataTypesに規定
94
95. SUGAR SWEET ROBOTICS CO., LTD.
/*
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でデータを返す
96. SUGAR SWEET ROBOTICS CO., LTD.
/*
Manipulator Common Interface (Common Commands)
- 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_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
マニピュレーター情報構造体
97. SUGAR SWEET ROBOTICS CO., LTD.
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
関節のリミット設定
98. SUGAR SWEET ROBOTICS CO., LTD.
/*
Manipulator Common Interface (Middle Level Commands)
- 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_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
(手先の相対位置座標指定移動)
99. SUGAR SWEET ROBOTICS CO., LTD.
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
(直行座標系における手先速度)
100. SUGAR SWEET ROBOTICS CO., LTD.
マニピュレータ共通インタフェース対応ロボット
• 埼玉大学琴坂研究室で開発され
ている
• http://design.mech.saitama-u.ac.jp/
research/rtm_industrial/rtm_industrial.html
• 三菱電機,DENSOウェーブ,
ヤマハ,VSTONEの教育用
ロボット
• 旧版共通インターフェース対応
• 前川製作所 OROCHI
• カワダロボティクス
Nextage Open
100
埼玉大学琴坂研究室
101. SUGAR SWEET ROBOTICS CO., LTD.
VSTONEアカデミックスカラロボット
• 垂直多関節型ロボットアー
ム
• 回転3軸+手先上下1軸
+ハンド開閉
• 共通インターフェースを
使えば手先位置指示可能
• カメラなどの環境認識セ
ンサを組み合わせて使う
101
https://www.vstone.co.jp/robotshop
102. SUGAR SWEET ROBOTICS CO., LTD.
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
103. SUGAR SWEET ROBOTICS CO., LTD.
共通インターフェーステスト用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
104. SUGAR SWEET ROBOTICS CO., LTD.
まとめ
• サービスポートについて紹介
• サービスポートの作成方法について紹介
– IDLについて紹介
– C++版のサービスポートの実装方法につ
いて紹介
104
105. SUGAR SWEET ROBOTICS CO., LTD.
OpenRTM-aist入門その6
105
株式会社SUGAR SWEET ROBOTICS 代表取締役
早稲田大学基幹理工学部表現工学科 尾形哲也研究室 客員次席研究員
芝浦工業大学SIT研究所 客員研究員
菅 佑樹
107. SUGAR SWEET ROBOTICS CO., LTD.
rtshell
• コマンドラインからRTCを操作
• http://www.openrtm.org/openrtm/ja/node/
869
• pip install rtshellでインストール
• OpenRTM-aist Python版も必要
• RT System Editor相当の機能がある
• 保存するxmlファイルはRT System Editorと共通
107
108. SUGAR SWEET ROBOTICS CO., LTD.
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
109. SUGAR SWEET ROBOTICS CO., LTD.
rtshell練習
• ConsoleInCompとConsoleOutCompを起動
• rtresurrect MySystem.xml (システムファイルを読み込んで接続・
コンフィグ)
• rtstart MySystem.xml (システムファイルの参加RTCをアクティ
ブ化)
• rtstop MySystem.xml (インアクティブ化)
• ConsoleInはscanfを待ってるので1などを入力しないと進まな
い
• rtteardown MySystem.xml
109
110. SUGAR SWEET ROBOTICS CO., LTD.
rtshellの便利機能その他
• rtlog : 出力データポートのデータをログ.
入力データポートにログを再生
• rtprint :出力データポートのデータをコ
ンソールに表示
• rtinject :入力ポートにデータを投げ込む
• rtconf :RTCのコンフィグ変更
110
111. SUGAR SWEET ROBOTICS CO., LTD.
RTno (あーるてぃーの)
• Arduino + RT = RTno
• Arduinoを簡単にRTコンポーネントにするライブラリ
• 自作の回路やデバイスをRTコンポーネントにできる
• http://ysuga.net/?p=124
111
112. SUGAR SWEET ROBOTICS CO., LTD.
V-REPシミュレータ
• RTコンポーネントとの通信に対応した動
力学シミュレータ V-REP
• 使い方はこちら
• http://ogata-lab.jp/?p=1264
112
113. SUGAR SWEET ROBOTICS CO., LTD.
OpenHRI
• 音声コミュニケーション用のRTC群
• http://openhri.readthedocs.org/en/
latest/index-ja.html
• http://openrtp.sakura.ne.jp/openhri/
• Juliusを使った音声認識
• OpenJTalkを使った発話
113
114. SUGAR SWEET ROBOTICS CO., LTD.
その他
• RTM on Android
• Androidアプリから直接RTCと通信
• Choreonoid
• ロボットの振り付け
• 動力学シミュレータ機能も
114
115. SUGAR SWEET ROBOTICS CO., LTD.
OpenRTM-aist入門 その5
115
株式会社SUGAR SWEET ROBOTICS 代表取締役
早稲田大学基幹理工学部表現工学科 尾形哲也研究室 客員次席研究員
芝浦工業大学SIT研究所 客員研究員
菅 佑樹
116. SUGAR SWEET ROBOTICS CO., LTD.
RTCをうまく使う
• システムを分割する指針
• RTCの粒度をどうするか
• 階層的アプローチを紹介
• RTCのインターフェース (データポート,サービ
スポート,コンフィグレーション) をどうするか
• 設計指針について持論を紹介
• 実際にどう使っているか
• 移動ロボットフレームワークを紹介
116
117. SUGAR SWEET ROBOTICS CO., LTD.
ロボットミドルウェアの
典型的ユースケース
• 複数のロボット製品を統合してタスクを行う
• 例:危険物除去ロボット
• A: サーボコントローラ
• B: センサー
• C: ロボットアーム統合
• D: ロボットハンド統合
• E: 運動学モジュール
• F: ロボット台車
• G: システム統合
• H: 学習アルゴリズムモジュール
117
119. SUGAR SWEET ROBOTICS CO., LTD.
階層的アプローチ
• ロボット要素層(再利用性高い)
• ロボットのハードウェア単位 (モータ,センサ,マイク等)
• ロボット統合層(ロボット単位での再利用を見込める)
• 複数のロボット要素をまとめて一つの要素のようにする層
• 単位や座標系の変換等の統合
• コントローラ層(ロボットの再利用に付随して再利用が可能)
• 運動学や起動追従制御など,ロボットハードウェア構成に依存が大きい
アルゴリズム
• アルゴリズム層(再利用性が高い)
• 認識・分類,軌道計画等,ハードウェアへの依存が少ないアルゴリズム
• アプリケーション層(ドメインに依存.再利用性は低い)
• ドメインに依存したアプリケーション.GUIなど.
119
120. SUGAR SWEET ROBOTICS CO., LTD.
インターフェースをどうするか
• ロボット要素層
• センサやアクチュエータはデータポートが中心
• ハードウェアの設定値をコンフィグレーションに
• 初期化はonActivateで,周期処理をonExecute
• ロボット統合層(ロボット単位での再利用を見込める)
• 要素からのデータを統合して,組み立てたロボットに依存する座標変換などを行う
• データポート中心.TimedVelocityやTimedPoseなどの物理量を意識
• 変換をするのみであればコールバックを使うと良い
• コントローラ層(ロボットの再利用に付随して再利用が可能)
• サービスポートが中心.マニピュレータの共通インターフェースなどを参考に
• アルゴリズム層(再利用性が高い)
• サービスポートが中心.計算負荷が高く,要求とレスポンスの対応関係が明確
• アプリケーション層(ドメインに依存.再利用性は低い)
• アルゴリズムやコントローラを利用するためのサービスポート(要求側)が中心
120
121. SUGAR SWEET ROBOTICS CO., LTD.
RTM対応 移動ロボット
ナビゲーションフレームワーク
• 台車型移動ロボットのナビゲーションが目的
– すべてオープンソース
• マップ作成
– SLAMのみ
• 地図を用いたナビゲーション
– レーザーレンジセンサーを用いた位置推定
– 軌道計画
– 軌道追従
• 機能ごとにモジュール化しており,インターフェースを共通化して,入替えが可能
– 例:独自の移動ロボットを適用
– 例:独自のプランニング・パスフォロワーを適用
• 基本操作が可能なGUIを準備
121
122. SUGAR SWEET ROBOTICS CO., LTD.
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
123. SUGAR SWEET ROBOTICS CO., LTD.
フレームワークの特徴
• 上位のインターフェースはすべてサービスポート
• 処理の成功・失敗を返り値で受け取ることがで
きる
• サービスロボットの上位処理はデータフロー的
ではなく,イベント的
• MobileRobot.idlでインターフェースを定義済み
– これをインポートすればコンシューマ側は簡
単に作成できる
123
124. SUGAR SWEET ROBOTICS CO., LTD.
MobileRobot.idl
• 移動ロボットナビゲーションに関わるインターフェースやデータ型
を定義
• OpenRTM-aist同梱のExtendedDataTypes.idlや
InterfaceDataTypes.idlを再利用
• 今回のように独自IDLからincludeする場合は,自分のIDLフォ
ルダにこれらのIDLをコピーしておく必要がある
• 統一されたインターフェース設計思想
– 値渡しでデータを渡し,参照渡しでデータを受け取る.返り値
で処理の成否を受け取る
124
125. SUGAR SWEET ROBOTICS CO., LTD.
IDLファイル
125
#include "BasicDataType.idl"
#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
126. SUGAR SWEET ROBOTICS CO., LTD.
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
127. SUGAR SWEET ROBOTICS CO., LTD.
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 {
/// Request Current Build Map Data
RETURN_VALUE requestCurrentBuiltMap(out OGMap map);
};
OGMapServerはマップを単純に配信するための
インターフェース
requestCurrentBuiltMapでマップを取得
128. SUGAR SWEET ROBOTICS CO., LTD.
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は軌道追従のためのインターフェース
129. SUGAR SWEET ROBOTICS CO., LTD.
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).
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;
};
/*!
* @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は,目標位置姿勢,
距離・角度許容差,
タイムリミットや最大速度が入る
130. SUGAR SWEET ROBOTICS CO., LTD.
IDLファイル全文 (MobileRobot.idl)
130
#include "BasicDataType.idl"
#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);
/// Request Current Build Map Data
RETURN_VALUE requestCurrentBuiltMap(out OGMap map);
};
/*!
* @interface OGMapServer
* @brief Occupancy Grid Map Service Interface
*/
interface OGMapServer {
/// Request Current Build Map Data
RETURN_VALUE requestCurrentBuiltMap(out OGMap map);
};
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;
};
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);
};
};
#endif
131. SUGAR SWEET ROBOTICS CO., LTD.
マップの作成
• ロボットに北陽URG等のレーザースキャナを搭載
• ロボットの移動情報 (オドメトリ) とスキャン情報か
らマップを作成
• ロボットの移動はジョイスティックなどで指令を送る
131
134. SUGAR SWEET ROBOTICS CO., LTD.
コンフィグレーションの設定
• UrgRTCのCOMポートをコンフィグレーション
(port_name) で指定
• geometry_x, y, zで,センサの位置=ロボットの座
標系 (車軸中心) からのオフセットを指定 (単位[m])
134
136. SUGAR SWEET ROBOTICS CO., LTD.
GUIの利用
• Start Mappingボタンでマッ
ピング開始
• NavigationManagerの
targetVelocityポートを接続
していれば,右図のGUIジョ
イスティックが起動
• ジョイスティック (黄色い円)
を上に引っ張ると,ロボット
が前進
• 別のジョイスティックRTCを
使うことも可能
136
GUIジョイスティックStartボタン
137. SUGAR SWEET ROBOTICS CO., LTD.
GUIの利用
• Save Mapでマップを保存
• マップデータは二つのファ
イルで保存される
– ****.png : グリッド
マップのデータ
– ****.yaml : マップの原
点やピクセル/長さの
比の情報
137
Saveボタン
139. SUGAR SWEET ROBOTICS CO., LTD.
Start Mapping時
GUI
SLAM
マッピング開始命令
GUIからSLAMモジュールにマッピング開始を指⽰
139
*OGMapperインターフェース
140. SUGAR SWEET ROBOTICS CO., LTD.
Mapping中
スキャンデータ
SLAM
オドメトリ
レンジスキャナのデータとロボットのオドメトリからマップを作成
140
ロボット
レンジセンサ
141. SUGAR SWEET ROBOTICS CO., LTD.
Mapping中の表示
スキャンデータ
SLAM
推定自己位置
ロボットの⾃⼰位置とスキャンデータをデータポートから取得
サービスポートで現在のマップデータを取得.重ねて表⽰
141
現在のマップデータを
要求し取得
レンジセンサ
*OGMapperインターフェース
143. SUGAR SWEET ROBOTICS CO., LTD.
Save Map時
SLAM
サービスポートで現在のマップデータを取得.保存.
143
現在のマップデータを
要求し取得
GUI
*OGMapperインターフェース
145. SUGAR SWEET ROBOTICS CO., LTD.
北陽電機 URG
• 主なコンフィグレーション
– スキャナのロボット座
標系での位置,オフセッ
ト [m]
• geometry_x
• geometry_y
• geometry_z
– COMポート番号 (COM3
とか)
• port_name
145
• レーザーセンサ URGのRTC
• 出⼒ポート
• range : RangeData
• レーザースキャナ出⼒
146. SUGAR SWEET ROBOTICS CO., LTD.
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
• 推定⾃⼰位置 (オドメトリ)
147. SUGAR SWEET ROBOTICS CO., LTD.
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]
148. SUGAR SWEET ROBOTICS CO., LTD.
NavigationManager
• ナビゲーション用GUI
• 入力ポート
– currentPose : TimedPose2D
• ロボットの自己位置 (表示用)
– range : RangeData
• LiDAR入力 (表示用)
– camera : CameraImage
• カメラ画像表示
• 出力ポート
– targetVelocity : TimedVelocity2D
• GUIジョイスティックでの操作時に接続
• サービスポート
– mapperService : RTC::OGMapper
(Required)
• SLAMモジュールを接続.マッピングの
開始・終了・マップ取得
148
149. SUGAR SWEET ROBOTICS CO., LTD.
ナビゲーション
• ロボットに北陽URG等のレーザースキャナを搭載
• ロボットの移動情報 (オドメトリ) とスキャン情報
をマップとマッチングして自己位置を高精度に推定
• マップ情報から軌道計画を自動化
• 計画した軌道への自動的な追従
149
152. SUGAR SWEET ROBOTICS CO., LTD.
コンフィグレーションの設定
• UrgRTCのCOMポートをコン
フィグレーション
(port_name) で指定
• geometry_x, y, zで,ロボッ
トの座標系 (車軸中心) からの
オフセットを指定 (単位[m])
• MapServerのマップファイル
のファイル名を設定
– ****.yamlを設定すること
152
154. SUGAR SWEET ROBOTICS CO., LTD.
GUIの利用
• MapServerにマップが読み込まれていれ
ば,マップが表示され,ロボットの位置と
レンジセンサのデータが表示される
• マップ上の目標位置をクリックするとゴー
ルを指定できる
154
155. SUGAR SWEET ROBOTICS CO., LTD.
GUIの利用
• マップ上のゴールを指定すると,ゴー
ルの姿勢を選択するダイアログが表
示される
• 赤い円内をクリックすると,ゴール
姿勢 (マップ座標系での姿勢) を指
定できるので,OKする
• マップ上に指定した姿勢でのゴール
が出現する (円の中心が位置,直線
の伸びている方向がロボットのX方
向)
155
158. SUGAR SWEET ROBOTICS CO., LTD.
Activate時 (自己位置推定)
まず,MapServerにマップを要求して取得
オドメトリとスキャンデータからマップ内での⾃⼰位置を推定
158
ロボット
スキャンデータ
レンジセンサ
オドメトリ マップデータを要求
*OGMapServerインターフェース
159. SUGAR SWEET ROBOTICS CO., LTD.
Activate時 (表示)
GUIはまず,MapServerにマップを要求して取得
推定⾃⼰位置とスキャンデータをマップに重ねて表⽰
159
⾃⼰位置推定
スキャンデータ
レンジセンサ
推定自己位置
マップデータを要求
GUI
*OGMapServerインターフェース
160. SUGAR SWEET ROBOTICS CO., LTD.
ゴール設定時
GUI内で処理は完結.クリックした位置を
表⽰したマップ上の位置に変換して記録
160
⾃⼰位置推定 マップデータを要求
GUI
*OGMapServerインターフェース
161. SUGAR SWEET ROBOTICS CO., LTD.
Plan Path時
ロボットの位置をスタート,指定されたゴール,MapServerから
取得したマップ,パスフォローの許容誤差等のパラメータを⽤い
てPathPlannerにプランニングを要求し,Pathを取得・表⽰
161
⾃⼰位置推定RTC
レンジセンサ
推定自己位置
GUI
軌道計画
*PathPlannerインターフェース
162. SUGAR SWEET ROBOTICS CO., LTD.
Follow時
⽬標軌道を引数として,軌道追従要求をPathFollowerに要求.成功・失敗を返
り値として得る
ロボットの⾃⼰位置と軌道を照らし合わせながら,ロボットの⽬標速度を送信
162
⾃⼰位置推定RTC
推定自己位置
GUI
軌道追従要求
ロボットへの速度指令
軌道追従RTC
*PathFollowerインターフェース
164. SUGAR SWEET ROBOTICS CO., LTD.
北陽電機 URG
• 主なコンフィグレーション
– スキャナのロボット座
標系での位置,オフセッ
ト
• geometry_x
• geometry_y
• geometry_z
– COMポート番号
• port_name
164
• レーザーセンサ URGのRTC
• 出⼒ポート
• range : RangeData
• レーザースキャナ出⼒
165. SUGAR SWEET ROBOTICS CO., LTD.
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
• 推定⾃⼰位置 (オドメトリ)
166. SUGAR SWEET ROBOTICS CO., LTD.
NavigationManager
• ナビゲーション用GUI
• 入力ポート
– currentPose : TimedPose2D
• ロボットの自己位置 (表示用)
– range : RangeData
• LiDAR入力 (表示用)
– camera : CameraImage
• カメラ画像表示
• 出力ポート
– targetVelocity : TimedVelocity2D
• GUIジョイスティックでの操作時に接続
• サービスポート
– mapServer : RTC::OGMapServer
• マップ配信モジュールを接続.マップ取得
– pathPlanner : RTC::PathPlanner
• 軌道計画モジュールを接続
– pathFollower : RTC::PathFollower
• 軌道追従モジュールを接続 166
167. SUGAR SWEET ROBOTICS CO., LTD.
MapServer
• マップデータを管理・出力
• サービスポート
– mapServer :
MapServer
• マップデータの出力
167
• 主なコンフィグレーション
• filename
• マップデータのファイル名.Yamlファイル
を指定すること.
• Yamlファイルとpngファイルは同じフォル
ダに配置すること
• 実⾏ディレクトリからの相対パスか,絶対
パスで指定.
• よくわからなければ
C:mapmy_may.yamlなどが無難
168. SUGAR SWEET ROBOTICS CO., LTD.
Localization_MRPT
• モンテカルロ法による自己位置推定
• 入力ポート
– range : RangeData
• LiDAR入力
– odometry : TimedPose2D
• オドメトリ
• 出力ポート
– estimatedPose : TimedPose2D
• 推定自己位置
• サービスポート
– mapServer : MapServer
• マップデータの取得
168
• 主なコンフィグレーション
• Dieter Fox,KLD-sampling: Adaptive particle
filters. In Advances in Neural Information
Processing Systems 14,2002
169. SUGAR SWEET ROBOTICS CO., LTD.
PathPlanner_MRPT
• パスプランニング
(MRPT使用)
• サービスポート
– pathPlanner :
PathPlanner
• パスプランニング
169
• 主なコンフィグレーション
• 円柱型ロボット(と仮定した場合)の半径
• robotRadius
• 軌道追従の位置・姿勢許容差
• pathDistanceTolerance
• pathHeadingTolerance
• 最終ゴール地点の位置・姿勢許容差
• goalDistanceTolerance
• pathHeadingTolerance
170. SUGAR SWEET ROBOTICS CO., LTD.
SimplePathFollower
• Path2D型の経路への追従
• 入力ポート
– currentPose : TimedPose2D
• 現在位置
• 出力ポート
– velocity : TimedVelocity2D
• 目標速度
• サービスポート
– pathFollower : PathFollower
• パス追従命令取得
170
• 主なコンフィグレーション
• distanceToTranslationGain
• Pathとの位置のズレと並進移動速度のゲイン
• distanceToRotationGain
• Pathとの位置のズレと回転移動速度のゲイン
• directionToTranslationGain
• Pathとの向きのズレと並進移動速度のゲイン
• directionToRotationGain
• Pathとの向きのズレと回転移動速度のゲイン
• approachDistanceGain
• 最後の終着点との距離と速度のゲイン
• approachDirectionGain
• 最後の終着点の⽅向と回転速度のゲイン
171. SUGAR SWEET ROBOTICS CO., LTD.
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との向きのズレと回転移動速度のゲイン
172. SUGAR SWEET ROBOTICS CO., LTD.
SimplePathFollowerアルゴリズム
• 最近傍パスがゴールを含む場合は
ゴールアプローチモードになる
• まずゴール点まで移動
– ゴールとロボットとの距離を
ΔX
– ゴール・ロボット間の直線と
ロボットの向きとの差分をΔH
• Vx = K5 * ΔX
• Va = K6 * ΔH
– ゴール点のdistanceTolerance
内で停止
• ゴール点の姿勢までその場で回転
し,headingToleranceに収まれば
ゴール到達
172
m
l n
ΔX
ΔH
• 主なコンフィグレーションとの対応
• approachDistanceGain = K5
• 最後の終着点との距離と速度のゲイン
• approachDirectionGain = K6
• 最後の終着点の⽅向と回転速度のゲイン
176. SUGAR SWEET ROBOTICS CO., LTD.
ロボットRTCの入替え
独⾃ロボットへのナビゲーション機能の実装
176
TimedVelocity2D⼊⼒,TimedPose2D出⼒
177. SUGAR SWEET ROBOTICS CO., LTD.
軌道計画RTCの改良
軌道計画アルゴリズムを改良
RRT-CONNECTなどの実装
177
PathPlannerサービスポート (Provided) を実装