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

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

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

【簡単IoT】ESP8266(+Amazon echo)で寝室からの呼び出しボタンを作ってみた

はじめに

このGWに二段ベッドを導入してようやく娘が一人で寝れる環境を作ることができた。ただ、残念ながら、娘は毎晩12時頃に目が覚めてしまい、寂しくて泣いてしまうのである。今までなら無言でリビングに出てくるのだが、二段ベッドの上だと寝ぼけてベッドから落ちるのが怖いらしい。かといって、大声で「おとーさーん!」と叫ばれると、下で寝てる息子を起こしかねない。

そこで、今回は寝室からリビングへ呼び出し通知するデバイス(要はナースコール)みたいなのを簡単に作ってみたいと思う。

要件

・寝室ベッドの横に呼び出しボタンを設置。それを押すとリビングで通知音が鳴る事。(寝室では音が鳴らない)
・夜中でも押しやすい様な大きなボタン
・電源から遠いので、コードレス推奨。

amazon echoによる家電制御を導入してからようやく我が家で認められる様になったAlexa! ここらで更に活躍を広げるため、娘の呼び出し通知機能を追加していこうと思う。

ハードウェア

コードレス(無線伝送/電池駆動)かつ超シンプル制御という要件から考えて、今回はArduino対応無線モジュールesp8266を使ってみたいと思う。

【ESP8266

www.switch-science.com
動作確認をするならこれが一番簡単。USB電源供給だしPC書き込み可能だし、書き込みボタンをも搭載されてるし。ただ今回は電池駆動にするため、USBコネクタは冗長になりそうなので、下記のモジュールそのものを使う。
www.switch-science.com
ソフト書き込み時は、ブレッドボード経由でUSBシリアル変換モジュール経由でPCから書き込む。

【ボタン】
娘と近くのコーナリングに行って押しやすそうなボタンを選ぶ。LEDプッシュライト NIT-BP1D で400円くらい。
www.kohnan-eshop.com

【3.3V出力用レギュレータ】
購入したプッシュライトが単4電池×3本だったので、3.6~4.5V→3.3V(マイコン電源)に変換するDC/DCとして、LM3671搭載 3.3V出力DC-DCコンバータを使ってみた。
https://www.switch-science.com/catalog/2638/


接続はシンプルなので文章のみ。
LEDプッシュライトからは、電池電源(3.6~4.5V)とGNDの2本を引っ張り、3.3Vレギュレータで変換した後、ESP8266の電源として使用。
今回の使い方としては、特に他に端子仕様しないので、超シンプル。

完成イメージはこちら。



f:id:motokiinfinity8:20180506174213j:plain
f:id:motokiinfinity8:20180506174201j:plain
f:id:motokiinfinity8:20180506174321j:plain




ソフトウェア

ESP8266に実際に書き込んだソースコードは以下。要は、ボタン押し=マイコン起動したら、リビングにあるラズパイに対してHttpメソッドのGetを投げるだけ。うん、すごく簡単。サンプルコード程度。

#include <ESP8266WiFi.h>
const char* ssid     = ***************;
const char* password = **************;
const char* host = "192.168.0.**";    //リモコン制御用RaspberryのIPアドレス
const char* path = "/bedroom_call";

void setup() {
  Serial.begin(115200);
  delay(100);

  // We start by connecting to a WiFi network
  Serial.print("Connecting to ");
  Serial.println(ssid);

  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.println("WiFi connected");  
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
  Serial.print("connecting to ");
  Serial.println(host);
}
  
void loop() {
  // Use WiFiClient class to create TCP connections
  WiFiClient client;
  const int httpPort = 1880;  //Node-REDの使用ポート
  if (!client.connect(host, httpPort)) {
    Serial.println("connection failed");
    return;
  }
   String body = "bedroom_call=1";
     
  // サーバにリクエストを送信(GET)
  client.print(String("GET ") + path +" HTTP/1.1\r\n" +
               "Host: " + host +":1880"+ "\r\n" + 
               "Connection: close\r\n\r\n");

  delay(1000); // Can be changed
  if (client.connected()) { 
    client.stop();  // DISCONNECT FROM THE SERVER
  }

  // 20秒待ち
  delay(20000); // Can be changed
}


本体ラズパイ側では、Node-REDを使用。寝室ボタンからのGETメソッドを受信したら、あらかじめ用意していたwavファイル(「~ちゃんが呼んでるでー!」)を再生するだけ。Google homeでは任意の音声を再生できるのだか、Amazon echoでは現時点でできなそう。text to speechはあるけど機械っぽい発音がいまいちなので。なので、echoをBluetoothスピーカーにしてラズパイから音声再生させる手段にしたいと思う。

なお、音声再生には node-red-contrib-spealkerpiを使用。

npm install node-red-contrib-speakerpi

でインストール可。


ほんま簡単。

ここでトラブル。Amazon echoから音声再生ができない!


Bluetooth経由でAmazon echo と接続。ごちゃごちゃやって、結果として paplay ではechoからの音声再生できたのですが、Node-REDからのSpeaker-Outノードで再生させようとすると、ラズパイ自身(HDMI or Audio Out)からの出力になってしまう。。。困った。

とりあえず、他にやりたい事もあったので、一旦はラズパイ自身に Audio DAC+スピーカを追加して、とりあえずロールアウト。
ちょっと悔しい。

この辺りの解決策、知っている人がいたら、是非とも教えて欲しいものだ。

作成結果

作成後の動画がこちら。

www.youtube.com

ピンポンパンポンの呼び出し音と、優しい関西弁での呼び掛け。text to speechでは出来ないコミカルな呼び出しボタンが出来ました。実際に、寝室の扉を閉めておけば、リビング音は聞こえないので、下の子を起こすこともない!

f:id:motokiinfinity8:20180506152005j:plain

