チュートリアル(Raspberry Pi Mouse、RTM講習会)

チュートリアル(RTM講習会、Windows、第2部)

はじめに

このページではシミュレーター上の Raspberry Pi マウスを操作するためのコンポーネントの作成手順を説明します。

/ja/node/6198

資料のダウンロード

まずは資料をダウンロードしてください。

ZIPファイルは Lhaplus 等で展開してください。

インターネットに接続できない環境で講習会を実施している場合がありますので、その場合は配布のUSBメモリーに入れてあります。

シミュレーター

シミュレーターは Open Dynamics Engine(ODE) という物理演算エンジンと ODE 付属の描画ライブラリ(drawstuff)を使用して開発しています。 OpenGL が動作すれば動くので、大抵の環境で動作するはずです。

以下の Raspberry Piマウス というロボットのシミュレーションができます。

/jp/node/6005

シミュレーター上の Raspberry Pi マウスの動力学計算、接触応答だけではなく、距離センサーのデータも現実のロボットに近い値を再現するようにしています。

Raspberry Piマウスの仕様

Raspberry Piマウスはアールティが販売している独立二輪駆動型の移動ロボットです。

/ja/node/6550

ラズパイマウスの仕様
CPU Raspberry Pi 2 Model B
モーター ステッピングモーターST-42BYG020 2個
モータードライバー SLA7070MRPT 2個
距離センサー 赤色LED+フォトトランジスタ(ST-1K3) 4個
モニター用赤色LED 4個
ブザー 1個
スイッチ 3個
バッテリー LiPo3セル(11.1V)1000mAh 1個

作成する RTコンポーネント

  • RobotController コンポーネント

RaspberryPiMouseSimulator コンポーネントと接続してシミュレーター上のロボットを操作するためのコンポーネントです。

RobotController コンポーネントの作成

GUI(スライダー)によりシミュレーター上のロボットの操作を行い、センサー値が一定以上の時には自動的に停止するコンポーネントの作成を行います。

/ja/node/6310

作成手順

作成手順は以下の通りです。

  • 開発環境の確認
  • コンポーネントの仕様を決める
  • RTC Builderによるソースコードのひな形の作成
  • ソースコードの編集
  • コンポーネントの動作確認

開発環境の確認

以下の環境を想定しています。

コンポーネントの仕様

RobotController は目標速度を出力するアウトポート、センサー値を入力するインポート、目標速度や停止するセンサー値を設定するコンフィギュレーションパラメーターを持っています。

コンポーネント名称 RobotController
InPort
ポート名 in
TimedShortSeq
説明 センサー値
OutPort
ポート名 out
TimedVelocity2D
説明 目標速度
Configuration
パラメーター名 speed_x
double
デフォルト値 0.0
制約 -1.5<x<1.5
Widget slider
Step 0.01
説明 直進速度の設定
Configuration
パラメーター名 speed_r
double
デフォルト値 0.0
制約 -2.0<x<2.0
Widget slider
Step 0.01
説明 回転速度の設定
Configuration
パラメーター名 stop_d
int
デフォルト値 30
説明 停止するセンサー値の設定

TimedVelocity2D 型について

2次元平面上の移動ロボットの移動速度を格納するデータ型である TimedVelocity2D 型を使用します。

     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;
     };
 
 
     struct TimedVelocity2D
     {
           Time tm;
           Velocity2D data;
     };

このデータ型にはX軸方向の速度vx、Y軸方向の速度vy、Z軸周りの回転速度vaが格納できます。

vxvyvaはロボット中心座標系での速度を表しています。


/ja/node/6042

vxはX方向の速度、vyはY方向の速度、vaはZ軸周りの角速度です。

Raspberry Pi マウスのように2個の車輪が左右に取り付けられているロボットの場合、横滑りしないと仮定するとvyは0になります。

直進速度vx、回転速度vaを指定することでロボットの操作を行います。

距離センサーのデータについて

Raspberry Pi マウスの距離センサーのデータは物体との距離が近づくほど大きな値を出力するようになっています。


rpm14.png

デバイスファイルから取得した数値 実際の距離[m]
1394 0.01
792 0.02
525 0.03
373 0.04
299 0.05
260 0.06
222 0.07
181 0.08
135 0.09
100 0.10
81 0.15
36 0.20
17 0.25
16 0.30

シミュレーターではこの値を再現して出力しています。 RobotController コンポーネントではこの値が一定以上の時に自動的に停止する処理を実装します。

RobotController コンポーネントのひな型の生成

RobotController コンポーネントの雛型の生成は、RTCBuilder を用いて行います。

RTCBuilder の起動

Eclipse では、各種作業を行うフォルダーを「ワークスペース」(Work Space)とよび、原則としてすべての生成物はこのフォルダーの下に保存されます。 ワークスペースはアクセスできるフォルダーであれば、どこに作っても構いませんが、このチュートリアルでは以下のワークスペースを仮定します。

  • C:\workspace

まずは Eclipse を起動します。 Windows 8.1の場合は「スタート」>「アプリビュー(右下矢印)」>「OpenRTM-aist 1.2.0」>「OpenRTP」をクリックすると起動できます。

最初にワークスペースの場所を尋ねられますので、上記のワークスペースを指定してください。

/ja/node/6550

すると、以下のようなWelcomeページが表示されます。


/ja/node/6550
Eclipse の初期起動時の画面

Welcomeページはいまは必要ないので左上の「×」ボタンをクリックして閉じてください。

右上の [Open Perspective] ボタンをクリックしてください。

/ja/node/6026
パースペクティブの切り替え

「RTC Builder」を選択することで、RTCBuilder が起動します。メニューバーに「カナヅチとRT」の RTCBuilder のアイコンが表示されます。

/ja/node/6550
パースペクティブの選択

新規プロジェクトの作成

RobotController コンポーネントを作成するために、RTC Builder で新規プロジェクトを作成する必要があります。

左上の [Open New RTCBuilder Editor] のアイコンをクリックしてください。

/ja/node/6057
RTC Builder 用プロジェクトの作成

「プロジェクト名」欄に作成するプロジェクト名 (ここでは RobotController) を入力して [終了] ボタンをクリックします。

/ja/node/6310

指定した名称のプロジェクトが生成され、パッケージエクスプローラ内に追加されます。

/ja/node/6310

生成したプロジェクト内には、デフォルト値が設定された RTC プロファイル XML(RTC.xml) が自動的に生成されます。

RTC プロファイルエディタの起動

RTC.xml が生成された時点で、このプロジェクトに関連付けられているワークスペースとして RTCBuilder のエディタが開くはずです。 もし起動しない場合はパッケージエクスプローラーの RTC.xml をダブルクリックしてください。

/ja/node/6026

プロファイル情報入力とコードの生成

まず、いちばん左の「基本」タブを選択し、基本情報を入力します。先ほど決めた RobotController コンポーネントの仕様(名前)の他に、概要やバージョン等を入力してください。 ラベルが赤字の項目は必須項目です。その他はデフォルトで構いません。

  • モジュール名: RobotController
  • モジュール概要: 任意(Robot Controller component)
  • バージョン: 任意(1.0.0)
  • ベンダ名: 任意
  • モジュールカテゴリ: 任意(Controller)


/ja/node/6310
基本情報の入力


次に、「アクティビティ」タブを選択し、使用するアクションコールバックを指定します。

RobotController コンポーネントでは、onActivated()、onDeactivated()、onExecute() コールバックを使用します。下図のように①の onAtivated をクリック後に②のラジオボタンにて [ON] にチェックを入れます。 onDeactivated、onExecute についても同様の手順を行います。


