OGIMOノート~家族のためのモノづくり~

OGIMOテックノート ~家族のためのモノづくり~

重度障害の息子を持つ父親エンジニアの備忘録。自作の電子工作おもちゃ/リハビリ器具/ロボット関係の製作記録、思った事を残していきます

M5Stack(ESP32)を使い始めてみた

はじめに

 ちまたでブームになっている Arduino対応無線マイコンESP32が搭載されたモジュールキット”m5stack"!  2か月ほど前に衝動買いしていたものの、別件を優先してすっかり積みマイコン化していましたが、ようやく開封!

f:id:motokiinfinity8:20180722175725j:plain


M5Stackとは

 M5StackはEspressif社のArduino対応無線(Wifi/BL)マイコンESP32をベースに、ディスプレイ、スピーカ、バッテリー、SDカードスロット、ボタン、USBやGroveのコネクタを5cm四方の基板に搭載した統合開発モジュール。なので、ESP32マイコンを組み込む前のSDKボードとして、またM5stack単体としても工作品に導入できる。見た目もコンパクトで、液晶に顔を表現するだけでかわいくなる!
やはり、見た目は大事だ。

esp32マイコンは、Arduino UNOに比べて、
 ・マイコン周波数 → 30倍 (ESP32: 80MHz240MHz@2コア / Uno:16MHz)
 ・RAM容量 → 260倍(ESP32:520kB / Uno:2kB)
 ・ROM容量 → 128倍 (ESP32:4MB / Uno:32kB)
それでして、価格は esp32 の方が安いのだから、もう純正Arduinoマイコン系は出番がなくなるのではなかろうか。

個人的な使い道

ちょっと考えただけでも、いろんな工作品に即時導入できそう
 ① 小型ロボットのメインマイコンに出来る(液晶画面/スピーカがあるのは素敵)
   rosserialでHost PCからもROS制御できるので子ロボット向き
 ② リハビリ靴 ver.4のスピーカ部として使えそう
   (ver.3の課題になったディスプレイ課題がこれで解決する!)
 ③ 玄関お出迎えロボやボタン押しロボットにも使える
  (もともとESP32で簡易ロボを作ろうとしてたが、
   ディスプレイがある方が愛着がわきそう)
 ④ 業務でMEMSセンサーの性能実験をする際の簡易モニターに使える
  (シリアル値確認のためにPCを持ち運ばなくて良い!)
  ⑤ 子供のおもちゃ魔改造用!!

そんな事を視野に入れながら、今回のM5Stack 立ち上げで簡単にディスプレイ表示&スピーカ再生をしてみた。

うーん、いい感じ♪
これを自家製ピタゴラ装置の一つにしてしまったりも出来そう!(息子がピタゴラスイッチ大好きなので間違いなく喜ぶ!)
という訳で、さっそく模索開始!


環境構築

環境構築はそんなに難しくない。esp32環境が入っている前提であれば、M5stackのGitHubからダウンロードしてlibraryに加えてやればよい。
github.com


なお、esp32のArduino開発環境を入れるならば、こちらを参考するとよさそう
www.mgo-tec.com

M5stack(esp32)でサーボモータを動かしてみる

ここからは、M5stackで各モジュールを動かした際の注意事項を備忘録として記載しておく。意外と手間取ってしまった。

サーボーモータはArudinoでよく使われている"Servo.h"を使おうとしたが、結果としてはNGであった。

#include <Servo.h>
Servo myservo;

void setup() {
  myservo.attach( 1 );
}

void loop() {
  myservo.write(45);
  delay(5000);
}


で動かそうとしたら、、、

『警告:ライブラリServoはアーキテクチャ(avr, sam, samd, nrf52, stm32f4)に対応したものであり、アーキテクチャ(esp32)で動作するこのボードとは互換性がないかもしれません。 #error "This library only supports boards with an AVR, SAM, SAMD, NRF52 or STM32F4 processor."』

ですって。
ありゃま? ESP8266では使えてたのに。。。

どうやら調べてみたところ、ESP8266まではServo.h が使える ESP8266 等と Servo.h が使えない ESP32 だそうです。ソラシラナンダ

そこで、下記サイトを参考に、LEDを使用するledcWrite関数でPWM信号を使用するのがよさそうです。c( )関数でパラメータを設定し、ledcAttachPin( )関数で使用するピンを指定し、ledcWrite( )関数でPWM値を算出する。
ESP32: RCサーボの制御 | マイクロファン ラボ

ただし、使用CHには注意。M5stackの場合、CH0はスピーカ用、CH7はディスプレイのバックライト用で使っているので、それ以外を選ぶ必要がある。
間違って、CH0を使ってサーボを動かそうとしたら、スピーカノイズが発生してしまった。。

という訳で、CH3を使えばよさそう。このあたり、きちんとM5stackのハード構成を抑える必要がありそうだ。

#define LEDC_CHANNEL_3 3    //LEDCのチャンネル指定
#define LEDC_TIMER_BIT 16   //LEDCのPWMタイマーの精度設定
#define LEDC_SERVO_FREQ 50   //サーボ信号の1サイクル 50Hz:20ms
#define SERVO_PIN 2       //ServoPWMピン

int servo_pwm_count(int v)
{
  float vv = (v + 90) / 180.0 ;
  return (int)(65536 * (SERVO_MIN_WIDTH_MS + vv * (SERVO_MAX_WIDTH_MS -SERVO_MIN_WIDTH_MS)) / 20.0) ;
}

setup(){
   // servoモーター設定
  ledcSetup(LEDC_CHANNEL_3, LEDC_SERVO_FREQ, LEDC_TIMER_BIT) ; // 16ビット精度で制御
  ledcAttachPin(SERVO_PIN, LEDC_CHANNEL_3) ; // CH3をRC SERVOに
}

loop(){
    ledcWrite(LEDC_CHANNEL_3, servo_pwm_count(60)) ; 
}


M5stack(esp32)でディスプレイに表示を出してみる

ディスプレイドライバはILITEK 社の TFT LCD 用 ドライバー ILI9341 との事。詳細はこちらのサイトが詳しそう。
www.mgo-tec.com

どうやら SPI接続+バックライト制御の模様。M5stackの描画ライブラリは、ESP32のSPIレジスタに直接アクセスしてHW転送を使っている様で高速転送が期待できる模様。

SDカード上のJPEGデータを表示

一番楽な使い方。

  M5.Lcd.clear();
  M5.Lcd.drawJpgFile(SD, "/dora.jpg");

これだけでSDカードのルート直下にある"dora.jpg"が表示されるので簡単。
これは画面左上を原点とした場合の描画。任意の位置に描画させるためには以下の引数を設定すればよさそうな模様(まだ未実証)

M5.Lcd.drawJpgFile(fs::FS &fs, const char *path, uint16_t x, uint16_t y, uint16_t maxWidth, uint16_t maxHeight, uint16_t offX, uint16_t offY, jpeg_div_t scale);


公開されている顔アバターを使う

ロボットの顔としてディスプレイを使うため、顔の表示は必要!できれば、瞬きとかちょっとしたアクションの変化は付けたいなぁ。と探していたら、既にライブラリを作成されている方を発見!

github.com

サンプルコードを使わせてもらったが、とても使いやすく可愛い♪ 呼吸などのアクション動作をMainソフトから意識させないライブラリの作り方がとてもありがたい。一旦、ありがたく使わせていただきつつ、将来的には家族好みのキャラに作り替えていきたいなぁ、と。
是非とも、ドラえもん風のキャラクタ顔をここに表示させたい!

