Downloaded 25 times
![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](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-34-320.jpg)
![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](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-53-320.jpg)
![SUGAR SWEET ROBOTICS CO., LTD.
Kinectのデータ
• 要するに構造体
• Vector4
• ３次元位置
• NuiSkeletonData
• trackingState
• トラッキングの状態
• skeletonPosition[20]
• 各関節の位置
• 関節のインデックスをenumにしておく
• OpenRTM-aistではこの構造体定義をIDL
(Interface Definition Language) で書く
61](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-61-320.jpg)
![SUGAR SWEET ROBOTICS CO., LTD.
IDLでstructを定義する
module KINECT {
struct スケルトンデータ {
トラッキングした人間の関節位置[20] : Vector4
トラッキングの品質や状態
};
struct スケルトンフレーム {
タイムスタンプやフレーム番号 : Time
スケルトンデータ [ 6 ];
};
};
67
• 単純にKinectSDKでサポートしているstructをそのままIDLにしまし
た
• 上述のgithubサーバーからファイルをダウンロードしてIDLの中身
を見てみよう
• 大まかな構造は下記のようになっている](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-67-320.jpg)
![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;
};
};](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-68-320.jpg)
![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で使う
トラッキング出来ているか否か
トラックした人間一人分のデー
タ．関節部の位置
一回のスキャンで取れるデータ
のまとまり．最大６人](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-69-320.jpg)
![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](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-74-320.jpg)
![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型を使う
引数がないときは簡単．参照渡しが難しいので解説．](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-89-320.jpg)
![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型を使う
引数がないときは簡単．参照渡しが難しいので解説．](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-90-320.jpg)
![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
(手先の相対位置座標指定移動)](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-98-320.jpg)
![SUGAR SWEET ROBOTICS CO., LTD.
コンフィグレーションの設定
• UrgRTCのCOMポートをコンフィグレーション
(port_name) で指定
• geometry_x, y, zで，センサの位置＝ロボットの座
標系 (車軸中心) からのオフセットを指定 (単位[m])
134](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-134-320.jpg)
![SUGAR SWEET ROBOTICS CO., LTD.
北陽電機 URG
• 主なコンフィグレーション
– スキャナのロボット座
標系での位置，オフセッ
ト [m]
• geometry_x
• geometry_y
• geometry_z
– COMポート番号 (COM3
とか)
• port_name
145
• レーザーセンサ URGのRTC
• 出⼒ポート
• range : RangeData
• レーザースキャナ出⼒](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-145-320.jpg)
![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
• 推定⾃⼰位置 (オドメトリ）](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-146-320.jpg)
![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]](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-147-320.jpg)
![SUGAR SWEET ROBOTICS CO., LTD.
コンフィグレーションの設定
• UrgRTCのCOMポートをコン
フィグレーション
(port_name) で指定
• geometry_x, y, zで，ロボッ
トの座標系 (車軸中心) からの
オフセットを指定 (単位[m])
• MapServerのマップファイル
のファイル名を設定
– ****.yamlを設定すること
152](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-152-320.jpg)
![SUGAR SWEET ROBOTICS CO., LTD.
2. 文字列コマンド取得
RTC::ReturnCode_t StringNavigationCommander::onExecute(RTC::UniqueId ec_id)
{
…
// 文字列コマンドを取得
if (m_commandIn.isNew()) {
m_commandIn.read(); // m_commandにデータが格納される
std::string data = m_command.data; // string型に変換
std::cout << "[RTC::StringCommander] Receive Command (" << data << ")" << std::endl;
…
通常のデータポートの⽅法でデータを取得
198](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-198-320.jpg)
![SUGAR SWEET ROBOTICS CO., LTD.
3. ゴール位置設定
RTC::ReturnCode_t StringNavigationCommander::onExecute(RTC::UniqueId ec_id)
{
…
// Goalを設定する
Pose2D goal;
if (data == "GOTO_A") { // string型は==演算子が使える
goal.position.x = 1.0;
goal.position.y = 1.0;
goal.heading = 1.57;
} else if (data == "GOTO_B") {
goal.position.x = 1.0;
goal.position.y = 0.0;
goal.heading = 0.0;
} else { // 知らないコマンドはエラーメッセージを表示
std::cout << "[RTC::StringCommander] Error. Unknown Command (" << data << ")" << std::endl;
return RTC::RTC_OK; // onExecuteから通常終了
}
// 現在地と目的地が定まる
std::cout << "[RTC::StringCommander] Current Pose = (" << m_currentPose.data.position.x << ", "
<< m_currentPose.data.position.y << ", " << m_currentPose.data.heading << ")" << std::endl;
std::cout << "[RTC::StringCommander] Goal Pose = (" << goal.position.x << ", "
<< goal.position.y << ", " << goal.heading << ")" << std::endl;
…
取得⽂字列からゴールを⾃動設定
199](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-199-320.jpg)
![SUGAR SWEET ROBOTICS CO., LTD.
4. マップ要求
RTC::ReturnCode_t StringNavigationCommander::onExecute(RTC::UniqueId ec_id)
{
…
// マップを要求する
RTC::OGMap_var outMap; // マップを格納するための変数
RTC::RETURN_VALUE retval = this->m_OGMapServer->requestCurrentBuiltMap(outMap);
if (retval == RTC::RETURN_VALUE::RETVAL_OK) {
std::cout << "[RTC::StringCommander] Map Request OK." << std::endl;
} else {
std::cout << "[RTC::StringCommander] Map Request Unknown Error Code =" << retval << std::endl;
return RTC::RTC_ERROR;
}
…
サービスポートを使ってマップを取得
out型引数の使い⽅の例
200](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-200-320.jpg)
![SUGAR SWEET ROBOTICS CO., LTD.
5. 軌道計画
RTC::ReturnCode_t StringNavigationCommander::onExecute(RTC::UniqueId ec_id)
{
…
// 軌道計画の為のパラメータを調整
RTC::PathPlanParameter param;
param.map = outMap; // マップ
param.currentPose = this->m_currentPose.data; // スタート
param.targetPose = goal; // ゴール
param.distanceTolerance = 1.0; // 軌道からの位置のずれの許容差
param.headingTolerance = 1.0; // 軌道からの角度のずれの許容差
param.maxSpeed.vx = 5.0; param.maxSpeed.vy = 0.0; param.maxSpeed.va = 1.0; // 最大速度
param.timeLimit.sec = 9999; param.timeLimit.nsec = 0; // 許容時間
RTC::Path2D_var outPath; // 軌道を格納するための変数
retval = m_pathPlanner->planPath(param, outPath);
if (retval == RTC::RETURN_VALUE::RETVAL_OK) { // 成功
std::cout << "[RTC::StringCommander] Path Plan OK." << std::endl;
} else if (retval == RTC::RETURN_VALUE::RETVAL_NOT_FOUND) { // 見つからない (通常終了)
std::cout << "[RTC::StringCommander] Path Plan No Path Found" << std::endl;
return RTC::RTC_OK;
} else {
std::cout << "[RTC::StringCommander] Path Plan Unknown Error Code =" << retval << std::endl;
return RTC::RTC_ERROR;
}
…
サービスポートを使って軌道を取得 (out型引数)
201](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-201-320.jpg)
![SUGAR SWEET ROBOTICS CO., LTD.
6. 軌道追従
RTC::ReturnCode_t StringNavigationCommander::onExecute(RTC::UniqueId ec_id)
{
…
// 軌道追従を要求．終了するまでfollowPathはブロックする
retval = m_pathFollower->followPath(outPath);
if (retval == RTC::RETURN_VALUE::RETVAL_OK) { // 成功
std::cout << "[RTC::StringCommander] Follow OK." << std::endl;
} else { // 失敗も通常終了
std::cout << "[RTC::StringCommander] Follow Unknown Error Code =" << retval << std::endl;
return RTC::RTC_OK;
}
} // if (m_commandIn.isNew()) に対応
followPathで起動追従．追従終了までブロック
202](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-202-320.jpg)
![SUGAR SWEET ROBOTICS CO., LTD.
203
RTC::ReturnCode_t StringNavigationCommander::onExecute(RTC::UniqueId ec_id)
{
// 現在のロボットの位置を取得
if (m_currentPoseIn.isNew()) {
m_currentPoseIn.read(); // m_currentPoseにデータが格納される
}
// 文字列コマンドを取得
if (m_commandIn.isNew()) {
m_commandIn.read(); // m_commandにデータが格納される
std::string data = m_command.data; // string型に変換
std::cout << "[RTC::StringCommander] Receive Command (" << data << ")" << std::endl;
// Goalを設定する
Pose2D goal;
if (data == "GOTO_A") { // string型は==演算子が使える
goal.position.x = 1.0;
goal.position.y = 1.0;
goal.heading = 1.57;
} else if (data == "GOTO_B") {
goal.position.x = 1.0;
goal.position.y = 0.0;
goal.heading = 0.0;
} else { // 知らないコマンドはエラーメッセージを表示
std::cout << "[RTC::StringCommander] Error. Unknown Command (" << data << ")" << std::endl;
return RTC::RTC_OK; // onExecuteから通常終了
}
// 現在地と目的地が定まる
std::cout << "[RTC::StringCommander] Current Pose = (" << m_currentPose.data.position.x << ", "
<< m_currentPose.data.position.y << ", " << m_currentPose.data.heading << ")" << std::endl;
std::cout << "[RTC::StringCommander] Goal Pose = (" << goal.position.x << ", "
<< goal.position.y << ", " << goal.heading << ")" << std::endl;
// マップを要求する
RTC::OGMap_var outMap; // マップを格納するための変数
RTC::RETURN_VALUE retval = this->m_OGMapServer->requestCurrentBuiltMap(outMap);
if (retval == RTC::RETURN_VALUE::RETVAL_OK) {
std::cout << "[RTC::StringCommander] Map Request OK." << std::endl;
} else {
std::cout << "[RTC::StringCommander] Map Request Unknown Error Code =" << retval << std::endl;
return RTC::RTC_ERROR;
}](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-203-320.jpg)
![SUGAR SWEET ROBOTICS CO., LTD.
204
// 軌道計画の為のパラメータを調整
RTC::PathPlanParameter param;
param.map = outMap; // マップ
param.currentPose = this->m_currentPose.data; // スタート
param.targetPose = goal; // ゴール
param.distanceTolerance = 1.0; // 軌道からの位置のずれの許容差
param.headingTolerance = 1.0; // 軌道からの角度のずれの許容差
param.maxSpeed.vx = 5.0; param.maxSpeed.vy = 0.0; param.maxSpeed.va = 1.0; // 最大速度
param.timeLimit.sec = 9999; param.timeLimit.nsec = 0; // 許容時間
RTC::Path2D_var outPath; // 軌道を格納するための変数
retval = m_pathPlanner->planPath(param, outPath);
if (retval == RTC::RETURN_VALUE::RETVAL_OK) { // 成功
std::cout << "[RTC::StringCommander] Path Plan OK." << std::endl;
} else if (retval == RTC::RETURN_VALUE::RETVAL_NOT_FOUND) { // 見つからない (通常終了)
std::cout << "[RTC::StringCommander] Path Plan No Path Found" << std::endl;
return RTC::RTC_OK;
} else {
std::cout << "[RTC::StringCommander] Path Plan Unknown Error Code =" << retval << std::endl;
return RTC::RTC_ERROR;
}
// 軌道追従を要求．終了するまでfollowPathはブロックする
retval = m_pathFollower->followPath(outPath);
if (retval == RTC::RETURN_VALUE::RETVAL_OK) { // 成功
std::cout << "[RTC::StringCommander] Follow OK." << std::endl;
} else { // 失敗も通常終了
std::cout << "[RTC::StringCommander] Follow Unknown Error Code =" << retval << std::endl;
return RTC::RTC_OK;
}
} // if (m_commandIn.isNew()) に対応
return RTC::RTC_OK;
}](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-204-320.jpg)
![SUGAR SWEET ROBOTICS CO., LTD.
さらなる改良案
• ゴール名とゴール位置・姿勢を外部ファイルにし
て読み込む
– yamlファイルなどが使い易い
– ファイル名をコンフィグレーションにする
• 文字列を音声認識で出力する (OpenHRIを使う)
205
GOTO_A : [1.0, 1.0, 0.0],
GOTO_B : [1.0, 0.0, 1.57],
GOTO_C : [1.0, -1.0, 0.0]](https://image.slidesharecdn.com/openrtm-160113064233/85/OpenRTM-aist-205-320.jpg)
More Related Content
OpenRTM-aist入門
- 1. SUGAR SWEET ROBOTICSCO., LTD. OpenRTM-aist入門 その１ 株式会社SUGAR SWEET ROBOTICS 代表取締役 早稲田大学基幹理工学部表現工学科 尾形哲也研究室 客員次席研究員 芝浦工業大学SIT研究所 客員研究員 菅 佑樹 1
- 2. SUGAR SWEET ROBOTICSCO., LTD. RTMツールの使い方 • OpenRTM-aistを使うためのツールの使い 方を説明します． • RTコンポーネントを起動してもシステ ムの動作は始まらない • ポートを接続する • RTCの状態遷移を促す • RTCのコンフィグレーションを調整 2
- 3. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., LTD. ネームサービスとは • 実行中のRTCの管理 – OpenRTM-aistを使う場合，RTCを 使ったシステムでは最低１つ必要 – 異なるホストのRTCも登録可能 – 複数のネームサービスを併用可能 6 RTC1 RTC2 RTC3 ネーム サーバー ツール PC2 PC1
- 7. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., LTD. RT System Editorの使い方 • パースペクティブを「RT System Editor」に変更 – パースペクティブとはEclipse の作業画面のレイアウトタイ プ – メニュー＞「ウィンドウ」＞ 「パースペクティブを開く」 ＞「その他」 – 「RT System Editor」を選択 8
- 9. SUGAR SWEET ROBOTICSCO., LTD. RT System Editorの使い方 • 所望のネームサーバが見つからない場合は追加処理 – ネームサーバとは実行中のRTCの管理を行うサーバ • localhost (自分自身) • デフォルトでlocalhostを検索し，ネームサーバーが発見され れば追加される • Macでは不具合があるので，ネームサーバーを起動する前に RTSEを起動した方がいい • ネームサーバに起動したRTCが登録されていれば成功 • ConsoleInは • 「/localhost/お使いのPC名.host_cxt/ConsoleIn0.rtc」 という名前でネームサーバーに登録されているはず • 名前の登録ルールを変更することも簡単 • rtc.conf 9
- 10. SUGAR SWEET ROBOTICSCO., LTD. RT System Editorの使い方 • System Editorを開いてRTシステムを編集す る - File > Open On-Line System Editorもしくはツールバー 上のボタン (右図) - メインビューに空の「System Diagram」が表示される • ネームサービスビューからドラッグ＆ドロッ プ – 利用するRTCをすべて表示 • ポートとポートをドラッグ＆ドロップで接続 – 表示されるダイアログはOK 10
- 11. SUGAR SWEET ROBOTICSCO., LTD. RT System Editorの使い方 • すべてのRTCをACTIVATE • ConsoleInのウィンドウに「Please Input number」と表示 – 適当な数字を入れると ConsoleOut側に表示 – RTC間の通信が行われたことが分か る • DEACTIVATEする場合は，指令を 送ってからConsoleInに数字を送る 必要がある – （scanf入力待ちになっている） ACTIVATE DEACTIVATE 11
- 12. SUGAR SWEET ROBOTICSCO., LTD. まとめ • ツールの使い方 • ネームサービスが必ず必要 • RT System Editor (RTCの接続，Activate/ Deactivate) 12
- 13. SUGAR SWEET ROBOTICSCO., LTD. OpenRTM-aist入門 その２ 株式会社SUGAR SWEET ROBOTICS 代表取締役 早稲田大学基幹理工学部表現工学科 尾形哲也研究室 客員次席研究員 芝浦工業大学SIT研究所 客員研究員 菅 佑樹 13
- 14. SUGAR SWEET ROBOTICSCO., LTD. データポートを使う • OpenRTM-aistではポートは２種類 • データポート・・・データフロー型 • サービスポート・・・遠隔手続き型 • データポートはちょうど，制御のブロック線 図のようにRTシステムを組むことができる • ロボット台車の簡易シミュレータを使っ てRTC開発に慣れよう 14
- 15. SUGAR SWEET ROBOTICSCO., LTD. OpenRTM-aist学習用台車シミュレータ • Loader.batを実行 – シミュレータ – 仮想ジョイスティック – コントローラ – Java7以降が必要 • すべて接続してACTIVATE • ジョイスティックで操作 • コントローラGUIで位置や接触センサを確認 シミュレータ 仮想ジョイスティック コントローラGUI http://ysuga.net/?p=133 15
- 16. SUGAR SWEET ROBOTICSCO., LTD. 台車シミュレータの中身 • RT System Editorで表示 仮想ジョイスティック コントローラ シミュレータ内のロボット 目標速度 現在位置 接触センサ ジョイスティック入力 16
- 17. SUGAR SWEET ROBOTICSCO., LTD. 実習1．台車に指令を送ってみよう • 下記のMobileRobotのRTCは移動ロボットシミュレータ内の台車を表している 目標速度 「vel」 TimedVelocity2D型 現在位置 「pos」 TimedPose2D型 接触センサ 「bumper」 TimedBooleanSeq型 17 • 台車に直進と回転の指令を同時に送り，その場でぐるぐる回る動作をさせ てみよう
- 18. SUGAR SWEET ROBOTICSCO., LTD. RTCプログラミングの流れ • RTC Builderによるスケルトンコードの生成 • スケルトンコードの特定のイベントハンドラに独自のコードを追加 • on_initialized・・・CREATED->INACTIVE • on_activated・・・INACTIVE->ACTIVE • on_deactivated・・・ACTIVE->INACTIVE • on_execute・・・ACTIVE状態で周期的に呼ばれる • コンパイルし実行 18
- 19. SUGAR SWEET ROBOTICSCO., LTD. RTC Builder • パースペクティブをRTC Builderに変更 – Builder Editorで，RTCの骨格（スケルト ン）コードを作成できる • Builder Editorを開く – RTCが持つべき「イベントハンドラ」が実 装されているので，それを編集して自分の コードを差し込む 19
- 20. SUGAR SWEET ROBOTICSCO., LTD. RTC Builder • プロジェクト名の入力 – MyController • 画面下部のタブを切り替え ながら必要な情報を入力 20
- 21. SUGAR SWEET ROBOTICSCO., LTD. RTC Builder • 基本タブ 必要情報を入力 21
- 22. SUGAR SWEET ROBOTICSCO., LTD. RTC Builder • アクティビティタブ 1. onExecuteを選択 2. ONにする 22
- 23. SUGAR SWEET ROBOTICSCO., LTD. RTC Builder • データポートタブ • データ型が見つから無い場合はRTM_ROOT環境変数が設定されて無い • システムの詳細設定や，~/.bashrcなどを確認．必要によっては再起動． • TimedVelocity2DはTimedVector2Dと間違いやすい！！ 出力ポートを追加ポート名を編集 ポートの型を変更 ポートのソースコード上の変数名を入力 23
- 24. SUGAR SWEET ROBOTICSCO., LTD. RTC Builder • 言語タブ C++を選択 24 • 基本タブに戻る (下にスクロール) コード生成
- 25. SUGAR SWEET ROBOTICSCO., LTD. RTC Builder • MyControllerプロジェクトにソースコードが生 成される – Builder Editor（真ん中のウィンドウ）は閉じてよい 25
- 26. SUGAR SWEET ROBOTICSCO., LTD. RTCのソースコードを見つける • Eclipseは通常，自分のホームディレクトリ直下 のworkspaceフォルダにワークスペースを作る • Winなら C:Usersユーザ名workspace • Linuxなどなら/home/ユーザ名/workspace • ワークスペース内に，作成したRTCのソースコー ドを見つけることができる 26
- 27. SUGAR SWEET ROBOTICSCO., LTD. CMakeによるプロジェクト生成 • CMakeを起動 • CMakeLists.txtを CMakeにドラッグ ＆ドロップ 27
- 28. SUGAR SWEET ROBOTICSCO., LTD. ROBOTECH2012 RTミドルウエア講習会 CMake • 出力フォルダパスにbuildを追加 • RTCのフォルダが”…/workspace/MyController”の場合 • ”…/workspace/MyController/build”というフォルダを作る • Configureを押すとフォルダ作成確認のダイアログが出るのでOK 2012/7/11 28
- 29. SUGAR SWEET ROBOTICSCO., LTD. CMake2.8 • ビルド環境を選択 – Visual Studio 12 2013を選択 – Use default native compilersを選 択 • 赤い表示が出るが恐れず 「Generate」を押す – Visual Studio用プロジェクトファ イルが生成される 29
- 30. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., LTD. Visual Studio • build/src/MyController.slnファイルを ダブルクリック – Visual Studioが起動する – 複数のプロジェクトが存在 – MyController・・RTC本体 – MyControllerComp・・・RTCを単体の アプリケーションとして実行するための プロジェクト • MyControllerプロジェクト – MyController.cppを開く 31
- 32. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., LTD. 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
- 34. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., LTD. 実行 • ネームサービスの起動確認 • RTCの実行 – MyControllerCompを右クリック – 「デバッグ→インスタンス作成」で実行 • MobileRobotSim.batを実行するとシミュレー タのみ起動 • RT System Editorでの接続，ACTIVE化 36
- 37. SUGAR SWEET ROBOTICSCO., LTD. 実習2．台車の動きを調整 • コンフィグレーション機能を試す – コンフィグレーションとは，実行中 のRTCを調整するための機能 • 例：制御ゲインの調整 • コンフィグレーション機能を加え て実行中にMyControllerの機能を 調整する 進行方向 X θ Y 37
- 38. SUGAR SWEET ROBOTICSCO., LTD. RTC Builder • Eclipseのパースペクティブを RTC Builderに再変更 • パッケージエクスプローラの RTC.xmlファイルをダブルク リックする – 先ほど作成した情報が記録され ている 38
- 39. SUGAR SWEET ROBOTICSCO., LTD. RTC Builder • コンフィグレーションタブ – velocity_x というコンフィグレーションを追加 39
- 40. SUGAR SWEET ROBOTICSCO., LTD. 追加したコンフィグレーション • velocity_x : double型 – 変数名: velocity_x – デフォルト値: 0.05 • velocity_theta : double型 – 変数名: velocity_theta – デフォルト値: 1.0 40
- 41. SUGAR SWEET ROBOTICSCO., LTD. RTC Builder • 比較ダイアログ – ここでは「Generated」を選択 – 基本的に「Merge」を選択 • 新しいコードの変更点のみ反映 – Generatedは新しいコード側で上書きされるので，自分の記入したコードが消える • バックアップファイルがある (ファイル名末尾に日付が入る) ので心配要りま せん 41
- 42. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., LTD. 実行 • ネームサービスの起動確認 • RTCの実行 – MobileRobotSim.batを実行するとシミュレータのみ起動 • RT System Editorでの接続，ACTIVE化 • RT System Editor下部にConfiguration Viewに選択中のRTCの コンフィグが表示されるので変更して「適用」を選択する（適 用しないと反映されない） 43
- 44. SUGAR SWEET ROBOTICSCO., LTD. 実習3．位置の取得 • 現在位置をデータポートから受け取ってコンソールに表示する 目標速度 「vel」 TimedVelocity2D型 現在位置 「pos」 TimedPose2D型 接触センサ 「bumper」 TimedBooleanSeq型 • TimedPose2D型の入力ポートをMyControllerに追加する
- 45. SUGAR SWEET ROBOTICSCO., LTD. RTC Builder • 再度，RTCBuilderを開く • MyController / RTC.xmlを開く (たぶん，開きっぱなし？) • データポートタブに移動 → TimedPose2D型の入力ポート「pose」を追加 45
- 46. SUGAR SWEET ROBOTICSCO., LTD. 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
- 47. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., LTD. 実行 • ネームサービスの起動確認 • RTCの実行 – MobileRobotSim.batを実行するとシミュレータのみ起 動 • RT System Editorでの接続，ACTIVE化 • 実行時の周期が速すぎる？ 48
- 49. SUGAR SWEET ROBOTICSCO., LTD. rtc.conf • RTCの実行時の設定ファイル – ネームサーバのIPアドレス，ポート番号 – 実行周期，実行コンテキストの種類 – RTCの名前付け規則 – ログの有無，ログレベル – etc… • rtc.confで実行周期を変更（単位Hz） – 右クリック→アプリケーション→テキストエディタ – rtc.confを実行ファイルと同じ場所に置いて，実行ファイルを実行 – Visual C++でデバッグする場合は，プロジェクトファイルと同じディレクトリに置く （デフォルトでそこがカレントディレクトリになる） exec_cxt.periodic.rate: 1.0 49
- 50. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., LTD. 実習4．台車の接触スイッチ • TimedBooleanSeq 真偽型 (True/False)の 配列 目標速度 「vel」 TimedVelocity2D型 現在位置 「pos」 TimedPose2D型 接触センサ 「bumper」 TimedBooleanSeq型 配列の0番目要素＝右側スイッチ 配列の1番目要素＝左側スイッチ 51
- 52. SUGAR SWEET ROBOTICSCO., LTD. RTC Builderによるポートの追加 • InPort : TimedBooleanSeq型 – ポート名: bumper – 変数名： bumper 52
- 53. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., LTD. みなさんへの課題 • 接触スイッチの反応でロボットの動作を変える – 左右どちらかに旋回する？ • 位置の値を使って図形を描く – 正方形，星型などなど • 仮想ジョイスティックを使った操作 • お使いのロボット関連製品をRTC化する – onExecute以外の使えるイベントハンドラ例(RTC Builderで使えるように設定) • onActivated・・・Activate時に一回呼ばれる（初期化用） • onDeactivated・・・Deactivate時もしくはエラー状態遷移時に呼ばれる（終了処理） スティック入力「out」 TimedDoubleSeq型 各ボタンの出力値はGUIに記載されている 例： Up(0,1) → 0番目要素が0，1番目要素が1 最初はコンソールにどんなデータが来るか表示して試してみよう 54
- 55. SUGAR SWEET ROBOTICSCO., LTD. まとめ • ツールの使い方 – RT System Editor (RTCの接続，Activate/Deactivate) – RTC Builder (RTCのスケルトンコード生成) – CMake (Visual C++用プロジェクト生成) • コーディング方法 – データポート入出力 • TimedVelocity2D, TimedPose2D, TimedBooleanSeq – コンフィグレーション – rtc.confの設定 55
- 56. SUGAR SWEET ROBOTICSCO., LTD. さらなる発展 • RTCを探してみよう • http://openrtm.org >プロジェクト > RTコンポーネント • 独自のデータ型を使ってみよう • データポートに独自のデータ型を使うことができる • サービスポートを使ってみよう • RTCに関数型のインターフェースを追加でき，より柔軟なシステム設計ができ る • rtshellを使ってみよう • コマンドラインから使えるツール．スクリプトを書くことで開発を効率化で きる • RTCを接続・コンフィグ・アクティブ化するプログラムを作ってみよう • システム構築を自動化できる • 参考ページ： http://ysuga.net 56
- 57. SUGAR SWEET ROBOTICSCO., LTD. OpenRTM-aist入門 その3 株式会社SUGAR SWEET ROBOTICS 代表取締役 早稲田大学基幹理工学部表現工学科 尾形哲也研究室 客員次席研究員 芝浦工業大学SIT研究所 客員研究員 菅 佑樹 57
- 58. SUGAR SWEET ROBOTICSCO., LTD. 独自のデータ型を使ってみよう • 自分独自のデータ型で通信 • データ型がないときに行う • データを分割して送ると，ひとまとまりのデータなのに別のポー トになってしまう • わかりやすいメンバ名を付けられる (ただの配列ではできない) • 接続できるデータポートを制限できる (DoubleSeqだとどんな データでも送れるけど，どんなデータも繋がってしまう） • 設定ファイルを配布しないといけないのが若干手間 • すでにあるデータ型は再定義しないようにしよう (よく調べる か相談しよう) 58
- 59. SUGAR SWEET ROBOTICSCO., LTD. 独自のデータ型を使ってみよう • Kinectと連携してみよう • Kinect SDK for Windowsを使ったRTコンポーネ ント • KinectだけWindowsで動かして他のRTCは別のシ ステムで作っても良い • モーションキャプチャのためのデータ型が見つから なかったので作った • 一部のKinect用RTCはTimedDoubleSeqを使ってい るけど上述の問題で使いにくい 59
- 60. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., LTD. Kinectのデータ • 要するに構造体 • Vector4 • ３次元位置 • NuiSkeletonData • trackingState • トラッキングの状態 • skeletonPosition[20] • 各関節の位置 • 関節のインデックスをenumにしておく • OpenRTM-aistではこの構造体定義をIDL (Interface Definition Language) で書く 61
- 62. SUGAR SWEET ROBOTICSCO., LTD. Interface Definition Language • サービスポートの関数やデータ型を宣言するためのファイル形式 (.idlファイル) – CORBA, RMIやDDSなどの分散処理実現のために使われる (標 準化されている形式) – Javaに似た形式 – オブジェクト指向 • IDLファイルをコンパイルすると，C++やJava, Pythonのプログラ ムのスケルトンが生成される – IDLは中間言語 – C++やJavaなどの実装言語に変換すると，各言語ごとに若干利 用方法が変わるが，関数名などは変わらない – 今回はC++版を少しだけ解説 62
- 63. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., LTD. skeletonのデータを使う • 各関節の位置を得られる • 3次元位置にはPoint3Dなどのデータ型があるが，複数 の人間のトラッキング情報などを統一的に扱うにはデー タ型が無い • データ型があるか否かを確認するのが重要 • TimedDoubleSeq (double型の配列) などを安易に 使うと，異なるデータ同士でつなぐことができるよ うになり，他の人に迷惑 • 独自データ型を使う！！ 66
- 67. SUGAR SWEET ROBOTICSCO., LTD. IDLでstructを定義する module KINECT { struct スケルトンデータ { トラッキングした人間の関節位置[20] : Vector4 トラッキングの品質や状態 }; struct スケルトンフレーム { タイムスタンプやフレーム番号 : Time スケルトンデータ [ 6 ]; }; }; 67 • 単純にKinectSDKでサポートしているstructをそのままIDLにしまし た • 上述のgithubサーバーからファイルをダウンロードしてIDLの中身 を見てみよう • 大まかな構造は下記のようになっている
- 68. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., 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で使う トラッキング出来ているか否か トラックした人間一人分のデー タ．関節部の位置 一回のスキャンで取れるデータ のまとまり．最大６人
- 70. SUGAR SWEET ROBOTICSCO., LTD. IDLをEclipseに読ませる • OpenRTP起動 • Window > Preference • 開いたダイアログからRTCBuilder を選択 • Newボタンをクリックして，フォ ルダを指定 • 好きなフォルダを指定できるの で，例えば「マイドキュメント /idl」というフォルダを作り， 毎回そこにidlを入れれば良い • Eclipseを再起動する 70
- 71. SUGAR SWEET ROBOTICSCO., LTD. IDLを登録する • データポートを作る 時に選択肢が増える 71
- 72. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., LTD. KinectTest RTC • KinectTestというRTCを作ってみよう • Module名がKinectTest • NuiSkeletonFrame型の入力ポートを持つ • 頭よりも手の高さが上になったら手を挙げ ていると判断して表示 73
- 74. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., LTD. 発展編 • Kinectから受け取ったモーションキャプチャ情報を解析し て，移動ロボットを操作してみよう • 右手を右に開いたら右 • 左手を左に開いたら左 • 両手を前に向けたら前進 • RTCはどのようなインターフェースをもっていなければなら ないか • 入力： Kinectのフレームデータ • 出力： 移動ロボットへの速度指令 75
- 76. SUGAR SWEET ROBOTICSCO., LTD. まとめ • 独自のデータ型を定義する方 法について • まずはOpenRTM-aistで 定義されているデータ型を 検討すること • TimedDoubleSeqなどは 使わない事！！ • データ型については，UMLの クラス図を使って情報を交換 するといい • 他の人と相談するときに使 おう！ 76
- 77. SUGAR SWEET ROBOTICSCO., LTD. OpenRTM-aist入門 その４ 株式会社SUGAR SWEET ROBOTICS 代表取締役 早稲田大学基幹理工学部表現工学科 尾形哲也研究室 客員次席研究員 芝浦工業大学SIT研究所 客員研究員 菅 佑樹 77
- 78. SUGAR SWEET ROBOTICSCO., LTD. サービスポートの基本 78
- 79. SUGAR SWEET ROBOTICSCO., LTD. データポートだけでは不便 • データが欲しいときだけ処理をして欲しい • データポートはいつもpush型 • openやclose, resetのようなデータをともなわない通信をし たい • Long型で1を送ると・・・みたいなインターフェースはダ メ．別のLong型とつながっちゃうから • 複数のデータを同期して受け取りたい • データポートでもタイムスタンプがあるし不可能ではな いが・・・ 79
- 80. SUGAR SWEET ROBOTICSCO., LTD. サービスポートの概念 • OpenRTM-aistの機能 • RTCに関数呼び出しのインターフェースを付加 – 引数でデータを渡す – 返り値でデータを受け取る (引数によるデータ受け取りも可能．これなら複数のデー タを同期して受け取ることができる．) • データポートと比べて – データを必要なときだけ要求することができる – データの送受信の結果が得られる（成否など） – データの送受信に関わらない処理を実現できる • reset, initializeなど – 若干実装が面倒．やっぱりIDLが要る – 独自のインターフェースが乱立すると再利用性が下がる – 共通インターフェース（後述） 80
- 81. SUGAR SWEET ROBOTICSCO., LTD. サービスポートの概念 • サービスポートは「インターフェース」を持つ • インターフェースには極性 (Polarity) がある • Provider 提供側 (polarity=Provided) • リソースを提供する．関数が呼ばれ る側 – Consumer 利用側 (polarity=Required) • リソースを利用する．関数を呼ぶ側 • 一つのサービスポートが複数の提供・要求イン ターフェースを持つことができる • インターフェースのタイプが合えば接続で き，利用できる． • ポートをつなげるには全てのインターフェー スが実装されている必要がある • １ポート，１インターフェースの方が使い 易い 81 RTC1 RTC2 ProvidedRequired Service Port
- 82. SUGAR SWEET ROBOTICSCO., LTD. サービスポート作成の流れ • 1. サービスポートを追加 • サービスポート名を設定 • 2. サービスポートにインターフェースを追加 • 読み込むIDLファイルを選択 • インターフェース型を選択 • インターフェース名* • インスタンス名 • 極性 (Polarity) • 注意！ – インターフェース名が違うと繋げるのが面倒になる！ • インターフェース型を定義するファイルを作成 (IDLファイル) 82 RTC Interface Name : Interface Type Port Name
- 83. SUGAR SWEET ROBOTICSCO., LTD. Interface Definition Language • サービスポートの関数やデータ型を宣言するためのファイル 形式 (.idlファイル) – CORBAやDDSなどの分散処理実現のために使われる – Javaに似た形式 – オブジェクト指向 • IDLファイルをコンパイルすると，C++やJava, Pythonのプ ログラムのスケルトンが生成される – IDLは中間言語 – C++やJavaなどの実装言語に変換すると，各言語ごとに 若干利用方法が変わるが，関数名などは変わらない – 今回はC++版を少しだけ解説 83
- 84. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., LTD. RTC Builder • 「サービスポート」タブ選択 • 「Add Port」でポート追加 • 「ポート名」を設定 • 「Add Interface」でインターフェース追 加 • 「インターフェース名」「方向」「インス タンス名」「変数名」を設定 • 「IDLファイル」の「Browse」ボタンで IDLを読み込む • 「インターフェース型」を選択 • 「IDLパス」は「IDLファイルがあった フォルダを選択」 87
- 88. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., LTD. サービスポートの接続 • インターフェース名が同じであれ ば，問題なく接続できる． • インターフェース名が違う場合は， RT System Editorで「接続でき るインターフェースがない」と言 われるが，詳細ダイアログで接続 するインターフェースを追加・選 択すると接続できるようになる • インターフェース名を再利用 したいRTCと同じにするのが 大事 91
- 92. SUGAR SWEET ROBOTICSCO., LTD. 実践編 マニピュレーター共通インターフェース 92
- 93. SUGAR SWEET ROBOTICSCO., LTD. 共通インターフェース • RTCを定義しただけではロボット開発は便利にならない • インターフェースの共通化 • 同じタイプのロボットならば入れ替えが効くようにす る • できるだけ多くの機能をカバー．「実装してない」を許 す • ロボットアーム，移動ロボット，音声コミュニケーショ ン，カメラ • 関連するツールや知能モジュールが再利用しやすくなる！ 93
- 94. SUGAR SWEET ROBOTICSCO., LTD. マニピュレーター 共通インターフェース • 2013年に改定 (埼玉大学 琴坂研究室による) • ２つのレベルに分割 • 共通レベル (ManipulatorCommonInterface_Common) • 各関節レベルでの動作，位置獲得 • エラー情報取得・復帰 • 中位レベル (ManipulatorCommonInterface_Middle) • 各関節レベルでの動作 • 手先位置での動作 • 手先移動範囲の指定や，ツールおよび台座のオフセット • ハンドの開閉 • 共通するデータ型をManipulatorCommonInterface_DataTypesに規定 94
- 95. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., LTD. マニピュレータ共通インタフェース対応ロボット • 埼玉大学琴坂研究室で開発され ている • http://design.mech.saitama-u.ac.jp/ research/rtm_industrial/rtm_industrial.html • 三菱電機，DENSOウェーブ， ヤマハ，VSTONEの教育用 ロボット • 旧版共通インターフェース対応 • 前川製作所 OROCHI • カワダロボティクス Nextage Open 100 埼玉大学琴坂研究室
- 101. SUGAR SWEET ROBOTICSCO., LTD. VSTONEアカデミックスカラロボット • 垂直多関節型ロボットアー ム • 回転3軸＋手先上下1軸 ＋ハンド開閉 • 共通インターフェースを 使えば手先位置指示可能 • カメラなどの環境認識セ ンサを組み合わせて使う 101 https://www.vstone.co.jp/robotshop
- 102. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., LTD. まとめ • サービスポートについて紹介 • サービスポートの作成方法について紹介 – IDLについて紹介 – C++版のサービスポートの実装方法につ いて紹介 104
- 105. SUGAR SWEET ROBOTICSCO., LTD. OpenRTM-aist入門その６ 105 株式会社SUGAR SWEET ROBOTICS 代表取締役 早稲田大学基幹理工学部表現工学科 尾形哲也研究室 客員次席研究員 芝浦工業大学SIT研究所 客員研究員 菅 佑樹
- 106. SUGAR SWEET ROBOTICSCO., LTD. 便利なツール群を紹介 • rtshell • RTno • V-REPシミュレータのRTC対応 106
- 107. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., LTD. rtshell練習 • ConsoleInCompとConsoleOutCompを起動 • rtresurrect MySystem.xml (システムファイルを読み込んで接続・ コンフィグ) • rtstart MySystem.xml (システムファイルの参加RTCをアクティ ブ化) • rtstop MySystem.xml (インアクティブ化) • ConsoleInはscanfを待ってるので1などを入力しないと進まな い • rtteardown MySystem.xml 109
- 110. SUGAR SWEET ROBOTICSCO., LTD. rtshellの便利機能その他 • rtlog : 出力データポートのデータをログ． 入力データポートにログを再生 • rtprint ：出力データポートのデータをコ ンソールに表示 • rtinject ：入力ポートにデータを投げ込む • rtconf ：RTCのコンフィグ変更 110
- 111. SUGAR SWEET ROBOTICSCO., LTD. RTno (あーるてぃーの) • Arduino + RT = RTno • Arduinoを簡単にRTコンポーネントにするライブラリ • 自作の回路やデバイスをRTコンポーネントにできる • http://ysuga.net/?p=124 111
- 112. SUGAR SWEET ROBOTICSCO., LTD. V-REPシミュレータ • RTコンポーネントとの通信に対応した動 力学シミュレータ V-REP • 使い方はこちら • http://ogata-lab.jp/?p=1264 112
- 113. SUGAR SWEET ROBOTICSCO., LTD. OpenHRI • 音声コミュニケーション用のRTC群 • http://openhri.readthedocs.org/en/ latest/index-ja.html • http://openrtp.sakura.ne.jp/openhri/ • Juliusを使った音声認識 • OpenJTalkを使った発話 113
- 114. SUGAR SWEET ROBOTICSCO., LTD. その他 • RTM on Android • Androidアプリから直接RTCと通信 • Choreonoid • ロボットの振り付け • 動力学シミュレータ機能も 114
- 115. SUGAR SWEET ROBOTICSCO., LTD. OpenRTM-aist入門 その５ 115 株式会社SUGAR SWEET ROBOTICS 代表取締役 早稲田大学基幹理工学部表現工学科 尾形哲也研究室 客員次席研究員 芝浦工業大学SIT研究所 客員研究員 菅 佑樹
- 116. SUGAR SWEET ROBOTICSCO., LTD. RTCをうまく使う • システムを分割する指針 • RTCの粒度をどうするか • 階層的アプローチを紹介 • RTCのインターフェース (データポート，サービ スポート，コンフィグレーション) をどうするか • 設計指針について持論を紹介 • 実際にどう使っているか • 移動ロボットフレームワークを紹介 116
- 117. SUGAR SWEET ROBOTICSCO., LTD. ロボットミドルウェアの 典型的ユースケース • 複数のロボット製品を統合してタスクを行う • 例：危険物除去ロボット • A: サーボコントローラ • B: センサー • C: ロボットアーム統合 • D: ロボットハンド統合 • E: 運動学モジュール • F: ロボット台車 • G: システム統合 • H: 学習アルゴリズムモジュール 117
- 118. SUGAR SWEET ROBOTICSCO., LTD. RTCの粒度決定方法 • 階層的アプローチ 118 ロボット統合 コントローラ ロボット要素 アプリ アルゴリズム
- 119. SUGAR SWEET ROBOTICSCO., LTD. 階層的アプローチ • ロボット要素層（再利用性高い） • ロボットのハードウェア単位 (モータ，センサ，マイク等) • ロボット統合層（ロボット単位での再利用を見込める） • 複数のロボット要素をまとめて一つの要素のようにする層 • 単位や座標系の変換等の統合 • コントローラ層（ロボットの再利用に付随して再利用が可能） • 運動学や起動追従制御など，ロボットハードウェア構成に依存が大きい アルゴリズム • アルゴリズム層（再利用性が高い） • 認識・分類，軌道計画等，ハードウェアへの依存が少ないアルゴリズム • アプリケーション層（ドメインに依存．再利用性は低い） • ドメインに依存したアプリケーション．GUIなど． 119
- 120. SUGAR SWEET ROBOTICSCO., LTD. インターフェースをどうするか • ロボット要素層 • センサやアクチュエータはデータポートが中心 • ハードウェアの設定値をコンフィグレーションに • 初期化はonActivateで，周期処理をonExecute • ロボット統合層（ロボット単位での再利用を見込める） • 要素からのデータを統合して，組み立てたロボットに依存する座標変換などを行う • データポート中心．TimedVelocityやTimedPoseなどの物理量を意識 • 変換をするのみであればコールバックを使うと良い • コントローラ層（ロボットの再利用に付随して再利用が可能） • サービスポートが中心．マニピュレータの共通インターフェースなどを参考に • アルゴリズム層（再利用性が高い） • サービスポートが中心．計算負荷が高く，要求とレスポンスの対応関係が明確 • アプリケーション層（ドメインに依存．再利用性は低い） • アルゴリズムやコントローラを利用するためのサービスポート（要求側）が中心 120
- 121. SUGAR SWEET ROBOTICSCO., LTD. RTM対応 移動ロボット ナビゲーションフレームワーク • 台車型移動ロボットのナビゲーションが目的 – すべてオープンソース • マップ作成 – SLAMのみ • 地図を用いたナビゲーション – レーザーレンジセンサーを用いた位置推定 – 軌道計画 – 軌道追従 • 機能ごとにモジュール化しており，インターフェースを共通化して，入替えが可能 – 例：独自の移動ロボットを適用 – 例：独自のプランニング・パスフォロワーを適用 • 基本操作が可能なGUIを準備 121
- 122. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., LTD. フレームワークの特徴 • 上位のインターフェースはすべてサービスポート • 処理の成功・失敗を返り値で受け取ることがで きる • サービスロボットの上位処理はデータフロー的 ではなく，イベント的 • MobileRobot.idlでインターフェースを定義済み – これをインポートすればコンシューマ側は簡 単に作成できる 123
- 124. SUGAR SWEET ROBOTICSCO., LTD. MobileRobot.idl • 移動ロボットナビゲーションに関わるインターフェースやデータ型 を定義 • OpenRTM-aist同梱のExtendedDataTypes.idlや InterfaceDataTypes.idlを再利用 • 今回のように独自IDLからincludeする場合は，自分のIDLフォ ルダにこれらのIDLをコピーしておく必要がある • 統一されたインターフェース設計思想 – 値渡しでデータを渡し，参照渡しでデータを受け取る．返り値 で処理の成否を受け取る 124
- 125. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., LTD. マップの作成 • ロボットに北陽URG等のレーザースキャナを搭載 • ロボットの移動情報 (オドメトリ) とスキャン情報か らマップを作成 • ロボットの移動はジョイスティックなどで指令を送る 131
- 132. SUGAR SWEET ROBOTICSCO., LTD. マップ作成用フレームワーク (基本的な使い方) 132
- 133. SUGAR SWEET ROBOTICSCO., LTD. 全RTC起動し図のように接続 Mapper_ALL.batで起動 133
- 134. SUGAR SWEET ROBOTICSCO., LTD. コンフィグレーションの設定 • UrgRTCのCOMポートをコンフィグレーション (port_name) で指定 • geometry_x, y, zで，センサの位置＝ロボットの座 標系 (車軸中心) からのオフセットを指定 (単位[m]) 134
- 135. SUGAR SWEET ROBOTICSCO., LTD. 全RTC起動をアクティブ化 135
- 136. SUGAR SWEET ROBOTICSCO., LTD. GUIの利用 • Start Mappingボタンでマッ ピング開始 • NavigationManagerの targetVelocityポートを接続 していれば，右図のGUIジョ イスティックが起動 • ジョイスティック (黄色い円) を上に引っ張ると，ロボット が前進 • 別のジョイスティックRTCを 使うことも可能 136 GUIジョイスティックStartボタン
- 137. SUGAR SWEET ROBOTICSCO., LTD. GUIの利用 • Save Mapでマップを保存 • マップデータは二つのファ イルで保存される – ****.png : グリッド マップのデータ – ****.yaml : マップの原 点やピクセル/長さの 比の情報 137 Saveボタン
- 138. SUGAR SWEET ROBOTICSCO., LTD. マップ作成用フレームワーク (操作とRTシステムとの関係) 138
- 139. SUGAR SWEET ROBOTICSCO., LTD. Start Mapping時 GUI SLAM マッピング開始命令 GUIからSLAMモジュールにマッピング開始を指⽰ 139 *OGMapperインターフェース
- 140. SUGAR SWEET ROBOTICSCO., LTD. Mapping中 スキャンデータ SLAM オドメトリ レンジスキャナのデータとロボットのオドメトリからマップを作成 140 ロボット レンジセンサ
- 141. SUGAR SWEET ROBOTICSCO., LTD. Mapping中の表示 スキャンデータ SLAM 推定自己位置 ロボットの⾃⼰位置とスキャンデータをデータポートから取得 サービスポートで現在のマップデータを取得．重ねて表⽰ 141 現在のマップデータを 要求し取得 レンジセンサ *OGMapperインターフェース
- 142. SUGAR SWEET ROBOTICSCO., LTD. GUIジョイスティックでの操作 GUI SLAM 目標速度指令 targetVelocityをGUIから指⽰ 142 ロボット
- 143. SUGAR SWEET ROBOTICSCO., LTD. Save Map時 SLAM サービスポートで現在のマップデータを取得．保存． 143 現在のマップデータを 要求し取得 GUI *OGMapperインターフェース
- 144. SUGAR SWEET ROBOTICSCO., LTD. マップ作成用フレームワーク (各RTCの解説) 144
- 145. SUGAR SWEET ROBOTICSCO., LTD. 北陽電機 URG • 主なコンフィグレーション – スキャナのロボット座 標系での位置，オフセッ ト [m] • geometry_x • geometry_y • geometry_z – COMポート番号 (COM3 とか) • port_name 145 • レーザーセンサ URGのRTC • 出⼒ポート • range : RangeData • レーザースキャナ出⼒
- 146. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., LTD. NavigationManager • ナビゲーション用GUI • 入力ポート – currentPose : TimedPose2D • ロボットの自己位置 (表示用) – range : RangeData • LiDAR入力 (表示用) – camera : CameraImage • カメラ画像表示 • 出力ポート – targetVelocity : TimedVelocity2D • GUIジョイスティックでの操作時に接続 • サービスポート – mapperService : RTC::OGMapper (Required) • SLAMモジュールを接続．マッピングの 開始・終了・マップ取得 148
- 149. SUGAR SWEET ROBOTICSCO., LTD. ナビゲーション • ロボットに北陽URG等のレーザースキャナを搭載 • ロボットの移動情報 (オドメトリ) とスキャン情報 をマップとマッチングして自己位置を高精度に推定 • マップ情報から軌道計画を自動化 • 計画した軌道への自動的な追従 149
- 150. SUGAR SWEET ROBOTICSCO., LTD. ナビゲーション用フレームワーク (基本的な使い方) 150
- 151. SUGAR SWEET ROBOTICSCO., LTD. 全RTCを起動し図のように接続 Navi_ALL.batで起動 151
- 152. SUGAR SWEET ROBOTICSCO., LTD. コンフィグレーションの設定 • UrgRTCのCOMポートをコン フィグレーション (port_name) で指定 • geometry_x, y, zで，ロボッ トの座標系 (車軸中心) からの オフセットを指定 (単位[m]) • MapServerのマップファイル のファイル名を設定 – ****.yamlを設定すること 152
- 153. SUGAR SWEET ROBOTICSCO., LTD. 全RTCをActivate 153
- 154. SUGAR SWEET ROBOTICSCO., LTD. GUIの利用 • MapServerにマップが読み込まれていれ ば，マップが表示され，ロボットの位置と レンジセンサのデータが表示される • マップ上の目標位置をクリックするとゴー ルを指定できる 154
- 155. SUGAR SWEET ROBOTICSCO., LTD. GUIの利用 • マップ上のゴールを指定すると，ゴー ルの姿勢を選択するダイアログが表 示される • 赤い円内をクリックすると，ゴール 姿勢 (マップ座標系での姿勢) を指 定できるので，OKする • マップ上に指定した姿勢でのゴール が出現する (円の中心が位置，直線 の伸びている方向がロボットのX方 向) 155
- 156. SUGAR SWEET ROBOTICSCO., LTD. GUIの利用 • Plan Pathボタンでパスプランニング • Followボタンでロボットが追従開始 156
- 157. SUGAR SWEET ROBOTICSCO., LTD. ナビゲーション用フレームワーク (操作とRTシステムとの関係) 157
- 158. SUGAR SWEET ROBOTICSCO., LTD. Activate時 (自己位置推定) まず，MapServerにマップを要求して取得 オドメトリとスキャンデータからマップ内での⾃⼰位置を推定 158 ロボット スキャンデータ レンジセンサ オドメトリ マップデータを要求 *OGMapServerインターフェース
- 159. SUGAR SWEET ROBOTICSCO., LTD. Activate時 (表示) GUIはまず，MapServerにマップを要求して取得 推定⾃⼰位置とスキャンデータをマップに重ねて表⽰ 159 ⾃⼰位置推定 スキャンデータ レンジセンサ 推定自己位置 マップデータを要求 GUI *OGMapServerインターフェース
- 160. SUGAR SWEET ROBOTICSCO., LTD. ゴール設定時 GUI内で処理は完結．クリックした位置を 表⽰したマップ上の位置に変換して記録 160 ⾃⼰位置推定 マップデータを要求 GUI *OGMapServerインターフェース
- 161. SUGAR SWEET ROBOTICSCO., LTD. Plan Path時 ロボットの位置をスタート，指定されたゴール，MapServerから 取得したマップ，パスフォローの許容誤差等のパラメータを⽤い てPathPlannerにプランニングを要求し，Pathを取得・表⽰ 161 ⾃⼰位置推定RTC レンジセンサ 推定自己位置 GUI 軌道計画 *PathPlannerインターフェース
- 162. SUGAR SWEET ROBOTICSCO., LTD. Follow時 ⽬標軌道を引数として，軌道追従要求をPathFollowerに要求．成功・失敗を返 り値として得る ロボットの⾃⼰位置と軌道を照らし合わせながら，ロボットの⽬標速度を送信 162 ⾃⼰位置推定RTC 推定自己位置 GUI 軌道追従要求 ロボットへの速度指令 軌道追従RTC *PathFollowerインターフェース
- 163. SUGAR SWEET ROBOTICSCO., LTD. ナビゲーション用フレームワーク (各RTCの説明) 163
- 164. SUGAR SWEET ROBOTICSCO., LTD. 北陽電機 URG • 主なコンフィグレーション – スキャナのロボット座 標系での位置，オフセッ ト • geometry_x • geometry_y • geometry_z – COMポート番号 • port_name 164 • レーザーセンサ URGのRTC • 出⼒ポート • range : RangeData • レーザースキャナ出⼒
- 165. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., 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 ROBOTICSCO., LTD. MapServer • マップデータを管理・出力 • サービスポート – mapServer : MapServer • マップデータの出力 167 • 主なコンフィグレーション • filename • マップデータのファイル名．Yamlファイル を指定すること． • Yamlファイルとpngファイルは同じフォル ダに配置すること • 実⾏ディレクトリからの相対パスか，絶対 パスで指定． • よくわからなければ C:mapmy_may.yamlなどが無難
- 168. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., LTD. PathPlanner_MRPT • パスプランニング (MRPT使用) • サービスポート – pathPlanner : PathPlanner • パスプランニング 169 • 主なコンフィグレーション • 円柱型ロボット(と仮定した場合)の半径 • robotRadius • 軌道追従の位置・姿勢許容差 • pathDistanceTolerance • pathHeadingTolerance • 最終ゴール地点の位置・姿勢許容差 • goalDistanceTolerance • pathHeadingTolerance
- 170. SUGAR SWEET ROBOTICSCO., LTD. SimplePathFollower • Path2D型の経路への追従 • 入力ポート – currentPose : TimedPose2D • 現在位置 • 出力ポート – velocity : TimedVelocity2D • 目標速度 • サービスポート – pathFollower : PathFollower • パス追従命令取得 170 • 主なコンフィグレーション • distanceToTranslationGain • Pathとの位置のズレと並進移動速度のゲイン • distanceToRotationGain • Pathとの位置のズレと回転移動速度のゲイン • directionToTranslationGain • Pathとの向きのズレと並進移動速度のゲイン • directionToRotationGain • Pathとの向きのズレと回転移動速度のゲイン • approachDistanceGain • 最後の終着点との距離と速度のゲイン • approachDirectionGain • 最後の終着点の⽅向と回転速度のゲイン
- 171. SUGAR SWEET ROBOTICSCO., 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 ROBOTICSCO., LTD. SimplePathFollowerアルゴリズム • 最近傍パスがゴールを含む場合は ゴールアプローチモードになる • まずゴール点まで移動 – ゴールとロボットとの距離を ΔX – ゴール・ロボット間の直線と ロボットの向きとの差分をΔH • Vx = K5 * ΔX • Va = K6 * ΔH – ゴール点のdistanceTolerance 内で停止 • ゴール点の姿勢までその場で回転 し，headingToleranceに収まれば ゴール到達 172 m l n ΔX ΔH • 主なコンフィグレーションとの対応 • approachDistanceGain = K5 • 最後の終着点との距離と速度のゲイン • approachDirectionGain = K6 • 最後の終着点の⽅向と回転速度のゲイン
- 173. SUGAR SWEET ROBOTICSCO., LTD. ナビゲーションフレームワーク 応用 • フレームワークのカスタマイズ例 • 独自RTCによるフレームワークの利用 173
- 174. SUGAR SWEET ROBOTICSCO., LTD. ナビゲーション機能の カスタマイズ 174
- 175. SUGAR SWEET ROBOTICSCO., LTD. RTCごとの入れ替え • インターフェースが共通であれば，入れ替 えは容易 175
- 176. SUGAR SWEET ROBOTICSCO., LTD. ロボットRTCの入替え 独⾃ロボットへのナビゲーション機能の実装 176 TimedVelocity2D⼊⼒，TimedPose2D出⼒
- 177. SUGAR SWEET ROBOTICSCO., LTD. 軌道計画RTCの改良 軌道計画アルゴリズムを改良 RRT-CONNECTなどの実装 177 PathPlannerサービスポート (Provided) を実装
- 178. SUGAR SWEET ROBOTICSCO., LTD. 既存RTCの強化 • 外部のRTCと接続するインターフェースが 共通であれば，新たな入出力を追加するこ とは可能 178
- 179. SUGAR SWEET ROBOTICSCO., LTD. 軌道追従RTCの改良案 Urgからのスキャンデータを使って障害物を回避 ロボットの接触センサ情報を使って衝突時の緊急停⽌機能を追加 179 PathFollowerサービスポート (Provided) を実装 ロボットの⾃⼰位置 (TimedPose2D) を受け取って，速度指令 (TimedVelocity2D) を出⼒
- 180. SUGAR SWEET ROBOTICSCO., LTD. RTCの追加による フレームワーク機能強化 • 外部のRTCと接続するインターフェースが 共通であれば，新たな入出力とRTCを追加 することが可能 – フィルタリング – センサーの増設とセンサーフュージョン 180
- 181. SUGAR SWEET ROBOTICSCO., LTD. 自己位置同定の改良 IMUのRTCを使い，カルマンフィルタを実装して，レンジセンサを使わずに⾃⼰位置を出⼒ GPSを使って⾃⼰位置同定を改良 181 TimedVelocity2Dを受け取りロボットを動かし，TimedPose2Dでロボットの位置を送信 KF IMU
- 182. SUGAR SWEET ROBOTICSCO., LTD. 独自ロボットのフレームワーク対応 182
- 183. SUGAR SWEET ROBOTICSCO., LTD. 移動ロボットRTC • 右図のSpurRTCを入れ替える • 最低限度，以下のポートをもっていれば良い • 入力： targetVelocity:TimedVelocity2D • 出力：currentPose:TimedPose2D 183 • YPSpur対応RTコンポーネント • ⼊⼒ポート • targetVelocity : TimedVelocity2D • ⽬標速度 • poseUpdate : TimedPose2D • 現在位置の更新 • 出⼒ポート • currentVelocity : TimedVelocity2D • 現在速度 • currentPose : TimedPose2D • 推定⾃⼰位置 (オドメトリ）
- 184. SUGAR SWEET ROBOTICSCO., LTD. 手順 • RTCBuilderで所望の入出力ポートを持ったRTCの スケルトンを生成する • 既存のライブラリを使う場合は，CMakeLists.txt を変更してロボット用のライブラリをリンクする • CMakeLists.txtの編集方法についてはウェブの 情報や，既存のRTCを参考にすると良い • 後述のAriaRTCなどが参考になると思う • Buildして実行する 184
- 185. SUGAR SWEET ROBOTICSCO., LTD. すでに存在する移動ロボットRTC • SpurRTC • 筑波大学移動ロボット研究室のT-frogプロジェクトの成果Spurライブラリを使ったRTC • http://t-frog.com • https://openspur.org/redmine/projects • T-frogで同じく開発された移動ロボット制御基板を使ったロボットなら利用可能 • https://github.com/sugarsweetrobotics/SpurRTC • KobukiRTC • Yujin Robot社のKobukiで使えるRTC • http://kobuki.yujinrobot.com • TurtleBot2という名前でも販売されている． • https://github.com/rt-net/kobuki_rtc • RoombaRTC • iRobot社のRoombaに対応したRTC．研究用のRoomba Createや，Roomba 500シリー ズなどで動作確認 • https://github.com/sugarsweetrobotics/RoombaRTC 185
- 186. SUGAR SWEET ROBOTICSCO., LTD. すでに存在する移動ロボットRTC • AriaRTC • 米国MobileRobot社の移動ロボットに対応したライブラリ「Aria」を使ったRTC • http://mobilerobots.com • https://github.com/sugarsweetrobotics/AriaRTC • RaspberryPiMouseRTC • 株式会社アールティが販売しているRaspberryPiで動くマイクロマウスで動作する RTC • http://www.rt-shop.jp/index.php? main_page=product_info&cPath=1299_1395&products_id=3234 • https://github.com/rsdlab/RaspberryPiMouseRTC 186
- 187. SUGAR SWEET ROBOTICSCO., LTD. フレームワーク管理RTCの追加 187
- 188. SUGAR SWEET ROBOTICSCO., LTD. 一番やりたいのはココですよね？ 188
- 189. SUGAR SWEET ROBOTICSCO., LTD. 移動ロボットへの指令を変更 • 例えば・・・ – 音声で「○○に行け」と言ったら，ゴー ルを自動的に指定して軌道計画，追従を 行う – 環境センサで状況を判断して自動的に○○ を取ってくるロボット • 上位のサービスの設計をしましょう 189
- 190. SUGAR SWEET ROBOTICSCO., LTD. StringNavigationCommander • 文字列命令からゴール位置を設定して，軌道計画・追従を指令するコンポーネ ント • 入力ポート – command : TimedString • GOTO_Xのように指令文字列を受け取る – currentPose : TimedPose2D • 自己位置を取得 (軌道計画で必要) • サービスポート – mapServer : MapServer (Required) • マップを要求・取得 – pathPlanner : PathPlanner (Required) • 軌道計画を要求 – pathFollower : PathFollower (Required) • 軌道追従を要求 190
- 191. SUGAR SWEET ROBOTICSCO., LTD. StringNavigationManagerを 簡単に作成 • /idlフォルダにIDLファイルをコピー – BasicDataType.idl – InterfaceDataTypes.idl – ExtendedDataTypes.idl – MobileRobot.idl • RTC Builderで新規プロジェクトを作成 – StringNavigationManagerフォルダのRTC.xmlを インポート 191
- 192. SUGAR SWEET ROBOTICSCO., LTD. ポートの設定 • commandデータ入力ポート • currentPoseデータ入力ポート 192
- 193. SUGAR SWEET ROBOTICSCO., LTD. ポートの設定 • mapServerサービスポート – Required • PathPlannerサービスポート – Required 193
- 194. SUGAR SWEET ROBOTICSCO., LTD. ポートの設定 • pathFollowerサービスポート – Required 194
- 195. SUGAR SWEET ROBOTICSCO., LTD. 一旦，通常のビルドを行い 接続を確認 • CMakeしてビルド • ナビゲーション用RTC群を起動して，サー ビスポートの接続を確認しておく 195
- 196. SUGAR SWEET ROBOTICSCO., LTD. 196 RTC::ReturnCode_t StringNavigationCommander::onExecute(RTC::UniqueId ec_id) { if (⾃⼰位置データ受信してたら) { ⾃⼰位置を取得(); } if (⽂字列コマンド受信してたら) { ⽂字列コマンド取得(); if (コマンド == “GOTO_A”) { ゴール = (1.0, 0.0, 1.57); } else (コマンド == “GOTO_B”) { ゴール = (1.0, 1.0, 3.14); } else { return RTC::RTC_OK; // 何もせず終了 } マップ = マップ要求(); 軌道 = 軌道計画要求(マップ, ゴール, ⾃⼰位置, その他のパラメータ); 軌道追従要求(軌道); } return RTC::RTC_OK; } …自己位置取得① …文字列コマンド取得② …ゴール位置設定③ …マップ要求④ …軌道計画要求⑤ …軌道追従要求⑥
- 197. SUGAR SWEET ROBOTICSCO., LTD. 1. 自己位置データ取得 RTC::ReturnCode_t StringNavigationCommander::onExecute(RTC::UniqueId ec_id) { // 現在のロボットの位置を取得 if (m_currentPoseIn.isNew()) { m_currentPoseIn.read(); } … 通常のデータポートの⽅法でデータを取得 197
- 198. SUGAR SWEET ROBOTICSCO., LTD. 2. 文字列コマンド取得 RTC::ReturnCode_t StringNavigationCommander::onExecute(RTC::UniqueId ec_id) { … // 文字列コマンドを取得 if (m_commandIn.isNew()) { m_commandIn.read(); // m_commandにデータが格納される std::string data = m_command.data; // string型に変換 std::cout << "[RTC::StringCommander] Receive Command (" << data << ")" << std::endl; … 通常のデータポートの⽅法でデータを取得 198
- 199. SUGAR SWEET ROBOTICSCO., LTD. 3. ゴール位置設定 RTC::ReturnCode_t StringNavigationCommander::onExecute(RTC::UniqueId ec_id) { … // Goalを設定する Pose2D goal; if (data == "GOTO_A") { // string型は==演算子が使える goal.position.x = 1.0; goal.position.y = 1.0; goal.heading = 1.57; } else if (data == "GOTO_B") { goal.position.x = 1.0; goal.position.y = 0.0; goal.heading = 0.0; } else { // 知らないコマンドはエラーメッセージを表示 std::cout << "[RTC::StringCommander] Error. Unknown Command (" << data << ")" << std::endl; return RTC::RTC_OK; // onExecuteから通常終了 } // 現在地と目的地が定まる std::cout << "[RTC::StringCommander] Current Pose = (" << m_currentPose.data.position.x << ", " << m_currentPose.data.position.y << ", " << m_currentPose.data.heading << ")" << std::endl; std::cout << "[RTC::StringCommander] Goal Pose = (" << goal.position.x << ", " << goal.position.y << ", " << goal.heading << ")" << std::endl; … 取得⽂字列からゴールを⾃動設定 199
- 200. SUGAR SWEET ROBOTICSCO., LTD. 4. マップ要求 RTC::ReturnCode_t StringNavigationCommander::onExecute(RTC::UniqueId ec_id) { … // マップを要求する RTC::OGMap_var outMap; // マップを格納するための変数 RTC::RETURN_VALUE retval = this->m_OGMapServer->requestCurrentBuiltMap(outMap); if (retval == RTC::RETURN_VALUE::RETVAL_OK) { std::cout << "[RTC::StringCommander] Map Request OK." << std::endl; } else { std::cout << "[RTC::StringCommander] Map Request Unknown Error Code =" << retval << std::endl; return RTC::RTC_ERROR; } … サービスポートを使ってマップを取得 out型引数の使い⽅の例 200
- 201. SUGAR SWEET ROBOTICSCO., LTD. 5. 軌道計画 RTC::ReturnCode_t StringNavigationCommander::onExecute(RTC::UniqueId ec_id) { … // 軌道計画の為のパラメータを調整 RTC::PathPlanParameter param; param.map = outMap; // マップ param.currentPose = this->m_currentPose.data; // スタート param.targetPose = goal; // ゴール param.distanceTolerance = 1.0; // 軌道からの位置のずれの許容差 param.headingTolerance = 1.0; // 軌道からの角度のずれの許容差 param.maxSpeed.vx = 5.0; param.maxSpeed.vy = 0.0; param.maxSpeed.va = 1.0; // 最大速度 param.timeLimit.sec = 9999; param.timeLimit.nsec = 0; // 許容時間 RTC::Path2D_var outPath; // 軌道を格納するための変数 retval = m_pathPlanner->planPath(param, outPath); if (retval == RTC::RETURN_VALUE::RETVAL_OK) { // 成功 std::cout << "[RTC::StringCommander] Path Plan OK." << std::endl; } else if (retval == RTC::RETURN_VALUE::RETVAL_NOT_FOUND) { // 見つからない (通常終了) std::cout << "[RTC::StringCommander] Path Plan No Path Found" << std::endl; return RTC::RTC_OK; } else { std::cout << "[RTC::StringCommander] Path Plan Unknown Error Code =" << retval << std::endl; return RTC::RTC_ERROR; } … サービスポートを使って軌道を取得 (out型引数) 201
- 202. SUGAR SWEET ROBOTICSCO., LTD. 6. 軌道追従 RTC::ReturnCode_t StringNavigationCommander::onExecute(RTC::UniqueId ec_id) { … // 軌道追従を要求．終了するまでfollowPathはブロックする retval = m_pathFollower->followPath(outPath); if (retval == RTC::RETURN_VALUE::RETVAL_OK) { // 成功 std::cout << "[RTC::StringCommander] Follow OK." << std::endl; } else { // 失敗も通常終了 std::cout << "[RTC::StringCommander] Follow Unknown Error Code =" << retval << std::endl; return RTC::RTC_OK; } } // if (m_commandIn.isNew()) に対応 followPathで起動追従．追従終了までブロック 202
- 203. SUGAR SWEET ROBOTICSCO., LTD. 203 RTC::ReturnCode_t StringNavigationCommander::onExecute(RTC::UniqueId ec_id) { // 現在のロボットの位置を取得 if (m_currentPoseIn.isNew()) { m_currentPoseIn.read(); // m_currentPoseにデータが格納される } // 文字列コマンドを取得 if (m_commandIn.isNew()) { m_commandIn.read(); // m_commandにデータが格納される std::string data = m_command.data; // string型に変換 std::cout << "[RTC::StringCommander] Receive Command (" << data << ")" << std::endl; // Goalを設定する Pose2D goal; if (data == "GOTO_A") { // string型は==演算子が使える goal.position.x = 1.0; goal.position.y = 1.0; goal.heading = 1.57; } else if (data == "GOTO_B") { goal.position.x = 1.0; goal.position.y = 0.0; goal.heading = 0.0; } else { // 知らないコマンドはエラーメッセージを表示 std::cout << "[RTC::StringCommander] Error. Unknown Command (" << data << ")" << std::endl; return RTC::RTC_OK; // onExecuteから通常終了 } // 現在地と目的地が定まる std::cout << "[RTC::StringCommander] Current Pose = (" << m_currentPose.data.position.x << ", " << m_currentPose.data.position.y << ", " << m_currentPose.data.heading << ")" << std::endl; std::cout << "[RTC::StringCommander] Goal Pose = (" << goal.position.x << ", " << goal.position.y << ", " << goal.heading << ")" << std::endl; // マップを要求する RTC::OGMap_var outMap; // マップを格納するための変数 RTC::RETURN_VALUE retval = this->m_OGMapServer->requestCurrentBuiltMap(outMap); if (retval == RTC::RETURN_VALUE::RETVAL_OK) { std::cout << "[RTC::StringCommander] Map Request OK." << std::endl; } else { std::cout << "[RTC::StringCommander] Map Request Unknown Error Code =" << retval << std::endl; return RTC::RTC_ERROR; }
- 204. SUGAR SWEET ROBOTICSCO., LTD. 204 // 軌道計画の為のパラメータを調整 RTC::PathPlanParameter param; param.map = outMap; // マップ param.currentPose = this->m_currentPose.data; // スタート param.targetPose = goal; // ゴール param.distanceTolerance = 1.0; // 軌道からの位置のずれの許容差 param.headingTolerance = 1.0; // 軌道からの角度のずれの許容差 param.maxSpeed.vx = 5.0; param.maxSpeed.vy = 0.0; param.maxSpeed.va = 1.0; // 最大速度 param.timeLimit.sec = 9999; param.timeLimit.nsec = 0; // 許容時間 RTC::Path2D_var outPath; // 軌道を格納するための変数 retval = m_pathPlanner->planPath(param, outPath); if (retval == RTC::RETURN_VALUE::RETVAL_OK) { // 成功 std::cout << "[RTC::StringCommander] Path Plan OK." << std::endl; } else if (retval == RTC::RETURN_VALUE::RETVAL_NOT_FOUND) { // 見つからない (通常終了) std::cout << "[RTC::StringCommander] Path Plan No Path Found" << std::endl; return RTC::RTC_OK; } else { std::cout << "[RTC::StringCommander] Path Plan Unknown Error Code =" << retval << std::endl; return RTC::RTC_ERROR; } // 軌道追従を要求．終了するまでfollowPathはブロックする retval = m_pathFollower->followPath(outPath); if (retval == RTC::RETURN_VALUE::RETVAL_OK) { // 成功 std::cout << "[RTC::StringCommander] Follow OK." << std::endl; } else { // 失敗も通常終了 std::cout << "[RTC::StringCommander] Follow Unknown Error Code =" << retval << std::endl; return RTC::RTC_OK; } } // if (m_commandIn.isNew()) に対応 return RTC::RTC_OK; }
- 205. SUGAR SWEET ROBOTICSCO., LTD. さらなる改良案 • ゴール名とゴール位置・姿勢を外部ファイルにし て読み込む – yamlファイルなどが使い易い – ファイル名をコンフィグレーションにする • 文字列を音声認識で出力する (OpenHRIを使う) 205 GOTO_A : [1.0, 1.0, 0.0], GOTO_B : [1.0, 0.0, 1.57], GOTO_C : [1.0, -1.0, 0.0]