/ja/node/6310
アクティビティコールバックの選択


さらに、「データポート」タブを選択し、データポートの情報を入力します。 先ほど決めた仕様を元に以下のように入力します。なお、変数名や表示位置はオプションで、そのままで結構です。


  • InPort Profile:
    • ポート名: in
    • データ型: TimedShortSeq

  • OutPort Profile:
    • ポート名: out
    • データ型: TimedVelocity2D


/ja/node/6310
データポート情報の入力


次に、「コンフィギュレーション」タブを選択し、先ほど決めた仕様を元に、Configuration の情報を入力します。 制約条件および Widget とは、RTSystemEditor でコンポーネントのコンフィギュレーションパラメーターを表示する際に、スライダー、スピンボタン、ラジオボタンなど、GUI で値の変更を行うためのものです。

直進速度 speed_x、回転速度 speed_r はスライダーのより操作できるようにします。


  • speed_x
    • 名称: speed_x
    • データ型: double
    • デフォルト値: 0.0
    • 制約条件: -1.5<x<1.5
    • Widget: slider
    • Step: 0.01
  • speed_r
    • 名称: speed_r
    • データ型: double
    • デフォルト値: 0.0
    • 制約条件: -2.0<x<2.0
    • Widget: slider
    • Step: 0.01
  • stop_d
    • 名称: stop_d
    • データ型: int
    • デフォルト値: 30
    • Widget: text


/ja/node/6310
コンフィグレーション情報の入力


次に、「言語・環境」タブを選択し、プログラミング言語を選択します。 ここでは、C++(言語)を選択します。なお、言語・環境はデフォルト等が設定されておらず、指定し忘れるとコード生成時にエラーになりますので、必ず言語の指定を行うようにしてください。

/ja/node/6310
プログラミング言語の選択


最後に、「基本」タブにあ [コード生成] ボタンをクリックし、コンポーネントの雛型を生成します。


/ja/node/6310
雛型の生成(Generate)


プロジェクトを右クリックして、「表示方法」→「システム・エクスプローラー」を選択するとワークスペースをエクスプローラーで開くことができます。

/ja/node/6550

CMake によるビルドに必要なファイルの生成

RTC Builder で生成したコードの中には CMake でビルドに必要な各種ファイルを生成するための CMakeLists.txt が含まれています。 CMake を利用することにより CMakeLists.txt からVisual Studio のプロジェクトファイル、ソリューションファイル、もしくは Makefile 等を自動生成できます。

CMake(cmake-gui) の操作

CMake を利用してビルド環境の Configure を行います。 まずは CMake(cmake-gui) を起動してください。「スタート」>「アプリビュー(右下矢印)」>「CMake 3.11.2」>「CMake (cmake-gui)」をクリックすると起動できます。

/ja/node/6310
CMake GUI の起動とディレクトリーの指定

画面上部に以下のようなテキストボックスがありますので、それぞれソースコードの場所 (CMakeList.txtがある場所) と、ビルドディレクトリーを指定します。

  • Where is the soruce code
  • Where to build the binaries

ソースコードの場所は RobotController コンポーネントのソースが生成された場所で CMakeList.txt が存在するディレクトリーです。 デフォルトでは <ワークスペースディレクトリー>/RobotController になります。

このディレクトリーはエクスプローラから cmake-gui にドラックアンドドロップすると手入力しなくても設定されます。

ビルドディレクトリーとは、ビルドするためのプロジェクトファイルやオブジェクトファイル、バイナリを格納する場所のことです。 場所は任意ですが、この場合 <ワークスペースディレクトリー>/RobotController/build のように分かりやすい名前をつけた RobotController のサブディレクトリーを指定することをお勧めします。

Where is the soruce code C:\workspace\RobotController
Where to build the binaries C:\workspace\RobotController\build

指定したら、下の [Configure] ボタンをクリックします。すると下図のようなダイアログが表示されますので、生成したいプロジェクトの種類を指定します。 今回は Visual Studio 15 2017 とします。Visual Studio 2013や Visual Studio 2019を利用している方はそれぞれ変更してください。 またプラットフォームにはx64を設定します。32bit版をインストールしている場合はWin32を選択してください。

/ja/node/6310
生成するプロジェクトの種類の指定

ダイアログで [Finish] ボタンをクリックすると Configure が始まります。 問題がなければ下部のログウインドウに「Configuring done」と出力されますので、続けて [Generate] ボタンをクリックします。 「Generating done」と出ればプロジェクトファイル・ソリューションファイル等の出力が完了します。

なお、CMake は Configure の段階でキャッシュファイルを生成しますので、トラブルなどで設定を変更したり環境を変更した場合は [File] > [Delete Cache] でキャッシュを削除して Configure からやり直してください。

ヘッダ、ソースの編集

次に先ほど指定した build ディレクトリーの中の RobotController.sln をダブルクリックして Visual Studioを起動します。

※cmake-gui の新しいバージョンでは cmake-gui 上のボタンをクリックすることで起動できます。


/ja/node/6310

ヘッダ (include/RobotController/RobotController.h) およびソースコード (src/RobotController.cpp) をそれぞれ編集します。 Visual Studio のソリューションエクスプローラから RobotController.h、RobotController.cpp をクリックすることで編集画面が開きます。
/ja/node/6550

アクティビティ処理の実装

RobotController コンポーネントでは、コンフィギュレーションパラメーター(speed_x、speed_y)をスライダーで操作しその値を目標速度としてアウトポート(out)から出力します。 インポート(in) から入力された値を変数に格納して、その値が一定以上の場合は停止するようにします。


onActivated()、onExecute()、onDeactivated() での処理内容を下図に示します。

/ja/node/6310
アクティビティ処理の概要


ヘッダファイル (RobotController.h) の編集

センサー値を一時的に格納する変数 sensor_data を宣言します。

   private:
      int sensor_data[4];    //センサー値を一時格納する変数

ソースファイル (RobotController.cpp) の編集

下記のように、onActivated()、onDeactivated()、onExecute() を実装します。

 RTC::ReturnCode_t RobotController::onActivated(RTC::UniqueId ec_id)
 {
         //センサー値初期化
         for (int i = 0; i < 4; i++)
         {
                 sensor_data[i] = 0;
         }
 
         return RTC::RTC_OK;
 }

 RTC::ReturnCode_t RobotController::onDeactivated(RTC::UniqueId ec_id)
 {
         //ロボットを停止する
         m_out.data.vx = 0;
         m_out.data.va = 0;
         m_outOut.write();
 
         return RTC::RTC_OK;
 }

 RTC::ReturnCode_t RobotController::onExecute(RTC::UniqueId ec_id)
 {
         //入力データの存在確認
         if (m_inIn.isNew())
         {
             //入力データ読み込み
             m_inIn.read();
             for (int i = 0; i < m_in.data.length(); i++)
             {
                 //入力データ格納
                 if (i < 4)
                 {
                     sensor_data[i] = m_in.data[i];
                 }
             }
         }
 
         //前進するときのみ停止するかを判定
         if (m_speed_x > 0)
         {
             for (int i = 0; i < 4; i++)
             {
                 //センサー値が設定値以上か判定
                 if (sensor_data[i] > m_stop_d)
                 {
                         //センサー値が設定値以上の場合は停止
                         m_out.data.vx = 0;
                         m_out.data.va = 0;
                         m_outOut.write();
                         return RTC::RTC_OK;
                 }
             }
         }
         //設定値以上の値のセンサーが無い場合はコンフィギュレーションパラメーターの値で操作
         m_out.data.vx = m_speed_x;
         m_out.data.va = m_speed_r;
         m_outOut.write();
           return RTC::RTC_OK;
 }

