#◆ 前回記事
RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_005日目_Arduino+モーター制御 の続き
#◆ はじめに
前回はArduinoを使用してハードウェアボタン始動によるモーター制御を試行した。
今回は外部からBluetooth接続+シリアル通信により、遠隔操作での始動と停止を試行したい。
テスト開始 ならびに 終了のソフト対応と、将来的には自律航行の外部コマンドによる緊急キャンセルなどの用途に使用したい。
記事を書いていたら能書きがかなり長くなってしまったので先に結果のGIF画像を公開。
トライを実施した結果はこのようになった。
結構キビキビ動くので感動したっス。
※オシリから伸びている黒い線はラズパイの電源線。最終的には外す。
■ 今回の要件
- PC と RaspberryPi の間は有線を無くしたい
- 始動、停止、右左折、前進後進の指示をハードウェア操作なしに無線経由で外界から送り付けたい
- できることなら、電力消費を抑えたい
- RaspberryPi と Arduino の間の通信を ROS で繋ぎたい
■ Wi-Fiを選択せず、Bluetoothを選択した理由
- ルーターなどの中継器不要でピアツーピアで接続可能、環境がシンプルで済む
- 一般的には Wi-Fi よりも Bluetooth のほうが電力効率が優れていると言われている
- コマンドはせいぜい数バイトで表現できるシンプルなものでよく、通信スピードを要求しない
- シビアなスピードとデータ量を要求するセンサー値の取得やモーター制御は RaspberryPi と Arduino 間の有線通信で完結させればよく、 PC と RaspberryPi の間ではやり取りする必要が無い
■ 思い描く構成イメージ
実装に移る前に、構成イメージの概要を下図に簡単にまとめる。
#◆ 作業環境
[作業PC] Windows 10 Pro(一部、Ubuntu 16.04)
[対 象] RaspberryPi3 (Raspbian Stretch) + ROS kinetic
#◆ 参考にさせていただいたサイト、謝辞
RoboTakaoさん 極力ローコスト ロボット製作 ブログ
- Raspberry Pi Zero WでBluetooth経由でシリアル通信(ペアリングまで)
- Raspberry Pi Zero WでBluetooth経由でシリアル通信(シリアル接続)実はうまく行ってません
- Raspberry Pi Zero WでBluetooth経由でシリアル通信(シリアル接続)続報
- pythonでシリアル通信(Raspberry Pi Zero WのBluetoothでMacと接続)
てぃるとさん うごくものづくりのために
Polol社公式チュートリアル Arduino 32U4のピンマッピング
Takumi Funadaさん Arduino日本語リファレンス
nonbiri15さん Qiita
#◆ RaspberryPi に Bluetoothを使用するためのモジュールが導入されていることを確認
RaspberryPi で Bluetooth を使用する場合は Bluez というツールが導入されている必要があるらしい。
さっそく導入状況を確認してみる。
$ hcitool | grep ver
バージョン5.43 が導入済みのようだ。
動作状況を確認してみる。hciconfig というコマンドを使うらしい。
$ hciconfig
RUNNING
かつ errors:0
ということなので特に問題なく動いているようだ。
#◆ RaspberryPi側 から Windows側 へペアリングしてみる
まずは、RaspberryPi側でBluetooth設定プログラムを起動する。
REUDO RBK-3000BT
というものが最初から表示されているが、これはだいぶ前にペアリングしたBluetoothキーボードの表示。
ペアリング済みのデバイスがリストアップされるコマンドなのかな。
$ bluetoothctl
この時点で Bluetooth の信号をブロードキャストしているデバイスが自動的にリストアップされた。
DESKTOP-FFOP8U3
と表示されているデバイスが WindowsPC を指しているようだ。
MACアドレスは、4C:34:88:20:92:2E
との表示。
では、Bluetoothを有効にした Windows を RaspberryPi側 から探せるかどうかを確認してみる。
scan on
というコマンドを使用するらしい。
[bluetooth]# scan on
作業PC側のWindowsがRaspberryPi側でしっかり検出された。
RSSI
というのは電波強度を表す数値のようで、マイナス値で表示される。
絶対値の部分が小さければ小さいほど強い電波を受信している、ということらしい。
-52 電波強い → -90 電波弱い
WindowsのMACアドレス「4C:34:88:20:92:2E」を控えて scan を停止する。
[bluetooth]# scan off
では、早速ペアリングペアリングしてみる。
[bluetooth]# pair 4C:34:88:20:92:2E
再接続がスムーズにいくように RaspberryPi側で trust(信頼) 設定を行ってみる。
[bluetooth]# trust 4C:34:88:20:92:2E
ペアリングの設定が終わったので設定画面から抜ける。
[Ubuntu]# exit
※ここまでのペアリングにまつわる手順はWindows側の設定画面からでもできることを確認済み。
※ここまでやっても接続がうまくいかない場合は、Windows側のBluetooth設定画面から接続ボタンを押してみる。
#◆ RaspberryPi側にシリアルポートを登録
さて、ここまでの手順ではPCとRaspberry Piの間がBluetoothでつながるようになっただけなので、Bluetoothの通信経路上をシリアル通信で使えるようにしようと思う。
レガシーな表現でCOM通信、というやつかな。
イメージ的には RS-232C の物理線を Bluetooth の無線に置き換えただけ、ということをやりたい。
Bluetoothでシリアル通信を有効にするには、一部設定ファイルの修正と デーモン(Windowsで言うところのサービス?) の再起動が必要、とのことらしい。
下記ファイルを一部修正する。
$ sudo nano /etc/systemd/system/dbus-org.bluez.service
ExecStart=/usr/lib/bluetooth/bluetoothd
ExecStart=/usr/lib/bluetooth/bluetoothd --compat
ExecStartPost=/usr/bin/sdptool add SP
デーモン(bluetoothd) を再起動する。
$ sudo systemctl daemon-reload
$ sudo systemctl restart bluetooth
$ sudo chmod 777 /var/run/sdp
とりあえずこれだけで、RaspberryPi側でBluetooth越しのシリアル通信をするための準備は終わりのようだ。
#◆ Python と RFCOMM による 「PC ⇔ RaspberryPi」 間の通信試験
ここで、いきなりの専門用語登場に翻弄される。
RFCOMM
???
RFCOMM
とは、「Bluetoothのプロトコルスタックの一つで、L2CAP上でRS-232Cシリアルポートの転送機能をエミュレーションするもの。」ということらしい。
つまり、前のセクションで自分が書いたことを端的に表現する単語が RFCOMM
という技術で、既に呼び名があったことを後から知る。
では、RaspberryPi側で下記コマンドにより rfcomm を listen 状態(待ち受け状態)にしてみる。
$ sudo rfcomm listen /dev/rfcomm0 1
「Waiting for connection ...」と表示されたので、文字通り接続を待っている状態になったように見える。
作業用PC側でTeraTermを起動し、RaspberryPi側へCOM接続する。
TeraTermを開いた瞬間に勝手に「COM4」が選べる状態になっていた。
何も考えずに「OK」をクリックしてみる。
■ TeraTerm側
TeraTerm側は真っ黒。「何か文字を入力してみろ。」と言わんばかりに見える。
■ RaspberryPi側のターミナル
RaspberryPi側は何やらコネクションが確立されたような表示に更新された。
TeraTerm側に文字を打ちたくなる衝動を抑えて、RaspberryPi側で入力を受け付けるPythonプログラムを作成する。
RaspberryPi側で別のターミナルを起動して下記のスクリプトを入力すると、 print(s.read())
の直後に入力待ち状態となる。 (20秒間何も入力しないと自動的にタイムアウトする)
$ python3
>>> import serial
>>> s = serial.Serial('/dev/rfcomm0', 9600, timeout=20)
>>> print(s.read())
作業用PC側の TeraTerm で任意の文字1文字をキー入力すると、入力した1文字 「a」 が即座にRaspberryPi側のターミナル上に表示された。
ここまでの 「Bluetooth + シリアル通信」 の実装をROSのPublisherの実装に置き換えれば、Arduinoへコマンド送信できるようになるような気がする。
今回はキーが押されたら即座に1文字分の受信を行うため、 s.read()
という関数を使用したが、1行単位や指定文字数単位、などのいろいろな組み合わせで受信することができるようだ。
念の為、以下にシリアル通信の基本メソッド例を記載する。
■ 基本的なコマンド
参考1.read() 1文字受信
>>> print(s.read())
参考2.read(x) ( )内の数だけ受信
>>> print(s.read(5))
参考3.readline() 改行 \n まで受信
>>> print(s.readline())
参考4.RaspberryPi -> 作業PC 送信
>>> s.write(b'hello') #<--- byte列に変換することを示す 'b' を付ける。
参考5.シリアルのクローズ
>>> s.close()
#◆ ROS で RaspberryPi と Arduino の間をUSBシリアル通信
さて、PC → RaspberryPi → Python の流れでシリアル通信による単純なコマンドを送信する検証が終わったところで、下図赤枠の範囲、RaspberryPi → Python → ROS → Arduino の流れでコマンドを連携する手段を考える。
参考記事の師 「てぃるとさん」 曰く、
rosserial は、シリアル通信を用いて ROSと組み込みボード あるいは センサノード等と通信するためのパッケージ。
シリアル通信をするだけでは無く、ROSとその他のもの達との通信プロトコルも提供する。
rosserial を使うと、シリアル通信で接続されたボードが ROS側 からは ROSノード のように見える。
ボードと通信するには、rosserial によって生成されたノードに対してトピックを Publish / Subscribe する。
とのことだ。求めていたのはまさにこれ。
PC → Bluetooth(シリアル通信) → RaspberryPi とつながってきたコマンドを、rosserial の仕組みを使用して Arduino側 へなんとかしてPublishしたい。
では、早速 rosserial をインストールしてみる。
■ Raspberry Piへインストールする場合
$ sudo apt-get update
$ sudo apt-get install -y arduino arduino-core
$ cd catkin_ws/src
$ git clone https://github.com/ros-drivers/rosserial.git
$ cd ../
$ catkin_make
$ catkin_make install
$ source install/setup.bash
$ sudo usermod -a -G dialout pi
$ sudo nano ~/.bashrc
source /home/pi/catkin_ws/install/setup.bash #<--- 追記
一度、ArduinoIDEをデスクトップモードで開いて閉じると、homeパスにsketchbookフォルダが自動生成される (ssh接続などのheadlessモードで実行するとjavaエラーが発生する)
下記コマンドを実行する。
$ cd ~/sketchbook/libraries
$ rm -rf ros_lib
$ rosrun rosserial_arduino make_libraries.py . #<--- 最後の "." は必須
■ Ubuntuへインストールする場合
1.ArduinoIDEを起動し、スケッチ→ライブラリをインクルード→ライブラリを管理
2.検索ボックスに「rosserial」を入力
3.一覧に表示された「Rosserial Arduino Library」をインストール
RaspberryPiのUSB-AポートとArduinoのmicroUSBポートをUSBケーブルで接続し、下記コマンドを実行する。
ttyxxxx
の部分は環境に応じて異なると推測。
あらかじめUSBを接続しておかないと、 「chmod: '/dev/ttyACM0' にアクセスできません: そのようなファイルやディレクトリはありません」 というエラーになる。
$ sudo chmod 666 /dev/ttyACM0
$ cd /etc/udev/rules.d
$ sudo touch ttyACM0rule.rules
$ sudo nano ttyACM0rule.rules
KERNEL=="ttyACM0", MODE="0666"
念の為、パッケージのアップデートと.bashrcを実行しておく。
$ sudo apt upgrade
$ source ~/.bashrc
まずは基本中の基本、 pub/sub
を実装してみる。
ArduinoIDEの pubsub
サンプルスケッチは Publisher と Subscriber のサンプルを同時に実装してくれているので分かりやすい。
ArduinoIDEに導入されている rosserial のサンプルスケッチ 「pubsub」 を手を加えずそのまま Arduino へ書き込んで実行してみたところ・・・なんと! 動かない!
チュートリアルロジックのくせにいきなりハマる。。。
なんやねん、いったい。。。
めちゃくちゃ悩んでしまった。
エラーメッセージは下記のとおり。何か、エラーメッセージそのものが的を外している気がして仕方がない。
[INFO] [WallTime: 1399983521.604184] ROS Serial Python Node
[INFO] [WallTime: 1399983521.617853] Connecting to /dev/ttyACM0 at 115200 baud
[ERROR] [WallTime: 1399983538.726124] Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino
ちなみに、Arduinoへ書き込んだダメなサンプルスケッチは下記。
**pubsub.ino のダメな例**
/*
* rosserial PubSub Example
* Prints "hello world!" and toggles led
*/
#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
ros::NodeHandle nh;
void messageCb( const std_msgs::Empty& toggle_msg){
digitalWrite(13, HIGH-digitalRead(13)); // blink the led
}
ros::Subscriber<std_msgs::Empty> sub("toggle_led", messageCb );
std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);
char hello[13] = "hello world!";
void setup()
{
pinMode(13, OUTPUT);
nh.initNode();
nh.advertise(chatter);
nh.subscribe(sub);
}
void loop()
{
str_msg.data = hello;
chatter.publish( &str_msg );
nh.spinOnce();
delay(500);
}
試行錯誤の末、成功したスケッチは下記。変更点はわずか1行の追記のみ。
#define USE_USBCON
を追記。
**pubsub.ino の成功例**
/*
* rosserial PubSub Example
* Prints "hello world!" and toggles led
*/
#define USE_USBCON <--- ココを追記
#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
ros::NodeHandle nh;
void messageCb( const std_msgs::Empty& toggle_msg){
digitalWrite(13, HIGH-digitalRead(13)); // blink the led
}
ros::Subscriber<std_msgs::Empty> sub("toggle_led", messageCb );
std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);
char hello[13] = "hello world!";
void setup()
{
pinMode(13, OUTPUT);
nh.initNode();
nh.advertise(chatter);
nh.subscribe(sub);
}
void loop()
{
str_msg.data = hello;
chatter.publish( &str_msg );
nh.spinOnce();
delay(500);
}
どうやら同じ問題を抱える技術者は多いようだ。
rosserial を Indigo用 にグレードダウンして再インストールしたらうまくいった、だとか、ArduinoHardware.h のプログラムを書き換えたらうまっくいった、だとか、spinOnce()は必要ない、だとか、情報が錯綜しまくっている。
【参考トピック】 rosserial arduino can't connect
では、RspberryPiとArduinoをmicroUSBケーブルで接続したうえでターミナルをひとつ起動して、Masterを起動する。
$ roscore
RaspberryPi側 でターミナルをもうひとつ起動し、 rosserial を実行する。
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200
おぉ、 「Setup publisher on chatter」 と表示されたので、 "chatter" という名前で TOPIC が公開されたのかな。
もうひとつ別のターミナルを起動し、"chatter" トピックを見てみよう。
$ rostopic echo chatter
エコーを停止するまでArduino側から延々と 「hello world!」 を語り続けてくる。
Arduino側を Publisher として、RaspberryPi側で Subscribe することに成功したようだ。
環境の作り方は間違っていなかったということかな。
では、もうひとつ別のターミナルを起動し、"toggle_led" トピックに対してメッセージを送ってみる。
スケッチのプログラムを読むと、Arduinoがメッセージを受信したときには、LEDの点灯⇔消灯動作をするようだ。
$ rostopic pub /toggle_led std_msgs/Empty
Arduino基盤上に実装されている黄色LEDが点灯状態になった。
もう一度同じようにメッセージをpublishすると消灯状態に戻った。
publish も subscribe も正常に動作しているようだ。
#◆ Arduinoによるモーター制御用スケッチの作成
いよいよ、Windowsで発行したコマンドを Bluetooth で RaspberryPi を中継し、Arduino側で Subscribe してモーターを制御するスケッチを書いてみる。
前進、後進、右折、左折、停止 の5つのコマンドを受け付けるようにしたい。
■ コマンドの定義
コマンド | 動作 |
---|---|
f | 前進 (forward) |
b | 後進 (backward) |
r | 右折 (turn right) |
l | 左折 (turn left) |
s | 停止 (stop) |
前回作成したスケッチ 「MotorControl.ino」 を ROS の Subscriber として加工する。
**MotorControl.ino の内容**
#define USE_USBCON
#include <Zumo32U4.h>
#include <ros.h>
#include "std_msgs/String.h"
Zumo32U4Motors motors;
String cmd = "";
ros::NodeHandle nh;
void motorcontrol(const std_msgs::String& cmd_msg) {
// F : forward , B : backward
// R : turn right , L : turn left
// S : stop
if (strlen(cmd_msg.data) != 0)
{
cmd = cmd_msg.data;
}
else
{
cmd = "";
}
if (cmd == "f")
{
motors.setSpeeds(0, 0);
delay(2);
motors.setSpeeds(50, 50);
}
else if (cmd == "b")
{
motors.setSpeeds(0, 0);
delay(2);
motors.setSpeeds(-50, -50);
}
else if (cmd == "r")
{
motors.setSpeeds(0, 0);
delay(2);
motors.setLeftSpeed(100);
}
else if (cmd == "l")
{
motors.setSpeeds(0, 0);
delay(2);
motors.setRightSpeed(100);
}
else if (cmd == "s")
{
motors.setSpeeds(0, 0);
delay(2);
}
}
ros::Subscriber<std_msgs::String> sub("command", motorcontrol);
void setup()
{
nh.initNode();
nh.subscribe(sub);
}
void loop()
{
nh.spinOnce();
delay(1);
}
上記で作成したスケッチを ArduinoIDE から Arduino へ書き込み、試しにコマンドを送って動きを確認してみる。
$ rostopic pub -1 /command std_msgs/String -- 'f'
rostopic pub
: トピックへメッセージをpublishするコマンド
-1
: 1つのメッセージをpublishしたあとに終了するオプション
/command
: publish対象のトピックの名前
std_msgs/String
: メッセージの型
--
: 後の引数を表示しないためのオプション
'f'
: publishするコマンド文字、複数連続で指定する場合は半角空白で区切る
OK。 Arduino側のノードが、ちゃんと 「f」 をSubscribeして躯体を前進してくれた。
#◆ Bluetooth経由でシリアル通信を受信してROSのTOPICへモーター制御用コマンドをPublishするPythonの作成
時と場合に応じてかなり手を抜くタイプの人間のため、 002日目 でダウンロードした talker.py
を改造してそのまま使用することにする。
**talker.py の内容**
#!/usr/bin/env python
import rospy
import serial
from std_msgs.msg import String
def talker():
pub = rospy.Publisher('command', String, queue_size=10)
rospy.init_node('talker', anonymous=True)
s = serial.Serial('/dev/rfcomm0', 115200, timeout=0.01)
while not rospy.is_shutdown():
command = ""
command = s.read()
if command != "":
pub.publish(command)
print command
s.close()
if __name__ == '__main__':
try:
talker()
except rospy.ROSInterruptException:
pass
恒例の catkin_make でビルドを行う。
$ cd ~/catkin_ws
$ catkin_make
#◆ 一気通貫の動作確認
では、技術的な要素が揃ったところで、すべてを結合した状態で一気通貫の動作確認を行う。
PC → Bluetooth(シリアル通信) → RaspberryPi → Arduino
1. RaspberryPi側でRFCOMMを有効化
$ sudo rfcomm listen /dev/rfcomm0 1
2. Windows側でTeraTermを起動しCOMxへ接続
3. RaspberryPi側でMasterの起動
$ roscore
4. RaspberryPiとArduinoをmicroUSBで接続し、RaspberryPi側で rosserial を起動
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200
5. RaspberryPi側でシリアル通信の中継用Pythonプログラムを起動
$ cd ~/catkin_ws
$ source devel/setup.bash
$ rosrun tutorials talker.py
6. Windows側のTeraTermでコマンドを送信
やった! ついに無線でウィンウィン動かせた〜!!
◆ はじめに のGIF画像の動きへ戻る
#◆ おまけ(Arduino障害からの復旧)
あれこれ作業している途中で、Arduino上のチップに書き込まれたプログラムを破損させてしまったようで、起動もしなければ新たなスケッチの書き込みもできなくなり、八方塞がりとなって数時間途方に暮れた。。。
一応、アレコレやって、復旧に成功した手順を下記に残す。
1. Arduinoと作業用PCをUSBで接続する
2. Arduino基盤(32U4)上のリセットボタンを素早く2回押す
3. 黄色のLEDが8秒ほど明滅している間に下記のコマンドを素早く実行する。
- 「/dev/ttyACM*」の部分はLINUX系統の作業PC向け。
- Windows系統の作業PCの場合は「COM*」に読み替える必要がある。
- 「*」の部分は適宜数字に読み替え。
- LINUX系の場合は
sudo ls /dev/ttyACM*
と、コマンド実行して表示される /dev/ttyACM(数字) がシリアルポート名となる。
$ avrdude -c avr109 -p atmega32U4 -P /dev/ttyACM* -e
4. 上記コマンド実行後、10秒ほど待機していると、Arduino基板上の黄色LEDが明滅しっぱなしの状態になる。
5. 黄色のLEDが明滅しっぱなしの状態で、ArduinoIDEからスケッチを書き込むと、内部的に壊れたプログラムが新たなスケッチで上書きされて正常に起動するようになる。
正直、もうダメかと思った。。。
#◆ 本日のまとめ
- Arduino・・・ヘタするとあっという間に無応答になる・・・怖い・・・トラウマ・・・
- これしきのことをするためにターミナルを5個ほど起動する必要があるのが面倒 → その後、コマンドの後ろに 「&」 を付加するだけでバックグラウンド化できることを知る
- チュートリアルどおりにうまくいかないことが多すぎ・・・
- だけど、トーシローでも意外といける気がしてきてしまった
- そろそろ面倒くさくなってきたため、
source devel/setup.bash
を .bashrc に書こうと思う - 久々に過去の自分の IoT用バッテリの記事 を見たら、クソ記事のわりに 7,000View を超えていて吹いた
- 幻のIoT用微電流無停止バッテリ、2ヶ月前ぐらいに在庫復活してますよ。放電し続けるのは本意ではないので自分は採用しないけども。。。
- 【白色】IoT機器用モバイルバッテリー cheero Canvas 3200mAh
- 【黒色】IoT機器用モバイルバッテリー cheero Canvas 3200mAh
- 参考書籍は情報量が多すぎて、2KBぐらいしかバッファが無い脳ミソでは理解をする前にパンクする。集中力が続かなくて眠くなる。今後気になるアイデアだけを部分的にパクっていくことにする。
#◆ 次回予告
LiDAR か何かでマップ作成のようなことにチャレンジしてみたい。
ん〜、急激に難しくなりそう。
さすがに一回、参考書を手に取ろう。
オッサンの奮闘は続く。。。
#◆ 次回記事
RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_007日目_SLAM_MONO-VO(単眼カメラ視覚測定) へ続く