M5stack(esp32)でスピーカから音を鳴らしたい

ここが少しばかり苦労した。まず、M5stackとしてのwav/mp3ファイル再生のライブラリはなさそうで、ESP32で動作実績のあるライブラリを持ってくる必要がありそうだ。
ESP8266Audio / ESP8266_Spiramライブラリを使うのが推奨の様だ。M5stackのサンプルプログラムにも本ライブラリのヘッダ記載があった。

https://github.com/earlephilhower/ESP8266Audio
https://github.com/Gianbacchio/ESP8266_Spiram

一旦、
M5 Speaker and WAV files · Issue #40 · m5stack/M5Stack · GitHub
を参考に、ボタンを押す度にwavファイルが再生するコードを作成。

しかし、うまくいなかい。
課題1 : 1回目の再生終了後にノイズが発生して2回目に移行せず。
 ⇒ 試行錯誤の結果、都度 file = new AudioFileSourceSD関数を作り直したらうまくいった。ファイルポインタの位置を戻さないといけないのか?

課題2: WAV再生の場合、一度再生した後に無音ノイズが残る(MP3再生なら問題なし)
 ⇒ ライブラリのソースを見たところ、ライブラリ側の間違い発見!
AudioGeneratorWAV.cpp内のstop関数で停止処理がされておらず

bool AudioGeneratorWAV::stop()
{
  if (!running) return true;
  running = false;
  free(buff);
  buff = NULL;
  output->stop();  ★ここを追加
  return file->close();
}


なるほど、Output-> stop()が抜けていたから、無音再生しっぱなしだったのね。
という訳で、ローカルでライブラリ修正する事で無事に音源ファイルの連続再生ができた! (後ほどGithubに書いておこう)

という訳で、無事に音楽再生もクリア。参考コードは以下の形となった


AudioGeneratorWAV *wav;
AudioGeneratorMP3 *mp3;
AudioFileSourceSD *file_w,file_m;
AudioOutputI2S *out_w, out_m;
AudioFileSourceID3 *id3;

void setup() {
  M5.begin();

  //MP3の場合
  file_m = new AudioFileSourceSD("/sample.mp3");
  id3 = new AudioFileSourceID3(file_m);
  out_m = new AudioOutputI2S(0,1);
  out_m->SetOutputModeMono(true);
  out_m->SetGain(0.3);
  mp3 = new AudioGeneratorMP3();

  //WAVの場合
  file_w = new AudioFileSourceSD("/sample.wav");
  out_w = new AudioOutputI2S(0,1); 
  out_w->SetOutputModeMono(true);
  out_w->SetGain(0.3); 
  wav = new AudioGeneratorWAV(); 
}

void loop(){
  M5.update();
  if(M5.BtnC.wasPressed()){
    //MP3の場合
    file = new AudioFileSourceSD("/sample.mp3");
    id3 = new AudioFileSourceID3(file_m);
    out_m = new AudioOutputI2S(0,1);
    out_m->SetOutputModeMono(true);
    out_m->SetGain(0.3);
    mp3 = new AudioGeneratorMP3();

    mp3->begin(id3, out_m);
    while(mp3->isRunning()){
     if (!mp3->loop()) mp3->stop();
    }

    //WAVの場合
    file_w = new AudioFileSourceSD("/sample.wav");
    out_w = new AudioOutputI2S(0,1); 
    out_w->SetOutputModeMono(true);
    out_w->SetGain(0.3); 
    wav = new AudioGeneratorWAV(); 

    wav->begin(file_w, out_w);
    while(wav->isRunning()){
     if (!wav->loop()) wav->stop();
    }
  }

}

最後に

M5stackを扱った事のある電子工作技術者から見たら初歩的な内容だったかもしれないが、自分への備忘録もかねて記載してみた。いろんなマイコン/プロセッサ/PCを並行して触っていると細かい部分はすぐに忘れてしまうからなぁ。。「一週間前の自分は他人である」ってヤツですね(笑)

今回で M5stackの良さはかなり実感できたので、実際に我が家の工作物に導入していきたいと思う!

【自作リハビリ器具/電子工作】歩くと効果音の鳴るメロディ靴 Ver.3 を作ってみた

はじめに

以前に息子のリハビリのモチベーション向上&歩こうとする意欲を上げるために、『靴に体重を乗せると効果音の鳴るメロディ靴』を作ってみた。

ogimotokin.hatenablog.com

このコンセプト自体はよかった様で、最初は息子も「なんだなんだー?」と興味を持ってくれていたのですが、残念ながら長続きせず。
原因を分析したところ、
 ・靴とスピーカーボックスが有線ケーブルでつながっていて、足をケーブルに絡めてしまったりで運用に難色あり
 ・音だけだと息子の興味が長続きせず。。。

そんな中、先月に娘が「Nintendo LABOで弟が歩く練習が出来る靴を作ろう!」という事で、Nintendo LABOで『靴に体重を乗せると効果音の鳴るメロディ靴 Ver.2』を父娘合作で発明してみた。

これの反応はなかなか良好でした。良好だったポイントとしては
 ・足をフリフリすると靴も振動する(バイブレーション)
 ・足のフリフリに応じて、タブレット画面も光る&音が鳴る(音は娘チョイス)
でした。振動もさる事ながら、タブレットに大きく光が音と同時に見えるのがうけたみたい!

じゃあ、これを実際に運用していこうとしていたが、これも長続きせず。。。。

原因はというと、
妻&娘が毎日Nintendo Switchで別ゲームを遊び続けるので、リモコンが確保できない事!!!(笑)

まぁ、これだけが要因ではないのですが、せっかくなら専用で新たにメロディ靴を起こそうと思った訳です。

要求仕様

①靴部とスピーカ部はケーブルレス
 (可能ならスピーカも靴に入れたいが、まだそこまで至れず)
②スピーカ部はVer.1のものをできる限り流用する事
 (スピーカをもう一回作るのは面倒くさいので)
③靴に体重をかけた際に、靴を光らせる&振動させる
 (足を動かす事への興味を引き付ける目的)
④靴を光らせる際の体重量(閾値)は簡単に変更できる様にする
 (息子の成長に柔軟に対応できる様に)
⑤足が当たる事が懸念されるので靴部分のモジュールは壊れにくく作る
 (ちゃんとした外装をつける)

そんな完成品は以下。なお、靴部のモジュールは3Dプリンタで外装設計した筐体に収めてみた。
f:id:motokiinfinity8:20180715020713j:plain

TWELITE DIP導入

そもそもどうやってケーブルレスを実現するか、という事で、無線モジュールを検討開始。

WifiはAPが必要になるから宅外に持っていくとソフト書換えが発生して面倒やなぁ、
Bluetoothは親1対子2みたいな通信が簡単に構築できるか不安だなぁ、
と悩んでいた時、たまたま見つけたTWELITE DIPに注目!

TWELITEはモノワイヤレス株式会社のZigbee(2.4GH)無線モジュール。
消費電力が低い、簡単にネットワークが構築できる事、モジュールがコンパクトである事が特徴だ。

個人的に一番気に入ったのは、子機の入出力信号(デジタル/アナログ)が親機の入出力信号にミラーリングされる点。
つまり、無線接続であるのにあたかも有線でつなげている様な感覚で使える!

超簡単!標準アプリ(App_Twelite) - MONO-WIRELESS.COM