Visual Studio によるビルド

ビルドの実行

Visual Studioの [ビルド] >「ソリューションのビルド」を選択してビルドを行います。


/ja/node/6026
ビルドの実行


RobotController コンポーネントの動作確認

作成した RobotController をシミュレーターコンポーネントと接続して動作確認を行います。

NameService の起動

コンポーネントの参照を登録するためのネームサービスを起動します。


RT System Editorのネームサービス起動ボタンを押すと起動します。

/ja/node/6550

&color(red){※ 「Start Naming Service」をクリックしても omniNames が起動されない場合は、フルコンピュータ名が14文字以内に設定されているかを確認してください。

RobotController コンポーネントの起動

RobotController コンポーネントを起動します。

RobotController\build\src\Debug(もしくは、Release)フォルダーの RobotControllerComp.exe ファイルを実行してください。

シミュレーターコンポーネントの起動

このコンポーネントは先ほどダウンロードしたファイル(RTM_Tutorial_2018.zip)を展開したフォルダーの EXE/RaspberryPiMouseSimulatorComp.exe を実行すると起動します。

コンポーネントの接続

下図のように、RTSystemEditor にて RobotController コンポーネント、RaspberryPiMouseSimulator コンポーネントを接続します。

/ja/node/6310
コンポーネントの接続

コンポーネントのActivate

RTSystemEditor の上部にあります [All Activate] というアイコンをクリックし、全てのコンポーネントをアクティブ化します。 正常にアクティベートされた場合、下図のように黄緑色でコンポーネントが表示されます。


/ja/node/6550
コンポーネントのアクティブ化


動作確認

下図のようにコンフィギュレーションビューの [編集] ボタンからコンフィギュレーションを変更することができます。


/ja/node/6310

スライダーを操作してシミュレーター上の [Raspberry Pi] マウスの操作ができるかを確認してください。


/ja/node/6310
コンフィギュレーションパラメーターの変更


実機での動作確認

講習会で Raspberry Pi マウス実機を用意している場合は実機での動作確認が可能ですので、時間に余裕がある人は試してみてください。

手順は以下の通りです。

  • Raspberry Pi マウスの電源を投入する
  • Raspberry Pi マウスのアクセスポイントに接続
  • ポートの接続
  • コンポーネントのアクティブ化

電源を投入する

Raspberry PiマウスにはRaspberry Piの電源スイッチとモーターの電源スイッチの2つがあります。


rpm8.png


内側の電源スイッチをオンにするとRaspberry Piが起動します。


rpm9.png


電源を切る場合

Raspberry Piの電源を切る場合は、電源スイッチから直接オフにはしないようにしてください。 3つ並んだボタンの中央のボタンを数秒押すとシャットダウンが始まります。 10秒程度でRaspbianのシャットダウンが終了するため、その後に電源スイッチをオフにしてください。


/ja/node/6042

アクセスポイントに接続

アクセスポイントへの接続方法は以下のページを参考にしてください。

SSID、パスワードは Rasoberry Pi マウスに貼り付けたシールに記載してあります。

まず右下のネットワークアイコンをクリックしてください。


/ja/node/6042

次に一覧から raspberrypi_*** を選択してください。


/ja/node/6042

パスワードを入力してください。


/ja/node/6042

※ネットワークが切り替わった場合にネームサーバーへのコンポーネントの登録やポートの接続が失敗する場合があるのでネームサーバ、コンポーネントを一旦全て終了してください。 ネットワーク切り替え後に起動した場合には問題ないので、終了させる必要はありません。

RT System Editor上でネームサーバーを再起動するには「ネームサービスを起動」ボタンを再度クリックします。

/ja/node/6550

ネームサーバー追加

続いてRTシステムエディタの [ネームサーバー追加] ボタンで 192.168.11.1 を追加してください。


tutorial_raspimouse0.png tutorial_raspimouse1.png



すると以下の3つの RTC が見えるようになります。

/ja/node/6550

RaspberryPiMouseRTC は名城大学のロボットシステムデザイン研究室で開発されているラズパイマウス制御用の RTコンポーネントです。

ポートの接続

RTシステムエディタで RaspberryPiMouseRTC、RobotController コンポーネントを以下のように接続します。

/ja/node/6310

モーターの電源を投入する

動作の前に、モーターの電源スイッチをオンにしてください。 モーターの電源はこまめに切るようにしてください。


rpm10.png


アクティブ化

そして RTC をアクティブ化すると Raspberry Pi マウスの操作ができるようになります。

チュートリアル(RTM講習会、Ubuntu、第2部)

はじめに

このページではシミュレーター上の Raspberry Pi マウスを操作するためのコンポーネントの作成手順を説明します。

/jp/node/6198

資料のダウンロード

まずは資料をダウンロードしてください。

 git clone https://github.com/OpenRTM/RTM_Tutorial

インターネットに接続できない環境で講習会を実施している場合がありますので、その場合は配布のUSBメモリーに入れてあります。

シミュレーター

シミュレーターは Open Dynamics Engine(ODE) という物理演算エンジンと ODE 付属の描画ライブラリ(drawstuff)を使用して開発しています。 OpenGL が動作すれば動くので、大抵の環境で動作するはずです。

以下の Raspberry Piマウス というロボットのシミュレーションができます。

/jp/node/6005

シミュレーター上の Raspberry Pi マウスの動力学計算、接触応答だけではなく、距離センサーのデータも現実のロボットに近い値を再現するようにしています。

Raspberry Piマウスの仕様

Raspberry Piマウスはアールティが販売している独立二輪駆動型の移動ロボットです。

/ja/node/6550

ラズパイマウスの仕様
CPU Raspberry Pi 2 Model B
モーター ステッピングモーターST-42BYG020 2個
モータードライバー SLA7070MRPT 2個
距離センサー 赤色LED+フォトトランジスタ(ST-1K3) 4個
モニター用赤色LED 4個
ブザー 1個
スイッチ 3個
バッテリー LiPo3セル(11.1V)1000mAh 1個

作成する RTコンポーネント

  • RobotController コンポーネント

RaspberryPiMouseSimulator コンポーネントと接続してシミュレーター上のロボットを操作するためのコンポーネントです。

RobotController コンポーネントの作成

GUI(スライダー)によりシミュレーター上のロボットの操作を行い、センサー値が一定以上の時には自動的に停止するコンポーネントの作成を行います。

/ja/node/6310

作成手順

作成手順は以下の通りです。

  • 開発環境の確認
  • コンポーネントの仕様を決める
  • RTC Builder によるソースコードのひな形の作成
  • ソースコードの編集
  • コンポーネントの動作確認

動作環境・開発環境

Linux (ここでは Ubuntu 18.04 を仮定) 上に開発環境を構築します。

依存ライブラリのインストール

 sudo apt-get install gcc g++
 sudo apt-get install libomniorb4-dev omniidl omniorb-nameserver
 sudo apt-get install python-omniorb-omg omniidl-python
 sudo apt-get install cmake
 sudo apt-get install doxygen
 sudo apt-get install openjdk-8-jdk

 #Ubuntu 18.04の場合はjava8に切り替え
 sudo update-alternatives --config java

OpenRTM-aistのインストール

ubuntu 18.04 (64bit) の場合

 #C++版のインストール
 wget https://github.com/OpenRTM/OpenRTM-aist/releases/download/v1.2.0/OpenRTM-aist_1.2.0_ubuntu18.04_amd64_package.tar.gz
 tar xf OpenRTM-aist_1.2.0_ubuntu18.04_amd64_package.tar.gz
 cd OpenRTM-aist_1.2.0_ubuntu18.04_amd64_package
 sudo sh install-openrtm-deb-packages.sh
 cd ..

 #Python版のインストール
 wget https://github.com/OpenRTM/OpenRTM-aist-Python/releases/download/v1.2.0/OpenRTM-aist-Python_1.2.0_ubuntu18.04_amd64_package.tar.gz
 tar xf OpenRTM-aist-Python_1.2.0_ubuntu18.04_amd64_package.tar.gz
 cd OpenRTM-aist-Python_1.2.0_ubuntu18.04_amd64_package
 sudo sh install-openrtm-deb-packages.sh
 cd ..

 #RTSystemEditor/RTCBuilderのインストール
 wget https://github.com/OpenRTM/OpenRTP-aist/releases/download/v1.2.0/OpenRTP-aist_1.2.0_ubuntu18.04_amd64_package.tar.gz
 tar xf OpenRTP-aist_1.2.0_ubuntu18.04_amd64_package.tar.gz
 cd OpenRTP-aist_1.2.0_ubuntu18.04_amd64_package
 sudo sh install-openrtm-deb-packages.sh
 cd ..

eclipse起動後、RTSystemEditor でネームサーバに接続できない場合があります。その場合、/etc/hosts の localhost の行に自ホスト名を追記してください。

 $ hostname
 ubuntu1804 ← ホスト名は ubuntu1804
 $ sudo vi /etc/hosts

 127.0.0.1       localhost
 を以下のように変更
 127.0.0.1       localhost ubuntu1804

CMake のインストール

 $ sudo apt-get install cmake cmake-gui

cmake-guiのインストール

 $ sudo apt-get install cmake-qt-gui

Code::Blocks のインストール

Code::Blocks は C/C++ に対応した統合開発環境です。 以下のコマンドでインストールできます。

 $ sudo apt-get install codeblocks

最新版を入手したい場合は以下のコマンドを入力します。

 $ sudo add-apt-repository ppa:damien-moore/codeblocks-stable
 $ sudo apt-get update
 $ sudo apt-get install codeblocks

Premakeのインストール

ODEのビルドに必要です。

 $ sudo apt-get install premake4 freeglut3-dev

RaspberryPiMouseSimulator コンポーネント

シミュレーターコンポーネントについては手動でビルドを行います。 以下のコマンドを入力してください。

 $ wget https://raw.githubusercontent.com/OpenRTM/RTM_Tutorial_ROBOMECH2019/master/script/install_raspimouse_simulator.sh
 $ sudo sh install_raspimouse_simulator.sh

インターネットに接続できない環境で講習会を実施している場合がありますので、その場合は配布の USBメモリー内のスクリプトを起動してください。

 $ sudo sh install_raspimouse_simulator_usb.sh

コンポーネントの仕様

RobotController は目標速度を出力するアウトポート、センサー値を入力するインポート、目標速度や停止するセンサー値を設定するコンフィギュレーションパラメーターを持っています。

コンポーネント名称 RobotController
InPort
ポート名 in
TimedShortSeq
説明 センサー値
OutPort
ポート名 out
TimedVelocity2D
説明 目標速度
Configuration
パラメーター名 speed_x
double
デフォルト値 0.0
制約 -1.5<x<1.5
Widget slider
Step 0.01
説明 直進速度の設定
Configuration
パラメーター名 speed_r
double
デフォルト値 0.0
制約 -2.0<x<2.0
Widget slider
Step 0.01
説明 回転速度の設定
Configuration
パラメーター名 stop_d
int
デフォルト値 30
説明 停止するセンサー値の設定

TimedVelocity2D 型について

2次元平面上の移動ロボットの移動速度を格納するデータ型である TimedVelocity2D 型を使用します。

     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;
     };
 
 
     struct TimedVelocity2D
     {
         Time tm;
         Velocity2D data;
     };

