Welcome to Mashykom WebSite



RaspberryPi Mouseロボットの操作

 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程度の電圧を供給する電源(3ピンコネクター付きリチウムイオン電池)が必要です。上記のロボット本体にセットで付属していますが、充電器は付いていません。LiPo充電器LBC-010とACアダプタ(12V5A)+VH3ピンコネクタへの変換ケーブルのセットが上記のRT 社から入手可能です。¥8,250.-(税込)です。

 以下での説明では主に Python を用いて書かれたコードを使用します。Linux や C 言語の知識は前提としませんが、ROS がインストールされていることは必須です。ROS のインストールの方法については、このページを参照して下さい。

Last updated:2020.5.24 ( first uploaded : 2018.3.20)

pimouse_2.JPG
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

と入力します。その後、ラズパイの緑色の点滅が停止してから電源スイッチを切ってください。

pimouse.JPG
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()

 第1行目の#!/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 ファイルを実行可能な状態にしておきます。

このプログラムを実行するために、3つのターミナルを起動します。ssh 接続でラズパイのターミナルを3つ立ち上げておきます。最初のターミナルで、roscoreを立ち上げます。


$ roscore

2つ目のターミナルで、


$ rosrun pimouse_ros buzzer.py

とノードを立ち上げます。topicをパブリッシュするために、3つ目のターミナルから


$ 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’ を作成します。トピックのメッセージの型を独自に定めます。距離センサーは4つありますので、4つの値のやりとりが必要です。各センサーに名前をつける必要もあります。型を定めるために、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

の2行のコメントを外します。これは、message_generation をビルドする時に、message_runtime を使うという意味になります。

 次に、CMakeLists.txtを編集します。C言語のソースファイルをコンパイルさせるための makefile、CMake に読み込ませるファイルです。C言語の知識がない人はそうなんだと理解して進んでください。ディレクトリ pimouse_ros にある CMakeLists.txtを開いてください。変更する箇所は4箇所です。1つ目は、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という文を追加してください。

2つ目、add_message_filesというコマンドの行を探してください。デフォルトでは、その行以下が#でコメントアウトされています。#を外して、


## Generate messages in the 'msg' folder
add_message_files(
	FILES
	LightSensorValues.msg
)

と、LightSensorValues.msgを追加する。3つ目は、generate_messagesの行を探します。この行以下が#でコメントアウトされていますので、以下のように#を外します。

## Generate added messages and services with any dependencies listed here

generate_messages(
	DEPENDENCIES
	std_msgs
)

と修正します。

最後の4つ目は、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)は1秒間に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

と打つと、距離センサーが1秒間に1回光ります。


  最後に、モーターの制御に関するプログラムを作成します。以下では、煩雑ですが、roscore の起動を含めて3つのターミナルを立ち上げる必要があります。 モーターのデバイス・ファイルrtmotor_raw_{l,r}0へ回転数の周波数を送るノードを二つ作り、2種類のトピック、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ビット浮動小数点形式です。方向は3次元となります。

  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() は5つのメソッド、__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が1秒以上過去であれば、両方のモーターの周波数をゼロとして、using_cmd_velの値をFalseにします。言い換えると、トピックcmd_velを使ってモーターを回転させるときは、1秒間で回転が停止する。トピック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" 

と入力すると、モーターが回転して、1秒後に停止します。


  ターミナルを3つも立ち上げるのは煩雑です。そこで、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 を立ち上げて、2つ目のターミナルを立ち上げて、以下のように打ち込んでください。


$ roscd pimouse_ros/scripts
$ chmod +x motors2.py
$ rosrun pimouse_ros motors2.py

 さらに、3つ目のターミナルから、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 を立ち上げます。そして、2つ目のターミナルから以下のように順番にコマンドを入力します。


$ 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 の1行を 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の速度で直進をさせる部分と、4つの距離センサーの合計値が500を超えた時に、cmd/vel に0を送る部分からなります。

 次に、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ディレクトリに保存してください。

 最後に、ロボットを動かしてみましょう。デバイス・ドライバーを読み込み、モーターの電源スイッチを入れて、ロボットを壁から1メートルほど離して、壁に向けておいてください。そこで、ラズパイのターミナルで