運用を開始しましたが、娘は嬉しそうに使ってくれていて、早速活躍してます! 良かった♪ 「お父さん、こんなに早く出来たん!? すごい!」と尊敬と信頼も勝ち取りました(笑)

作成期間は約2晩(4時間×2日) ソフトは1h程度で、あとはハード構想&改造の時間。esp8266が思った以上に使えそうなのも収穫! これから使っていきます!

【ROS】ラズパイで始めるROS Bot入門③ ~Ubuntu MATEへROSインストール~

はじめに

先日の記事
ogimotokin.hatenablog.com
Ubuntu Server 16.04 LTSを立ち上げて開発していたが、色々と設定をいじっているとデスクトップ環境を壊してしまい、再インストールするはめに…

ただ、現状のUbuntu Server環境では下記の不満があった。
 ・起動が不安定。時々落ちる。
 ・環境インストールが地味に手間がかかる
 ・ VNC環境が構築できず、別クライアントPCからの遠隔開発ができない
ロボット開発という特性上、極力ロボットにHDMIケーブルは刺したくないので、やはりなんとかしておきたい。

という事で、色々と調べてみたら、Raspberry Pi に対してはUbuntu MATEディストリビューションが最近流行りの様だ。VNC環境の構築も簡単そうだしROSも普通に動きそうなので、この機会にUbuntu MATE 16.04 LTSに切り替えてみようと思う。

インストール手順

①公式サイトからUbuntu MATE 16.04 LTSのダウンロード

ubuntu-mate.org
ubuntu-mate-16.04.2-desktop-armhf-raspberry-pi.img.xz をダウンロード。xz形式で圧縮されているので、Windowsであれば「7-ZIP」などの解凍ソフトをつかってimgファイルを抽出。その後は、いつもの「win32diskimager」を使ってSDカードに書き込む。

②ラズパイセットアップ

書き込んだマイクロSDカードをラズパイに設定してAC電源入り。画面指示にしたがって、ネットワーク(無線)設定、言語設定、ユーザー名設定を行っていく。設定を終えたら、デスクトップが起動するまでひたすら待つ。片手間で出来るので楽ちん。

③日本語入力対応

日本語入力は以下のサイトを参考に。
deviceplus.jp
特にはまる事はなかった。

SSH/VNC設定

ssh設定は以下で実施。

sudo apt update
sudo apt upgrade
sudo apt install openssh-server

続いて、sshで使用する22番ポートの解放設定を実施。

sudo ufw allow 22 # sshの再起動
sudo /etc/init.d/ssh restart # 起動時にsshの起動
sudo systemctl enable ssh

他のPCからsshでログインできる事を確認すれば完了

VNC設定は以下で実施。

sudo apt-get install tightvncserver
vncserver :1

これでVNCサーバーが立ち上がるので、クライアントPCのVNCビューワーでIPアドレスにポート5091を指定すれば接続できる。
(この辺りはRasbianで実施した内容と同じ)

自動化する場合は、vncサーバの起動スクリプトの実行スクリプトをOS起動時に実行するため、以下のスクリプトを/etc/init.d/vncbootとして作成する。

#! /bin/sh
# /etc/init.d/vncboot
USER=pi
HOME=/home/pi
export USER HOME
case "$1" in
start)
echo "Starting VNC Server"
#Insert your favoured settings for a VNC session
su $USER -c '/usr/bin/vncserver :1 -geometry 1440x900 -depth 24'
;;
stop)
echo "Stopping VNC Server"
su $USER -c '/usr/bin/vncserver -kill :1'
;;
*)
echo "Usage: /etc/init.d/vncboot {start|stop}"
exit 1
;;
esac
exit 0

その後、以下のコマンドでinit.dで起動する様に修正する。

sudo chmod 755 /etc/init.d/vncboot
sudo update-rc.d vncboot defaults

これで再起動すればOK

★[2018/5/13追記]
無線AP接続設定を変更する場合、表示されるSSIDを選択するだけではうまくいかなかった。「新しいWi-Fiネットワークを作成」メニューに進み、「モード:クライアント」「セキュリティ:WPA & WPA2 Personal」「IPV4設定:DHCP自動 or 手動設定」を選択すれば変更できる。

⑤ROSセットアップ

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!

まとめ

Ubutu MATE+ROS環境が簡単に構築できてストレスなし。デスクトップ画面もカッコいいし。
前回に構築したOpen CV+Movidius環境がUbuntu MATEでも動くのかは不明だが、そこは慎重に見て行こう。

次回こそ、BOTの足回りの開発に着手できず、はず!

【簡単IoT】Node-REDを使ってAmazon echo で家電をON/OFFしてみた

動機

今年始めに購入したAmazon echo。残念ながら、天気予報と(ごくたまに)音楽再生する以外に使い道がなく、気がつけば電源すら抜かれてた始末(笑)
こりゃいかん!と思い、家電コントロールスキルを自作しようと企んでたのだが、ROSの勉強を優先させてしまい、放置状態。。

そんな中、昨日にNode-RED入門講座に遭遇して、「あれ、思ってた以上に簡単に作れそうかも!?」な感触を得たので、モチベーションの高いうちに早速取り組んでみた。

やりたいこと

amazon echoからリビングの「蛍光灯」「エアコン」「テレビ」「レコーダ」「扇風機」のON/OFFを音声操作する

・使い方想定1: 就寝直前にリビングの家電を消す場合。眠いのでリモコンを探すのが面倒。
・使い方想定2: 帰宅後にとっさに電気、エアコン等を付けたい場合。帰宅時はたいてい両手がふさがっているので、声で家電をONできると助かる。

実現するためのソフトウェア環境としては、Node-REDを使用する。Node-REDは、Webブラウザ上のGUIで簡単にWebサービスやハードウェアを接続するフローを書けるツール。
ラピッドプロト用ツールとして注目されている様なので、この使い方チュートリアルも踏まえてみた。

