Tutorial (Raspberry Pi Mouse)

このページでは RTM講習会での Raspberry Pi Mouse 操作手順を説明します。

Raspberry Pi Mouse (以下ラズパイマウス)はアールティが販売している二輪方式の移動ロボットです。 Raspberry Pi を搭載しているため Linux(Raspbian) 等での開発が可能です。

仕様

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

ダウンロード

最初に PC側で使用する RTC 等をダウンロードしてください。

robomech2017_tutorial.zip

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

電源の入れ方/切り方

電源の入れ方

内側のスイッチが Raspberry Pi の電源スイッチです。このスイッチをオンにすると電源が投入できます。

電源の切り方

電源を切る場合は真ん中のスイッチを1秒ほど押すと OS がシャットダウンするため、その後にスイッチを切ってください。
※スイッチを直接切るのはデータの破損などの危険があるため絶対にやらないでください。

Raspberry Pi への接続

Raspberry Pi へは原則として無線LANで接続するようにしてください。

無線LANアクセスポイントへの接続

まずは無線LANアダプタを取り付けた状態で Raspberry Pi Mouse の電源スイッチをオンにしてください。

しばらくすると無線LANアクセスポイントが起動するので、指定の SSID のアクセスポイントに接続してください。

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

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

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

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

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

LANケーブルでの接続

以下の作業は有線で接続する場合の作業なので、無線で接続する場合は不要です。

まず最初にLANケーブルで PC と Raspberry Pi を接続してください。

次に Raspberry Pi Mouse の電源スイッチをオンにしてください。

事前準備

このページの手順に従ってネームサーバー、RTシステムエディタを起動してください。予めネームサーバーを起動してある場合は再起動してください。

またネットワークインターフェースが2つ以上ある場合に通信に失敗する可能性があるため、有線で接続した場合は他のネットワークデバイスを無効にしてからネームサーバーを起動してください。

ネームサーバー追加

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

すると以下の2つの RTC が起動します。

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

RaspberryPiMouseRTC
InPort
名前	データ型	説明
target_velocity_in	RTC::TimedVelocity2D	目標速度
pose_update_in	RTC::TimedPose2D	位置設定
buzzer_hz_in	RTC::TimedShort	ブザー
led4bit_in	RTC::TimedBooleanSeq	LED
OutPort
名前	データ型	説明
current_velocity_out	RTC::TimedVelocity2D	現在の速度
current_pose_out	RTC::TimedPose2D	現在位置
ir_sensor_out	RTC::TimedShortSeq	距離センサーの計測値
switch3bit_out	RTC::TimedBooleanSeq	スイッチのオンオフ

TimedVelocity2D 型について

TimedVelocity2D 型は以下のように定義されています。

     struct Velocity2D
     {
         double vx;
         double vy;
         double va;
     };

     struct TimedVelocity2D
     {
         Time tm;
         Velocity2D data;
     };

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

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

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

vx、va を指定することでロボットの操作を行います。

サンプルコンポーネント起動

付属資料のstart_component_raspimouse.batを起動してください。 ※OpenRTM-aist Python版をインストールしていない場合、もしくはインストールに失敗している場合は個別に実行ファイル入りの USBメモリーを配布しますので、start_component_raspimouse_exe.batを利用してください。

すると以下の2つの RTC が起動します。

動作確認

まずはジョイスティックでラズパイマウスを操作してみます。

動作の前にモーター電源スイッチをオンにしておいてください。動作確認が終了したら、モーター電源はオフにするようにしてください。

RTシステムエディタで RaspberryPiMouseRTC、FloatSeqToVelocity、TkJoyStick を以下のように接続します。

そして RTC をアクティブ化するとジョイスティックでラズパイマウスの操作ができるようになります。

自作の RTC で制御

まずは FloatSeqToVelocity の out と RaspberryPiMouseRTC の target_velocity_in のコネクタを切断してください。

FloatSeqToVelocity と RaspberryPiMouseRTC の間に自作の RTC を接続して、距離センサーの値が一定以上になった場合に停止して音を鳴らすようにします。

ひな形コードの作成

RTC ビルダを起動してください。

起動したら新規にプロジェクトを作成します。

プロジェクト名は TestRasPiMouseCPP(TestRasPiMousePy) にします。

以下のように設定を行ってください。 C++、もしくは Python で作成します。

基本
モジュール名	TestRasPiMouseCPP、もしくはTestRasPiMousePy
アクティビティ
有効アクション	onInitialize、onExecute、onActivated、onDeactivated
データポート
InPort
名前	データ型	説明
velocity_in	RTC::TimedVelocity2D	入力目標速度
distance_sensor	RTC::TimedShortSeq	距離センサーの計測値
OutPort
名前	データ型	説明
velocity_out	RTC::TimedVelocity2D	出力目標速度
buzzer	RTC::TimedShort	ブザー
コンフィギュレーション
名前	型	説明
stop_distance	short	距離センサーで物体を検知した場合に前進しなくなる距離センサーの計測値、デフォルト値は300
言語・環境
言語	C++、もしくはPython