$ 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>	

このファイルをwall_around.launchという名前で、launchディレクトリに保存してください。
  デバイス・ドライバーを読み込み、モーターの電源スイッチを入れて、ロボットを壁から離して、壁に平行に向けておいてください。そして、
$ roslaunch pimouse_corridor wall_around.launch
と入力します。すると、ロボットが走り出して、壁から10センチほど離れて、壁や柱の凹凸を避けながら走ります。

pimouseの壁に沿った自動走行の動画です


ライブ映像を用いてロボットを走らせる


このセクションでは、ブラウザから操作できるロボットのコントロールパネルを作成し、スマホのブラウザからロボットを動かしてみます。ブラウザ上にロボットに搭載したwebカメラからの映像を表示することができるようにします。最初に、ROSでUSBカメラを使用できるようにしましょう。まずロボットにUSBカメラを搭載します。ロボットに電源を通電し、WiFi経由でssh接続をしてください。ロボットのデバイスドライバを読み込んでください。

raspi_camera.JPG
Raspberry Pi Mouse Robotに搭載したLogicool製 USBカメラ

  UVC規格のUSBカメラ、ここでは、Logicool ウエブカムC310を使いました。USBカメラをラズパイに接続すると、以下のように/dev/video0 というデバイスファイルが表示されます。
$ 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_make
ros-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」にあります。
  OpenCVのバージョンを確認します。バージョンが3系でも、モジュール名はcv2 です。
$ ipython
>>> import cv2
>>> print cv2.__version__
3.2.0-dev
と表示されるはずです。OpenCV3バージョンです。
  USBカメラとcv_cameraの動作確認をします。roscoreを立ち上げてから、cv_cameraのノードを立ち上げます。
$ rosrun cv_camera cv_camera_node
正常に動作していることを確認するために、別のターミナルから
$ rosrun echo /cv_camera/image_raw
と打ちます。数字の配列が画面上に表示されます。これが画像のデータです。[Ctrl+c]でrosrunを止めます。
  次に、mjpeg_serverの動作確認をします。先ほどの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を指定します。ロボットのカメラからの映像がブラウザに表示されると、成功です。
  ここから、webアプリを作成して、スマホからロボットを操作するためのパッケージの作成に入ります。インストールする必要があるソフトは、rosbridgeとBootstrapです。まず、rosbridgeをインストールします。
$ sudo apt install ros-kinetic-rosbridge-suite
このソフトrosbridgeは、ROS以外のソフトからROSの機能を利用するためのプロトコル、rosbridge protocolを使用するためのものです。非ROSのソフトがrosbridgeでROSの世界と通信をするときは、JSON(Javascript object notation)でデータをやり取りします。ROSのJSON APIです。
  作成するパッケージの名称をpimouse_monitorとします。webアプリを使うこのパッケージでは、htmlファイル、 Java script、cssファイルなどが必要になります。以下のようにパッケージを作ります。
$ cd ~/catkin_ws/src
$ catkin_create_pkg pimouse_monitor
$ roscd pimouse_monitor
$ mkdir scripts contents
$ ls
CMakeLists.txt contents package.xml pimouse_monitor
package.xml、CMakeLists.txtが作成されていることが分かります。
  次に、contentsディレクトリ内にBootstrapのパッケージを配置します。Bootstrapの公式ページhttps://getbootstrap.com/docs/3.3/getting-started#downloadの「Download Bootstrap」ボタンのリンク先をコピーします。これを以下のように、wgetの後にペーストします。
$ 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での動作確認はまだしていません。
  次に、ウェブサーバーを準備します。通常は、Apacheを使用しますが、ここでは、簡単化のために、Python 2.7に用意されている簡易なサーバーを使います。scriptsディレクトリ内にサーバー作成用のwebserver.pyファイルを以下のように作成します。
#!/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()がウェブサーバーを立ち上げている命令文です。最も単純なスクリプトにしたいときは、最後の1行だけでも動作しますが、ROSにはノードの初期化が必要です。
roscoreを立ち上げて、
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という3つのディレクトリがあります。トップページにindex.htmlファイルが見つかると、自動的にこのファイルを読み込んで処理します。
  htmlファイルを作成してみましょう。以下のスクリプトをindex1.htmlという名前にして、contentsディレクトリに保存してください。