同様の事はIFTTTを使ってもできる様なのだが、その場合「Alexa、○○○をトリガー」という表現になるらしく、それが個人的には気に入らない。Node-REDスキルの場合、「でんきをつけて」「でんきをけして」という表現になりこちらの方がしっくりくるという理由もある。

ハードウェア

f:id:motokiinfinity8:20180416003411j:plain

ラズベリーパイ

 Node-REDが動いて、極力安価な環境なものを使う。
今回は家に余っていたRaspberry Pi2を使った(環境セットアップ済)
最終的には、コスト&省電力を考えてRaspberry Pi Zeroに移植する想定。

・赤外線送信機
 各家電に送信するための赤外線送信回路。時間短縮のため
既存モジュールを購入する方針。

 候補1: irMagician (大宮技研)
irMagician – 高機能/低価格赤外線リモコン | 大宮技研 合同会社
 よく使われている赤外線送信モジュール。USB(CDC-ACM)でホストに接続、リモコンコードの学習が可能。¥4000程度

候補2: ADSIRE学習リモコン基盤(Bit Trade One)
bit-trade-one.co.jp
 ラズパイ用シールドを想定した赤外線送信モジュール。10個の学習コードをGPIOトリガで送信可能。送信光を9個搭載して広視野角に対応。¥5000程度

 今回は、赤外線送信可能範囲と使いやすさ(GPIO制御のみで送信可)に注目して、【候補2:ADSIRE】を選んだ。

環境構築

Node-RED環境を構築する。こちらのサイトを参考。
ここを見ればほとんどやりたい事ができる。

「アレクサ、LEDつけて」でRaspberry PiからLEDを点灯する – oh-maker

ここにある
1. Alexa Home Skill Bridgeのアカウント作成 (web)
2. 制御対象となるデバイスを登録 (Web)
3. Node-REDのAlexaスキルの有効化 (スマホのAlexaアプリ)
4.Node-REDにAlexa Home Skill Bridgeノードを追加 (ラズパイ)
を実施していく。

はまったのは4の部分。Node-REDのGUI画面でどんだけ頑張っても、
f:id:motokiinfinity8:20180415234708p:plain
Alexa Home Skill Bridgeが見えない。。。

調べた結果、どうやらNode-REDのバージョンが古かった模様

update-nodejs-and-nodered 

でNode-REDを入れなおして、再度4を実施すれば無事に環境構築完了!

ちなみに、「○○をつけて」の○○はスマホのAlexaアプリから変更できます。
例えば、私は上記2のデバイス登録で「ライト」と名前をつけましたが、Alexaアプリでは「でんき」と変更しました。
これで、「Alexa、でんきをつけて」「Alexa、でんきをけして」という命令に反応してくれます。

ソフトウェア実装

実装したノードの全体イメージは以下
f:id:motokiinfinity8:20180416005716j:plain


一例として、光灯をつけるノードの拡大。
f:id:motokiinfinity8:20180416005744j:plain

1: Alexa-homeからのdeviceノード(この場合「ライト」)を措定
2: switchノードで msg.payload==true時と msg.payload==false時の分岐を記述
3: triggerノードでパルスを作成。本ボードでは赤外線送信は Lトリガのため、 0パルスで設定
4: rpi-gpio outノードで該当するGPIOピンにつなげる

こんな感じで比較的直感的に書ける!

学習基板の10個のリモコンコードはあらかじめ個別に学習させておく。
学習基板の取説通りの手順で、
SW1:電気ONコード
SW2:電気OFFコード
SW3:エアコンONコード ※季節に応じて「暖房」「冷房」を学習し直す必要あり
SW4:エアコンOFFコード
SW5 : テレビ電源ボタンコード
SW6 : レコーダ再生ボタンコード
SW7 : 扇風機電源ボタンコード

を配置してそれに対応したGPIOを4で制御する。
テレビ/扇風機などはON/OFFが同じ電源ボタンのため、ONとOFFの区別がつかない(つまり、テレビの電源が付いている状態で「テレビをつけて」というとテレビは消えてしまう)がそこは今回は妥協する。

実装結果

ここまでで約2時間で完成!

無事に、リビング電灯、エアコン、テレビ、レコーダをAmazon echoで操作できた!

早速、外出先からの帰宅時に使ってみたが、息子を抱っこしながら「Alexa、電気を付けて!」と言ったら即座に電気がつくとなんだか嬉しかった!
おもわずAlexaに「ありがとう!」と言うと、「よかったー♪」と嬉しそうに答えてくれて、ちょっと距離が近づいた気持ちになりました。

これからも使っていこう!

【アーム制御】CRANE+を使ったROSアーム制御②

ogimotokin.hatenablog.com

前回はMove it!を使ってアームを任意座標に持ってくるプログラムを作成したが、
残念ながら応答速度が非常に遅い事 & 移動できない座標位置が非常に多く現状使い物にならなかった。

なので、Move It!を使わずにCRANE+に適した形で逆運動学を解いてサーボモータ角度を直接制御するアプローチをトライしてみた。

CRANE+に対する逆運動学アプローチ

物理系は素人なので、まずは逆運動学(Inverse Kinematics)の算出方法の勉強から。

tajimarobotics.com

•順運動学 … 関節サーボの目標角度から指先の座標がどの様になるかを算出する問題
•逆運動学 … 指先の目標座標から間接サーボの角度を算出する問題

という事。解き方としては幾何学的にN個の連立方程式を解く問題が一般的な様ですね。当然ながら、複数解は発生するし、関節の多いアームほど計算が複雑になってくる訳で、なかなか素人では難しい。

しかし、このCRANE+はアーム部は4軸と自由度が少なめなので、これを利用して簡略化して考えてみた。アームの構成をおさらいすると、以下のサイトを参照頂きたい。

