このページでは 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 等をダウンロードしてください。
ZIPファイルを Lhaplus 等で展開してください。
内側のスイッチが Raspberry Pi の電源スイッチです。 このスイッチをオンにすると電源が投入できます。
電源を切る場合は真ん中のスイッチを1秒ほど押すと OS がシャットダウンするため、その後にスイッチを切ってください。
※スイッチを直接切るのはデータの破損などの危険があるため絶対にやらないでください。
Raspberry Pi へは原則として無線LANで接続するようにしてください。
まずは無線LANアダプタを取り付けた状態で Raspberry Pi Mouse の電源スイッチをオンにしてください。
しばらくすると無線LANアクセスポイントが起動するので、指定の SSID のアクセスポイントに接続してください。
SSID、パスワードは Rasoberry Pi マウスに貼り付けたシールに記載してあります。
アクセスポイントへの接続方法は以下のページを参考にしてください。
まず右下のネットワークアイコンをクリックしてください。
以下の作業は有線で接続する場合の作業なので、無線で接続する場合は不要です。
まず最初にLANケーブルで PC と Raspberry Pi を接続してください。
このページ の手順に従ってネームサーバー、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 型は以下のように定義されています。
struct Velocity2D { double vx; double vy; double va; };
struct TimedVelocity2D { Time tm; Velocity2D data; };
vx、vy、va はロボット中心座標系での速度を表しています。
ラズパイマウスのように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 をアクティブ化するとジョイスティックでラズパイマウスの操作ができるようになります。
まずは FloatSeqToVelocity の out と RaspberryPiMouseRTC の target_velocity_in のコネクタを切断してください。
FloatSeqToVelocity と RaspberryPiMouseRTC の間に自作の RTC を接続して、距離センサーの値が一定以上になった場合に停止して音を鳴らすようにします。
RTC ビルダを起動してください。
以下のように設定を行ってください。 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) を起動します。
Where is the source code | RTCBuilder で生成したコードのフォルダー(C:\workspace\TestRasPiMouseCPP) |
Where to build the binaries | RTCBuilder で生成したコードのフォルダーの下に作成した build フォルダー(C:\workspace\TestRasPiMouseCPP\build) |
build ディレクトリーの TestRasPiMouseCPP.sln を開いてください。
次にコードの編集を行います。
Python の場合はまず変数の初期化部分を修正してください。
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 関数を呼び出すとデータが送信されます。
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の場合は以下のようになります。
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 に記述します。
private: int m_last_sensor_data[4];
Pythonの場合はコンストラクタに記述します。
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以上の場合には前進しているため障害物に接触するかもしれないと判定して、距離センサーの値が一定以上の場合は停止してブザーを鳴らします。
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の場合は以下のようになっています。
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システムを保存する場合は System Diagram 上で右クリックして [Save As...] を選択してください。
復元する場合は [Open and Restore] を選択して、先ほど保存したファイルを選択してください。
RTC を終了する場合はRTシステムエディタ上で RTC を [exit] してください。