Robotics 関連Site Map
|
RT社製の小型移動ロボット RaspberryPi Mouse を実際に使用して、ロボット操作の実際を体験してみます。したがって、RT 社製造のロボット「Raspberry Pi Mouse V2」が手元にあることを前提とします。2020年5月時点では V3 にバージョンアップされています。バージョン V2 からの変更点は、Raspberry Pi へ供給できる電流を強化(最大5A)したこと、および、パルスカウンタを追加（ステッピングモータドライバへのパルス指令を積算する）したことです。Raspberry Pi Mouse V3 ロボットの製造メーカーのサイトはこちらです。通常価格（税込で）、￥66,000.-です。
このページでは、Raspberry Pi 3 (Model B)を使用します。Raspberry Pi Mouse の操作を ROS を用いて実行するためには、Raspberry Pi へ Ubuntu をインストールする必要があります。Raspberry Pi への Ubuntu Mate 16.04 及び ROS のインストールはすでに行われていることを前提とします。RT社の「取扱説明書」を見ながら、この Raspberry Pi 3 をロボットに搭載してください。Raspberry Pi 4 を使用するときは対応するコネクターが必要です。Raspberry Pi 4 を用いたロボットの操作は Ubuntu Mate 20.04 でMouseロボットの操作のページにあります。
なお、ロボットを起動するためには、12v程度の電圧を供給する電源（３ピンコネクター付きリチウムイオン電池）が必要です。上記のロボット本体にセットで付属していますが、充電器は付いていません。LiPo充電器LBC-010とACアダプタ(12V5A)+VH3ピンコネクタへの変換ケーブルのセットが上記のRT 社から入手可能です。￥8,250.-(税込)です。
以下での説明では主に Python を用いて書かれたコードを使用します。Linux や C 言語の知識は前提としませんが、ROS がインストールされていることは必須です。ROS のインストールの方法については、このページを参照して下さい。
Last updated:2020.5.24 ( first uploaded : 2018.3.20)
Raspberry Pi Mouse V2 に搭載されたRaspberry Pi 3
Raspberry Pi Mouse のデバイス・ドライバーのインストール |
Raspberry Pi が Raspberry Pi Mouse ロボットに搭載されて、電池のコネクターが接続されていることを確認してください。WiFi 経由で Raspberry Pi への ssh 接続をするために必要なラズパイの IP アドレスがわかっているとします。
Raspberry Pi Mouse のメイン電源スイッチをオンにしてください。数分後、ラズパイの Ubuntu が起動したと思われたら、WiFi 経由で接続します。ラズパイのアドレスが192.168.10.10 であるとき、PCのターミナルから
$ ssh ubuntu@192.168.10.10.
と入力して接続します。ここでは、ubuntu はラズパイにログインするときのユーザー名です。パスワードを入力して、ラズパイにログインします。パスワードはubuntu mate をインストールした時に設定してあるので、それを使用します。ubuntu にログインすると、PC のターミナルは
ubuntu@Raspi:~$
と変化します。ここで、 Raspi は ssh 接続先のデバイスの名称で、ロボットに搭載された Raspberry Pi です。
Raspberry Pi 入門のページにて説明した通り、ラズベリーパイには GPIO という40ピンのコネクターが付いています。この GPIO を操作して、ロボットを動かします。GPIO インターフェースとロボットの機器（デバイス）を接続するためには、ロボット専用のデバイス・ドライバーが必要です。ラズパイ・マウスのデバイス・ドライバーのソース・コードは GitHubのrt-net/RaspberryPiMouse にあります。
ラズパイにログインできたら、デバイス・ドライバーをインストールします。以下のようにGitHubからcloneします。
ubuntu@Raspi:~$ git clone https://github.com/rt-net/RaspberryPiMouse.git ubuntu@Raspi:~$ cd RaspberryPiMouse
/RaspberryPiMouse/src/drivers/というディレクターの中にC言語のソースコード rtmouse.c が配置されています。rtmouse.c をコンパイルする必要があります。ビルド用のbashファイルはutilsのディレクトリにあります。
ubuntu@Raspi:~$ sudo apt install linux-headers-$(uname -r) build-essential ubuntu@Raspi:~$ cd utils ubuntu@Raspi:~$ ./build_install.bash
と入力すると、デバイスドライバーの rtmouse.ko が作成されるはずです。うまくいったら、rtmouse.ko が作成されていることを以下のように確認します。
ubuntu@Raspi:~$ cd ~/RaspberryPIMouse/src/drivers/ ubuntu@Raspi:~$ ls
この rtmouse.ko がデバイスドライバで、カーネル・モジュールと呼ばれます。linux のカーネルに後から組み込まれるモジュールです。カーネル・モジュール rtmouse.ko を linux カーネルに直接組み込む手続きが必要です。
ロボットのデバイスとGIOPピン番号との関係は「取扱説明書」に書いてある通りです。GPIOの状態を見てみましょう。/sys/class/gpio/というディレクトリがあることを確認します。ルート権限でlsを使います。
ubuntu@Raspi:~$ ls -l /sys/class/gpio/ ------------ 合計 0 -rwxrwx--- 1 root gpio 4096 2月 12 2016 export lrwxrwxrwx 1 root gpio 0 2月 12 2016 gpiochip0 -> ../../devices/platform/soc/3f200000.gpio/gpio/gpiochip0 lrwxrwxrwx 1 root gpio 0 2月 12 2016 gpiochip100 -> ../../devices/gpiochip2/gpio/gpiochip100 lrwxrwxrwx 1 root gpio 0 2月 12 2016 gpiochip128 -> ../../devices/gpiochip1/gpio/gpiochip128 -rwxrwx--- 1 root gpio 4096 2月 12 2016 unexport
と表示されます。GPIOの入出力ファイルが存在することが確認できました。以下では、ubuntu@Raspi:~ の明示は省略します。
ラズパイマウスを操作する手続きについては、上田 隆一著『RaspberryPiで学ぶROSロボット入門』に詳しく説明されていますので、ここでの手続きは大枠でそれに沿っています。この書籍の説明をそのままコピーしたのでは、作動しないケースもありますので、その都度修正を加えています。デバイスドライバのシェルスクリプトも修正の必要がありました。以下の通りに、シェルスクリプトを作成してください。
------------------------------------------------------ #!/bin/bash/ cd ~/RaspberryPiMouse/src/drivers/ sudo insmod rtmouse.ko sudo chmod 666 /dev/rt* sleep 1 echo 0 > /dev/rtmotoren0 ------------------------------------------------------
このファイルを、pimouse_driver.bash という名前で home ディレクトリに保存してください。insmod という命令はrtmouse.ko を直接カーネルに組み込ませるコマンドです。これは bash ファイルなので、呼び出して、実行するときは、
$ source pimouse_driver.bash (または . pimouse_driver.bash)
とします。ロボットのブザーがピッーという音を出して、LEDが光ります。組み込みの成功の印です。ロボットのデバイスドライバがカーネルに組み込まれているので、/dev/ の下にデバイスファイルが作成されています。
$ cd $ ls -l /dev/rt*
とコマンドを入力すると、ロボットのデバイスファイルの一覧が表示されます。
crw-rw-rw- 1 root root 241, 0 3月 3 17:07 /dev/rtbuzzer0 crw-rw-rw- 1 root root 232, 0 3月 3 17:07 /dev/rtcounter0 crw-rw-rw- 1 root root 233, 0 3月 3 17:07 /dev/rtcounter_l0 crw-rw-rw- 1 root root 234, 0 3月 3 17:07 /dev/rtcounter_r0 crw-rw-rw- 1 root root 242, 0 3月 3 17:07 /dev/rtled0 crw-rw-rw- 1 root root 242, 1 3月 3 17:07 /dev/rtled1 crw-rw-rw- 1 root root 242, 2 3月 3 17:07 /dev/rtled2 crw-rw-rw- 1 root root 242, 3 3月 3 17:07 /dev/rtled3 crw-rw-rw- 1 root root 239, 0 3月 3 17:07 /dev/rtlightsensor0 crw-rw-rw- 1 root root 235, 0 3月 3 17:07 /dev/rtmotor0 crw-rw-rw- 1 root root 237, 0 3月 3 17:07 /dev/rtmotor_raw_l0 crw-rw-rw- 1 root root 238, 0 3月 3 17:07 /dev/rtmotor_raw_r0 crw-rw-rw- 1 root root 236, 0 3月 3 17:07 /dev/rtmotoren0 crw-rw-rw- 1 root root 240, 0 3月 3 17:07 /dev/rtswitch0 crw-rw-rw- 1 root root 240, 1 3月 3 17:07 /dev/rtswitch1 crw-rw-rw- 1 root root 240, 2 3月 3 17:07 /dev/rtswitch2
rtled0を点灯したいときは、
$ echo 1 > /dev/rtled0
と入力します。消灯するときは
$ echo 0 > /dev/rtled0
とします。センサーを見たいときは
$ cat /dev/rtlightsensor0と入力します。
ステップモーターを回したいときには、まずロボットのモータースイッチを on に入れます。次に、
$ echo 1 > /dev/rtmotoren0 $ echo 400 400 1000 > /dev/rtmotor0
と順番に入力すると、モーターに通電し、400Hzで左右の車輪が順方向に回転します。1000ミリ秒回転し続けて、停止します。モーターに通電を止めるときは
$ echo 0 > /dev/rtmotoren0
とします。試験的にモーターを回転させるときは、ロボットの車輪を浮かせた状態にしておいてください。コマンドecho を使って、デバイスファイルに書き込むと、各デバイスを操作できます。色々と試して見てください。
ラズパイの終了は、
$ sudo shutdown now
と入力します。その後、ラズパイの緑色の点滅が停止してから電源スイッチを切ってください。
Raspberry Pi Mouse Robot/RT社製小型移動ロボット
RaspberryPi Mouse ロボットを操作する：トピックの利用 |
ROSの基本的使用法を理解していることを前提にします。ラズパイ・ロボットに電源が入り、PCのターミナルからssh接続が確立しているとします。ロボットのデバイス・ドライバーを読み込んでください。
$ source pimouse_driver.bash
と入力します。
まず最初に、ロボット操作用の基本的なパッケージを作成します。パッケージの名前をpimouse_rosとします。
~/catkin_ws/src/にパッケージを作成するために、
$ cd ~/catkin_ws/src/ $ catkin_create_pkg pimouse_ros std_msgs rospy
と入力します。catkin_create_pkgはパッケージを作成させるコマンドです。パッケージの名称の後に、ビルドに必要なライブラリのリストを書きます。rospyを明記しているのは、Pythonでコードを書くためです。（ROSのAPIのほとんどはC++とPythonで書かれています。）pimouse_ros というディレクトリができて、package.xmlとCMakelists.txt が自動的に作成されます。
$ cd pimouse_ros $ls
と入力すると、生成されたファイルが表示されます。
pimoue_ros をワークスペースに組み込むために、
$ cd ~/catkin_ws $ catkin_make
とします。
ROSのページで説明した通り、ノード間での通信はTopicを介したメッセージの送信・受信としてプログラムされる。このロボットでのノードはデバイス・ファイアルにある rtbuzzer、rtsensors、rtled、や rtmotors などである。
ブザーのノードを立ち上げ、他のノードからメッセージ（周波数）を受け取り、それをデバイス・ファイルに書き込むというプログラムを書きましょう。
#!/usr/bin/env python
import rospy
from std_msgs.msg import UInt16
def write_freq(hz=0):
bfile = "/dev/rtbuzzer0"
try:
with open(bfile,"w") as f:
f.write(str(hz) + "\n")
except IOError:
rospy.logerr("can't write to " + bfile)
def recv_buzzer(data):
write_freq(data.data)
if __name__ == '__main__':
rospy.init_node('buzzer')
rospy.Subscriber("buzzer", UInt16, recv_buzzer)
rospy.spin()
第１行目の#!/usr/bin/env python は、ファイルを実行ファイルとして用いる時の決まりです。実行ファイルとして読み込まれる時、if __name__ == '__main__':以下のコマンドから実行されます。
メセージの型を16ビット整数に定めるために、from std_msgs.msg import UInt16 と記述します。
コールバック関数 write_freq()とrecv_buzzer() を定義します。関数 write_freq(hz)では、デバイス・ファイル/dev/rtbuzzer0 を変数名 bfile として、この bfile を f という名称で開き、str(hz) の内容を書き込んでいます。書き込みが失敗するときの、エラー処理を except IOError で記述します。
その後、'buzzer'というトピックのノードを立ち上げ、このトピックのメッセージを購読して、rec_buzzer 関数で処理します。rospy.spin() は無限ループで受信を待つという意味です。
以上のスクリプトをファイル名 buzzer.py として/scripts/ディレクトリに保存します。
$ roscd pimouse_ros $ mkdir scripts $ cd scripts $ vi buzzer.py $ chmod +x buzzer.py
と入力します。chmod +x buzzer.pyで、python ファイルを実行可能な状態にしておきます。
このプログラムを実行するために、３つのターミナルを起動します。ssh 接続でラズパイのターミナルを３つ立ち上げておきます。最初のターミナルで、roscoreを立ち上げます。
$ roscore
２つ目のターミナルで、
$ rosrun pimouse_ros buzzer.py
とノードを立ち上げます。topicをパブリッシュするために、３つ目のターミナルから
$ rostopic pub -1 '/buzzer' std_msgs/UInt16 1000
と打ちます。'/buzzer'がトピック名で、メッセージの型がstd_msgs/UInt16であり、周波数がhz=1000という意味になります。すると、ブザーがピーとなります。停止するために
$ rostopic pub -1 '/buzzer' std_msgs/UInt16 0
と周波数をゼロとして入力します。音が止まります。2番目のターミナルで、[contrl + c]を入力して、python プログラムを終了します。
次に、距離センサーからの値をデバイス・ファイルから読み込んで、別のノードに伝える新しいノード 'lightsensors’ を作成します。トピックのメッセージの型を独自に定めます。距離センサーは４つありますので、４つの値のやりとりが必要です。各センサーに名前をつける必要もあります。型を定めるために、msgというディレクトリを作成します。以下のように入力します。
$ roscd pimouse_ros $ mkdir msg $ cd msg
msgディレクトリの中に以下のようなメッセージの型を定めるファイルを作成します。
int16 right_forward int16 right_side int16 left_side int16 left_forward int16 sum_all int16 sum_forward
このスクリプトをファイル名 LightSensorValues.msg とします。大文字小文字に注意。次に、ノード'lightsensors'を立ち上げるプログラムを作成します。scriptsディレクトリの中に、lightsensors1.pyというファイルを作成してください。
#!/usr/bin/env python
import sys, rospy
from pimouse_ros.msg import LightSensorValues
rospy.init_node('lightsensors')
このスクリプトはノード'lightsensors'を立ち上げるだけの機能しかありません。from pimouse_ros.msg import LightSensorValues は'lightsensors'からのメッセージの型を組み込むためのコマンドです。このファイルを実行可能なファイルにするために、以下のように打ち込んでください。
$ roscd pimouse_ros/scripts $ chmod +x lightsensors1.py
roscoreを立ち上げて、
$ rosrun pimouse_ros lightsensors1.py
と打つと、エラーが出ます。なぜなら、pimouse_ros.msg というモジュールがまだ組み込まれていないからです。このモジュールを組み込むためには、catkin_makeが必要となります。
まず、package.xmlとCMakeLists.txtを編集します。package.xmlを開いて、以下のように二つの行のコメントを外して、上書き保存します。
------ <!-- Example: --> <author email="xxxxx@aol.com">Koichi Mashiyama</author> <!-- The *_depend tags are used to specify dependencies --> <!-- Dependencies can be catkin packages or system dependencies --> <!-- Examples: --> <!-- Use build_depend for packages you need at compile time: --> <build_depend>message_generation</build_depend> <!-- Use buildtool_depend for build tool packages: --> <!-- <buildtool_depend>catkin</buildtool_depend> --> <!-- Use run_depend for packages you need at runtime: --> <run_depend>message_runtime</run_depend> <!-- Use test_depend for packages you need only for testing: --> <!-- <test_depend>gtest</test_depend> --> -----
具体的には、
の２行のコメントを外します。これは、message_generation をビルドする時に、message_runtime を使うという意味になります。
次に、CMakeLists.txtを編集します。C言語のソースファイルをコンパイルさせるための makefile、CMake に読み込ませるファイルです。C言語の知識がない人はそうなんだと理解して進んでください。ディレクトリ pimouse_ros にある CMakeLists.txtを開いてください。変更する箇所は４箇所です。１つ目は、find_packageという行を見つけてください。
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS rospy std_msgs message_generation #追加する )
とmessage_genaraionという文を追加してください。
２つ目、add_message_filesというコマンドの行を探してください。デフォルトでは、その行以下が#でコメントアウトされています。#を外して、
## Generate messages in the 'msg' folder add_message_files( FILES LightSensorValues.msg )
と、LightSensorValues.msgを追加する。３つ目は、generate_messagesの行を探します。この行以下が#でコメントアウトされていますので、以下のように#を外します。
## Generate added messages and services with any dependencies listed here generate_messages( DEPENDENCIES std_msgs )
と修正します。
最後の４つ目は、catkin_packageの行を探します。この行以下の#のうちCATKIN_DEPENDSの行の#だけを外し、その行にmessage_runtimeを追加します。以下のようになります。
## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include # LIBRARIES pimouse_ros CATKIN_DEPENDS rospy std_msgs message_runtime # DEPENDS system_lib )
これで、CMakeLists.txtの修正は完了です。修正したファイルを上書き保存してください。
最後に、catkin_makeをします。
$ cd ~/catkin_ws/ $ catkin_make $ source ~/.bashrc
と入力します。これで、パッケージは完成です。
roscoreを立ち上げて、別のターミナルから
$ rosrun pimouse_ros lightsensors1.py
と入力して、エラーが出なければ、成功です。また、LightSensorValuesから生成されたファイルは以下のように入力すると、
$ cd ~/catkin_ws/devel/ $ find . | grep LightSensorValues --------- ./share/common-lisp/ros/pimouse_ros/msg/_package_LightSensorValues.lisp ./share/common-lisp/ros/pimouse_ros/msg/LightSensorValues.lisp ./share/common-lisp/ros/raspimouse_ros/msg/_package_LightSensorValues.lisp ./share/common-lisp/ros/raspimouse_ros/msg/LightSensorValues.lisp ./share/gennodejs/ros/pimouse_ros/msg/LightSensorValues.js ./share/gennodejs/ros/raspimouse_ros/msg/LightSensorValues.js ./share/roseus/ros/pimouse_ros/msg/LightSensorValues.l ./share/roseus/ros/raspimouse_ros/msg/LightSensorValues.l ./lib/python2.7/dist-packages/pimouse_ros/msg/_LightSensorValues.py ./lib/python2.7/dist-packages/pimouse_ros/msg/_LightSensorValues.pyc ./lib/python2.7/dist-packages/raspimouse_ros/msg/_LightSensorValues.py ./lib/python2.7/dist-packages/raspimouse_ros/msg/_LightSensorValues.pyc ./include/pimouse_ros/LightSensorValues.h ./include/raspimouse_ros/LightSensorValues.h
と表示されます。
距離センサーからの値を読めるようにしましょう。そこで、lightsensors1.pyを以下のように書き換えます。
#!/usr/bin/env python
import sys, rospy
from pimouse_ros.msg import LightSensorValues
if __name__ == '__main__':
devfile = '/dev/rtlightsensor0'
rospy.init_node('rtlightsensors')
pub = rospy.Publisher('lightsensors', LightSensorValues, queue_size=1)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
try:
with open(devfile,'r') as f:
data = f.readline().split()
data = [int(e) for e in data ]
d = LightSensorValues()
d.right_forward = int(data[0])
d.right_side = int(data[1])
d.left_side = int(data[2])
d.left_forward = int(data[3])
# d.sum_all = sum(data)
# d.sum_forward = data[0] + data[3]
pub.publish(d)
except:
rospy.logerr("cannot open " + devfile)
rate.sleep()
devfile = '/dev/rtlightsensor0'はデバイス・ファイルrtlightsensor0をdevfileという名称にします。pub = rospy.Publisher('lightsensors', LightSensorValues, queue_size=1)は、トピックlightsensorsを発信するインスタンスpubを作成します。メッセージの型はLightSensorValuesで定義されていて、バッファサイズが1という意味です。rate = rospy.Rate(10)は１秒間に10回の割合でプログラムを実行するというインスタンスを作成しています。while not rospy.is_shutdown():はプログラムが「ctrl+c」の入力で終了するまで無限ループで続くことを意味します。debfileの内容をdataに格納して、変数dの中に読み込んでいます。pub.publish(d)はd の内容を発信しています。#の行はコメント行で、エラーの関係で、ここでは無視します。このファイルをlightsensors2.pyという名前で保存します。roscoreを立ち上げ、
$ roscd pimouse_ros/scripts/ $ chmod +x lightsensors2.py $ rosrun pimouse_ros lightsensors2.pyと入力します。別のターミナルから
$ source ~/.bashrc $ rostopic echo lightsensors right_forward: 19 right_side: 15 left_side: -6 left_forward: 14 --- right_forward: 31 right_side: 30 left_side: 12 left_forward: 30 ---
と入力すると、センサーの値が表示されます。rostopic echo lightsensorsは、トピック名lightsensorsからのメッセージを表示させるコマンドです。
距離センサーで計測する頻度を変えることができるように、プログラムを拡張します。以下のプログラムを作成して
#!/usr/bin/env python
import sys, rospy
from pimouse_ros.msg import LightSensorValues
def get_freq():
f = rospy.get_param('lightsensors_freq',10)
try:
if f <= 0.0:
raise Exception()
except:
rospy.logerr("value error: lightsensors_freq")
sys.exit(1)
return f
if __name__ == '__main__':
devfile = '/dev/rtlightsensor0'
rospy.init_node('lightsensors')
pub = rospy.Publisher('lightsensors', LightSensorValues, queue_size=1)
freq = get_freq()
rate = rospy.Rate(freq)
while not rospy.is_shutdown():
try:
with open(devfile,'r') as f:
data = f.readline().split()
data = [ int(e) for e in data ]
d = LightSensorValues()
d.right_forward = data[0]
d.right_side = data[1]
d.left_side = data[2]
d.left_forward = data[3]
d.sum_all = sum(data)
d.sum_forward = data[0] + data[3]
pub.publish(d)
except IOError:
rospy.logerr("cannot write to " + devfile)
f = get_freq()
if f != freq:
freq = f
rate = rospy.Rate(freq)
rate.sleep()
lightsensors.pyとして、scriptsディレクトリに保存してください。パラメータ値の設定がないときは、初期値(hz=10)のままで作動します。上と同じく、roscoreを立ち上げ、
$ roscd pimouse_ros/scripts/ $ chmod +x lightsensors.py $ rosparam set lightsensors_freq 1 $ rosrun pimouse_ros lightsensors.py
と打つと、距離センサーが１秒間に１回光ります。
最後に、モーターの制御に関するプログラムを作成します。以下では、煩雑ですが、roscore の起動を含めて３つのターミナルを立ち上げる必要があります。 モーターのデバイス・ファイルrtmotor_raw_{l,r}0へ回転数の周波数を送るノードを二つ作り、２種類のトピック、motor_rawとcmd_velを使います。motor_rawはこのロボット固有のものなので、新しくメッセージの型を作る必要があります。cmd_velはROSに付属しているシミュレータturtlesimで使用されている同名のトピックを使います。
motor_rawで使用するメッセージの型は以下のファイルを作成して
int16 left_hz int16 right_hz
ファイル名をMotoFreqs.msgとして、msgディレクトリに保存します。cmd_velの型はturtlesimに従いgeometry_msgs/Twistを使用します。この型の定義内容は以下のコマンドを打ち込むと表示できます。
$ rosmsg show geometry_msgs/Twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z
linearは速度、angularは角速度に対応します。数値は64ビット浮動小数点形式です。方向は３次元となります。
MotorFreqs.msgは独自の型なので、モジュールに組み込むためにCMakeLists.txtに記述して置く必要があります。CMakeLists.txtを開いて、add_message_filesの下に以下のように追加します。
## Generate messages in the 'msg' folder add_message_files( FILES LightSensorValues.msg MotorFreqs.msg )
モーター操作用のプログラムを以下のように作成します。
#!/usr/bin/env python
import sys, rospy, math
from pimouse_ros.msg import MotorFreqs
from geometry_msgs.msg import Twist
class Motor():
def __init__(self):
if not self.set_power(True): sys.exit(1)
rospy.on_shutdown(self.set_power)
self.sub_raw = rospy.Subscriber('motor_raw', MotorFreqs, self.callback_raw_freq)
self.sub_cmd_vel = rospy.Subscriber('cmd_vel', Twist, self.callback_cmd_vel)
self.last_time = rospy.Time.now()
self.using_cmd_vel = False
def set_power(self,onoff=False):
en = "/dev/rtmotoren0"
try:
with open(en,'w') as f:
f.write("1\n" if onoff else "0\n")
self.is_on = onoff
return True
except:
rospy.logerr("cannot write to " + en)
return False
def set_raw_freq(self,left_hz,right_hz):
if not self.is_on:
rospy.logerr("not enpowered")
return
try:
with open("/dev/rtmotor_raw_l0",'w') as lf,\
open("/dev/rtmotor_raw_r0",'w') as rf:
lf.write(str(int(round(left_hz))) + "\n")
rf.write(str(int(round(right_hz))) + "\n")
except:
rospy.logerr("cannot write to rtmotor_raw_*")
def callback_raw_freq(self,message):
self.set_raw_freq(message.left_hz,message.right_hz)
def callback_cmd_vel(self,message):
forward_hz = 80000.0*message.linear.x/(9*math.pi)
rot_hz = 400.0*message.angular.z/math.pi
self.set_raw_freq(forward_hz-rot_hz, forward_hz+rot_hz)
self.using_cmd_vel = True
self.last_time = rospy.Time.now()
if __name__ == '__main__':
rospy.init_node('motors')
m = Motor()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
if m.using_cmd_vel and rospy.Time.now().to_sec() - m.last_time.to_sec() >= 1.0:
m.set_raw_freq(0,0)
m.using_cmd_vel = False
rate.sleep()
このファイルにmotors1.pyと名前をつけて、scriptsディレクトリに保存してください。このスクリプトの説明を簡単にします。
from pimouse_ros.msg import MotorFreqs と from geometry_msgs.msg import Twist は、モーターへ送るメッセージの型を組み入れています。
クラスmotor() は５つのメソッド、__init__(self), set_power(self,onoff=False), set_raw_freq(self,left_hz,right_hz), callback_raw_freq(self,message), callback_cmd_vel(self,message), から構成されています。これらのメソッドのコードは読み込まれますが、実際の実行は、if __name__ == '__main__':から呼び出される時に、実現されます。
__init__(self):内では、モータに通電がないときはプログラムから抜けます。通電されている場合には、 rospy.on_shutdown(self.set_power)で、シャットダウンの入力があるときは、終了の処理を行いますが、そうでないときは、self.sub_raw = rospy.Subscriber('motor_raw', MotorFreqs, self.callback_raw_freq)を実行します。つまり、トピック'motor_raw'のメッセージの型をMotorFreqで受け取って、コールバック関数callback_raw_freqで処理します。
self.sub_cmd_vel = rospy.Subscriber('cmd_vel', Twist, self.callback_cmd_vel)は、トピック'cmd_vel'からのメッセージを受け取って、関数callback_cmd_velで処理します。
関数callback_raw_freqは受け取ったメッセージをそのままset_raw_freqに渡します。callback_cmd_velは、直進の速度（messege.linear.x）と角速度（messege.angular.x）を左右の周波数に変換して、set_raw_freqに渡します。この後、using_cmd_velの値をTrueとして、時刻をlast_timeに入れています。
main部分のwhile not rospy.is_shutdown():以下では、using_cmd_velの値がTrueであるとき、last_timeが１秒以上過去であれば、両方のモーターの周波数をゼロとして、using_cmd_velの値をFalseにします。言い換えると、トピックcmd_velを使ってモーターを回転させるときは、１秒間で回転が停止する。トピックmotor_rawを使用するときは、自動停止しませんので、停止のためのキー入力が必要です。
このファイルはこれ以降順次拡張されていきます。
このMotorFreq.msgを組み込んだプログラムでモーターを操作するためには、catkin_makeをする必要があります。catkin_makeの後、（roscoreが起動していないときは、roscoreを立ち上げ）
$ cd ~/catkin_ws/ $ catkin_make $ source ~/.bashrc $ roscd pimouse_ros/scripts $ chmod +x motors1.py $ rosrun pimouse_ros motors1.py
と入力して、motors1.pyを実行させます。別のターミナルを開いて、
$ rostopic pub /motor_raw pimouse_ros/MotorFreqs "left_hz: 100 (tab キーを打ち） right_hz:100"
と入力すると、hz=100でモーターが回転します。止めるときは
$ rostopic pub /motor_raw pimouse_ros/MotorFreqs "left_hz: 0 (tab キーを打ち） right_hz:0"
と打つと停止します。geometry_msgsを用いてモーターを回転されるときは、例えば、
$ rostopic pub /cmd_vel geometry_msgs/Twist "linear: x: 0.0 y: 0.0 z: 0.0 angular: x: 0.0 y: 0.0 z: 1.0"
と入力すると、モーターが回転して、１秒後に停止します。
ターミナルを３つも立ち上げるのは煩雑です。そこで、launchを使います。launchディレクトリを作成します。
$ roscd pimouse_ros $ mkdir launch $ cd lauch $ vi pimouse.launch
と入力して、以下のようなpimouse.launchファイルを作成します。
<launch>
<node pkg="pimouse_ros" name="lightsensors" type="lightsensors.py" required="true" />
<node pkg="pimouse_ros" name="buzzer" type="buzzer.py" required="true" />
<node pkg="pimouse_ros" name="motors" type="motors.py" required="true" />
</launch>このlaunchファイルに対応させて、スクリプトを以下のようにシンボリック・リンクを貼って統一します。
$ roscd pimouse_ros/scripts $ ln -s motors1.py motors.pyこのlaunchファイルを起動すれば、
$ roslaunch pimouse_ros pimouse.launch
roscoreが立ち上がり、launchファイルに記述されたスクリプトが実行されます。一つのターミナルで済みます。
RaspberryPi Mouse ロボットを操作する：サービスの利用 |
トピックを介した通信はメッセージを送受するノードが互いに待つことはなく、非同期で動作します。サービスを利用すると、相手のノードの処理を待ち、その結果を受け取ることができます。サービスを実装するプログラムを作成します。特に、モーターのオンオフに関するサービスを実装します。std_msgsに対応するサービス版std_srvs パッケージを利用します。
pimouse_rosパッケージでstd_srvsを使用するためには、対応するpackage.xmlとCMakeLists.txtに追加修正が必要となります。まず、package.xmlを開いて、以下のようにstd_srvsの文を追加してください。
（略） <build_depend>message_generation</build_depend> <build_depend>std_msgs</build_depend> <build_depend>std_srvs</build_depend> （略） <run_depend>message_runtime</run_depend> <run_depend>std_msgs</run_depend> <run_depend>std_srvs</run_depend> （略）
CMakeLists.txt を開いて、find_package の箇所と catkin_package の箇所に以下のような追加をしてください。
find_package(catkin REQUIRED COMPONENTS rospy std_msgs std_srvs #追加 message_generation ) （略） catkin_package( # INCLUDE_DIRS include # LIBRARIES pimouse_ros CATKIN_DEPENDS rospy std_msgs std_srvs message_runtime #std_srvs追加 # DEPENDS system_lib )
これで準備は終わりました。一度、catkin_makeしておきましょう。
$ cd ~/catkin_ws/ $ catkin_make $ source ~/.bashrc
サービスを実装したモーター操作用のプログラムを以下のように作成します。以下のコードは上田 隆一著『RaspberryPiで学ぶROSロボット入門』から援用しました。motors1.pyをコピーして、それに以下のような追加修正をして
#!/usr/bin/env python
#encoding: utf8
import sys, rospy, math
from pimouse_ros.msg import MotorFreqs
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse #追加
class Motor():
def __init__(self):
if not self.set_power(False): sys.exit(1) #モータの電源を切る（TrueをFalseに）
rospy.on_shutdown(self.set_power)
self.sub_raw = rospy.Subscriber('motor_raw', MotorFreqs, self.callback_raw_freq)
self.sub_cmd_vel = rospy.Subscriber('cmd_vel', Twist, self.callback_cmd_vel)
self.srv_on = rospy.Service('motor_on', Trigger, self.callback_on) #追加
self.srv_off = rospy.Service('motor_off', Trigger, self.callback_off) #追加
self.last_time = rospy.Time.now()
self.using_cmd_vel = False
def set_power(self,onoff=False):
en = "/dev/rtmotoren0"
try:
with open(en,'w') as f:
f.write("1\n" if onoff else "0\n")
self.is_on = onoff
return True
except:
rospy.logerr("cannot write to " + en)
return False
def set_raw_freq(self,left_hz,right_hz):
if not self.is_on:
rospy.logerr("not enpowered")
return
try:
with open("/dev/rtmotor_raw_l0",'w') as lf,\
open("/dev/rtmotor_raw_r0",'w') as rf:
lf.write(str(int(round(left_hz))) + "\n")
rf.write(str(int(round(right_hz))) + "\n")
except:
rospy.logerr("cannot write to rtmotor_raw_*")
def callback_raw_freq(self,message):
self.set_raw_freq(message.left_hz,message.right_hz)
def callback_cmd_vel(self,message):
forward_hz = 80000.0*message.linear.x/(9*math.pi)
rot_hz = 400.0*message.angular.z/math.pi
self.set_raw_freq(forward_hz-rot_hz, forward_hz+rot_hz)
self.using_cmd_vel = True
self.last_time = rospy.Time.now()
def onoff_response(self,onoff): #以下3つのメソッドを追加
d = TriggerResponse()
d.success = self.set_power(onoff)
d.message = "ON" if self.is_on else "OFF"
return d
def callback_on(self,message): return self.onoff_response(True)
def callback_off(self,message): return self.onoff_response(False)
if __name__ == '__main__':
rospy.init_node('motors')
m = Motor()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
if m.using_cmd_vel and rospy.Time.now().to_sec() - m.last_time.to_sec() >= 1.0:
m.set_raw_freq(0,0)
m.using_cmd_vel = False
rate.sleep()
このファイルをmotors2.pyとして/pimouse_ros/scriptsディレクトリに保存してください。
動作を確認してみます。ロボットのモーターの電源スイッチを入れて、ラズパイのターミナルを立ち上げて、
$ roscore
と roscore を立ち上げて、２つ目のターミナルを立ち上げて、以下のように打ち込んでください。
$ roscd pimouse_ros/scripts $ chmod +x motors2.py $ rosrun pimouse_ros motors2.py
さらに、３つ目のターミナルから、on/offの動作を確認します。
$ rosservice call /motor_on ----------------- success: True message: ON $ rosservice call /motor_off ----------------- success: True message: OFF
モーターに通電して、車輪が固定され、次に、モータへの通電が切れて車輪が自由になります。
最後に、/dev/rtmotor0へ左右のモーターをミリ単位で回転させたい時間を指定できるようなサービスの型を定義します。このサービスをTimedMotion.srvで定義します。TimedMotion.srvは、次のセクション以降のロボット制御では使用しないので、スキップしても構いません。
pimouse_ros の下に srv ディレクトリを作成して、そこに、ファイルを保存します。以下のファイルを作成して、保存してください。
int16 left_hz int16 right_hz uint32 duration_ms --- bool success
左右の車輪への信号は16ビットの整数、回転時間については32ビットの符号なし整数としています。この型の情報を持つクラスを埋め込むために、ここでcatkin_makeします。その前に、CMakeLists.txtのadd_service_filesのある行を探して、以下のようにコメントを外して、TimedMotion.srvを追加してください。
add_service_files( FILES TimedMotion.srv )
この修正が終わったら、
$ cd ~/catkin_ws $ catkin_make
とcatkin_makeしてください。
TimedMotion.srvを利用してモーターを動作させるために、Motors2.py をコピーして、motors.py を作成します。シンボリック・リンクの motors.py を削除してください。motors.py は以下の通りです。
#!/usr/bin/env python
#encoding: utf8
import sys, rospy, math
from pimouse_ros.msg import MotorFreqs
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse
from pimouse_ros.srv import TimedMotion #追加
class Motor():
def __init__(self):
if not self.set_power(False): sys.exit(1) #モータの電源を切る（TrueをFalseに）
rospy.on_shutdown(self.set_power)
self.sub_raw = rospy.Subscriber('motor_raw', MotorFreqs, self.callback_raw_freq)
self.sub_cmd_vel = rospy.Subscriber('cmd_vel', Twist, self.callback_cmd_vel)
self.srv_on = rospy.Service('motor_on', Trigger, self.callback_on)
self.srv_off = rospy.Service('motor_off', Trigger, self.callback_off)
self.srv_tm = rospy.Service('timed_motion', TimedMotion, self.callback_tm) #追加
self.last_time = rospy.Time.now()
self.using_cmd_vel = False
def set_power(self,onoff=False):
en = "/dev/rtmotoren0"
try:
with open(en,'w') as f:
f.write("1\n" if onoff else "0\n")
self.is_on = onoff
return True
except:
rospy.logerr("cannot write to " + en)
return False
def set_raw_freq(self,left_hz,right_hz):
if not self.is_on:
rospy.logerr("not enpowered")
return
try:
with open("/dev/rtmotor_raw_l0",'w') as lf,\
open("/dev/rtmotor_raw_r0",'w') as rf:
lf.write(str(int(round(left_hz))) + "\n")
rf.write(str(int(round(right_hz))) + "\n")
except:
rospy.logerr("cannot write to rtmotor_raw_*")
def callback_raw_freq(self,message):
self.set_raw_freq(message.left_hz,message.right_hz)
def callback_cmd_vel(self,message):
forward_hz = 80000.0*message.linear.x/(9*math.pi)
rot_hz = 400.0*message.angular.z/math.pi
self.set_raw_freq(forward_hz-rot_hz, forward_hz+rot_hz)
self.using_cmd_vel = True
self.last_time = rospy.Time.now()
def onoff_response(self,onoff): #以下3つのメソッドを追加
d = TriggerResponse()
d.success = self.set_power(onoff)
d.message = "ON" if self.is_on else "OFF"
return d
def callback_on(self,message): return self.onoff_response(True)
def callback_off(self,message): return self.onoff_response(False)
def callback_tm(self,message):
if not self.is_on:
rospy.logerr("not enpowered")
return False
dev = "/dev/rtmotor0"
try:
with open(dev,'w') as f:
f.write("%d %d %d\n" %
(message.left_hz,message.right_hz,message.duration_ms))
except:
rospy.logerr("cannot write to " + dev)
return False
return True
if __name__ == '__main__':
rospy.init_node('motors')
m = Motor()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
if m.using_cmd_vel and rospy.Time.now().to_sec() - m.last_time.to_sec() >= 1.0:
m.set_raw_freq(0,0)
m.using_cmd_vel = False
rate.sleep()
# Copyright 2016 Ryuichi Ueda
# Released under the BSD License.
# To make line numbers be identical with the book, this statement is written here. Don't move it to the header.
動作確認をしましょう。モーターを浮かせておいて、モーターの電源スイッチを入れて、ラズパイのターミナルで roscore を立ち上げます。そして、２つ目のターミナルから以下のように順番にコマンドを入力します。
$ rosrun pimouse_ros motors.py $ rosservice call /motor_on $ rosservice call /timedMotion "left_hz: 400 #tabキーをうつ\ right_hz:400 #tabキーをうつ\ duraion_ms:500" #enterキーを打つ
「#tabキーを打つ」などのコメントは入力しないでください。左右の車輪が400Hzで回転して、500ミリ秒後に停止します。回転数を変えたり、動作時間を変化させて、実行することを試みてください。モーターへの通電を停止するためには
$ rosservice call /motor_off
と入力してください。うまくいったら、成功です。
このセクションで作成したpimouse_rosパッケージの構造は以下のようになっています。
~/catkin_ws/src/pimouse_ros$ tree . ├── CMakeLists.txt ├── launch │ ├── pimouse.launch │ ├── msg │ ├── LightSensorValues.msg │ └── MotorFreqs.msg ├── package.xml ├── scripts │ ├── buzzer.py -> buzzer2.py │ ├── buzzer1.py │ ├── buzzer2.py │ ├── lightsensors.py │ ├── lightsensors1.py │ ├── lightsensors2.py │ ├── motors.py │ ├── motors1.py │ └── motors2.py ├── srv └── TimedMotion.srv （ここはなくても良い）
RaspberryPi Mouse ロボットの自律走行 |
このセクションでは、ラズパイ・マウスを壁の手前でストップさせたり、壁に沿って走らせることをします。このような操作を実現するパッケージをpimouse_corridorという名称で作成します。また、pimouse_rosパッケージをこれに組み込んで利用します。pimouse_corridorパッケージを作成するために
$ cd ~/catkin_ws/src/ $ catkin_create_pkg pimouse_corridor
と入力します。pimouse_ros パッケージを利用することを pimouse_corridor/package.xml に念のために、以下のような行を書き込んでおきましょう。
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<run_depend>rospy</run_depend>
<run_depend>pimouse_ros</run_depend>
pimouse_ros の１行を run_depend の一覧の最後に追加します。ここで、一度、catkin_make しておきましょう。
$ cd ~/catkin_ws $ catkin_make $ source ~/.bashrc
前のセクションと同様に、pimouse_corriodr/scripts/ディレクトリを作ります。
$ roscd pimouse_corridor $ mkdir scripts $ cd scripts
このディレクトリ（/scripts/）の中にラズパイ・マウスを走らせるプログラムを配置します。最初に、壁の手前で停止させるwall_stop.pyを作成します。以下のファイルを書いて、保存してください。
#!/usr/bin/env python
import rospy,copy
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse
from pimouse_ros.msg import LightSensorValues
class WallTrace():
def __init__(self):
self.cmd_vel = rospy.Publisher('/cmd_vel',Twist,queue_size=1)
self.sensor_values = LightSensorValues()
rospy.Subscriber('/lightsensors', LightSensorValues, self.callback_lightsensors)
def callback_lightsensors(self,messages):
self.sensor_values = messages
def run(self):
rate = rospy.Rate(10)
data = Twist()
while not rospy.is_shutdown():
data.linear.x = 0.2
data.angular.z = 0
if self.sensor_values.sum_all >= 500:
data.linear.x = 0
data.angular.z = 0
self.cmd_vel.publish(data)
rate.sleep()
if __name__ == '__main__':
rospy.init_node('wall_trace')
rospy.wait_for_service('/motor_on')
rospy.wait_for_service('/motor_off')
rospy.on_shutdown(rospy.ServiceProxy('/motor_off',Trigger).call)
rospy.ServiceProxy('/motor_on',Trigger).call()
w = WallTrace()
w.run()
このプログラムの関数run()は、基本的に、cmd_velを用いてロボットに0.2m/sの速度で直進をさせる部分と、４つの距離センサーの合計値が500を超えた時に、cmd/vel に０を送る部分からなります。
次に、launchファイルを作成します。そのために、pimouse_corridor の下に launch ディレクトリを作ります。
$ roscd pimouse_corridor $ mkdir launch $ cd launch
以下のような、wall_stop.launchファイルを作成します。
<launch>
<include file="$(find pimouse_ros)/launch/pimouse.launch" />
<node pkg="pimouse_corridor" name="wall_stop" type="wall_stop.py" />
</launch>
このファイルをlaunchディレクトリに保存してください。
最後に、ロボットを動かしてみましょう。デバイス・ドライバーを読み込み、モーターの電源スイッチを入れて、ロボットを壁から１メートルほど離して、壁に向けておいてください。そこで、ラズパイのターミナルで
$ roslaunch pimouse_corridor wall_stop.launch
と入力します。すると、ロボットが走り出して、壁の10センチ手前で停止します。
次に、ロボットを壁沿に走らせることを試みましょう。このために、wall_around.pyというスクリプト作成します。以下のファイルを書いて、
#!/usr/bin/env python
import rospy,copy,math
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse
from pimouse_ros.msg import LightSensorValues
class WallAround():
def __init__(self):
self.cmd_vel = rospy.Publisher('/cmd_vel',Twist,queue_size=1)
self.sensor_values = LightSensorValues()
rospy.Subscriber('/lightsensors', LightSensorValues, self.callback_lightsensors)
def callback_lightsensors(self,messages):
self.sensor_values = messages
def wall_front(self,ls):
return ls.left_forward > 50 or ls.right_forward > 50
def too_right(self,ls):
return ls.right_side > 50
def too_left(self,ls):
return ls.left_side > 50
def run(self):
rate = rospy.Rate(20)
data = Twist()
data.linear.x = 0.0
data.angular.z = 0.0
while not rospy.is_shutdown():
data.linear.x = 0.3
if self.wall_front(self.sensor_values):
data.angular.z = - math.pi
elif self.too_right(self.sensor_values):
data.angular.z = math.pi
elif self.too_left(self.sensor_values):
data.angular.z = - math.pi
else:
e = 1.0 * (50 - self.sensor_values.left_side)
data.angular.z = e * math.pi / 180.0
self.cmd_vel.publish(data)
rate.sleep()
if __name__ == '__main__':
rospy.init_node('wall_trace')
rospy.wait_for_service('/motor_on')
rospy.wait_for_service('/motor_off')
rospy.on_shutdown(rospy.ServiceProxy('/motor_off',Trigger).call)
rospy.ServiceProxy('/motor_on',Trigger).call()
w = WallAround()
w.run()
scriptsディレクトリに保存してください。さらに、launchファイルを作成します。
<launch>
<include file="$find pimouse_ros)/launch/pimouse.launch" />
<node pkg="pimouse_corridor" name="wall_around" type="wall_around.py" />
</launch> $ roslaunch pimouse_corridor wall_around.launchと入力します。すると、ロボットが走り出して、壁から10センチほど離れて、壁や柱の凹凸を避けながら走ります。
pimouseの壁に沿った自動走行の動画です
ライブ映像を用いてロボットを走らせる |
Raspberry Pi Mouse Robotに搭載したLogicool製 USBカメラ
$ ls /dev/video0 /dev/video0ラズパイマウス・ロボットのデバイスファイルはすでに組み込まれているので、カメラを使用するためのROSパッケージをインストールするだけです。以下のように順次打ち込んでインストールします。
$ sudo apt install ros-kinetic-cv-bridge $ sudo apt install ros-kinetic-cv-camera $ sudo apt install ros-kinetic-image-transport-plugins $ cd ~/catkin_ws/src $ git clone git@github.com:RobotWebTools/mjpeg_server.git $ cd ~/catkin_ws $catkin_makeros-kinetic-cv-bridge はROSでOpenCVを使用するためのパッケージで、ros-kinetic-cv-camera はカメラから画像を取得して、ROSにメッセージとして配信するためのパッケージです。ros-kinetic-image-transport-plugins はROSで様々な形式の画像をトピックでやり取りするためのパッケージです。以上のインストールで、OpenCVソフトは自動的にインストールされています。mjpeg_server は、画像のメッセージを受け取って、ウエブブラウザに配信するサーバーのためのパッケージです。実は、web_video_serverというパッケージがあるのですが、理由はわからないのですが、現時点で(2017年12月）、このパッケージはうまくインストールできません。なので、少し古いですが、mjpeg_serverを利用します。ちなみに、web_video_serverのソースコードは「git@github.com:RobotWebTools/web_video_server.git」にあります。
$ ipython >>> import cv2 >>> print cv2.__version__ 3.2.0-devと表示されるはずです。OpenCV3バージョンです。
$ rosrun cv_camera cv_camera_node正常に動作していることを確認するために、別のターミナルから
$ rosrun echo /cv_camera/image_rawと打ちます。数字の配列が画面上に表示されます。これが画像のデータです。[Ctrl+c]でrosrunを止めます。
$ rosrun mjpeg_server mjpeg_server _port:=10000と打ちます。_port:=10000はウエッブのポート番号を10000にしています。これを指定しないとき、デフォルトでは、8080となっています。WiFi接続されたPCまたはスマホのブラウザから、ロボットのIPアドレスが192.168.10.10のとき
http://192.168.10.10:10000/stream?topic=/cv_camera/image_rawとURLを指定します。ロボットのカメラからの映像がブラウザに表示されると、成功です。
$ sudo apt install ros-kinetic-rosbridge-suiteこのソフトrosbridgeは、ROS以外のソフトからROSの機能を利用するためのプロトコル、rosbridge protocolを使用するためのものです。非ROSのソフトがrosbridgeでROSの世界と通信をするときは、JSON(Javascript object notation)でデータをやり取りします。ROSのJSON APIです。
$ cd ~/catkin_ws/src $ catkin_create_pkg pimouse_monitor $ roscd pimouse_monitor $ mkdir scripts contents $ ls CMakeLists.txt contents package.xml pimouse_monitorpackage.xml、CMakeLists.txtが作成されていることが分かります。
$ roscd pimouse_monitor/contents $ wget https://github.com/twbs/bootstrap/releases/download/v3.3.7/bootstrap-3.3.7-dist.zip $ unzip bootstrap-3.3.7-dist.zipこれで、bootstrapのフォルダーが生成されますが、その内容物だけをcontentsディレクトリ内に置きたいので
$ mv bootstrap-3.3.7-dist/* ./ $ rmdir bootstrap-3.3.7-dist/ $ rm bootstrap-3.3.7-dist.zipと入力して、整理します。Bootstrapの現在のバージョンは4.0.0です。バージョン3.3.7での動作確認はできていますが、4.0.0での動作確認はまだしていません。
#!/usr/bin/env python
import rospy, os
import SimpleHTTPServer
def kill():
os.system("kill -KILL " + str(os.getpid()))
os.chdir(os.path.dirname(__file__) + "/../contents")
rospy.init_node("webserver")
rospy.on_shutdown(kill)
SimpleHTTPServer.test()
Python 2.7に内臓されているSimpleHTTPServerというモジュールを組み込みます。関数kill()はシャットダウンされた時の処理の仕方を記述します。os.chdir(os.path.dirname(__file__) + "/../contents")はディレクトリcontentsにパスを移動させて、カレントディレクトリにするための記述です。rospy.init_node("webserver")は'webserver'ノードを立ち上げるためのものです。SimpleHTTPServer.test()がウェブサーバーを立ち上げている命令文です。最も単純なスクリプトにしたいときは、最後の１行だけでも動作しますが、ROSにはノードの初期化が必要です。roscd pimouse_monitor/scripts chmod +x webserver.py rosrun pimouse_monitor webserver.pyと入力すると、HTTPサーバーが開始されたことが表示されます。ここに表示されたポート番号が8000であるとき、スマホのブラウザから
http://192.168.10.10:8000とURLを指定すると、contents内のファイルが表示されます。css、fonts、jsという３つのディレクトリがあります。トップページにindex.htmlファイルが見つかると、自動的にこのファイルを読み込んで処理します。
<!DOCTYPE html>
<html lang="ja">
<head>
<meta charset="utf-8">
<meta http-equiv="X-UA-Compatible" content="IE=edge">
<meta name="viewport" content="width=device-width, initial-scale=1">
<title>Pimouse Monitor</title>
<!-- Bootstrap -->
<link href="css/bootstrap.min.css" rel="stylesheet">
</head>
<body>
<nav class="navbar navbar-default navbar-fixed-top">
<div class="container">
<div class="navbar-header">
<a class="navbar-brand" href="#">Pimouse Monitor</a>
</div>
</div>
</nav>
<div class="container" style="padding-top:60px">
<div class="row">
<div class="col-md-4">.col-md-4</div>
<div class="col-md-4">.col-md-4</div>
<div class="col-md-4">.col-md-4</div>
</div>
</div>
<!-- jQuery (necessary for Bootstrap's JavaScript plugins) -->
<script src="js/jquery.min.js"></script>
<!-- Include all compiled plugins (below), or include individual files as needed -->
<script src="js/bootstrap.min.js"></script>
</body>
</html>
<div class="row">
<div class="container" style="padding-top:60px">
---
</div>
</div>
<script src="js/jquery.min.js"></script>
<script src="https://ajax.googleapis.com/ajax/libs/jquery/3.3.1/jquery.min.js"></script>
$ wget https://code.jquery.com/jquery-3.3.1.min.jsとすると、最新バージョン（jquery-3.3.1.min.js）がダウンロードできます。minは容量を小さくしたファイルを意味します。bootstrap.min.jsはBootstrapをダウンロードした時にインストールされます。
$ ln -s index1.html index.htmlと打ちます。
$ roscd pimouse_monitor $ mkdir launch $ cd launch以下のスクリプトにmonitor1.launchと名前をつけて、launchディレクトリに保存してください。
<launch>
<node pkg="pimouse_monitor" name="webserver" type="webserver.py" args="8000" />
<include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch">
<arg name="port" value="9000"/>
</include>
<include file="$(find pimouse_ros)/launch/pimouse.launch" />
</launch>
$ roslaunch pimouse_monitor monitor1.launchroslaunchが立ち上がると、ターミナルに以下のように表示されます。
... logging to /home/ubuntu/.ros/log/af61e67e-2d86-11e8-a688-b827eb3b9c1b/roslaunch-Raspi-2413.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://localhost:38721/
SUMMARY
========
PARAMETERS
* /rosapi/params_glob: [*]
* /rosapi/services_glob: [*]
* /rosapi/topics_glob: [*]
* /rosbridge_websocket/address:
* /rosbridge_websocket/authenticate: False
* /rosbridge_websocket/bson_only_mode: False
* /rosbridge_websocket/delay_between_messages: 0
* /rosbridge_websocket/fragment_timeout: 600
* /rosbridge_websocket/max_message_size: None
* /rosbridge_websocket/params_glob: [*]
* /rosbridge_websocket/port: 9000
* /rosbridge_websocket/retry_startup_delay: 5
* /rosbridge_websocket/services_glob: [*]
* /rosbridge_websocket/topics_glob: [*]
* /rosdistro: kinetic
* /rosversion: 1.12.7
NODES
/
buzzer (pimouse_ros/buzzer.py)
lightsensors (pimouse_ros/lightsensors.py)
motors (pimouse_ros/motors.py)
rosapi (rosapi/rosapi_node)
rosbridge_websocket (rosbridge_server/rosbridge_websocket)
webserver (pimouse_monitor/webserver.py)
auto-starting new master
process[master]: started with pid [2425]
ROS_MASTER_URI=http://localhost:11311
$curl http://localhost:9000 Can "Upgrade" only to "WebSocket".ubuntu@Raspi:~$と表示されますので、確認できます。WiFi経由で無線LAN接続されたPCやスマホのブラウザから
http://192.168.10.10:8000とURLを指定すると、index1.htmlのページが表示されます。
<!DOCTYPE html>
<html lang="ja">
<head>
<meta charset="utf-8">
<meta http-equiv="X-UA-Compatible" content="IE=edge">
<meta name="viewport" content="width=device-width, initial-scale=1">
<title>Pimouse Monitor</title>
<!-- Bootstrap -->
<link href="css/bootstrap.min.css" rel="stylesheet">
</head>
<body>
<nav class="navbar navbar-default navbar-fixed-top">
<div class="container">
<div class="navbar-header">
<a class="navbar-brand" href="#">Pimouse Monitor</a>
</div>
</div>
</nav>
<div class="container" style="padding-top:60px">
<div class="row">
<div class="col-md-4" id="lightsensors"><span id="lightsensors"></span></div>
<div class="col-md-4">.col-md-4</div>
<div class="col-md-4">.col-md-4</div>
</div>
</div>
<!-- jQuery (necessary for Bootstrap's JavaScript plugins) -->
<script src="js/jquery.min.js"></script>
<!-- Include all compiled plugins (below), or include individual files as needed -->
<script src="js/bootstrap.min.js"></script>
<script src="js/eventemitter2.min.js"></script>
<script src="js/roslib.min.js"></script>
<script src="main.js"></script>
</body>
</html>
<script src ="https://static.robotwebtools.org/EventEmitter2/current/eventemitter2.min.js"></script>
<script src="https://static.robotwebtools.org/roslibjs/current/roslib.min.js"></script>
var ros = new ROSLIB.Ros({ url : 'ws://' + location.hostname + ':9000' });
ros.on('connection', function() {console.log('websocket: connected'); });
ros.on('error', function(error) {console.log('websocket error: ', error); });
ros.on('close', function() {console.log('websocket: closed');});
var ls = new ROSLIB.Topic({
ros : ros,
name : '/lightsensors',
messageType : 'pimouse_ros/LightSensorValues'
});
ls.subscribe(function(message) {
str = JSON.stringify(message);
document.getElementById("lightsensors").innerHTML = str;
//$("#lightsensors").html(str); //このように書いてもよい
console.log(str);
});
このJavascriptファイルは、index2.htmlの中で
<script src="main.js"></script>
$ ln -s main2.js main.jsと貼ります。launchファイルを立ち上げ、
$ roslaunch pimouse_monitor monitor1.launchWiFi経由で接続されたPCやスマホのブラウザから
http://192.168.10.10:8000とURLを指定すると、ブラウザに距離センサーの値が表示されます。この表示の仕方では読みにくいので、もっとわかりやすい表示に改善する必要があります。
<!DOCTYPE html>
<html lang="ja">
<head>
<meta charset="utf-8">
<meta http-equiv="X-UA-Compatible" content="IE=edge">
<meta name="viewport" content="width=device-width, initial-scale=1">
<title>Pimouse Monitor</title>
<link href="css/bootstrap.min.css" rel="stylesheet">
</head>
<body>
<nav class="navbar navbar-default navbar-fixed-top">
<div class="container">
<div class="navbar-header">
<a class="navbar-brand" href="#">Pimouse Monitor</a>
</div>
</div>
</nav>
<div class="container" style="padding-top:60px">
<div class="row">
<div class="col-md-4" id="lightsensors">
<h2>Light Sensors</h2>
<table class="table table-striped">
<tr><th>DIRECTION</th><th>VALUE</th></tr>
<tr><td>left_forward</td><td id="left_forward"></td></tr>
<tr><td>left_side</td><td id="left_side"></td></tr>
<tr><td>right_side</td><td id="right_side"></td></tr>
<tr><td>right_forward</td><td id="right_forward"></td></tr>
<tr><td>sum_all</td><td id="sum_all"></td></tr>
<tr><td>sum_forward</td><td id="sum_forward"></td></tr>
</table>
</div>
<div class="col-md-4">.col-md-4</div>
<div class="col-md-4">.col-md-4</div>
</div>
</div>
<!-- jQuery (necessary for Bootstrap's JavaScript plugins) -->
<script src="js/jquery.min.js"></script>
<!-- Include all compiled plugins (below), or include individual files as needed -->
<script src="js/bootstrap.min.js"></script>
<script src="js/eventemitter2.min.js"></script>
<script src="js/roslib.min.js"></script>
<script src="main.js"></script>
</body>
</html>
var ros = new ROSLIB.Ros({ url : 'ws://' + location.hostname + ':9000' });
ros.on('connection', function() {console.log('websocket: connected'); });
ros.on('error', function(error) {console.log('websocket error: ', error); });
ros.on('close', function() {console.log('websocket: closed');});
var ls = new ROSLIB.Topic({
ros : ros,
name : '/lightsensors',
messageType : 'pimouse_ros/LightSensorValues'
});
ls.subscribe(function(message) {
for( e in message ){
document.getElementById(e).innerHTML = message[e];
}
});
$ rm index.html $ rm main.js $ ln -s index3.html index.html $ ln -s main3.js main.jsと貼ります。launchファイルを立ち上げ、
$ roslaunch pimouse_monitor monitor1.launchWiFi経由で接続されたPCやスマホのブラウザから
http://192.168.10.10:8000とURLを指定すると、ブラウザに距離センサーの値が以下のような表形式で表示されます。
Pimouse Monitor Light Sensors DIRECTION VALUE left_forward -5 left_side 17 right_side 19 right_forward 11 sum_all 42 sum_forward 6次に、カメラの映像をブラウザに写しましょう。index3.htmlをコピーして、子画面のdiv要素に以下のような、object要素を追加します。
<div class="col-md-4">
<h2>Camera</h2>
<object id="camstream" style="width:320px; height:240px"></object>
</div>
document.getElementById('camstream').data='http://' + location.hostname + ':10000/stream?topic=cv_camera_node/image_raw';
$ rm index.html $ rm main.js $ ln -s index4.html index.html $ ln -s main4.js main.jsカメラ映像を取り込むために、launchファイルを拡張します。元のlaunchファイルに、cv_cameraとmjpeg_serverのノードを加えて、以下のファイルを作成します。
<launch>
<node pkg="pimouse_monitor" name="webserver" type="webserver.py" args="8000" />
<include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch">
<arg name="port" value="9000"/>
</include>
<include file="$(find pimouse_ros)/launch/pimouse.launch" />
<node pkg="cv_camera" name="cv_camera_node" type="cv_camera_node" >
<param name="rate" value="10.0" />
<param name="image_width" value="320" />
<param name="image_height" value="240" />
</node>
<node pkg="mjpeg_server" name="mjpeg_server" type="mjpeg_server" >
<param name="port" value="10000" />
</node>
</launch>
<!DOCTYPE html>
<html lang="ja">
<head>
<meta charset="utf-8">
<meta http-equiv="X-UA-Compatible" content="IE=edge">
<meta name="viewport" content="width=device-width, initial-scale=1">
<title>Pimouse Monitor</title>
<!-- Bootstrap -->
<link href="css/bootstrap.min.css" rel="stylesheet">
</head>
<body>
<nav class="navbar navbar-default navbar-fixed-top">
<div class="container">
<div class="navbar-header">
<a class="navbar-brand" href="#">Pimouse Monitor</a>
</div>
</div>
</nav>
<div class="container" style="padding-top:60px">
<div class="row">
<div class="col-md-4" id="lightsensors">
<h2>Light Sensors</h2>
<table class="table table-striped">
<tr><th>DIRECTION</th><th>VALUE</th></tr>
<tr><td>left_forward</td><td id="left_forward"></td></tr>
<tr><td>left_side</td><td id="left_side"></td></tr>
<tr><td>right_side</td><td id="right_side"></td></tr>
<tr><td>right_forward</td><td id="right_forward"></td></tr>
<tr><td>sum_all</td><td id="sum_all"></td></tr>
<tr><td>sum_forward</td><td id="sum_forward"></td></tr>
</table>
</div>
<div class="col-md-4">
<h2>Camera</h2>
<object id="camstream" style="width:300px;height:240px" ></object>
</div>
<div class="col-md-4">
<h2>Motors <button type="button" class="btn btn-danger" id="motor_on">ON</button>
<button type="button" class="btn btn-primary" id="motor_off">OFF</button></h2>
<div id="touchmotion" class="well" style="width:300px;height:300px;cursor:pointer">
<span id="vel_fw">0</span>[mm/s] <span id="vel_rot">0</span>[deg/s]
</div>
</div>
</div>
</div>
<!-- jQuery (necessary for Bootstrap's JavaScript plugins) -->
<script src="js/jquery.min.js"></script>
<!-- Include all compiled plugins (below), or include individual files as needed -->
<script src="js/bootstrap.min.js"></script>
<script src="js/eventemitter2.min.js"></script>
<script src="js/roslib.min.js"></script>
<script src="main.js"></script>
</body>
</html>
var ros = new ROSLIB.Ros({ url : 'ws://' + location.hostname + ':9000' });
ros.on('connection', function() {console.log('websocket: connected'); });
ros.on('error', function(error) {console.log('websocket error: ', error); });
ros.on('close', function() {console.log('websocket: closed');});
var ls = new ROSLIB.Topic({
ros : ros,
name : '/lightsensors',
messageType : 'pimouse_ros/LightSensorValues'
});
ls.subscribe(function(message) {
for( e in message ){
document.getElementById(e).innerHTML = message[e];
}
});
var on = new ROSLIB.Service({
ros : ros,
name : '/motor_on',
messageType : 'std_srvs/Trigger'
});
var off = new ROSLIB.Service({
ros : ros,
name : '/motor_off',
messageType : 'std_srvs/Trigger'
});
$('#motor_on').on('click', function(e){
on.callService(ROSLIB.ServiceRequest(),function(result){
if(result.success){
$('#motor_on').attr('class','btn btn-danger');
$('#motor_off').attr('class','btn btn-default');
}
});
});
$('#motor_off').on('click', function(e){
off.callService(ROSLIB.ServiceRequest(),function(result){
if(result.success){
$('#motor_on').attr('class','btn btn-default');
$('#motor_off').attr('class','btn btn-primary');
}
});
});
var vel = new ROSLIB.Topic({
ros : ros,
name : '/cmd_vel',
messageType : 'geometry_msgs/Twist'
});
function pubMotorValues(){
fw = $('#vel_fw').html();
rot = $('#vel_rot').html();
fw = parseInt(fw)*0.001;
rot = 3.141592*parseInt(rot)/180;
v = new ROSLIB.Message({linear:{x:fw,y:0,z:0}, angular:{x:0,y:0,z:rot}});
vel.publish(v);
}
$('#touchmotion').on('click', function(e){
rect = $('#touchmotion')[0].getBoundingClientRect();
x = e.pageX - rect.left - window.pageXOffset;
y = e.pageY - rect.top - window.pageYOffset;
vel_fw = (rect.height/2 - y)*3;
vel_rot = rect.width/2 - x;
$('#vel_fw').html(parseInt(vel_fw));
$('#vel_rot').html(parseInt(vel_rot));
});
setInterval(pubMotorValues,100);
document.getElementById('camstream').data = 'http://'
+ location.hostname
+ ':10000/stream?topic=/cv_camera_node/image_raw';
$ rm index.html $ rm main.js $ ln -s index5.html index.html $ ln -s main5.js main.jsroslaunchを立ち上げます。
$ roslaunch pimouse_monitor monitor.launchこのコマンドが成功すると、ターミナルに以下のようが表示が続けてでます。
[INFO] [1521858663.643483]: Rosbridge WebSocket server started on port 9000 Corrupt JPEG data: 1 extraneous bytes before marker 0xd0 Corrupt JPEG data: 1 extraneous bytes before marker 0xd5 Corrupt JPEG data: 2 extraneous bytes before marker 0xd0 Corrupt JPEG data: 3 extraneous bytes before marker 0xd1 Corrupt JPEG data: 2 extraneous bytes before marker 0xd5 Corrupt JPEG data: 1 extraneous bytes before marker 0xd6 Corrupt JPEG data: 1 extraneous bytes before marker 0xd6 Corrupt JPEG data: 1 extraneous bytes before marker 0xd3 Corrupt JPEG data: 2 extraneous bytes before marker 0xd2 Corrupt JPEG data: 2 extraneous bytes before marker 0xd1 Corrupt JPEG data: 4 extraneous bytes before marker 0xd7この表示が出ているとき、スマホのブラウザから
http://192.168.10.10:8000とURLを指定すると、ブラウザに距離センサーの値、カメラの映像、およびモーターのコントローラーが以下のように表示されます。