<!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>

コードはgetbootstrapのサンプルソースで提供されてる標準的なもので、タイトルがふさわしい名前に変更されています。

<div class="row">
<div class="container"  style="padding-top:60px">
--- 
</div>
</div>

はgridというサンプルコードと同じです。4列表示の画面を1画面ごと3行に分けて表示させるものです。nav要素の記述に従い、画面の最上部にPimouse Monitorと表示されます。


<script src="js/jquery.min.js"></script>


の行は、インターネット上のCDNのURLを指定して、


<script src="https://ajax.googleapis.com/ajax/libs/jquery/3.3.1/jquery.min.js"></script>


と書くことの方が一般的です。ここでは安定性を重視して、ダウンロードしたjquery.min.jsのソースファイルをディレクトリjsに保存しています。また、jQuery公式ホームページのダウンロードのリンクをコピーして、
$ wget https://code.jquery.com/jquery-3.3.1.min.js
とすると、最新バージョン(jquery-3.3.1.min.js)がダウンロードできます。minは容量を小さくしたファイルを意味します。bootstrap.min.jsはBootstrapをダウンロードした時にインストールされます。
  index1.htmlという名前では自動的に読み込まれないので、index.htmlにシンボリック・リンクを貼ります。
$ ln -s index1.html index.html
と打ちます。
  最後に、launchファイルを作成します。
$ 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>

pimouse_monitorパッケージはpimouse_rosパッケージを部分的に使用しますので、launchファイルの中に埋め込みます。rosbridgeも使用します。webserver.pyのポート番号を8000としています。rosbridgeはポート9000番を使用すると記述していますが、これはメモ程度のものです。
  launchファイルを立ち上げます。
$ roslaunch pimouse_monitor monitor1.launch
roslaunchが立ち上がると、ターミナルに以下のように表示されます。
... 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

rosbridgeからのレスポンスを見るためには、別のターミナルから以下のように打つと、
$curl http://localhost:9000
Can "Upgrade" only to "WebSocket".ubuntu@Raspi:~$
と表示されますので、確認できます。WiFi経由で無線LAN接続されたPCやスマホのブラウザから
http://192.168.10.10:8000
とURLを指定すると、index1.htmlのページが表示されます。
[Ctrl+c]でroslaunchを停止してください。
  距離センサーの値をブラウザに表示するプログラムを作成します。index1.htmlにrosbridgeによる通信部分を書き加えたindex2.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>

id="lightsensors"という記述は、以下のmain.jsファイルの中で
document.getElementById("lightsensors").innerHTML = str;
を対応させるためのidで、ブラウザの表示に埋め込む場所を示しています。 jsファイルのeventemitter2.min.jsとroslib.min.jsはダウンロードして、ディレクトリjsに保存したJavascriptのソースファイルです。ROSの通信に必要なファルです。以下のリンク先からコピーできますが、ソースファイルを保存しないときは、インターネットのリンク先を

<script src ="https://static.robotwebtools.org/EventEmitter2/current/eventemitter2.min.js"></script>
<script src="https://static.robotwebtools.org/roslibjs/current/roslib.min.js"></script>


とプログラムの中で指定すると、利用できます。
  以下のスクリプトを作成し、名前をmain2.jsとしてcontentsディレクトリに保存します。
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>