www.rt-shop.jp

まず、水平方向の制御はアーム根本のサーボモータ(Joint1)のみなので、水平方向(xy平面)の座標軸からサーボモータ角度は一意に決まる。一方、垂直方向を決めるサーボモータ数は3軸(つまり3変数)だが、2軸(Joint2/Joint3)をアーム位置制御用、1軸(Joint4)をアーム角度制御用という様に役割分担をすれば、単純化できそう!

という事でトライ!

f:id:motokiinfinity8:20180405235358j:plain
f:id:motokiinfinity8:20180405235653j:plain
f:id:motokiinfinity8:20180405235754j:plain



高校数学で勉強した余弦定理を使えば、とりあえず計算出来た!

これをコード実装した。一部抜粋。

#define L_B2 0.076	// base_link〜Shoulder(id2)のLink長さ
#define L_23 0.0825	// Shoulder(id2)~Elbow(id3)のLink長さ
#define L_34 0.094	// Elbow(id3)〜Wrist(id4)のLink長さ
#define L_4G 0.10	// Wrist(id4)〜Gripper_link先のLink長さ

void ArmCtrlTrack::timerCallback(const ros::TimerEvent&){
	// JOYCONからの操作情報をアップデート(ARM)
	if(pos_delta.x !=0 || pos_delta.y != 0 || pos_delta.z != 0){
		arm_pose.pose.position.x += pos_delta.x;
		arm_pose.pose.position.y += pos_delta.y;
		arm_pose.pose.position.z += pos_delta.z;
		ROS_WARN("[Manual]Move Arm_position[%f, %f, %f]", arm_pose.pose.position.x, arm_pose.pose.position.y, arm_pose.pose.position.z);
	}
	arm_MoveIK(arm_pose.pose.position, wrist_arg, &servo_arg);
}

void ArmCtrlTrack::arm_MoveIK(const geometry_msgs::Point gripper_position, const float wrist_arg, CraneARM_Servo_Arg *servo_arg){
	
	float theta_2d, theta_3d, theta_4d;
	float R_24 = sqrt(pow(gripper_position.x,2)+pow(gripper_position.y,2))-L_4G*cos(wrist_arg);
	float Z_24 = gripper_position.z + L_4G*sin(wrist_arg)- L_B2;

	theta_2d = acos((pow(L_23,2)-pow(L_34,2)+pow(R_24,2)+pow(Z_24,2))/(2*L_23*sqrt(pow(R_24,2)+pow(Z_24,2))));
	theta_3d = acos((pow(L_34,2)-pow(L_23,2)+pow(R_24,2)+pow(Z_24,2))/(2*L_34*sqrt(pow(R_24,2)+pow(Z_24,2))));
	theta_4d = atan2(Z_24, R_24);
	servo_arg->id2.data = theta_2d + atan2(Z_24, R_24)-M_PI/2;
	servo_arg->id3.data = theta_2d + theta_3d;
	servo_arg->id4.data = theta_4d-theta_3d+wrist_arg;

	servo_arg->id1.data = atan2(gripper_position.y, gripper_position.x);
	//ROS_WARN("[calc] id2:%f, id3:%f, id4:%f", servo_arg->id2.data, servo_arg->id3.data, servo_arg->id4.data);
	//ROS_WARN("[calc] id1:%f", servo_arg->id1.data);

	// サーボ角度Publish
	if(servo_arg->id4.data >= -M_PI/2 ||servo_arg->id4.data <= M_PI/2) joint4_pub.publish(servo_arg->id4);
	if(servo_arg->id3.data >= -M_PI   ||servo_arg->id3.data <= M_PI)   joint3_pub.publish(servo_arg->id3);
	if(servo_arg->id2.data >= -M_PI/2 ||servo_arg->id2.data <= M_PI/2) joint2_pub.publish(servo_arg->id2);
	joint1_pub.publish(servo_arg->id1);

	return;
}

解けない場合は servo_arg->idx.data = nan になるので、その場合はPublishさせない様にしないといけない点に注意。

実機動作結果

動画は以下
youtu.be

おぉ、MoveIt!使用時に比べて、座標指定に対する応答が早いっ!
PS4コントローラのアナログスティックの動きに追従してくれるので、もう少し作り込めばUFOキャッチャーみたいな操作感になりそう!


次回は、カメラによる物体認識機能も加えて、ペットボトルの位置を自動検出してボトルを把持するプログラムを作ってみよう!

【アーム制御】CRANE+を使ったROSアーム制御①

はじめに

Turtlebot用ロボットアームを期間限定でレンタルできたので、勉強がてら遊んでみたいと思う。
うまく使えそうならば、ロボットアームを別途自作して、息子の食事介護支援などに応用できればよいなぁ、と企んでいます。

最初の目標 : グリッパー位置を任意の三次元位置に持っていく事。

使用ハードウェア

RT社製CRANE+
www.rt-shop.jp

アーム自由度4軸+ハンド部1軸だそうです。サーボモータはDynamixel AX-12A。電源は12V。
USBシリアル通信で制御きるので、HostはROSが動く環境であれば何でもよさそう。

HW動作確認は、以下のサイトが詳しいのでこちらを見ながら進める。
個別のサーボモータを動かすことができればOK
www.rt-shop.jp

ソフトウェア環境構築

5つのサーボモータの角度をそれぞれアームの期待動作に応じて、個別計算&更新していく手段がまず考えられるが、
せっかくROSを使うのでROS既存パッケージを使った動作を試してみたいと思う。
そこで、マニュピュレータ制御用パッケージ moveit! を使ったアーム制御環境を構築する
参考にしたのは、以下のサイト
ROSを使用した画像処理とマニピュレータ制御 by gbiggs
このサイトに従って進めていく。
まずはインストール。