このデータ型にはX軸方向の速度vx、Y軸方向の速度vy、Z軸周りの回転速度vaが格納できます。

vxvyvaはロボット中心座標系での速度を表しています。


/ja/node/6042

vxはX方向の速度、vyはY方向の速度、vaはZ軸周りの角速度です。

Raspberry Piマウスのように2個の車輪が左右に取り付けられているロボットの場合、横滑りしないと仮定するとvyは0になります。

直進速度vx、回転速度vaを指定することでロボットの操作を行います。

距離センサーのデータについて

Raspberry Pi マウスの距離センサーのデータは物体との距離が近づくほど大きな値を出力するようになっています。


rpm14.png

デバイスファイルから取得した数値 実際の距離[m]
1394 0.01
792 0.02
525 0.03
373 0.04
299 0.05
260 0.06
222 0.07
181 0.08
135 0.09
100 0.10
81 0.15
36 0.20
17 0.25
16 0.30

シミュレーターではこの値を再現して出力しています。 RobotController コンポーネントではこの値が一定以上の時に自動的に停止する処理を実装します。

RobotController コンポーネントのひな型の生成

RobotController コンポーネントの雛型の生成は、RTCBuilder を用いて行います。

RTCBuilder の起動

Eclipse では、各種作業を行うフォルダーを「ワークスペース」(Work Space)とよび、原則としてすべての生成物はこのフォルダーの下に保存されます。 ワークスペースはアクセスできるフォルダーであれば、どこに作っても構いませんが、このチュートリアルでは以下のワークスペースを仮定します。

  • /home/ユーザー名/workspace

まずは Eclipse を起動します。 OpenRTP を展開したディレクトリーに移動して以下のコマンドを入力します。

 $ ./openrtp

最初にワークスペースの場所を尋ねられますので、上記のワークスペースを指定してください。

/ja/node/6058

すると、以下のような Welcome ページが表示されます。


/ja/node/6026
Eclipse の初期起動時の画面

Welcome ページはいまは必要ないので左上の「×」ボタンをクリックして閉じてください。

右上の [Open Perspective] ボタンをクリックしてください。

/ja/node/6026
パースペクティブの切り替え

「RTC Builder」を選択することで、RTCBuilderが起動します。メニューバーに「カナヅチとRT」の RTCBuilder のアイコンが現れます。

/ja/node/6026
パースペクティブの選択

新規プロジェクトの作成

RobotController コンポーネントを作成するために、RTC Builder で新規プロジェクトを作成する必要があります。

左上の [Open New RTCBuilder Editor] のアイコンをクリックしてください。

/ja/node/6057
RTC Builder 用プロジェクトの作成

「プロジェクト名」欄に作成するプロジェクト名 (ここでは RobotController) を入力して [終了] をクリックします。

/ja/node/6310

指定した名称のプロジェクトが生成され、パッケージエクスプローラ内に追加されます。

/ja/node/6310

生成したプロジェクト内には、デフォルト値が設定された RTC プロファイル XML(RTC.xml) が自動的に生成されます。

RTC プロファイルエディタの起動

RTC.xmlが生成された時点で、このプロジェクトに関連付けられているワークスペースとして RTCBuilder のエディタが開くはずです。 もし起動しない場合はパッケージエクスプローラーの RTC.xml をダブルクリックしてください。

/ja/node/6026

プロファイル情報入力とコードの生成

まず、いちばん左の「基本」タブを選択し、基本情報を入力します。先ほど決めた RobotController コンポーネントの仕様(名前)の他に、概要やバージョン等を入力してください。 ラベルが赤字の項目は必須項目です。その他はデフォルトで構いません。

  • モジュール名: RobotController
  • モジュール概要: 任意(Robot Controller component)
  • バージョン: 任意(1.0.0)
  • ベンダ名: 任意
  • モジュールカテゴリ: 任意(Controller)