から読み込まれると、ブラウザに距離センサーの値を表示します。第1行目のvar ros = new ROSLIB.Ros({ url : 'ws://' + location.hostname + ':9000' });は、{}内のオブジェクト・リテラルを引数としてROSLIB.Rosに渡して、オブジェクト変数rosを作成しています。{}内の記述はrosbridgeのサーバーのurlがロボットのIPアドレスで、ポート番号が9000であり、通信のプロトコルにWeb Socketを用いることを意味します。var ls = new ROSLIB.Topic({・・・{);は、ROSのトピックのオブジェクトを定義をしています。ls.subscribe(function(message) {・・・}は、トピックからのメッセージを処理するところです。その中で、str = JSON.stringify(message)はJSON形式で受け取ったメッセージを文字列に変換しています。
シンボリックリンクを
$ ln -s main2.js main.js
と貼ります。launchファイルを立ち上げ、
$ roslaunch pimouse_monitor monitor1.launch
WiFi経由で接続されたPCやスマホのブラウザから
http://192.168.10.10:8000
とURLを指定すると、ブラウザに距離センサーの値が表示されます。この表示の仕方では読みにくいので、もっとわかりやすい表示に改善する必要があります。
  htmlファイルを拡張しましょう。それに合わせて同時に、Javascriptファイルも修正します。距離センサーの値の表示を表形式して、それに対応するようにjsファイルを修正します。index2.htmlをコピーして、以下のように修正を加えて、index3.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>   
    <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>

main2.htmlをコピーして、以下のように修正を加えて、main3.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.launch
WiFi経由で接続された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>

このファイルをindex4.htmlとして、保存します。これに対応するJSファイルを作成します。main3.jsをコピーして、main4.jsファイルを作成します。追加修正は以下の行をスクリプトの最下部に追加します。
document.getElementById('camstream').data='http://' + location.hostname + ':10000/stream?topic=cv_camera_node/image_raw';

このコードはカメラの映像をobject要素のdataという属性に埋め込むための処理をします。このコードが読み込まれると、ブラウザに映像が表示されます。 前と同じく、シンボリックリンクを貼り直します。
$ 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>

このファイルをmonitor.launchと名前をつけて、保存してください。
  最後に、モーターのコントローラーをスマホのブラウザに表示させるためのプログラムを組みましょう。モーターの子画面を作るために、新しいhtmlファイルを以下の通り作成します。index5.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">
          <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]&nbsp;<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>

子画面用のdiv要素のMotors関係の部分が新しく追加されたコードです。この子画面には、モータースイッチのON/OFFボタンとタッチパッド仕様のモーターのコントローラーが配置されています。タッチパッドの上側(下側)をタッチすると、中心からの距離に応じてロボットが速度を変えて前進(後退)します。左側(右側)をタッチする、右(左)方向に回転します。
次に、JSファイルを作成します。以下のスクリプトをmain5.jsとして保存してください。
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';

このスクリプトはjQueryを使用することを前提しています。var on = new ROSLIB.Service 以下がmotor制御に関する新しい部分です。まず、サービス/motor_on、/motor_offに対応するオブジェクトonとoffを作っています。その後に、オン・オフのボタンがタッチされた時の処理が書かれています。ONがタッチされた('click')時の処理が$('#motor_on').on('click', function(e)以下の関数で記述され、OFFがタッチされた('click')時の処理が$('#motor_off').on('click', function(e)以下の記述文になります。ROSのサービスを使用するために、callserviceというメソッドを用いて、サービスから返ってきたデータがresultに入ります。モーターの電源が入っている場合、ボタンの色を変化させています。
モーターへの速度に関するメッセージに関する部分は、var vel = new ROSLIB.Topic以下になります。トピック/cmd_velを作成しています。function pubMotorValues()はメッセージのデータを作ってパブリッシュします。IDが'vel_fwと'vel_rot'のHTMLの要素から文字列を読み取り、それぞれfwとrotに入れて、単位変換します。 Roslibを用いてvel.publish(v)で送信しています。
前と同じく、シンボリックリンクを貼り直します。
$ rm index.html
$ rm main.js
$ ln -s index5.html index.html
$ ln -s main5.js main.js
roslaunchを立ち上げます。
$ 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を指定すると、ブラウザに距離センサーの値、カメラの映像、およびモーターのコントローラーが以下のように表示されます。

monitor.png


ただし、この画面はPCなので子画面が横に表示されていますが、スマホのブラウザでは子画面は縦に表示されます。

 Motorsの[on]をクリックして、四角形枠内の画面上のどこかをタッチすると、タッチした場所に依存した速度でモーターが回転します。モーターを止めるときは、[off]をクリックします。

**************************************************************
日本語音声でロボットを走らせる
**************************************************************

ーーー以下、省略ーーー

このページのトップに戻る

トップページに戻る