sudo apt-get install ros-kinetic-dynamixel-motor
sudo apt-get install ros-kinetic-moveit-*

上記で一旦インストールを完了させた後、CRANE+のROSパッケージをダウンロードしコンパイルする。

cd ~/catkin_ws/src/
git clone https://github.com/gbiggs/crane_plus_arm.git
cd ../
catkin_make

CRANE+のパッケージの詳細説明は引用サイトに記載されていますので、ここでは割愛。
ちなみに、私が途中でハマったのは、USB制御ポートが異なった場合の対応方法。
defaultでは、/dev/ttyUSB0 で制御する想定なのだが、Host PCのUSB構成次第ではここ以外を使わざるを得ない場合がある。
その場合は、crane_plus_arm/crane_plus_hardware/config/servo_controller_manager.yaml 内のport_name以下を変更すればよい。

moveitを使って任意位置にアームを移動

まず第一段階として、ユーザー操作で任意の位置にアームを動かす制御プログラムを作成する。
アーム位置を設定する手段として、PS4用コントローラを使用する。
PS4コントローラのアナログスティックを動かして、アーム位置をマニュアルで変更できる様にする。

制御ノートの追加

ワークスペース内に制御パッケージを追加する。

cd ~/catkin_ws/src/
catkin_create_pkg arm_control roscpp moveit_core moveit_ros_planning_interface moveit_visual_tools moveit_msgs moveit_commander tf actionlib control_msgs geometry_msgs shape_msgs trajectory_msgs

パッケージ内のCMakeLists.txtで以下を追加する

①find_packageに記載追加
find_package(Boost REQUIRED
  system
  filesystem
  date_time
  thread
)
②include_directoriesにBoostのディレクトリを追加。
include_directories(
  ${catkin_INCLUDE_DIRS}
  ${Boost_INCLUDE_DIR}
)
③コンパイル情報を追加
add_executable(ArmCtrlTrack src/arm_ctrl_track.cpp)
target_link_libraries(ArmCtrlTrack
 ${catkin_LIBRARIES}
 ${Boost_LIBRARIES}
)
制御プログラムの作成

C++で実際に実装する。ポイントとしては、
 ・createTimer関数で定期的に実行関数をCallbackして、その関数内でarm座標をpublish(moveit関数を使用)
 ・PS4コントローラ(joy)のSubscribe関数からアナログスティック情報を取得して座標情報(private変数)を都度更新
である。

アーム移動は、参照サイト同様に、geometry_msgs::PoseStamped型で座標定義してMoveIt!関数を使って移動先を指示&実行する。この時の座標系はbase_link、初期座標は arm.setNamedTarget("resting")に移動した後は、奥行きX = 0.18[m]、水平Y=0[m]、高さZ=0.10[m]からスタートする。この座標設定はCRANE+が移動可能な場所を実験的に求めている。

グリッパー開閉は、参照サイト同様に、actionlibを利用し、control_msgs/GripperCommandActionアクションを使います。control_msgs/GripperCommandGoal.positionにグリッパー幅を指示する。初期値は0.10[m]でOpen状態とする。

#include <ros/ros.h>
#include <ros/wall_timer.h>
#include <math.h>
#include <sensor_msgs/Joy.h>
#include <actionlib/client/simple_action_client.h>
#include <moveit/move_group_interface/move_group.h>
#include <control_msgs/GripperCommandAction.h>
#include <geometry_msgs/PoseStamped.h>

class ArmCtrlTrack{
public:
	ArmCtrlTrack();
	~ArmCtrlTrack();
	void SetInitialPose();
	void timerCallback(const ros::TimerEvent&);
	void JoyConCallback(const sensor_msgs::Joy &joy_msg);
private:
	ros::Timer timer;
	ros::NodeHandle nh;
	ros::Subscriber joy_sub;
	geometry_msgs::PoseStamped arm_pose;
	geometry_msgs::Point pos_delta;			// 現在位置からのposition差分値
	control_msgs::GripperCommandGoal g_goal;
	float g_delta;

	moveit::planning_interface::MoveGroup arm_{"arm"};
	actionlib::SimpleActionClient<control_msgs::GripperCommandAction> gripper_{"/crane_plus_gripper/gripper_command"};
};

ArmCtrlTrack::ArmCtrlTrack(){
	timer   = nh.createTimer(ros::Duration(0.1), &ArmCtrlTrack::timerCallback, this);
	joy_sub = nh.subscribe("joy", 10, &ArmCtrlTrack::JoyConCallback, this);

	arm_pose.header.frame_id = "base_link";
	arm_pose.pose.position.x = 0.18;
	arm_pose.pose.position.y = 0.0;
	arm_pose.pose.position.z = 0.10;
	arm_pose.pose.orientation.x = 0.0;
	arm_pose.pose.orientation.y = 0.707106;
	arm_pose.pose.orientation.z = 0.0;
	arm_pose.pose.orientation.w = 0.707106;
	pos_delta.x = 0;
	pos_delta.y = 0;
	pos_delta.z = 0;
	g_delta = 0;
	gripper_.waitForServer();
	SetInitialPose();
}

ArmCtrlTrack::~ArmCtrlTrack(){
}

void ArmCtrlTrack::SetInitialPose(){
	// Planning interface for the arm
	arm_.setPoseReferenceFrame("base_link");
	arm_.setNamedTarget("resting");
	arm_.asyncMove();

	g_goal.command.position = 0.1;
	gripper_.sendGoal(g_goal);
}