[コード生成] ボタンをクリックするとコードが生成されます。

距離センサー

ラズパイマウスの距離センサーは近ければ近いほど大きな値を出力します。

参考までにですが、/dev/rtlightsensor0のデバイスファイルから取得できる数値とセンサーまでの距離との関係は以下のようになっています。

デバイスファイルから取得した数値	実際の距離[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

プロジェクト生成

コードが生成できたら C++ の場合は CMake で Visual Studio のプロジェクト(Ubuntu の場合は Code::Blocks)を生成してください。

まずCMake (cmake-gui) を起動します。

Windows 7

Windows 8.1

起動したらソースコードのディレクトリー、ビルドを行うディレクトリーに以下を指定します。括弧内は eclipse の作業ディレクトリーをC:\workspaceにした場合の例です。

Where is the source code	RTCBuilder で生成したコードのフォルダー(C:\workspace\TestRasPiMouseCPP)
Where to build the binaries	RTCBuilder で生成したコードのフォルダーの下に作成した build フォルダー(C:\workspace\TestRasPiMouseCPP\build)

[Configure] ボタン → [Generate] ボタンをクリックすると Visual Studio のプロジェクトが生成されます。

ソースコードの編集

build ディレクトリーの TestRasPiMouseCPP.sln を開いてください。

次にコードの編集を行います。

Python の場合はまず変数の初期化部分を修正してください。

TestRasPiMousePy.py

     def __init__(self, manager):
         #self._d_velocity_in = RTC.TimedVelocity2D(*velocity_in_arg)
         self._d_velocity_in = RTC.TimedVelocity2D(RTC.Time(0,0),RTC.Velocity2D(0,0,0))

         #self._d_distance_sensor = RTC.TimedShortSeq(*distance_sensor_arg)
         self._d_distance_sensor = RTC.TimedShortSeq(RTC.Time(0,0),[])

         #self._d_velocity_out = RTC.TimedVelocity2D(*velocity_out_arg)
         self._d_velocity_out = RTC.TimedVelocity2D(RTC.Time(0,0),RTC.Velocity2D(0,0,0))

         #self._d_buzzer = RTC.TimedShort(*buzzer_arg)
         self._d_buzzer = RTC.TimedShort(RTC.Time(0,0),0)

まずは onExecute で入力速度をそのまま出力するコードを書いてみます。
C++の場合は以下のようになります。
isNew 関数で新規の入力データが存在するかを確認して、read 関数で変数(m_velocity_in)に格納します。そして m_velocity_out に出力データを格納してwrite 関数を呼び出すとデータが送信されます。

src/TestRaspiMouseCPP.cpp

     if (m_velocity_inIn.isNew())
     {
         m_velocity_inIn.read();
         //入力速度をそのまま出力
         m_velocity_out.data.vx = m_velocity_in.data.vx;
         m_velocity_out.data.vy = m_velocity_in.data.vy;
         m_velocity_out.data.va = m_velocity_in.data.va;
         setTimestamp(m_velocity_out);
         m_velocity_outOut.write();
 
 
     }

Pythonの場合は以下のようになります。

TestRasPiMousePy.py

         if self._velocity_inIn.isNew():
             data = self._velocity_inIn.read()
             #入力速度をそのまま出力する
             self._d_velocity_out.data.vx = data.data.vx
             self._d_velocity_out.data.vy = data.data.vy
             self._d_velocity_out.data.va = data.data.va
             OpenRTM_aist.setTimestamp(self._d_velocity_out)
             self._velocity_outOut.write()

次に距離センサーの計測値が一定以上の場合に停止する処理を記述します。
常に距離センサーのデータが入力されるとは限らないので、センサーのデータを格納する変数を宣言します。

C++の場合は TestRasPiMouseCPP.h に記述します。

include/TestRasPiMouseCPP/TestRasPiMouseCPP.h

  private:
      int m_last_sensor_data[4];

Pythonの場合はコンストラクタに記述します。

TestRasPiMousePy.py

     def __init__(self, manager):
         OpenRTM_aist.DataFlowComponentBase.__init__(self, manager)
 
         self._last_sensor_data = [0, 0, 0, 0]

次に onExecute に停止する処理を記述します。
C++の場合は以下のようになっています。
まずインポート distance_sensor に isNew 関数で新規にデータが入力されたかを確認して、入力されている場合は read 関数で読み込みます。そして変数 m_last_sensor_data に格納します。
そしてインポート velocity_in で受信したデータの vx が0以上の場合には前進しているため障害物に接触するかもしれないと判定して、距離センサーの値が一定以上の場合は停止してブザーを鳴らします。

src/TestRaspiMouseCPP.cpp

 RTC::ReturnCode_t TestRasPiMouseCPP::onExecute(RTC::UniqueId ec_id)
 {
     //データを新規に受信した場合に、データを m_last_sensor_data を格納する
     if (m_distance_sensorIn.isNew())
     {
         m_distance_sensorIn.read();
         if (m_distance_sensor.data.length() == 4)
         {
             for (int i = 0; i < 4; i++)
             {
                 m_last_sensor_data[i] = m_distance_sensor.data[i];
             }
         }
     }
 
     if (m_velocity_inIn.isNew())
     {
         m_velocity_inIn.read();
         //vxが0以上(前進)のときのみ停止するか判定する
         if (m_velocity_in.data.vx > 0)
         {
             for (int i = 0; i < 4; i++)
             {
                 //センサーの計測値がstop_distance以上の時に前進しないようにする
                 if (m_last_sensor_data[i] > m_stop_distance)
                 {
                     //停止する
                     m_velocity_out.data.vx = 0;
                     m_velocity_out.data.vy = 0;
                     m_velocity_out.data.va = 0;
                     setTimestamp(m_velocity_out);
                     m_velocity_outOut.write();
                     
                     //ブザーを鳴らす
                     m_buzzer.data = 50;
                     setTimestamp(m_buzzer);
                     m_buzzerOut.write();
 
                     return RTC::RTC_OK;
                 }
             }
         }
         //ブザーを止める
         m_buzzer.data = 0;
         setTimestamp(m_buzzer);
         m_buzzerOut.write();
 
         //入力速度をそのまま出力
         m_velocity_out.data.vx = m_velocity_in.data.vx;
         m_velocity_out.data.vy = m_velocity_in.data.vy;
         m_velocity_out.data.va = m_velocity_in.data.va;
         setTimestamp(m_velocity_out);
         m_velocity_outOut.write();
      }
     
   return RTC::RTC_OK;
 }

Pythonの場合は以下のようになっています。

TestRasPiMousePy.py

     def onExecute(self, ec_id):
         #データを新規に受信した場合に、データをm_last_sensor_dataを格納する
         if self._distance_sensorIn.isNew():
             data = self._distance_sensorIn.read()
             if len(data.data) == 4:
                 self._last_sensor_data = data.data[:]
 
 
         
         if self._velocity_inIn.isNew():
             data = self._velocity_inIn.read()
             #vxが0以上(前進)のときのみ停止するか判定する
             if data.data.vx > 0:
                 for d in self._last_sensor_data:
                     #センサーの計測値がstop_distance以上の時に前進しないようにする
                     if d > self._stop_distance[0]:
                         #停止する
                         self._d_velocity_out.data.vx = 0
                         self._d_velocity_out.data.vy = 0
                         self._d_velocity_out.data.va = 0
                         OpenRTM_aist.setTimestamp(self._d_velocity_out)
                         self._velocity_outOut.write()
 
                         #ブザーを鳴らす
                         self._d_buzzer.data = 50
                         OpenRTM_aist.setTimestamp(self._d_buzzer)
                         self._buzzerOut.write()
                         
                         return RTC.RTC_OK
 
             #ブザーを止める
             self._d_buzzer.data = 0
             OpenRTM_aist.setTimestamp(self._d_buzzer)
             self._buzzerOut.write()
 
             #入力速度をそのまま出力する
             self._d_velocity_out.data.vx = data.data.vx
             self._d_velocity_out.data.vy = data.data.vy
             self._d_velocity_out.data.va = data.data.va
             OpenRTM_aist.setTimestamp(self._d_velocity_out)
             self._velocity_outOut.write()
 
         return RTC.RTC_OK

コードの編集が終わったら C++ の場合はビルドしてください。
ビルドに成功すると build\src\Release(Debug) に TestRasPiMouseCPPComp.exe が生成されます。

動作確認

TestRasPiMouseCPPComp.exe (TestRasPiMouseCPPComp.py) をダブルクリックして起動してください。
TestRaspiMouseCPP (TestRaspiMousePy) を以下のように接続してください。

最後に RTC をアクティブ化して動作確認してください。

RTシステム保存

RTシステムを保存する場合は System Diagram 上で右クリックして [Save As...] を選択してください。

RTシステム復元

復元する場合は [Open and Restore] を選択して、先ほど保存したファイルを選択してください。

RTC 終了

RTC を終了する場合はRTシステムエディタ上で RTC を [exit] してください。

RT-Component	153.5
RT-Middleware	35
Tools	22
Documentation	2

OpenRTM-aist