/ja/node/6310
基本情報の入力


次に、「アクティビティ」タブを選択し、使用するアクションコールバックを指定します。

RobotController コンポーネントでは、onActivated()、onDeactivated()、onExecute() コールバックを使用します。下図のように①の onAtivated をクリック後に②のラジオボタンにて [ON] にチェックを入れます。 onDeactivated、onExecute についても同様の手順を行います。


/ja/node/6310
アクティビティコールバックの選択


さらに、「データポート」タブを選択し、データポートの情報を入力します。 先ほど決めた仕様を元に以下のように入力します。なお、変数名や表示位置はオプションで、そのままで結構です。


  • InPort Profile:
    • ポート名: in
    • データ型: TimedShortSeq

  • OutPort Profile:
    • ポート名: out
    • データ型: TimedVelocity2D


/ja/node/6310
データポート情報の入力


次に、「コンフィギュレーション」タブを選択し、先ほど決めた仕様を元に、Configuration の情報を入力します。 制約条件および Widget とは、RTSystemEditor でコンポーネントのコンフィギュレーションパラメーターを表示する際に、スライダー、スピンボタン、ラジオボタンなど、GUI で値の変更を行うためのものです。

直進速度 speed_x、回転速度 speed_r はスライダーのより操作できるようにします。


  • speed_x
    • 名称: speed_x
    • データ型: double
    • デフォルト値: 0.0
    • 制約条件: -1.5<x<1.5
    • Widget: slider
    • Step: 0.01
  • speed_r
    • 名称: speed_r
    • データ型: double
    • デフォルト値: 0.0
    • 制約条件: -2.0<x<2.0
    • Widget: slider
    • Step: 0.01
  • stop_d
    • 名称: stop_d
    • データ型: int
    • デフォルト値: 30
    • Widget: text


/ja/node/6310
コンフィグレーション情報の入力


次に、「言語・環境」タブを選択し、プログラミング言語を選択します。 ここでは、C++(言語) を選択します。なお、言語・環境はデフォルト等が設定されておらず、指定し忘れるとコード生成時にエラーになりますので、必ず言語の指定を行うようにしてください。

/ja/node/6310
プログラミング言語の選択


最後に、「基本」タブにある [コード生成] ボタンをクリックし、コンポーネントの雛型を生成します。


/ja/node/6310
雛型の生成(Generate)


※ 生成されるコード群は、eclipse起動時に指定したワークスペースフォルダーの中に生成されます。 現在のワークスペースは、[ファイル] > [ワークスペースの切り替え...] で確認することができます。

CMake によるビルドに必要なファイルの生成

RTC Builder で生成したコードの中には CMake でビルドに必要な各種ファイルを生成するための CMakeLists.txt が含まれています。 CMake を利用することにより CMakeLists.txt から Visual Studio のプロジェクトファイル、ソリューションファイル、もしくは Makefile 等を自動生成できます。

CMake(cmake-gui) の操作

CMake を利用してビルド環境の Configure を行います。 まずは CMake(cmake-gui) を起動してください。

 $ cmake-gui
/ja/node/6311
CMake GUI の起動とディレクトリーの指定

画面上部に以下のようなテキストボックスがありますので、それぞれソースコードの場所 (CMakeList.txt がある場所) と、ビルドディレクトリーを指定します。

  • Where is the soruce code
  • Where to build the binaries

ソースコードの場所は RobotController コンポーネントのソースが生成された場所で CMakeList.txt が存在するディレクトリーです。 デフォルトでは <ワークスペースディレクトリー>/RobotController になります。

このディレクトリーはエクスプローラから cmake-gui にドラックアンドドロップすると手入力しなくても設定されます。

ビルドディレクトリーとは、ビルドするためのプロジェクトファイルやオブジェクトファイル、バイナリを格納する場所のことです。 場所は任意ですが、この場合 <ワークスペースディレクトリー>/RobotController/build のように分かりやすい名前をつけた RobotController のサブディレクトリーを指定することをお勧めします。

Where is the soruce code /home/ユーザー名/RobotController
Where to build the binaries /home/ユーザー名/RobotController\build

指定したら、下の [Configure] ボタンをクリックします。すると下図のようなダイアログが表示されますので、生成したいプロジェクトの種類を指定します。 今回は CodeBlocks - Unix Makefiles を指定します。 Code::Blocks を使わない場合は Unix Makefiles を使ってください。

/ja/node/6026
生成するプロジェクトの種類の指定

また cmake-gui を使用しない場合は以下のコマンドでファイルを生成できます。

 $ mkdir build
 $ cd build
 $ cmake .. -G "CodeBlocks - Unix Makefiles"

ダイアログで [Finish] をクリックすると Configure が始まります。問題がなければ下部のログウインドウに「Configuring done」と出力されますので、続けて [Generate] ボタンをクリックします。 「Generating done」と出ればプロジェクトファイル・ソリューションファイル等の出力が完了します。

なお、CMake は Configure の段階でキャッシュファイルを生成しますので、トラブルなどで設定を変更したり環境を変更した場合は [File] > [Delete Cache] を選択して、キャッシュを削除してから Configure からやり直してください。

ヘッダ、ソースの編集

次に先ほど指定した build ディレクトリーの中の RobotController.cbp をダブルクリックしてCode::Blocks を起動します。

ヘッダ (include/RobotController/RobotController.h) およびソースコード (src/RobotController.cpp) をそれぞれ編集します。 Code::BlocksのProjectsからRobotController.h、RobotController.cpp をクリックすることで編集画面が開きます。

/ja/node/6311

64bitの環境の場合に Code::Blocks の動作が不安定になることがあります。 その場合は code completion というプラグインを無効化すると動作することがあります。

「Plugins」>「Manage plugins...」を選択します。

/ja/node/6057/

「code completion」を選択して [Disable] ボタンをクリックします。

/ja/node/6057/

動作しないときはこの手順を試してください。

アクティビティ処理の実装

RobotController コンポーネントでは、コンフィギュレーションパラメーター(speed_x、speed_y)をスライダーで操作しその値を目標速度としてアウトポート(out)から出力します。 インポート(in)から入力された値を変数に格納して、その値が一定以上の場合は停止するようにします。


onActivated()、onExecute()、onDeactivated() での処理内容を下図に示します。

/ja/node/6310/
アクティビティ処理の概要


ヘッダファイル (RobotController.h) の編集

センサー値を一時的に格納する変数 sensor_data を宣言します。

   private:
      double sensor_data[4];    //センサー値を一時格納する変数

ソースファイル (RobotController.cpp) の編集

下記のように、onActivated()、onDeactivated()、onExecute() を実装します。

 RTC::ReturnCode_t RobotController::onActivated(RTC::UniqueId ec_id)
 {
     //センサー値初期化
     for (int i = 0; i < 4; i++)
     {
         sensor_data[i] = 0;
     }
 
   return RTC::RTC_OK;
 }

 RTC::ReturnCode_t RobotController::onDeactivated(RTC::UniqueId ec_id)
 {
     //ロボットを停止する
     m_out.data.vx = 0;
     m_out.data.va = 0;
     m_outOut.write();
 
   return RTC::RTC_OK;
 }

 RTC::ReturnCode_t RobotController::onExecute(RTC::UniqueId ec_id)
 {
     //入力データの存在確認
     if (m_inIn.isNew())
     {
         //入力データ読み込み
         m_inIn.read();
         for (int i = 0; i < m_in.data.length(); i++)
         {
             //入力データ格納
             if (i < 4)
             {
                 sensor_data[i] = m_in.data[i];
             }
         }
     }
 
     //前進するときのみ停止するかを判定
     if (m_speed_x > 0)
     {
         for (int i = 0; i < 4; i++)
         {
             //センサ値が設定値以上か判定
             if (sensor_data[i] > m_stop_d)
             {
                 //センサ値が設定値以上の場合は停止
                 m_out.data.vx = 0;
                 m_out.data.va = 0;
                 m_outOut.write();
                 return RTC::RTC_OK;
             }
         }
     }
     //設定値以上の値のセンサーが無い場合はコンフィギュレーションパラメーターの値で操作
     m_out.data.vx = m_speed_x;
     m_out.data.va = m_speed_r;
     m_outOut.write();
   return RTC::RTC_OK;
 }