void ArmCtrlTrack::JoyConCallback(const sensor_msgs::Joy &joy_msg){
	// 左スティック 垂直方向 : ARM y軸移動, 水平方向: ARM x軸方向、
	// 右スティック 垂直方向 : ARM Z軸移動, 水平方向, Griper Open/close
	pos_delta.x = joy_msg.axes[5] / 100 * ARM_UPDATE_TS * 2;
	pos_delta.y = joy_msg.axes[0] /100 * ARM_UPDATE_TS  *2;
	pos_delta.z = joy_msg.axes[1] / 100 * ARM_UPDATE_TS *2;
	g_delta     = joy_msg.axes[JOY_AXES_RIGHT_HORIZONTAL] / 100 * ARM_UPDATE_TS * 5;
}

void ArmCtrlTrack::timerCallback(const ros::TimerEvent&){
	// JOYCONからの操作情報をアップデート(ARM)
	if(pos_delta.x !=0 || pos_delta.y != 0 || pos_delta.z != 0){
		arm_pose.pose.position.x += pos_delta.x;
		arm_pose.pose.position.y += pos_delta.y;
		arm_pose.pose.position.z += pos_delta.z;
		ROS_WARN("[MOVEIT]Move Arm_position[%f, %f, %f]", arm_pose.pose.position.x, arm_pose.pose.position.y, arm_pose.pose.position.z);

		// ARM更新
		arm_.setPoseReferenceFrame("base_link");
		arm_.setPoseTarget(arm_pose);
		arm_.setGoalTolerance(0.02);
		arm_.asyncMove();
	}

	// JOYCONからの操作情報をアップデート(GRIPPER)
	if(g_delta != 0){
		g_goal.command.position += g_delta;
		if(g_goal.command.position < 0)	g_goal.command.position = 0;

		ROS_WARN("[MOVEIT]Move Gripper_position[%f]", g_goal.command.position);
		gripper_.sendGoal(g_goal);
	}
}

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

実装していく中でハマったポイントは以下の二点。(参照サイトに記載されたサンプルコードとの差分)
①move実行関数を arm_.Move() で実装した場合、動作完了するまでMove()関数から抜けられずそのまま座標更新ができなかった。そこで、非同期処理のarm_.asyncMove()に置き換える事で座標更新ができる様になった
②最初はmoveit::planning_interface::MoveGroup arm_("_arm")を:timerCallback関数内で都度定義していたが、その処理時間が想定以上にかかっていてtimerCallback処理時間溢れが発生していた。そこで、Classのprivate変数にコンストラクタ処理を含めて用意する方法に変更した。 (C++でClass変数設定時にコンストラクタを記載する手順を知らなくて苦戦した。。{}で記載できるんですね)
③コンストラクタ処理でgripper_.waitForServer()は必要。これがないと、ActionServerが準備できていない状態でClient側からコマンドが投げられてしまいエラーとなってしまう。

動作させるためのLaunchファイルも作成。制御用パッケージ内にlaunchフォルダを作成して、その中に以下の記載を追加。

[arm_ctrl_tracking.launch]
<?xml version="1.0"?>
    <include file="$(find crane_plus_hardware)/launch/start_arm_standalone.launch" />
    <include file="$(find crane_plus_moveit_config)/launch/move_group.launch" />
    <node pkg="joy" name="joycon" type="joy_node"/>
    <node pkg="arm_control" name="ArmCtrlTrack" type="ArmCtrlTrack"  output="screen"/>
</launch>
動作確認

動画撮影中。

意図した動作イメージを実現する事ができ、当初の目標は達成できた気がする。
しかし、応答速度が非常に遅い事 & 移動できない座標位置が非常に多い。。。。どうやらMoveit!自体は6軸アーム前提のソフトウェアのため、CRANE+の様な4軸アームに対しては逆運動学の式が最適に解けないのでは??
なので、次回はMoveit!を使用せず、CRANE+のハードウェア仕様に特化して逆運動学を自力で解くプログラム作成にチャレンジしてみようと思う。果たして、レンタル期間中に間に合うか!?

【組込みDeep Learning】Movidius + raspberry pi+ROS環境構築

Movidius を使ってみる

f:id:motokiinfinity8:20180209212816j:plain

Caffe/Tensorflowに対応した組み込み向けVPUスティックデバイス Movidius。 Intelに買収される前から目をつけていたのですが、いつの間にやらここまで有名になるとは。
ラズパイの様なGPUを持たないマシンでもDeep Learningをエッジ側で使えるので、使いこなせれば非常に便利であろう。

自作ROSロボットで、子供の顔を認識させて追従させる様なロボットを作りたかったので、ここはDeep Learning環境の構築に前向きにトライしてみます!

まずは、SDKを立ち上げてみた。
ここのサイトを参考にました。

【Movidius™NCS&RaspberryPi】リアルタイム物体認識【TensorFlow】 - Qiita

Tensorflowのインストール

ARM用Tensorflowはビルドでトラブりがちなので、コンパイル済を取ってくる。

wget https://github.com/lhelontra/tensorflow-on-arm/releases/download/v1.3.1/ten
sorflow-1.3.1-cp35-none-linux_armv7l.whl
sudo pip3 install tensorflow-1.3.1-cp35-none-linux_armv7l.whl 
SDKのインストール

公式サイトにしたがってインストール。バージョンは1.11.00

git clone http://github.com/Movidius/ncsdk && cd ncsdk && make install && make examples

OpenCVもビルドしようとするがこれは失敗するはず。前投稿の手順にしたがって事前にインストールしておくこと。

サンプル動作をさせてみる

SDK内にCaffe,Tensorflowの学習済モデルをMovidius用graphファイルに変換したものが入っているので、これを利用する。とりあえず参考サイト通り、GoogleのInceptionV3モデルを使ってみた。

cd ncsdk/examples/tensorflow/inception_v3
python3 run.py

で実行。例えば、下記のルリコシボタンインコの写真の場合、認識結果は以下。
f:id:motokiinfinity8:20180209223407j:plain

                                                                                                          • -