子機へのデジタル入力(4本)→親機からのデジタル出力(4本)
子機へのアナログ入力(4本)→親機からのデジタルPWM出力(4本)
親機へのデジタル入力(4本)→子機からのデジタル出力(4本)
親機へのアナログ入力(4本)→子機からのデジタルPWM出力(4本)

要求仕様で上げていた「スピーカー部はVer1(有線)と共用できる事」も実現できそう!
うまくやればソフト開発なしで出来る気がしてきた!!
という事で TWELITEを導入してみる事に。

全体ハードシステム

Ver1(有線版)と 今回のVer.3(TWELITE無線版)のシステム比較が以下の図。

f:id:motokiinfinity8:20180715010926j:plain

Ver.1では、靴からの感圧センサー値を見て、ある閾値以上の値になればスピーカ出音& LED制御という仕組みが入っていたので、その制御仕様をそのまま使えそうな事が確認できた。LED+振動モータをスピーカ部から制御せず、靴部内で完結させる案も考えたが将来的な仕様拡張性を優先して、スピーカ部で制御を集約する仕様とした。
(例えば、左足に体重をかけた後、右足に体重をかけた後でないと、左足のLED+振動モーターがONにならない様な仕様に変更する場合、左右両方の状態が分かるスピーカ部で制御する様がやりやすいから)

TWELITE設定

「超簡単!標準アプリ(App_Twelite)」モードを使うので、特に購入時からソフトの書き換えの必要はなし。ただし、1対2通信を行うため、設定値のみ変更を行った。
変更方法は下記参照
設定変更(インタラクティブ)モード - MONO-WIRELESS.COM

・a: set Application ID を変更 (なんでもよいが、全モジュールで統一させる)
・z: PWM周波数 を10KHzに変更 (LPF回路でリップル成分を減らすため)

回路設計(子機側)

コンパクトにしたいため、電源としてはボタン電池を採用。振動モーター等を動かすので電流容量は必要と思ったので、CR3023を使用。1個当たり3Vだが、TWELITE DIPは3.3V駆動でやや不安が残るため、「CR3023×2 (6V) → LDOで3.3V電圧駆動」とした。LEDは10mA程度でも十分光量が確保できそうなため、端子に直接(抵抗経由で)接続した。ただし、振動モーターは同程度の電流をひっぱる懸念があったため端子は別に分けた。

回路設計(親機側)

親機側はスピーカ部と有線接続する形とした。スピーカ部のマイコン端子電圧は5V、TWELITE DIP端子電圧は3.3Vなので、両者の接続のためレベルシフタICを間に挟んだ。注意すべきは、子機から来る感圧センサー値。子機側ではアナログ値で取得して、親機ではPWM値で来るため、5Vへのレベルシフト後にRCフィルタでLPFを形成してアナログ電圧にしてやる必要がある。
フィルタ計算に使ったサイトは以下。

CRローパス・フィルタ計算ツール

手元にある部品で極力カットオフ周波数を落とせる組み合わせが、C=1uF/R=510Ω であった。これでカットオフ周波数は約312Hz。1kHzのPWMに対してはかなり原信号成分が残ってしまうため、PWM周波数をdefaultから一桁分変更する事にした。まぁ、趣味なのでこの程度で良いだろう。

出来上がった回路は以下。
f:id:motokiinfinity8:20180715014607j:plain


なお、ブレッドボード図は、Fritzingで作成している。
Fritzing Download

外装設計

コンパクトに設計すべき子機側をくみ上げてみた。ユニバーサル基板にうまく積層できたのではなかろうか。


f:id:motokiinfinity8:20180715021012j:plain



ここまで来たら、筐体も付けたくなる!
ちょうど良い勉強の機会なので、自力で3D-CADで筐体設計もしてみた。

使用するソフトは fusion360
参考書を読みながら、試行錯誤しながら作ってみた。

3D-CADでの出来上がりはこんな感じ。サイズ感が分からないので、ユニバーサル基板や電池パックも書いてみた。
このおかげでかなり仕上がりのイメージがつかめた!やはり、視覚化するってとても大事。

f:id:motokiinfinity8:20180715014909j:plain

そして、3Dプリンタを所有する友人にお願いして作ってもらったのが以下。

f:id:motokiinfinity8:20180715020820j:plain


完成動画

マリオのコイン音の場合


息子の好きなピタゴラスイッチ音の場合

やはり、好きな音楽になると食いつきが違う!母の手を引っ張ってきて、「押してー!」とアピールしている様子が見える。
この様に、その時の好みに応じて柔軟に対応できるのが、このメロディ靴の良いところ。

LEDと振動モータをつけたのも良い傾向ですが、LEDを靴につけた分、下向きの姿勢になってしまうのが残念だった。
やはりLEDは体の正面にする必要がありそうですね。失敗したなぁ(>_<)
こりゃあ、さらに改良版が必要そうだ。

先は長いなぁ~ 頑張ろう!

OpenPoseを使って姿勢検出を試してみた

はじめに

ワークマシンでGPU環境を構築したので、早速いろいろと試してみたくなる。

子供らの遊び相手や息子のリハビリ支援に応用できそうなロボットを作る上で、子供らの姿勢状態を把握する仕組みは欲しいなぁ、と前から思っていたので、今回は姿勢推定(Human pose stimation)として有名どころであるOpenPoseを動かしてみた。

OpenPoseは、カーネギーメロン大学のZhe CaoらによってCVPR2017で発表された「Realtime Multi-Person 2D Pose Estimation using Part Affinity Fields」の有名な実装ライブラリで、人の関節位置合計18点を求めることができるとの事。

news.mynavi.jp

サンプルコード環境があるので、早速動かして動作確認してみた。

環境インストール

・CPU : Intel Core i7-8750H
・メモリ : 16GB
GPU : GTX1060
・OS : Ubuntu16.04 LTS

基本的には、CMU-Perceptual-Computing-Labのインストールマニュアルを参照すれば良い。
https://github.com/CMU-Perceptual-Computing-Lab/openpose/blob/master/doc/installation.md

ただし、先に言っておくと、、、私の場合、Cuda/cudnnバージョン関連で苦労して、なかなかうまくいなかった。

まずは関連パッケージのインストール

sudo apt-get install cmake-qt-gui
sudo apt-get install libviennacl-dev

その後、Git Clone

git clone https://github.com/CMU-Perceptual-Computing-Lab/openpose

その後、cmake-guiGUI起動する。
・[Where is the source code] → git cloneしてきたディレクトリのパス、[..../openpose/]を指定
・[Where to build the binaries] → [..../openpose/build]を指定
[Configure]ボタンを押して、[Unix Makefile]を選択して、[finish]。
最後に[Generate]ボタンを押して完了

その後、以下のコマンドでビルドをしかける。

cd build/
make -j`nproc`

動作確認(トラブルあり)

動作確認のため、サンプル動画を使って姿勢検出をしてみた。

./build/examples/openpose/openpose.bin --video examples/media/video.avi

しかし、ここでエラー…

Check failed: status == CUDNN_STATUS_SUCCESS (3 vs. 0) CUDNN_STATUS_BAD_PARAM

むむむ、cudnn関連でエラーがでて、コアダンプしている始末。 ググッて調べていたところ、CUDA/cudnnのバージョンに依存していそうな雰囲気。
なので、バージョンを変更していろいろとトライ。

・CUDA=9.2/ cudnn=7.1  ⇛ NG (同様のエラーメッセージ)
・CUDA=8.0/ cudnn=5.1 ⇛ NG (同様のエラーメッセージ)
・CUDA=8.0/ cudnn=6.0 ⇛ OK!