Code::Blocks によるビルド

ビルドの実行

Code::Blocksの [ビルド] ボタンをクリックしてビルドを行います。


/ja/node/6311
ビルドの実行


RobotController コンポーネントの動作確認

作成した RobotController をシミュレーターコンポーネントと接続して動作確認を行います。

NameService の起動

コンポーネントの参照を登録するためのネームサービスを起動します。


 $ rtm-naming

RobotController コンポーネントの起動

RobotController コンポーネントを起動します。

RobotController/build/srcフォルダーの RobotControllerComp ファイルを実行してください。

 $ ./RobotControllerComp

シミュレーターコンポーネントの起動

RaspberryPiMouseSimulator コンポーネントをインストールしたディレクトリー({install_raspimouse_simulator.shを実行したディレクトリ}/RasPiMouseSimulatorRTC/build)に移動後、下記のコマンドにて起動できます。

 $ ./src/RaspberryPiMouseSimulatorComp

コンポーネントの接続

下図のように、RTSystemEditorにて RobotController コンポーネント、RaspberryPiMouseSimulator コンポーネントを接続します。

/ja/node/6310
コンポーネントの接続

コンポーネントの Activate

RTSystemEditor の上部にあります [All Activate] というアイコンをクリックし、全てのコンポーネントをアクティブ化します。 正常にアクティベートされた場合、下図のように黄緑色でコンポーネントが表示されます。


/ja/node/6310
コンポーネントのアクティブ化


動作確認

下図のようにコンフィギュレーションビューの [編集] ボタンからコンフィギュレーションを変更することができます。


/ja/node/6311

スライダーを操作してシミュレーター上の Raspberry Pi マウスの操作ができるかを確認してください。


/ja/node/6310
コンフィギュレーションパラメーターの変更


実機での動作確認

講習会で Raspberry Pi マウス実機を用意している場合は実機での動作確認が可能です。

手順は以下の通りです。

  • Raspberry Pi マウスの電源を投入する
  • Raspberry Pi マウスのアクセスポイントに接続
  • ポートの接続
  • コンポーネントのアクティブ化

電源を投入する

Raspberry PiマウスにはRaspberry Piの電源スイッチとモーターの電源スイッチの2つがあります。


rpm8.png


内側の電源スイッチをオンにするとRaspberry Piが起動します。


rpm9.png


電源を切る場合

Raspberry Piの電源を切る場合は、電源スイッチから直接オフにはしないようにしてください。 3つ並んだボタンの中央のボタンを数秒押すとシャットダウンが始まります。 10秒程度でRaspbianのシャットダウンが終了するため、その後に電源スイッチをオフにしてください。


/ja/node/6042

アクセスポイントに接続

SSID、パスワードは Rasoberry Pi マウスに貼り付けたシールに記載してあるので、その SSID に接続してください。

※ネットワークが切り替わった場合にネームサーバーへのコンポーネントの登録やポートの接続が失敗する場合があるのでネームサーバ、コンポーネントを一旦全て終了してください。 ネットワーク切り替え後に起動した場合には問題ないので、終了させる必要はありません。

RT System Editor上でネームサーバーを再起動するには「ネームサービスを起動」ボタンを再度クリックします。

/ja/node/6550

ネームサーバー追加

続いてRTシステムエディタの [ネームサーバー追加] ボタンで 192.168.11.1 を追加してください。


tutorial_raspimouse0.png tutorial_raspimouse1.png



すると以下の2つの RTC が見えるようになります。

/ja/node/6311

RaspberryPiMouseRTC は名城大学のロボットシステムデザイン研究室で開発されているラズパイマウス制御用の RTコンポーネントです。

ポートの接続

RTシステムエディタで RaspberryPiMouseRTC、RobotController コンポーネントを以下のように接続します。

/ja/node/6310

モーターの電源を投入する

動作の前に、モーターの電源スイッチをオンにしてください。 モーターの電源はこまめに切るようにしてください。


rpm10.png


アクティブ化

そして RTC をアクティブ化すると Raspberry Pi マウスの操作ができるようになります。

チュートリアル(RTM講習会、第3部)

このページではRaspberry PiマウスとLEGO Mindstorms EV3を連携したRTシステムの構築を行います。

Raspberry Piマウスをアクセスポイントとして、ノートPCとEV3をアクセスポイントに接続します。

※Raspberry Piマウスと同じ番号のEV3を使用するようにしてください。


/ja/node/6552

EV3のデバイス

EV3 には以下のデバイスが付属しています。

ジャイロセンサー
45505_GyroSensor.jpg
確度モード: 精度 +/- 3°
角速度モード: 最大 440 deg/sec
サンプリングレート 1,000 Hz
カラーセンサー
45506_color.jpg
計測: 赤色光の反射光、 周囲の明るさ、色
検出カラー数: 8色 (無色、黒、青、緑、黄、赤、白、茶)
サンプリングレート 1,000 Hz
距離 約1mm~18mm(アフレル調査値)
タッチセンサー
45507_TouchSensor.jpg
オン (1), オフ (0)
スイッチ可動域: 約4mm
超音波センサー
45504_UltrasonicSensor.jpg
距離計測可能範囲: 3cmから250cm
距離計測精度: +/- 1 cm
前面電飾: 点灯:超音波発信中、 点滅:超音波観測中
EV3 Lモーター
45502_LargeMotor.jpg
フィードバック: 1°単位
回転数: 160から170RPM
定格トルク: 0.21 N・m (30oz*in)
停動トルク: 0.42 N・m (60oz*in)
重さ: 76 g
EV3 Mモーター
45503_MediumMotor.jpg
フィードバック 1°単位
回転数: 240から250RPM
定格トルク: 0.08 N・m (11oz*in)
停動トルク: 0.12 N・m (17oz*in)
重さ: 36 g

EV3の組立て

まず、EV3本体を土台に装着します。


/ja/node/6552

次に25cmケーブルでEV3と左右のLモーターを接続します。


45502_LargeMotor.jpg
Lモーター


Lモーター右 ポート C 25cmケーブル
Lモーター左 ポート B 25cmケーブル


/ja/node/6552

ケーブルに接続するポート、デバイス名は記載してあります。

他のデバイスを取り付ける場合は、チュートリアル(EV3)を参考にしてください。

EV3との接続

ノートPCとRaspberry Piの接続

第二部の、実機での動作確認まで完了してください。 この時点でノートPCとアクセスポイントのRaspberry Piが接続されているはずです。


/ja/node/6552

EV3の電源の入れ方/切り方

電源の入れ方

中央のボタンを押せば電源が投入されます。


/ja/node/6041

電源の切り方

EV3 の電源を切る場合は最初の画面で EV3 本体の左上の戻るボタンを押して「Power Off」を選択してください。


/ja/node/6041


/ja/node/6041

再起動

再起動する場合は最初の画面で EV3 本体の左上の戻るボタンを押して「Reboot」を選択してください。