91 lorikeet 0.52588
93 bee eater 0.11914
89 macaw 0.023178
12 goldfinch, Carduelis carduelis 0.0059319
137 European gallinule, Porphyrio porphyrio 0.0055695

                                                                                                            • -

lorikeet (インコ)なので正解!
なお、認識時間は 3200×2400の写真で 0.53秒。ざっくり計算でVGA(640×480)ならば30fpsは出そうな雰囲気(実際は画像処理のオーバーヘッドの方が大きそう) なかなか優秀だと思う。

なお、run.py のコードは以下

from mvnc import mvncapi as mvnc
import sys
import numpy
import cv2
import time

path_to_networks = './'
path_to_images = '../../data/images/'
graph_filename = 'graph'
image_filename = path_to_images + 'bird.jpg'

start = time.time()

#mvnc.SetGlobalOption(mvnc.GlobalOption.LOGLEVEL, 2)
devices = mvnc.EnumerateDevices()
if len(devices) == 0:
    print('No devices found')
    quit()

device = mvnc.Device(devices[0])
device.OpenDevice()

print("Open device : {0}".format(time.time()-start) + "[sec]")

#Load graph
with open(path_to_networks + graph_filename, mode='rb') as f:
    graphfile = f.read()

#Load preprocessing data
mean = 128
std = 1/128

#Load categories
categories = []
with open(path_to_networks + 'categories.txt', 'r') as f:
    for line in f:
        cat = line.split('\n')[0]
        if cat != 'classes':
            categories.append(cat)
    f.close()
    print('Number of categories:', len(categories))

#Load image size
with open(path_to_networks + 'inputsize.txt', 'r') as f:
    reqsize = int(f.readline().split('\n')[0])

graph = device.AllocateGraph(graphfile)

print("Open graph : {0}".format(time.time()-start) + "[sec]")

img = cv2.imread(image_filename).astype(numpy.float32)
dx,dy,dz= img.shape
delta=float(abs(dy-dx))
if dx > dy: #crop the x dimension
    img=img[int(0.5*delta):dx-int(0.5*delta),0:dy]
else:
    img=img[0:dx,int(0.5*delta):dy-int(0.5*delta)]
img = cv2.resize(img, (reqsize, reqsize))

img=cv2.cvtColor(img,cv2.COLOR_BGR2RGB)

for i in range(3):
    img[:,:,i] = (img[:,:,i] - mean) * std

print('Start download to NCS...')
graph.LoadTensor(img.astype(numpy.float16), 'user object')
output, userobj = graph.GetResult()

print("get tensor result: {0}".format(time.time()-start) + "[sec]")

top_inds = output.argsort()[::-1][:5]

print(''.join(['*' for i in range(79)]))
print('inception-v3 on NCS')
print(''.join(['*' for i in range(79)]))
for i in range(5):
    print(top_inds[i], categories[top_inds[i]], output[top_inds[i]])

print(''.join(['*' for i in range(79)]))
graph.DeallocateGraph()
device.CloseDevice()
print('Finished')

MovidiusのROS対応

上記のMovidius SDKでは、Python3を使っていた。残念ながら、ROSは、Python2.7系がdefaultのため、Python3系を使うのはなかなかハードルが高そう。どうしようか悩んでいたところ、以下のサイトを発見。
github.com

どうやら、公式にROS対応している様だ。これはありがたい。

Install NCSDK v1.11.00
Install NC APP Zoo

が必要とのことで、GitHub - movidius/ncappzoo: Contains examples for the Movidius Neural Compute Stick.から NC APPをインストール

git clone https://github.com/movidius/ncappzoo
mv ncappzoo /opt/movidius/ncappzoo
ROSパッケージをインストール

参考サイトの手順にしたがって進める。