という事で、無事に姿勢検出できた!
しかし、CUDA/cudnnバージョン依存があるのは厄介だなぁ。。 (CUDA9.0環境でも動いた情報があるので、後日また見てみようと思う)

姿勢検出結果

Webカメラの画像の確認方法は以下のコマンド

./build/examples/openpose/openpose.bin

これを使って、子供たちの姿勢検出ができるかを試してみた。
f:id:motokiinfinity8:20180709233943p:plainf:id:motokiinfinity8:20180709233647p:plain
f:id:motokiinfinity8:20180709234319p:plainf:id:motokiinfinity8:20180709234320p:plain

腕を曲げてもらったり、ズリバイしてもらったりしましたが、それなりの精度で予測している事がよくわかる。
(我が家として、ズリバイでも姿勢を検出できている事が好印象)

最高峰のGPU GTX1080 を持ってしても、8fpsとは、、、なかなか重たい処理やなぁ。
最終的には、本処理をロボット内に搭載しようと考えていたので、最低でもJetson TXシリーズで実用レベルの負荷で動いてほしいところ。そこは慎重に処理削減に取り組む必要がありそう。


ただ、このアルゴリズムを応用すれば、Kinectなどの距離検出カメラをつかわず単眼での距離算出もできるみたい。体の構成から算出した個人同定も目指せそう。
息子がプロンボードやリハビリをしている際に、姿勢が崩れているかを判定できるし、それに応じてモニター画面を変えるなど、いろいろとリハビリ支援に活かせる可能性もありそう。

ソースコード中身に踏み込んだサイトは以下
robonchu.hatenablog.com


姿勢情報の使い方やアプリケーションへの応用方法、ROSなどのロボット本体への組み込みを考えて、これからもう少し中身を見ていきたいと思う。

【環境構築】Ubuntu16.04+GTX1080+Cuda9.2+cudnn7.1+ROS Kineticのワークマシン導入

はじめに

本格的にDeep Learningを学んでみようということで、自宅ワークマシンとして UbuntuGPU環境を構築してみた。ロボットの認識や行動計画に応用できればよいなぁ〜
(息子のリハビリ計画や行動把握に応用できれば尚良しだが、そこは使い方をしっかり考えないと…)

[環境]
・CPU : Intel Core i7-7700
・メモリ : 16GB
GPU : GTX1080
・OS : Ubuntu16.04 LTS

以下は導入手順を、備忘録も兼ねて記載していく

Ubuntu16.04導入

ここの手順は省略。
 ・ USBメモリにBootファイルを書き込む
 ・ USBメモリを挿入してUbuntu起動。画面手順にしたがってUbuntu
  (日本語版を選択)
 ・apt-get update → apt-get upgrade で最新に更新

あとは、日本語版Ubuntuディレクトリ名が初期状態では日本語になっているのが個人的には気持ち悪いので、これを英語に変更しておく

$ LANG=C xdg-user-dirs-gtk-update
$ LANG=ja_jp.UTF8

ROS kineticのインストール

ROS Wiki
http://wiki.ros.org/ROSberryPi/Installing%20ROS%20Kinetic%20on%20the%20Raspberry%20Pi
にしたがってインストール

$ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
$ sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
$ sudo apt-get update
$ sudo apt-get upgrade
$ sudo apt-get install ros-kinetic-desktop-full
$ sudo rosdep init
$ rosdep update
$ echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc 
$ source ~/.bashrc

完了したらroscoreを起動する事を確認できればOK!

GTX1080ドライバのインストール

リポジトリを登録してドライバをインストールします。GTX1080に限らず NVidia GPUは共通ドライバになっている様だ。現状の最新は 396 (2018-6-4)のため、これをインストールする。

sudo add-apt-repository ppa:graphics-drivers/ppa
sudo apt-get update
sudo apt-get install nvidia-396
sudo apt-get install mesa-common-dev
sudo apt-get install freeglut3-dev

ここで 再起動した後、nvidia-smiコマンドを叩いて、GPU情報が表示されればOK

$ nvidia-smi 
+-----------------------------------------------------------------------------+
| NVIDIA-SMI 396.26                 Driver Version: 396.26                    |
|-------------------------------+----------------------+----------------------+
| GPU  Name        Persistence-M| Bus-Id        Disp.A | Volatile Uncorr. ECC |
| Fan  Temp  Perf  Pwr:Usage/Cap|         Memory-Usage | GPU-Util  Compute M. |
|===============================+======================+======================|
|   0  GeForce GTX 1080    Off  | 00000000:01:00.0  On |                  N/A |
| N/A   40C    P8    11W /  N/A |    261MiB /  8111MiB |      0%      Default |
+-------------------------------+----------------------+----------------------+
                                                                               
+-----------------------------------------------------------------------------+
| Processes:                                                       GPU Memory |
|  GPU       PID   Type   Process name                             Usage      |
|=============================================================================|
|    0      1295      G   /usr/lib/xorg/Xorg                           181MiB |
|    0      1874      G   compiz                                        77MiB |
+-----------------------------------------------------------------------------+


Cuda9.2のインストール

現在の最新版は Cuda9.2なので、これを入れてみる

sudo apt-key adv --fetch-keys http://developer.download.nvidia.com/compute/cuda/repos/ubuntu1604/x86_64/7fa2af80.pub
echo "deb https://developer.download.nvidia.com/compute/cuda/repos/ubuntu1604/x86_64 /" | sudo tee /etc/apt/sources.list.d/cuda.list
sudo apt-get update
sudo apt-get -o Dpkg::Options::="--force-overwrite" install cuda-9-2 cuda-drivers
echo 'export PATH=/usr/local/cuda-9.2/bin${PATH:+:${PATH}}' >> ~/.bashrc
echo 'export LD_LIBRARY_PATH=/usr/local/cuda-9.2/lib64${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}}' >> ~/.bashrc
source ~/.bashrc

cudnn7.1のインストール

現在の最新版はcudnn7.1なので、これを入れてみる。

https://developer.nvidia.com/rdp/cudnn-download からcuDNN 7.1 for CUDA 9.2のdebパッケージをダウンロードする。
以下の3つを使用する
・cuDNN v7.1.* Runtime Library for Ubuntu16.04 (Deb)
・cuDNN v7.1.* Developer Library for Ubuntu16.04 (Deb)
・cuDNN v7.1.* Code Samples and User Guide for Ubuntu16.04 (Deb)
ダウンロードしたフォルダで以下のコマンドを実行して、cuDNNをインストールする

sudo dpkg -i libcudnn7_7.1*+cuda9.2_amd64.deb
sudo dpkg -i libcudnn7-dev_7.1*+cuda9.2_amd64.deb
sudo dpkg -i libcudnn7-doc_7.1*+cuda9.2_amd64.deb 

GPU動作確認

ここらで一度動作確認してみたいと思う。
/usr/local/cuda/samples/
に移動すると、Cuda動作確認のためのサンプルコードがあるので、これを動かしてみる。

良く見かけるケムリのシミュレーション表示をさせるコードを動かしてみる

cd 5_Simulations/smokeParticles
make
./smokeParticles

この様にケムリがフワフワ動く動画が確認できる

f:id:motokiinfinity8:20180705092433p:plain

ここで先ほどの nvidia-smi コマンドを動かしてGPU使用率が動いていることを確認する

nvidia-smi -l 1