リセット

ev3dev の起動が途中で停止する場合には、中央ボタン、戻るボタン(左上)、左ボタンを同時押ししてください。画面が消えたら戻るボタンを離すと再起動します。


/ja/node/6041

Raspberry PiとEV3の接続

EV3の電源を投入してください。

起動後にRaspberry Piに自動接続します。 自動接続できた場合は、EV3の画面左上にIPアドレスが表示されます。 IPアドレスは192.168.11.yyyが表示されます。


/ja/node/6384

ネームサーバー、RTCの起動

EV3の画面上の操作でネームサーバーとRTCを起動します。

EV3 の操作画面から「File Browser」→「scripts」を選択してください。

ネームサーバー、RTCはstart_rtcs.shのスクリプトを実行することで起動します。

 ------------------------------
 192.168.11.yyy
 ------------------------------
         File Browser
 ------------------------------
 /home/robot/scripts
 ------------------------------
 ../
 Component/
 ・・
 [start_rtcs.sh                 ]
 ------------------------------


/ja/node/6384

ネームサーバー追加

RTシステムエディタから、192.168.11.yyyのネームサーバーに接続してください。


tutorial_raspimouse0.png tutorial_ev3_irex22.png



この時点でRTシステムエディタのネームサービスビューにはlocalhost、192.168.11.1、192.168.11.yyyのネームサーバーが登録されています。 192.168.11.yyyのネームサーバーに登録されているRTCの名前はEducatorVehicle1となります。


/ja/node/6552

  • localhost
    • RobotController0
  • 192.168.11.1
    • RaspberryPiMouseRTC0
    • OpenCVCamera0
    • artp0
  • 192.168.11.yyy
    • EducatorVehicle1

動作確認

RaspberryPiMouseRTC0(192.168.11.1)とEducatorVehicle1(192.168.11.yyy)をシステムダイアグラム上で接続してください。 EducatorVehicle0の現在の速度出力をRaspberryPiMouseRTC0の目標速度入力に接続することで、EV3の動きにRaspberry Piマウスが追従するようになります。


/ja/node/6552

RTCをアクティベートしてEducator Vehicleの車輪を転がすと、Raspberry Piマウスがそれに合わせて動作します。


/ja/node/6552

自由課題

これで実習は一通り終了ですが、時間が余っている場合は以下のような課題に挑戦してみてください。

EV3のタッチセンサのオンオフでRaspberry Piマウスを操作

EV3のタッチセンサーのオンオフでRaspberry Piマウスを前進後退させるRTシステムを作成します。


45507_TouchSensor.jpg
タッチセンサー


タッチセンサー接続

EV3とタッチセンサーを35cmケーブルで接続してください。

タッチセンサー右 ポート 3 35cmケーブル
タッチセンサー左 ポート 1 35cmケーブル

RTCの作成

以下のような仕様のRTCを作成します。

コンポーネント名称 SampleTouchSensor
InPort
ポート名 touch
TimedBooleanSeq
説明 タッチセンサーのオンオフ
OutPort
ポート名 target_velocity
TimedVelocity2D
説明 目標速度
Configuration
パラメーター名 speed
double
デフォルト値 0.2
説明 タッチセンサがオンの時の直進速度の設定

アクティビティでonExecuteを有効にしてください。

SampleTouchSensorのonExecute関数に以下のように記述します。

 RTC::ReturnCode_t SampleTouchSensor::onExecute(RTC::UniqueId ec_id)
 {
     //新規データの確認
     if (m_touchIn.isNew())
     {
         //データの読み込み
         m_touchIn.read();
         //配列の要素数が1以上かを確認
         if (m_touch.data.length() == 2)
         {
             //0番目のデータがオンの場合は直進する指令を出力
             //0番目のデータは右側のタッチセンサに対応
             if (m_touch.data[0])
             {
                 //目標速度を格納
                 m_target_velocity.data.vx = m_speed;
                 m_target_velocity.data.vy = 0;
                 m_target_velocity.data.va = 0;
                 setTimestamp(m_target_velocity);
                 //データ出力
                 m_target_velocityOut.write();
             }
             //1番目のデータがオンの場合は後退する指令を出力
             //1番目のデータは左側のタッチセンサに対応
             else if (m_touch.data[1])
             {
                 //目標速度を格納
                 m_target_velocity.data.vx = -m_speed;
                 m_target_velocity.data.vy = 0;
                 m_target_velocity.data.va = 0;
                 setTimestamp(m_target_velocity);
                 //データ出力
                 m_target_velocityOut.write();
             }
             //オフの場合は停止する
             else
             {
                 //目標速度を格納
                 m_target_velocity.data.vx = 0;
                 m_target_velocity.data.vy = 0;
                 m_target_velocity.data.va = 0;
                 setTimestamp(m_target_velocity);
                 //データ出力
                 m_target_velocityOut.write();
             }
         }
     }
   return RTC::RTC_OK;
 }

RTシステム作成

データポートを以下のように接続後、タッチセンサをオンオフするとRaspberry Piが前進後退します。


/ja/node/6552

ジョイスティックコンポーネントで2台同時に操作

以下GUIジョイスティックでRaspberry Piマウス、EV3を操作するRTシステムを作成します。


/ja/node/6552

ジョイスティックコンポーネント起動

ジョイスティックコンポーネントはOpenRTM-aist Python版のサンプルにあります(TkJoyStickComp.py)。 ジョイスティックコンポーネントは、Windows 8.1の場合は「スタート」>「アプリビュー(右下矢印)」>「OpenRTM-aist 1.2.0」>「Python_Examples」をクリックして、エクスプローラーで「TkJoyStickComp.bat」をダブルクリックして起動してください。

RTC作成

TkJoyStickComp.pyのアウトポートのデータ型はTimedFloatSeq型であるため、TimedVelocity2D型に変換するRTCを作成する必要があります。

以下のような仕様のRTCを作成してください。

コンポーネント名称 FloatSeqToVelocity
InPort
ポート名 in
TimedFloatSeq
説明 変換前のデータ
OutPort
ポート名 out
TimedVelocity2D
説明 変換後のデータ
Configuration
パラメーター名 rotation_by_position
double
デフォルト値 -0.02
説明 ジョイスティックのX座標の位置に対する角速度の変化量
Configuration
パラメーター名 velocity_by_position
double
デフォルト値 0.002
説明 ジョイステックのY座標に対する速度の変化量

アクティビティはonExecuteをオンにしてください。

onExecute関数を以下のように編集してください。

 RTC::ReturnCode_t FloatSeqToVelocity::onExecute(RTC::UniqueId ec_id)
 {
     //新規データの確認
     if (m_inIn.isNew())
     {
         //データの読み込み
         m_inIn.read();
         //配列のデータ数確認
         if (m_in.data.length() >= 2)
         {
             //目標速度格納
             m_out.data.vx = m_in.data[1] * m_velocity_by_position;
             m_out.data.vy = 0;
             m_out.data.va = m_in.data[0] * m_rotation_by_position;
             setTimestamp(m_out);
             //目標速度出力
             m_outOut.write();
         }
     }
   return RTC::RTC_OK;
 }

RTシステム作成

以下のようにデータポートを接続してください。


/ja/node/6552

EV3をしゃべらせる

EducatorVehicleRTCのsoundという名前のインポートに文字列(TimedString型)を入力すると、EV3が発声します。

RTC作成

以下のような仕様のRTCを作成してください。

コンポーネント名称 SpeechSample
OutPort
ポート名 out
TimedString
説明 発話する文字列

アクティビティはonExecuteをオンにしてください。