# Building
cd ~/catkin_ws/src
git clone https://github.com/intel/ros_intel_movidius_ncs.git
cd ros_intel_movidius_ncs
git checkout master
cd ~/catkin_ws
catkin_make
# Installation
catkin_make install
source install/setup.bash
# Copy label files from this project to the installation location of NCSDK
cp ~/catkin_ws/src/ros_intel_movidius_ncs/data/labels/* /opt/movidius/ncappzoo/data/ilsvrc12/

ただし、x86ベースのため、catkine_make時に sse4 エラーが発生する。そのため、
ros_intel_movidius_ncs/src/CMakeLists.txt にある下記コマンドオプションをコメントアウトしておく

  # Add x86 intrinsic compiler support
  #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse4.1")
  #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mf16c")
  #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native")

先ほどと同じ様に、、GoogleのInceptionV3モデルを使ってみる。

cd /opt/movidius/ncappzoo/tensorflow/inception_v3
make

RuntimeError: module compiled against API version 0xc but this version of numpy is 0xb
Segmentation fault (core dumped)
Makefile:41: recipe for target 'weights' failed
make: *** [weights] Error 139

でコケた。どうやら、numpyが1.13.0と1.11.0の2つが多重インストールされている様なので、pip3 uninstall numpy を2回かけた後、pip3 install numpy で 1.14.0をインストールしてみる。これで、ビルドが進んだ!

ちなみに、AlexNetを使ってみる場合は以下のコマンド。

cd /opt/movidius/ncappzoo/caffe/AlexNet
make

その後、USBカメラからの映像を確認する

roslaunch movidius_ncs_launch ncs_stream_classification_example.launch camera_topic:="/usb_cam/image_raw"

これで行けそうな見込み! と思いきや、

process[movidius_ncs_example_stream_classification-1]: started with pid [15135]

から動かず映像でず。解析はもう少しかかりそう。

ラズパイ(Ubuntu16.04)にOpenCV 3.3.0をインストール

さて、ROS(kinetic)環境を構築した訳だが、本格的なロボット開発をする前に、環境構築で苦戦が予想される画像処理&Deep Learning環境を先に構築しておきたいと思う。
まずは、ラズパイ(ubuntu16.04)環境にOpenCVを入れる。将来的にトラッキングなども行いたいので、3系を入れることとする。

Open CV 3.3.0 インストール

qiita.com
を参考に。

ただし、このままだとmake時にコケるため、以下の二点は注意が必要

 ① .bashrcから ROS向けの下記設定をコメントアウトしておく必要あり
(build時に ROSのOpenCV関連パッケージを見に行ってしまうらしい)

#source /opt/ros/kinetic/setup.bash
 
 ② cmake時のオプションは変更が必要

/usr/include/GL/glext.h:466:19: error: conflicting declaration ‘typedef std::ptrdiff_t GLsizeiptr’
typedef ptrdiff_t GLsizeiptr;

どうやらQT4 のパッケージが OpenGL ESベースでコンパイルされているのに、Open GLを指定していたため、定義の競合が起きた様子。そのため、OPENGL=ON → OPENGL-ES=ONに変更すれば解決!


念の為、手順メモを残す。

sudo apt-get install cmake libeigen3-dev libgtk-3-dev qt5-default freeglut3-dev \
libvtk6-qt-dev libtbb-dev ffmpeg libdc1394-22-dev libavcodec-dev libavformat-dev \
libswscale-dev libjpeg-dev libjasper-dev libpng++-dev libtiff5-dev \
libopenexr-dev libwebp-dev libhdf5-dev libpython3.5-dev python3-numpy \
python3-scipy python3-matplotlib libopenblas-dev liblapacke-dev

wget https://github.com/opencv/opencv/archive/3.3.0.tar.gz

tar xvf 3.3.0.tar.gz

mkdir opencv-3.3.0/build && cd opencv-3.3.0/build
cmake -G "Unix Makefiles" --build . -D BUILD_CUDA_STUBS=OFF -D BUILD_DOCS=OFF \
-D BUILD_EXAMPLES=OFF -D BUILD_JASPER=OFF -D BUILD_JPEG=OFF -D BUILD_OPENEXR=OFF \
-D BUILD_PACKAGE=ON -D BUILD_PERF_TESTS=OFF -D BUILD_PNG=OFF -D BUILD_SHARED_LIBS=ON \
-D BUILD_TBB=OFF -D BUILD_TESTS=OFF -D BUILD_TIFF=OFF -D BUILD_WITH_DEBUG_INFO=ON \
-D BUILD_ZLIB=OFF -D BUILD_WEBP=OFF -D BUILD_opencv_apps=ON -D BUILD_opencv_calib3d=ON \
-D BUILD_opencv_core=ON -D BUILD_opencv_cudaarithm=OFF -D BUILD_opencv_cudabgsegm=OFF \
-D BUILD_opencv_cudacodec=OFF -D BUILD_opencv_cudafeatures2d=OFF -D BUILD_opencv_cudafilters=OFF \
-D BUILD_opencv_cudaimgproc=OFF -D BUILD_opencv_cudalegacy=OFF -D BUILD_opencv_cudaobjdetect=OFF \
-D BUILD_opencv_cudaoptflow=OFF -D BUILD_opencv_cudastereo=OFF -D BUILD_opencv_cudawarping=OFF \
-D BUILD_opencv_cudev=OFF -D BUILD_opencv_features2d=ON -D BUILD_opencv_flann=ON \
-D BUILD_opencv_highgui=ON -D BUILD_opencv_imgcodecs=ON -D BUILD_opencv_imgproc=ON \
-D BUILD_opencv_java=OFF -D BUILD_opencv_ml=ON -D BUILD_opencv_objdetect=ON \
-D BUILD_opencv_photo=ON -D BUILD_opencv_python2=ON -D BUILD_opencv_python3=ON \
-D BUILD_opencv_shape=ON -D BUILD_opencv_stitching=ON -D BUILD_opencv_superres=ON \
-D BUILD_opencv_ts=ON -D BUILD_opencv_video=ON -D BUILD_opencv_videoio=ON \
-D BUILD_opencv_videostab=ON -D BUILD_opencv_viz=OFF -D BUILD_opencv_world=OFF \
-D CMAKE_BUILD_TYPE=RELEASE -D WITH_1394=ON -D WITH_CUBLAS=OFF -D WITH_CUDA=OFF \
-D WITH_CUFFT=OFF -D WITH_EIGEN=ON -D WITH_FFMPEG=ON -D WITH_GDAL=OFF -D WITH_GPHOTO2=OFF \
-D WITH_GIGEAPI=ON -D WITH_GSTREAMER=OFF -D WITH_GTK=ON -D WITH_INTELPERC=OFF -D WITH_IPP=ON \
-D WITH_IPP_A=OFF -D WITH_JASPER=ON -D WITH_JPEG=ON -D WITH_LIBV4L=ON -D WITH_OPENCL=ON \
-D WITH_OPENCLAMDBLAS=OFF -D WITH_OPENCLAMDFFT=OFF -D WITH_OPENCL_SVM=OFF -D WITH_OPENEXR=ON \
-D WITH_OPENGL-ES=ON -D WITH_OPENMP=OFF -D WITH_OPENNI=OFF -D WITH_PNG=ON -D WITH_PTHREADS_PF=OFF \
-D WITH_PVAPI=OFF -D WITH_QT=ON -D WITH_TBB=ON -D WITH_TIFF=ON -D WITH_UNICAP=OFF \
-D WITH_V4L=ON -D WITH_VTK=OFF -D WITH_WEBP=ON -D WITH_XIMEA=OFF -D WITH_XINE=OFF \
-D WITH_LAPACKE=ON -D WITH_MATLAB=OFF ..

make -j4

sudo make install


これでOK! ちゃんとインストールされたか確認するためには、
python3 で

import cv2
cv2.__version__  → '3.3.0'

と表示されれば問題なし!

次は、いよいよ画像認識環境の立ち上げです!