(1秒ごとに表示)
|   0  GeForce GTX 1080    Off  | 00000000:01:00.0  On |                  N/A |
| N/A   49C    P2    44W /  N/A |    440MiB /  8111MiB |     23%      Default |

ちなみに、ノートPC(GEFORCE MX150搭載)にも同様の環境を構築して、ケムリシミュレーション時のGPU負荷を見たところ、常に100%状態だった。如何に GTX1080の処理性能が高いかを実感した。(まだまだ序の口だとは思いますが)

CPU/GPUの負荷&温度確認

これから先の検討の中で、どの程度の負荷状態かを把握しておきたいと思ったところ、簡易なGUI表示ツールである psensor を見つけた。

sudo apt-get install psensor

でインストールして、psensorで起動。
CPU/GPUの負荷%や温度がグラフ化されるので便利!

まとめ

とりあえず、ワークマシンにUbuntu16.04+GTX1080+Cuda9.2+cudnn7.1+ROS Kineticを構築した。OSインストールの機会はまたありそうなので、その時への備忘録として残しておく。

次はワークマシンに対して、DeepLearning環境を構築していきたいと思う。

【ROS】 Turtlebot3 のシミュレーション環境を構築してみた

はじめに

自宅で自作ロボットのNavigation環境を構築しようとしているが、モータ周りの動作が安定せず苦戦中。
市販で購入してきたギア内蔵モータのギア比が低めなのか、ちょっと動かすだけで半周したりとなかなかのじゃじゃ馬っぷり。

このままハードウェアの調整だけで時間を使っていくのが勿体ないので、気分転換に市販ロボットで勉強してみようと思う。

とはいえ、Turtlebot3 を購入するのはボーナス前のこの時期にはきついので、とりあえずシミューレション環境でお勉強。ちょうど手元のノートPCを一新して、Windows10とUbuntu16.04をDual Bootできる様にしたので、この環境に構築してみたいと思う。

手順

こちらのサイトを参考に、立ち上げてみる。
demura.net
emanual.robotis.com

$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_gazebo_plugin git

で一通りのTurtlebot3環境をclone。 .bashrcに以下の環境変数を追加。

export TURTLEBOT3_MODEL=burger

そして、catkin_makeをかけてGazebo起動!

$ roslaunch turtlebot3_gazebo  turtlebot3_world.launch 

んっ!? なんかエラーが出て起動しない。。。