onExecute関数を以下のように編集してください。

 RTC::ReturnCode_t SpeechSample::onExecute(RTC::UniqueId ec_id)
 {
     std::cout << "Please input: ";
     std::string ret;
     //文字入力
     std::cin >> ret;
     //データに格納
     m_out.data = CORBA::string_dup(ret.c_str());
     setTimestamp(m_out);
     //データ出力
     m_outOut.write();
 
   return RTC::RTC_OK;
 }

文字列(const char*)をデータポートで出力する際はCORBA::string_dup関数で文字列をコピーする必要があります。

 m_out.data= CORBA::string_dup("abc");

RTシステム作成

以下のようにデータポートを接続してください。


/ja/node/6552

マーカーの追従

Raspberry Piマウスを起動すると、OpenCVCameraコンポーネントとarptコンポーネントが起動します。 OpenCVCameraコンポーネントは画像を取得するコンポーネント、artpコンポーネントは画像データからマーカの位置姿勢を計算して出力するコンポーネントです。


/ja/node/6552

Raspberry Piマウスがマーカーに追従するRTシステムを作成します。

カメラの装着

まずはカメラをRaspberry Piマウスに装着します。

以下の土台部品をRaspberry Piマウスに取り付けていきます。


/ja/node/6552

部品①をRaspberry Piマウスの上部に装着してください。 左から押し込むようにして取り付けます。


/ja/node/6552


この時、左側の突起がプレートを挟むように取り付けてください。


/ja/node/6552

部品②を部品①の左側に上から差し込んでください。


/ja/node/6552



/ja/node/6552

部品③を左側から部品②に差し込んでください。


/ja/node/6552

最後にカメラを搭載して、USBケーブルをRaspberry Piに差し込んだら完成です。


/ja/node/6552



/ja/node/6552


RTC作成

以下の仕様でRTCを作成してください。

コンポーネント名称 testARToolKit
InPort
ポート名 marker_pos
TimedPose3D
説明 マーカーの位置
OutPort
ポート名 target_vel
TimedVelocity2D
説明 ロボットの目標速度
Configuration
パラメーター名 x_distance
double
デフォルト値 0.5
説明 マーカーまでの目標距離(X軸)
Configuration
パラメーター名 y_distance
double
デフォルト値 0
説明 マーカーまでの目標距離(Y軸)
Configuration
パラメーター名 x_speed
double
デフォルト値 0.1
説明 X軸方向移動速度
Configuration
パラメーター名 r_speed
double
デフォルト値 0.2
説明 回転方向移動速度
Configuration
パラメーター名 error_range_x
double
デフォルト値 0.1
説明 X軸方向目標距離の許容範囲
Configuration
パラメーター名 error_range_y
double
デフォルト値 0.05
説明 Y軸方向目標距離の許容範囲

アクティビティはonExecuteをONにしてください。

onExecuteを以下のように編集してください。

 RTC::ReturnCode_t testARToolKit::onExecute(RTC::UniqueId ec_id)
 {
     //新規データの確認
     if (m_marker_posIn.isNew())
     {
         m_target_vel.data.vx = 0;
         m_target_vel.data.vy = 0;
         m_target_vel.data.va = 0;
 
         //データの読み込み
         m_marker_posIn.read();
         //マーカーの位置(X軸)が目標距離(X軸)よりも大きい場合
         if (m_marker_pos.data.position.x > m_x_distance + m_error_range_x/2.0)
         {
             m_target_vel.data.vx = m_x_speed;
         }
         //マーカーの位置(X軸)が目標距離(X軸)よりも小さい場合
         else if (m_marker_pos.data.position.x < m_x_distance - m_error_range_x/2.0)
         {
             m_target_vel.data.vx = -m_x_speed;
         }
         //マーカーの位置(Y軸)が目標距離(Y軸)よりも大きい場合
         else if (m_marker_pos.data.position.y > m_y_distance + m_error_range_y/2.0)
         {
             m_target_vel.data.va = m_r_speed;
         }
         //マーカーの位置(Y軸)が目標距離(Y軸)よりも小さい場合
         else if (m_marker_pos.data.position.y < m_y_distance - m_error_range_y/2.0)
         {
             m_target_vel.data.va = -m_r_speed;
         }
         setTimestamp(m_target_vel);
         //データ書き込み
         m_target_velOut.write();
     }
   return RTC::RTC_OK;
 }

RTシステム作成

データポートを以下のように接続してください。


/ja/node/6552

RTCをアクティベートしてカメラの前でマーカーを動かして、Raspberry Piマウスが移動するかを確認してください。

チュートリアル(RTM講習会、第4部)

はじめに

このページではLibreOffice Calc用RTCによるRTCの動作確認手順について説明します。 Calcのセルの値をInPortに入力、OutPortの出力した値をセルに表示することで対象RTCの挙動を確認できます。

/jp/node/6586

RTM講習会ではUSBメモリでポータブル版LibreOfficeとRTCを配布します。 Windowsで実行できます。 UbuntuはPython3用のomniORBのパッケージがないため実行できません。講習会ではノートPCを貸し出します。

この実習では第2部で作成したRobotControllerコンポーネントを使用します。

LibreOfficeとは?

表計算、パワーポイント、ワープロ機能等を提供するオフィススイートです。 フリーソフトとして公開されており、今回の講習会では以下のポータブル版を使用します。

LibreOffice Calc用RTCの起動

配布したUSBメモリ内のポータブル版LibreOffice\run_CalcRTC.batを実行します。

LibreOffice Calcが起動するため、RTC起動ボタンをクリックすることでOOoCalcControlというRTCを起動します。

/jp/node/6586

OutPortの接続

RobotControllerのOutPortと接続し、Calcで出力データの確認ができるようにします。 Calcの操作ダイアログ起動ボタンをクリックしてください。

/jp/node/6586

まずは出力データを確認するOutPortと接続します。 ツリー表示ボタンを押下してネームサーバーに登録されたRTCのポート一覧を表示後、ツリーからRobotController0のoutを選択します。

/jp/node/6586

次に一部設定を変更します。

列を移動させるのチェックを外してください。 このチェックが有効の場合、データを受信する度にセルの位置が移動するモードで動作します。 グラフに描画する場合は位置が移動するモードを使用しますが、今回は単純に値を確認したいだけのためチェックを外します。

列番号の右のボックスにCと入力してください。 これで2行目のAC列のセルにOutPortの出力データを表示するようになりました。

設定完了後、作成ボタンを押してください。

/jp/node/6586

OutPortの動作確認

RT System Editor上でRTCをアクティブ化して動作を確認してください。

/jp/node/6586

この状態でコンフィギュレーションパラメータを操作してCalcのセルの値が変化するかを確認してください。

/jp/node/6586

InPortの接続

RobotControllerのInPortと接続し、Calcからデータの入力を行うようにします。

ツリー表示ボタンを押下してネームサーバーに登録されたRTCのポート一覧を表示後、ツリーからRobotController0のinを選択します。

/jp/node/6586

次に一部設定を変更します。

列を移動させるのチェックを外してください。

列番号の右のボックスにDと入力してください。 これで3行目のAC列のセルにOutPortの出力データを表示するようになりました。

設定完了後、作成ボタンを押してください。

/jp/node/6586

InPortの動作確認

RT System Editor上でRTCをアクティブ化して動作を確認してください。

この状態でコンフィギュレーションパラメータで前進する速度をOutPortから出力するように操作してください。 その後、Calcの3行目のAC列のセルに31以上の値を入力するか、30以下の値を入力するかで動作が変化するかを確認してください。

/jp/node/6586