process[rosout-1]: started with pid [4186]
started core service [/rosout]
process[gazebo-2]: started with pid [4194]
process[gazebo_gui-3]: started with pid [4211]
process[spawn_urdf-4]: started with pid [4220]
[ INFO] [1528459620.691972547]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1528459620.692481908]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
Error [parser.cc:581] Unable to find uri[model://sun]
Error [parser.cc:581] Unable to find uri[model://ground_plane]
[ INFO] [1528459622.261795752, 0.022000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.

どうやらmodelファイルが足りないらしい。いろいろと調べたら下記サイトに対策を発見。

How do you completely uninstall old versions of gazebo? - Gazebo: Q&A Forum

サイト通りに従って、
https://bitbucket.org/osrf/gazebo_modelsからosrf-gazebo_models.zipをダウンロード
②解凍ファイルを~/.gazebo/models に移動 (フォルダがなければ新規作成)
③ .bashrcに以下の記述を追加

export GAZEBO_PLUGIN_PATH=~/catkin_ws/src/turtlebot3_gazebo_plugin/build:${GAZEBO_PLUGIN_PATH}
export GAZEBO_MODEL_PATH=~/catkin_ws/src/turtlebot3_gazebo_plugin/models:~/catkin_ws/.gazebo/models:${GAZEBO_MODEL_PATH}

これで再度launchファイルを動かすと、、、起動した!!

ふぅ、焦った。。。 (環境開発はこれだから大変だ。苦行にならずにすみそう)

$  roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

で無事にGazebo上の Turtlebot3 を動かせた!

f:id:motokiinfinity8:20180608214931p:plain

この環境で Rvizが動くかも確認してみた。別ターミナルで以下のコマンドでRviz起動

roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch

f:id:motokiinfinity8:20180608215904p:plain

よし、Rvizでしっかりと LaserScan値も読めています。良い感じ!
シミュレータがあるって、よいねぇ〜



とりあえず、実機の調整に飽きたら、こちらのSimulation環境でNavigationを動かしていこう。

情報量は少なめですが、備忘録がわりとして今日はここまで

【ROS】Nintendo LABO(ニンテンドーラボ) Toy-Con+ESP8266を使ってROS対応二輪ロボットを動かしてみた

完成したもの

出来たものはこちら

youtu.be
f:id:motokiinfinity8:20180524220642j:plain

Nintendo LABOのバイクToy-Conを使って、ロボットを操作できる様にしてみた。
アクセスハンドルをひねるとロボットが前に動き出し、ハンドルを左右に傾けるとロボットも左右に移動する様な使い勝手。
ハンドル加減と曲がり具合はまだ調整中ですが、体験価値としては面白いかと。
娘も楽しそうに遊んでくれてました!(息子にはまだ早いので、息子向けにはToy-Conの見直しから)

ちなみに、ROS上で動かしているので、将来的には自立移動等と組み合わせたりも出来そう♪


作成動機

前回記事
ogimotokin.hatenablog.com
の続きで、「さーて! GW中にencorderからodometryを作成して、いよいよnavigationと結合するか~」と意気込んでいたのですが、
Nintendo LABO発売!
によってすべての計画が崩れてしまいました!
これは面白い!

あっという間に、娘のお手て(マーカー)を追いかけるロボットが出来ました(^-^)
これはただのダンボールで遊ぶおもちゃじゃないですね。子供の成功体験(作った!出来た!動いた!)を積ませて、「上手にできた!楽しい! もっと色々作りたい!」と好奇心を高めさせてくれる素敵な工作教材です(^-^)
ただ簡単に作るだけじゃなく、しっかり原理も学べるという。子供向けに「IRカメラ」とか「振動の早さ(周波数)を変える」とかをかんたんに説明していたりして、これはすごい!
さすが任天堂! 本当に子供心を掴むのがうまいですね!

気が付けばGWも終わり、すっかり計画破綻。
しかし、この体験価値を生かせないだろうか?と考え、「いっそ、娘の作ったToy-CONで実際のロボットも動いたら面白んでないか?」という事で早速トライしてみました。

システム構成(案)

思いついたのは以下の案
f:id:motokiinfinity8:20180524222806p:plain


Arduino対応無線モジュールesp8266(ESP-WROOM-02)マイコンをROS対応して、Nintendo Switch Joy-CONのジャイロセンサ情報をキャプチャーするという手段。Nintendo LABO側はトイコンガレージを使って、2つのジャイロセンサーの角度に応じて赤外線送信命令を送信するプログラムを作成してやればよいので、これからNintendo LABOをROS対応したと言えなくはない。

という訳で、まずはesp8266のROS対応から。
これは Wifi経由でrosserialノードを使う事ができる様だ。下記サイトを参考にしてプログラムを作成してみた。

GitHub - agnunez/espros: ROS serial for ESP8266 over WiFi

serverPortは11411を使用。clientからserverへの接続はesp8266サンプルプログラムと同様。加えて

  ros::NodeHandle nh;
  nh.getHardware()->setConnection(server, serverPort);
  nh.initNode();

を追加するイメージ。その後はrosserialと同じ使い方でpub/sub定義をすれば良い。(完成コードは後述)

課題

順調に制作を進めていたのだが、本家のNintendo LABOの開発が進まず。一番のブロッカーは娘と一緒に遊ぶ時間があわず通常キット5個が完成していない状況。元々娘と一緒に遊ぶ約束で購入したため、私が勝手にトイコンガレージを触ろうとすると
「わたしがまだやってないのに、先にやらんといてーーーー!!」
という始末。これは困った…

システム構成案(改)

という訳で、娘の決済がおりず、やむなくシステム変更。
f:id:motokiinfinity8:20180524222808p:plain

Conの代わりにジャイロセンサ×2を購入して、Toy-Conの段ボールに張り付ける。1つはハンドル部、1つは胴体部。

SODIAL(R) GY-521 MPU-60503軸ジャイロ+3軸加速度センサ
Amazon CAPTCHA

センサーの使い方はこちらのサイトを参考にした
こじ研(ESP センシング編)
https://www.ei.tohoku.ac.jp/xkozima/lab/espTutorial2.html

2つのジャイロセンサーとはI2Cで並列接続。区別できる様に片側のAD0ピンは電源PU抵抗をつけてHにしておく。
胴体部をAD0ピンをPUしてI2Cアドレス0x69、ハンドル部は未実装にしてI2Cアドレス0x68とする。
x,y,z方向の加速度情報を読み出し(読み出しは2byte×2、リトルエンディアンである事に注意)、分解能で割って加速度(G)に変換する。その後、加速度からセンサー対地角度を求める。あとは、ハンドル部(toy_acc)と胴体部(base_acc)との差分をもとにハンドルのひねり量を算出する。

そう、、、お気づきの通り、ここでもう Nintendo LABOは関係なくなってしまった(笑)

作成コード

①esp8266側 toycon_ros.ino

#include <ESP8266WiFi.h>
#include <ros.h>
#include <time.h>
#include <Wire.h>
#include <geometry_msgs/Twist.h>

#define MPU6050_TOY_ADDR 0x68
#define MPU6050_BASE_ADDR 0x69
#define MPU6050_AX  0x3B
#define MPU6050_AY  0x3D
#define MPU6050_AZ  0x3F
#define MPU6050_GX  0x43
#define MPU6050_GY  0x45
#define MPU6050_GZ  0x47

float base_acc_x, base_acc_y, base_acc_z;
float toy_acc_x,  toy_acc_y,  toy_acc_z;
float base_acc_angle_x, base_acc_angle_y, base_acc_angle_z;
float toy_acc_angle_x, toy_acc_angle_y, toy_acc_angle_z;

const char* ssid     = "********************";
const char* password = "******************";
IPAddress server(192,168,11,3);      // Set the rosserial socket ROSCORE SERVER IP address
const uint16_t serverPort = 11411;    // Set the rosserial socket server port

void setupWiFi() {                    // connect to ROS server as as a client
  if(DEBUG){
    Serial.print("Connecting to ");
    Serial.println(ssid);
    WiFi.begin(ssid, password);
  }
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
  }
  if(DEBUG){
    Serial.println("");
    Serial.println("WiFi connected");
    Serial.println("IP address: ");
    Serial.println(WiFi.localIP());
  }
}

// ROS nodes //
ros::NodeHandle nh;
geometry_msgs::Twist cmdvel_msg; 
ros::Publisher pub_cmdvel("/toycon/cmd_vel", &cmdvel_msg); 
 
void setup() {
  Serial.begin(115200);
  setupWiFi();
  delay(2000);
// Ros objects constructors   
  nh.getHardware()->setConnection(server, serverPort);
  nh.initNode();
  nh.advertise(pub_cmdvel);

  //  i2c as a master
  Wire.begin();
  Wire.beginTransmission(MPU6050_BASE_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission();

  Wire.beginTransmission(MPU6050_TOY_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission();
}

void loop() {
  if (nh.connected()) {
    //  send MPU6050_BASE_ADDR
    //  send start address
    Wire.beginTransmission(MPU6050_BASE_ADDR);
    Wire.write(MPU6050_AX);
    Wire.endTransmission();  
    Wire.requestFrom(MPU6050_BASE_ADDR, 14);

    //  get 14bytes
    short int AccX, AccY, AccZ;
    AccX = Wire.read() << 8;  AccX |= Wire.read();
    AccY = Wire.read() << 8;  AccY |= Wire.read();
    AccZ = Wire.read() << 8;  AccZ |= Wire.read();

    //  X/Y方向を反転
    AccX = AccX * -1.0;
    AccY = AccY * -1.0;

    // 取得した加速度値を分解能で割って加速度(G)に変換する
    base_acc_x = AccX / 16384.0; //FS_SEL_0 16,384 LSB / g
    base_acc_y = AccY / 16384.0;
    base_acc_z = AccZ / 16384.0;

    // 加速度からセンサ対地角を求める
    base_acc_angle_x = atan2(base_acc_x, base_acc_z) * 360 / 2.0 / PI;
    base_acc_angle_y = atan2(base_acc_y, base_acc_z) * 360 / 2.0 / PI;
    base_acc_angle_z = atan2(base_acc_x, base_acc_y) * 360 / 2.0 / PI;


    //  send MPU6050_TOY_ADDR
    //  send start address
    Wire.beginTransmission(MPU6050_TOY_ADDR);
    Wire.write(MPU6050_AX);
    Wire.endTransmission();  
    Wire.requestFrom(MPU6050_TOY_ADDR, 14);

    //  get 14bytes
    AccX = Wire.read() << 8;  AccX |= Wire.read();
    AccY = Wire.read() << 8;  AccY |= Wire.read();
    AccZ = Wire.read() << 8;  AccZ |= Wire.read();

    // 取得した加速度値を分解能で割って加速度(G)に変換する
    toy_acc_x = AccX / 16384.0; //FS_SEL_0 16,384 LSB / g
    toy_acc_y = AccY / 16384.0;
    toy_acc_z = AccZ / 16384.0;

    // 加速度からセンサ対地角を求める
    toy_acc_angle_x = atan2(toy_acc_x, toy_acc_z) * 360 / 2.0 / PI;
    toy_acc_angle_y = atan2(toy_acc_y, toy_acc_z) * 360 / 2.0 / PI;
    toy_acc_angle_z = atan2(toy_acc_x, toy_acc_y) * 360 / 2.0 / PI;

    // cmd_vel判定① : linear.x
    // TOY y角とBASE y角に対してTOY y角が90°以上になった場合、Baseとの差分角分の前進命令を送る
    float max_linear = 0.5;
    float start_angle = 92;
    float max_angle = 145;
    float diff_angle;
    diff_angle = toy_acc_angle_y - base_acc_angle_y;
    if(diff_angle < 0 )  diff_angle = 360 + diff_angle;
    
    if(diff_angle  < start_angle || base_acc_angle_y > 90 || base_acc_angle_y < -90){
      cmdvel_msg.linear.x = 0.0;
    }else if(diff_angle  > max_angle){
      cmdvel_msg.linear.x = max_linear;
    }else{
      cmdvel_msg.linear.x = max_linear * ( diff_angle - start_angle) / (max_angle - start_angle); 
    }

    // cmd_vel判定② :angular.z
    // BASE x角に応じて、左右方向の命令を送る
    if(cmdvel_msg.linear.x == 0.0){
      cmdvel_msg.angular.z = 0.0;
    }else if(base_acc_angle_x >= 35){
      cmdvel_msg.angular.z =0.5 * cmdvel_msg.linear.x /  max_linear;
      cmdvel_msg.linear.x = 0.0;  
    }else if(base_acc_angle_x <= -35){
      cmdvel_msg.angular.z =-0.5 * cmdvel_msg.linear.x /  max_linear;
      cmdvel_msg.linear.x = 0.0;  
    }else if(abs(base_acc_angle_x) < 35){
      cmdvel_msg.angular.z = base_acc_angle_x / 35.0;
    }

    // cmd_vel publish
    pub_cmdvel.publish( &cmdvel_msg );  
  } else {
    Serial.println("Not Connected");
  }
  nh.spinOnce();
  // Loop aprox. every  
  delay(50);  // milliseconds
}

raspberry pi側 aruduono_namual.launch

<?xml version="1.0"?>
<launch>
    <!-- Start joycon -->
    <node pkg="joy" name="joycon" type="joy_node"/>

    <!-- Start alphabot_manual node -->
    <node pkg="alphabot" name="AlphaBotManual" type="AlphaBotManual"  output="screen"/>

    <!-- Set to your serial port of esp8266 -->
    <arg name="toycon_serial_port" default="tcp"/>
    <arg name="toycon_baud_rate" default="57600"/>
    <node pkg="rosserial_python" type="serial_node.py" name="toycon_node" output="screen">
        <param name="port" type="string" value="$(arg toycon_serial_port)"/>
        <param name="baud" type="int" value="$(arg toycon_baud_rate)"/>
    </node>
</launch>
||<</span>

** 最後に
何気に、ESP8266をrosserialで動かす事が出来たのが一番の成果ではなかろうか。

【ROS】ラズパイで始めるROS BOT入門④ ~WaveShare社Alphabotの移動制御対応~

はじめに

ようやくラズパイ環境を構築できたので、いよいよ実物ロボットを使った自律移動を学んでいく。しかし、ここで問題になるのは、どのロボットを購入するかである。
安いと言われているTurtlebot3 Burgerで約7万円、RT社のマイクロマウスでも約5万円。小遣い制の父親エンジニアにはなかなか決裁が取れない様な金額である。

別途、mBot(1.3万円)は持っているのだが、これはモーターエンコーダなしのため、SLAM環境構築には厳しいと考える。
モーターエンコーダ付きの既存二輪走行ロボットでなんか安いのないかなぁー、とAmazonをさまよっていると……1万円以下で発見!!!


AlphaBot - Robotics

WaveShare社のAlphabotなる二輪走行ロボット。サイト情報からは、モーターエンコーダ搭載とあり、お値段はラズパイなしで当時(2017/11)で6500円!
Webサイトで調べるも全然レビュー例がないのではげしく不安だが、「この値段なら失敗してもまだなんとかなる!」と思い購入!

Alphabot組み立て

とりあえず無事に到着したので、組み立ててみる。
メーカーサイトにWikiがあり、組み立て説明書も以下のサイトにある。

AlphaBot - Waveshare Wiki
とりあえず手順に従って組み立てていく。
f:id:motokiinfinity8:20180507022501j:plainf:id:motokiinfinity8:20180508020809j:plain


一旦組み立て完了後の写真。思ってたより大きかった。Raspberry pi3 と Arduino Unoはボディ間にはさむ想定。(ただし、Arduinoはケースを外し、底はテープで絶縁する必要あり)

f:id:motokiinfinity8:20180508021104j:plain

各デバイス(モーター/エンコーダ/ライントレーサー/赤外線センサー)をRaspberry / Arduino のどちらと接続するかをジャンパーで切り替えれる様だ(左側の黄色ジャンパ) これは便利。また、RaspberryとArduino間はUARTで繋がってる様だ。これもスイッチでON/OFFできる。

f:id:motokiinfinity8:20180508021825j:plain

タイヤのエンコーダ。ギアボックス付きモーターにエンコーダ(黒い穴空き円盤)があり、フォトダイオードでパルス検出するタイプ。オシロでデジタル信号を確認したところ、キレイには取れていた!

不満点

便利に感じる面もあるが不満点も多々あり。
・底の補助輪(金属)の回転摩擦が大きく、移動するとガラガラ音が鳴る
・タイヤの付きが甘く外れやすい
・前後の重量バランスがシビアで、後ろ重心になるとタイヤが空回りしてしまう
・ラズパイのUSB電源とHDMIコネクタが内部モーター手前のため、ロボット状態でのケーブル抜き差し不可。
・バッテリーは見た目乾電池だが、乾電池ではなくリチウム電池18650を購入する必要あり。
私はamazonにて、以下のメーカー品を購入したのだが、安全最優先で保護回路付きにしたため、保護回路分電池が長く、無理やり押し込まないと入らない


結果、値段相当ではある印象。
まぁ、これを使いこなしてこそエンジニア!という事で、もう少し頑張ってみたいと思う。

ROSでのcmd_vel対応 (Arduinoマイコンでの左右モーター制御)

まずは走行ロボット制御の基礎である前後左右移動を実装する。操作はPS4コントローラのアナログ(左)スティックで行う。
ブロック図は以下。

f:id:motokiinfinity8:20180511014736j:plain

ラズパイ側では、PS4コントローラのjoyノードから通知されるjoy情報から速度情報(geometry::twist型/cmd_vel)を生成して、arduinoマイコンに通知する。arduinoマイコンはcmd_vel情報から、左右のモーターに必要なPWM値を算出して信号出力するイメージだ。

モーターの必要トルクについては不勉強なため、カット&トライアルを実施。モーターはPWMで制御。現状ではバッテリー80%で80/256くらいの回転数で動き出せる様だ。

Arduinoとのrosserial通信

個人的にROSの非常にありがたい機能であるroserial。これはシリアル通信を使ったROSメッセージを送る汎用プロトコル になり、これがらあれば非力な非OSのマイコンでもclientとしてのROS動作を実現できると非常に使いやすい!

rosserial 導入
sudo apt-get install ros-kinetic-rosserial-arduino ros-kinetic-rosserial
cd <sketchbook>/libraries
rm -rf ros_lib
rosrun rosserial_arduino make_libraries.py .

arduinoWindowsで書きこみたい場合は、作成したros_libをWindows内のarduino/library に置けばオッケー!(ros.hでインクルードエラーが置きなければ良い)

UART使用に向けて

Ubuntu MATE@Raspberry PiではdefaultでUARTが使えないため、以下の設定を実施してUARTを使用可能にする必要がある
①/boot/config.txtの以下の設定を有効にする
 core_freq=250
② /boot/cmdline.txtを以下に変更する
 dwc_otg.lpm_enable=0 console=tty1 root=/dev/mmcblk0p2 rootfstype=ext4 elevator=deadline rootwait quiet splash
③ raspi-configを実行
 「3:Interface Option」→「P5: Serial」→「login shel accessible: No」→「enable:Yes」
④再起動

作成コード

①raspberry側 alphabot_manual.launch

<?xml version="1.0"?>
<launch>
    <!-- Start joycon -->
    <node pkg="joy" name="joycon" type="joy_node"/>

    <!-- Start alphabot_manual node -->
    <node pkg="alphabot" name="AlphaBotManual" type="AlphaBotManual"  output="screen"/>

    <!-- Set to your serial port of Arduino -->
    <arg name="serial_port" default="/dev/ttyS0"/>
    <arg name="baud_rate" default="57600"/>
    <node pkg="rosserial_python" type="serial_node.py" name="serial_node" output="screen">
        <param name="port" type="string" value="$(arg serial_port)"/>
        <param name="baud" type="int" value="$(arg baud_rate)"/>
    </node>
</launch>



②raspberry側 alphabot_manual.cpp

#include <ros/ros.h>
#include <ros/wall_timer.h>
#include <math.h>
#include <sensor_msgs/Joy.h>
#include <geometry_msgs/Twist.h>

typedef enum {
	JOY_AXES_MANUAL_VERTICAL     = 1,   // Up or Down     (PS4:左スティック)      [joy_msg.axes]
	JOY_AXES_MANUAL_HORIZONTAL   = 0,   // Right or Left  (PS4:左スティック)      [joy_msg.axes]
} JOY_AXES;

typedef enum {
	JOY_BUTTON_MANUALBOOST        = 0,   // Set Manual Mode  (PS4:□ボタン)  [joy_msg.buttons]
} JOY_BUTTON;

#define UPDATE_TS 0.05

class AlphabotManual{
public:
	AlphabotManual();
	~AlphabotManual();
	void timerCallback(const ros::TimerEvent&);
	void JoyConCallback(const sensor_msgs::Joy &joy_msg);
private:
	ros::Timer timer;
	ros::NodeHandle nh;
	ros::Subscriber joy_sub;
	ros::Publisher twist_pub;
	geometry_msgs::Twist cmd_vel,p_cmd_vel;
};

AlphabotManual::AlphabotManual(){
	timer        = nh.createTimer(ros::Duration(UPDATE_TS), &AlphabotManual::timerCallback, this);
	joy_sub      = nh.subscribe("joy", 10, &AlphabotManual::JoyConCallback, this);
	twist_pub    = nh.advertise<geometry_msgs::Twist>("alphabot/cmd_vel", 10);
}

AlphabotManual::~AlphabotManual(){
}

/***************************************************************************
* PS4コントローラによる操作
***************************************************************************/
void AlphabotManual::JoyConCallback(const sensor_msgs::Joy &joy_msg){ 
	int boost = (joy_msg.buttons[JOY_BUTTON_MANUALBOOST] == 1) ? 3 : 1;
	cmd_vel.linear.x = joy_msg.axes[JOY_AXES_MANUAL_VERTICAL]*0.25*boost;
	cmd_vel.angular.z = joy_msg.axes[JOY_AXES_MANUAL_HORIZONTAL]*0.5*boost;
   	return;
}

void AlphabotManual::timerCallback(const ros::TimerEvent&){
	// cmd_vel更新
	twist_pub.publish(cmd_vel);
	if(p_cmd_vel.linear.x != cmd_vel.linear.x || p_cmd_vel.angular.z != cmd_vel.angular.z ){
		ROS_INFO("[Manual mode]linear.x: %0.2f, angular.z: %0.2f",cmd_vel.linear.x, cmd_vel.angular.z);
		p_cmd_vel = cmd_vel;
	}	
	return;
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "AlphaBotManual");
	AlphabotManual client;
	ros::spin();
	return 0;
}



arduino側 alphabot_cmdvel.ino

#include <ros.h>
#include <geometry_msgs/Twist.h>
//駆動に必要な最低PCMパルス数
#define L_PULSE_START 80 //駆動に必要な最低PCMパルス数
#define R_PULSE_START 80 //駆動に必要な最低PCMパルス数
#define PULSE_1MPS 80 //1.0m/sに必要なPCMパルス数

ros::NodeHandle nh;
int cmdvel_cnt=0;
int l_motor=0, r_motor=0;
std_msgs::Int32 count_msg;
void MotorCmdCallback(const geometry_msgs::Twist& msg){
cmdvel_cnt = 0;

// 進行方向設定
if(msg.linear.x > 0.0){
  l_motor = (int)(L_PULSE_START + PULSE_1MPS * msg.linear.x);
  r_motor = (int)(R_PULSE_START + PULSE_1MPS * msg.linear.x);
}else if(msg.linear.x < 0.0){
  l_motor = (int)(-1*L_PULSE_START + PULSE_1MPS * msg.linear.x);
  r_motor = (int)(-1*R_PULSE_START + PULSE_1MPS * msg.linear.x);
}else{
  l_motor = 0;
  r_motor = 0;
}
// 角度方向設定
if(msg.angular.z > 0.0){
  if(msg.linear.x == 0.0){
    l_motor = (int)(-1*L_PULSE_START - PULSE_1MPS * msg.angular.z * 0.75 /2);
    r_motor = (int)( R_PULSE_START + PULSE_1MPS * msg.angular.z * 0.75 /2); 
  }else if(msg.linear.x > 0.0){
    r_motor += PULSE_1MPS * msg.angular.z;
    l_motor -= PULSE_1MPS * msg.angular.z*0.5;
  }else{
    r_motor -= PULSE_1MPS * msg.angular.z;
    l_motor += PULSE_1MPS * msg.angular.z*0.5;
   }
 }else if(msg.angular.z < 0.0){
    if(msg.linear.x == 0.0){
      l_motor = (int)( L_PULSE_START + PULSE_1MPS * msg.angular.z * -0.75 /2);
      r_motor = (int)( -1 * R_PULSE_START - PULSE_1MPS * msg.angular.z * -0.75 /2); 
    }else if(msg.linear.x > 0.0){
      l_motor += PULSE_1MPS * msg.angular.z * -1;
      r_motor -= PULSE_1MPS * msg.angular.z*0.5 * -1;
  } else{
      l_motor -= PULSE_1MPS * msg.angular.z * -1;
      r_motor += PULSE_1MPS * msg.angular.z*0.5 * -1;
  }
}
  //限度設定
  if(l_motor > 255) l_motor = 255;
  if(l_motor < -255) l_motor = -255;
  if(r_motor > 255) r_motor = 255;
  if(r_motor < -255) r_motor = -255;
return;
}
bool MotorRun(int LS,int RS){ 
  if(LS >= 0 && LS <= 255){
    analogWrite(5, LS); 
    digitalWrite(14, HIGH); 
    digitalWrite(15, LOW);
  }
  if(LS < 0 && LS >= -255){
    analogWrite(5, abs(LS)); 
    digitalWrite(14, LOW); 
    digitalWrite(15, HIGH);
  } 
  if(RS >= 0 && RS <= 255){
    analogWrite(6, RS); 
    digitalWrite(17, HIGH); 
    digitalWrite(16, LOW);
  }
  if(RS < 0 && RS >= -255){
    analogWrite(6, RS); 
    digitalWrite(17, LOW); 
    digitalWrite(16, HIGH);
  }
  if  (RS > 255 || RS < -255 || LS > 255 || LS < -255){
    return false;
  } 
  return true;
}

bool MotorBrake(){ 
  digitalWrite(5,LOW);
  digitalWrite(6,LOW);
}
ros::Subscriber<geometry_msgs::Twist> sub_cmdvel("alphabot/cmd_vel", MotorCmdCallback);
void setup(){
  nh.initNode();
  nh.subscribe(sub_cmdvel);
}

void loop(){
//cmd_vel設定の反映
  if(cmdvel_cnt <= 20){ //cmd_vel命令が 1000ms(50*20)以内の場合
    MotorRun(l_motor, r_motor); 
    cmdvel_cnt++; 
  }else{
    MotorBrake(); 
  }
nh.spinOnce();
delay(50);
}



これで無事にPS4コントローラから二輪走行ロボットを動かすことが出来た。左右のトルクばらつきなどでまっすく進まないケースがあるため、そこについては微調整が必要になりそうだ。次回はOdometryによる自己位置推定のベース作成に行きたいと思う