Welcome to Mashykom WebSite
Raspberry Pi 4を用いたRobot(例えば、RT/RaspberryPI Mouseなど)の操作をROS(Robot Operating System)を用いて実行するためには、ROSをインストールする必要があります。ROSはUbuntuをサポートしているので、Ubuntu ディストリビューションをインストールします。ここでは、Ubuntu Desktop 20.04 LTS の ARM64 バージョンがないので、 Ubuntu Mate 20.04 LTS(64bit)をインストールします。ここで使用するリモートPCは Macです。Ubuntu Server 20.04 LTS の Raspberry Pi へのインストールとROSの活用についての説明は、このページに あります。
使用するRasPiはRaspberry Pi 4(Model B) です。その構成と機能は以下の通りです。
SoC(System on Chip): BCM2711
64-bit CPU： Cortex-A72(ARM v8) 4コア(1.5GHz),メモリ：2~8GB、LPDDR2
ネットワークコネクタ：Ethernet, WiFi, Bluetooth, BLE、 USB2及びUSB3コネクタ：各2個、
GPIO拡張ピン：40個、SDカードコネクター：microSD、オーディオ：φ3.5mm4極ミニジャック
電源コネクタはUSB-C(5.1V,3000mA以上必要）、ディスプレイ接続はmicroHDMIコネクタです。
Raspberry Piを作動させるためには、電源用USB-microCケーブル、モニター(TV)との接続用 microHDMIーHDMIケーブル、USB接続のキーボードとマウス、OSがインストールされたmicroSDカードが必要です。Raspberry Piにはハードディスクが内蔵されていないので、microSDカードにOSをインストールして用います。
micro SDカードへの Ubuntu Mate 20.04 LTS のインストールの方法を説明します。PCのブラウザを開いて、Raspberry Pi用のOSインストーラを公式ホームページ からダウンロードする。推奨されている方法は、Raspberry Pi Imager を最初にダウンロードして、この Imager をPCにインストールする、その後に、この Imager を起動して インスットールする OS 本体をSD カードに書き込むという方法です。
Ububtu Mate ではデスクトップが利用できますが、VNC Server の設定が面倒なので、 リモートPCからはコンソールを用いたコマンドラインで行います。従って、基本的にはPCから ssh 接続で操作します。Ubuntu OS の取扱説明はLinux OS 入門のページ を参照ください
Ubuntu Mate 20.04 LTS をインストールした後で、ROSのインストール手順を説明します。2022年2月現在、ROSのLTS最新バージョンはROS Noetic Ninjemys です。サポートするプラットフォームは Ubuntu 20.04 です。Python3をサポートしています。
Last updated: 2022.2.25
Ubuntu Mate のダウンロードとインストール
Ubuntu Mate のダウンロードサイト に行って、Ubuntu Mate 20.04 LTS（64bit）のzipファイルをPCにダウンロードします。解凍展開しておきます。
Ubuntu Mate メージファイルをmicroSDカードにコピーする必要があります。このために、Raspberry Pi の公式ページ から Raspberry Pi Imager をダウンロード、インストールします。次に、この Raspberry Pi Imager を起動します。operating system の choose OS で、use custom をクリックして、PC内に用意したUbuntu Mate 20.04LTS イメージファイルを選択します。Choose Storage でmicroSD カードを選択して、write します。書き込みが終わったら、Raspberry Pi Imager を終了します。
以下の手順で boot を行います。
Ubuntu Mate 書き込み済みのmicroSDカードを、Raspberry Pi のSDカードスロットに挿入する。
USB接続キーボードをRaspberry Pi のUSB端子に接続する。
モニタのHDMIケーブルをRaspberry Pi のHDMI端子に接続する。
USB充電器をRaspberry Pi のmicro USB端子に接続する。
最後に、USB充電器をAC電源コンセントに接続し、Raspberry Pi を起動させる。
microSDをRaspiに挿入して、起動すると、Boot が開始します。ログイン画面が表示されます。表示に従って、日本語、キーボード、ロケーション、wifi などの設定を行います。
まず、aptのパッケージリストを更新してアップグレードしておきます。
$ sudo apt update
$ sudo apt upgrade
と入力して、確認します。WiFi のIP アドレスを確認するために、以下のコマンドを打ちます。
$ ip a
------
1: lo: mtu 65536 qdisc noqueue state UNKNOWN group default qlen 1000
link/loopback 00:00:00:00:00:00 brd 00:00:00:00:00:00
inet 127.0.0.1/8 scope host lo
valid_lft forever preferred_lft forever
inet6 ::1/128 scope host
valid_lft forever preferred_lft forever
2: eth0: mtu 1500 qdisc fq_codel state DOWN group default qlen 1000
link/ether b8:27:eb:c8:82:e9 brd ff:ff:ff:ff:ff:ff
3: wlan0: mtu 1500 qdisc fq_codel state UP group default qlen 1000
link/ether b8:27:eb:9d:d7:bc brd ff:ff:ff:ff:ff:ff
inet 192.168.11.1/24 brd 192.168.11.255 scope global dynamic wlan0
valid_lft 172136sec preferred_lft 172136sec
inet6 fe80::ba27:ebff:fe9d:d7bc/64 scope link
valid_lft forever preferred_lft forever
Raspberry Pi のWiFiのIPアドレスは inet 192.168.11.1 であることが確認できます。リモートPCから操作するために、以下のコマンドを入力します。
$ sudo systemctl enable ssh
ここからは、リモートPCでの操作になります。Raspberry Pi からキーボードとモニターを外します。
Ubuntu Mate に VNC server をインストールするのは煩雑なので、Ubuntu Mate とPCの間でファイル共有をした方が便利です。SSHで Ubuntu Mate とPCの間でファイル共有を可能にするためには、mac OS の場合には若干手数がかかります。FUSE for macOSというソフトをインストールする必要があります。
FUSE for macOSのページ から、「FUSE for macOS」および「SSHFS」をダウンロードします。前者がMacで標準対応していないファイルシステムを扱う枠組みを提供し、後者が実際にSSHをつかってファイルをやり取りするためのソフトです。
FUSE for macOSのインストールでは、サードパーティー製のソフトなので、個別に許可する必要があります。FUSE for macOS をインストールする最後に、このソフトに対するアプリケーション実行の許可を認証する要求が表示されます。「セキュリティ環境設定を開く」をクリックし、ダウンロードしたアプリケーション実行の「許可」をクリックします。これをしないとインストールできません。
次に、ダウンロードしたSSHFSをインストールします。pkgファイルを開くとインストーラーが起動します。使用許諾に同意する場合は「同意する」をクリックして進めます。
最後に、 Ubuntu Mate のディレクトリを参照できるようにします。ソフトの環境が整ったので、 Ubuntu Mate のディレクトリをMac上のFinderやターミナルから見られるようにしてみましょう。まず Ubuntu Mate のディレクトリをマウントする場所を用意しておく必要があります。ホームに「macfuse」という名前のディレクトリを作成します。マウントするには、ターミナルから以下の形式のコマンドを実行します。 Ubuntu mate のユーザー名とIPアドレスを使用します。
$ sshfs koichi@192.168.**.**:/home/koichi ~/macfuse
finderを開くと、ユーザーのホームに「macFUSE Volume 0 (sshfs)」というディレクトリが作成されています。
マウントを解除するには、umountコマンドを使用します。
$ umount macfuse
Raspi を操作するPCのターミナルから
$ ssh ubuntu@192.168.11.1
と入力します。パスワードが要求されるので、パスワードを入力します。これで、ラズパイと接続します。以後、PCのターミナルからのリモート操作となります。
以下、ROS Noetic Ninjemys のインストールの手順を説明します。「packages.ros.org.」からのソフトを受け入れるためにOSなどをセットアップします（長いですが全部でコマンド１行文です）。以下のコマンドを入力します。
$ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
以下のコマンドでキーをセットアップする。
$ sudo apt install curl # if you haven't already installed curl
$ curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
インストールするにあたって、始めに、Ubuntu package indexをアップデートする。
$ sudo apt update
次のコマンドでros-noetic-desktopをインストールする。GUIを含めた基本セットをインストールしたい場合は、ros-kinetic-desktop-fullを選択して、インストールしてください。(GUIを用いたシミュレーションをしない場合は、ros-kinetic-desktopでも十分です。)
$ sudo apt install ros-noetic-desktop-full
サイズが約470MBありますので、ダウンロードとインストールに若干の時間がかかります。ROS環境変数を設定するために、
$ echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
$ source ~/.bashrc
と入力して、bashファイルをシェルに組み込む。更に、PythonのROSパッケージなどを組み込むために
$ sudo apt install python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential
$ sudo apt install python-rosdep
とコマンドを入力する。
ROSを使用する前に、rosdepを初期化し、アップデートする必要があります。
$ sudo rosdep init
$ rosdep update
-----
reading in sources list data from /etc/ros/rosdep/sources.list.d
Hit https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/osx-homebrew.yaml
Hit https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/base.yaml
Hit https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/python.yaml
Hit https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/ruby.yaml
Hit https://raw.githubusercontent.com/ros/rosdistro/master/releases/fuerte.yaml
Query rosdistro index https://raw.githubusercontent.com/ros/rosdistro/master/index-v4.yaml
--
略
--
Add distro "melodic"
Add distro "noetic"
Add distro "rolling"
updated cache in /home/koichi/.ros/rosdep/sources.cache
これで、一応のインストールは終了します。インストールが成功していれば、
$ roscore
とコマンド入力すると、ROSの本体が作動します。以下の通りに、ROSが起動されたことが表示されます。
... logging to /home/koichi/.ros/log/77388430-9900-11ec-bc26-3d615ff99864/roslaunch-Raspi-5046.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://Raspi:33755/
ros_comm version 1.15.14
SUMMARY
========
PARAMETERS
* /rosdistro: noetic
* /rosversion: 1.15.14
NODES
auto-starting new master
process[master]: started with pid [5056]
ROS_MASTER_URI=http://Raspi:11311/
setting /run_id to 77388430-9900-11ec-bc26-3d615ff99864
process[rosout-1]: started with pid [5066]
started core service [/rosout]
ROSの終了は「Ctrl」キーを押しながら「c」キーを押します。詳細は、この説明 を読んでください。このページにあるコマンドをコピペする方がベターかもしれません。ROSの使い方ガイドは、このサイトの説明 を参照してください。
ロボットに一連の動作をさせるためのプログラム群を納める作業スペースをcatkinワークスペースと言います。ROSを使用するにあたって最初にすることはこのcatkinワークスペースを作成することです。正確に言い換えると、例えば「catkin_ws」というわかりやすい名称のディレクトリを作成することです。このディレクトリの中にコードや設定ファイルを納めると、ロボットを動かす時に必要なもの一式が「catkin_make」というコマンド一つで生成できます。詳しい説明は公式チュートリアル を参照してください。そこからのコピペがベターかもしれません。
以下のコマンドを入力して、作業用のワークスペースを作成します。
$ source /opt/ros/noetic/setup.bash
$ sudo apt install python3-empy # if pytho3-empy has not been installed
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/
$ catkin_make
このコマンド「catkin_make」はC言語のビルド・システムのmakeに対応していて、C++でのコードのコンパイルに関係する仕事をしてます。この結果、「source」ディレクトリの中に「CMakeLists.txt」が作成され、「catkin_ws」ディレクトリの下に「devel」と「build」という名称の二つのディレクトリが生成されます。いくつかの「setup.*sh」ファイルも生成されます。なお、この catkin_make は システム Pythoh3 を使用しますので、このPython へのパスが通っている必要があります。
このワークスペース関連の設定をシェルに読み込ませるために、
$ source ~/catkin_ws/devel/setup.bash
と入力する必要があります。
$ echo $ROS_PACKAGE_PATH
と打って、Terminalの標示からcatkin_ws/srcへのパスが確認できれば成功です。
ロボットに一連の動作をさせるための自作のプログラム・ファイル作成した時、これらを一つのパッケージにしてROSのワークスペース(catkin_ws)にビルドする必要があります。このパッケージ名をros_testとするとき、以下のようなコマンド入力が必要です。
$ cd ~/catkin_ws/src
$ catkin_create_pkg ros_test std_msgs rospy roscpp
ここでのcatkin_create_pkgはパッケージの雛形を作ることを指示します。ros_test のフォルダーが作成されます。Terminalには
Created file ros_test/CMakeLists.txt
Created file ros_test/package.xml
Created folder ros_test/include/ros_test
Created folder ros_test/src
Successfully created files in /home/ubuntu/catkin_ws/src/ros_test. Please adjust the values in package.xml.
と表示されます。CMakeLists.txtはROSのcmakeというビルド・システムの設定ファイルです。package.xmlはROSのパッケージ管理システムが利用するファイルです。このファイルの中にはこのパッケージが利用するモジュール（rospy、roscpp、std_msgsなど）の依存関係が書かれています。ここまで来たら一度catkin_wsに戻って、catkin_makeする必要があります。
$ cd ~/catkin_ws
$ catkin_make
このコマンドを実行すると、
Base path: /home/ubuntu/catkin_ws
Source space: /home/ubuntu/catkin_ws/src
Build space: /home/ubuntu/catkin_ws/build
Devel space: /home/ubuntu/catkin_ws/devel
Install space: /home/ubuntu/catkin_ws/install
####
#### Running command: "make cmake_check_build_system" in "/home/ubuntu/catkin_ws/build"
####
####
#### Running command: "make -j4 -l4" in
Base path: /home/koichi/catkin_ws
Source space: /home/koichi/catkin_ws/src
Build space: /home/koichi/catkin_ws/build
Devel space: /home/koichi/catkin_ws/devel
Install space: /home/koichi/catkin_ws/install
####
#### Running command: "make cmake_check_build_system" in "/home/koichi/catkin_ws/build"
####
-- Using CATKIN_DEVEL_PREFIX: /home/koichi/catkin_ws/devel
-- Using CMAKE_PREFIX_PATH: /home/koichi/catkin_ws/devel;/opt/ros/kinetic
-- This workspace overlays: /home/koichi/catkin_ws/devel;/opt/ros/kinetic
-- Using PYTHON_EXECUTABLE: /usr/bin/python
-- Using Debian Python package layout
-- Using empy: /usr/bin/empy
-- Using CATKIN_ENABLE_TESTING: ON
-- Call enable_testing()
-- Using CATKIN_TEST_RESULTS_DIR: /home/koichi/catkin_ws/build/test_results
-- Found gtest sources under '/usr/src/gtest': gtests will be built
-- Using Python nosetests: /usr/bin/nosetests-2.7
-- catkin 0.7.8
-- BUILD_SHARED_LIBS is on
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- ~~ traversing 4 packages in topological order:
-- ~~ - beginner_tutorials
-- ~~ - raspimouse_ros
-- ~~ - ros_start
-- ~~ - ros_test
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- +++ processing catkin package: 'beginner_tutorials'
-- ==> add_subdirectory(beginner_tutorials)
-- +++ processing catkin package: 'raspimouse_ros'
-- ==> add_subdirectory(raspimouse_ros)
-- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
-- Generating .msg files for action raspimouse_ros/Music /home/koichi/catkin_ws/src/raspimouse_ros/action/Music.action
-- raspimouse_ros: 10 messages, 2 services
-- +++ processing catkin package: 'ros_start'
-- ==> add_subdirectory(ros_start)
-- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
-- Generating .msg files for action ros_start/GoUntilBumper /home/koichi/catkin_ws/src/ros_start/action/GoUntilBumper.action
-- ros_start: 7 messages, 1 services
-- +++ processing catkin package: 'ros_test'
-- ==> add_subdirectory(ros_test)
-- Configuring done
-- Generating done
-- Build files have been written to: /home/koichi/catkin_ws/build
####
#### Running command: "make -j2 -l2" in "/home/koichi/catkin_ws/build"
####
と表示されます。これで、自作のパッケージがROSで利用できるようになります。ROSの中のディレクトリを探すとき、cdの代わりにroscdを使います。以下のコマンドでパッケージが構成されていることを確認しましょう。
$ roscd ros_test
$ ls
ロボットを動かすための一連のプログラムをこのパッケージに書き込んでいくことになります。
Raspberry Pi Mouse のデバイス・ドライバーのインストール
製造メーカーのサイトはこちら です。RT社の「取扱説明書」を見ながら、この Raspberry Pi 3 をロボットに搭載してください。（Raspberry Pi 4 を使用するときは対応するコネクターが必要です。）
なお、ロボットを起動するためには、12v程度の電圧を供給する電源（３ピンコネクター付きリチウムイオン電池）が必要です。上記のロボット本体にセットで付属していますが、充電器は付いていません。LiPo充電器LBC-010とACアダプタ(12V5A)+VH3ピンコネクタへの変換ケーブルのセットが上記のRT 社から入手可能です。
以下での説明では主に Python を用いて書かれたコードを使用します。Linux や C 言語の知識は前提としませんが、ROS がインストールされていることは必須です。ROS のインストールの方法については、このページを 参照して下さい。
Raspberry Pi が Raspberry Pi Mouse ロボットに搭載されて、電池のコネクターが接続されていることを確認してください。WiFi 経由で Raspberry Pi への ssh 接続をするために必要なラズパイの IP アドレスがわかっているとします。
Raspberry Pi Mouse のメイン電源スイッチをオンにしてください。数分後、ラズパイの Ubuntu が起動したと思われたら、WiFi 経由で接続します。ラズパイのアドレスが192.168.11.1 であるとき、PCのターミナルから
$ ssh koichi@192.168.11.1
と入力して接続します。ここでは、koichi はラズパイにログインするときのユーザー名です。パスワードを入力して、ラズパイにログインします。
Raspberry Pi 入門のページ にて説明した通り、ラズベリーパイには GPIO という40ピンのコネクターが付いています。この GPIO を操作して、ロボットを動かします。GPIO インターフェースとロボットの機器（デバイス）を接続するためには、ロボット専用のデバイス・ドライバーが必要です。ラズパイ・マウスのデバイス・ドライバーのソース・コードは GitHubのrt-net/RaspberryPiMouse にあります。
ラズパイにログインできたら、デバイス・ドライバーをインストールします。以下のようにGitHubからcloneします。
$ git clone https://github.com/rt-net/RaspberryPiMouse.git
$ cd RaspberryPiMouse
/RaspberryPiMouse/src/drivers/というディレクターの中にC言語のソースコード rtmouse.c が配置されています。Raspberry Pi 4 を使用するときには、RT社のマニュアル に従って、以下の修正をします。
Raspberry Pi 4で本ドライバを使用する際にはrtmouse.cの行 「#define RASPBERRYPI 2」 を 「#define RASPBERRYPI 4」に書き換えてビルドする必要があります。
※./utils/build_install.bashを実行すると、Raspberry Piのモデルに合わせてrtmouse.cが自動で書き換わります。
// define the Raspberry Pi version here
// Raspberry Pi 1 B/A/B+/A+: 1
// Raspberry Pi 2 B : 2
// Raspberry Pi 3 B/A+/B+ : 2
// Raspberry Pi 4 B : 4
#define RASPBERRYPI 2
上記の修正後、rtmouse.c をコンパイルする必要があります。ビルド用のbashファイルはutilsのディレクトリにあります。以下のコマンドを入力します。
$ sudo apt install linux-headers-$(uname -r) build-essential
$ cd utils
$ ./build_install.bash
----
make -C /usr/src/linux-headers-5.4.0-1052-raspi M=/home/koichi/RaspberryPiMouse/src/drivers V=0 clean
make[1]: Entering directory '/usr/src/linux-headers-5.4.0-1052-raspi'
make[1]: Leaving directory '/usr/src/linux-headers-5.4.0-1052-raspi'
make -C /usr/src/linux-headers-5.4.0-1052-raspi M=/home/koichi/RaspberryPiMouse/src/drivers V=0 modules
make[1]: Entering directory '/usr/src/linux-headers-5.4.0-1052-raspi'
CC [M] /home/koichi/RaspberryPiMouse/src/drivers/rtmouse.o
Building modules, stage 2.
MODPOST 1 modules
CC [M] /home/koichi/RaspberryPiMouse/src/drivers/rtmouse.mod.o
LD [M] /home/koichi/RaspberryPiMouse/src/drivers/rtmouse.ko
make[1]: Leaving directory '/usr/src/linux-headers-5.4.0-1052-raspi'
上記のようにデバイスドライバーの rtmouse.ko が作成されるはずです。ブザーが鳴って終了します。うまくいったら、rtmouse.ko が作成されていることを以下のように確認します。
$ cd /home/ubuntu/RaspberryPiMouse/src/drivers
$ ls
----
Makefile Module.symvers rtmouse.ko rtmouse.mod.o
Makefile.header_from_apt modules.order rtmouse.mod rtmouse.o
Makefile.header_from_source rtmouse.c rtmouse.mod.c
この rtmouse.ko がデバイスドライバで、カーネル・モジュールと呼ばれます。linux のカーネルに後から組み込まれるモジュールです。カーネル・モジュール rtmouse.ko を linux カーネルに直接組み込む手続きが必要です。
ロボットのデバイスとGIOPピン番号との関係は「取扱説明書」に書いてある通りです。GPIOの状態を見てみましょう。/sys/class/gpio/というディレクトリがあることを確認します。ルート権限でlsを使います。
$ ls -l /sys/class/gpio/
----
total 0
--w------- 1 root root 4096 Feb 25 15:31 export
lrwxrwxrwx 1 root root 0 Feb 25 15:31 gpiochip0 -> ../../devices/platform/soc/3f200000.gpio/gpio/gpiochip0
lrwxrwxrwx 1 root root 0 Feb 25 15:31 gpiochip100 -> ../../devices/gpiochip1/gpio/gpiochip100
lrwxrwxrwx 1 root root 0 Feb 25 15:31 gpiochip504 -> ../../devices/platform/soc/soc:firmware/soc:firmware:expgpio/gpio/gpiochip504
--w------- 1 root root 4096 Feb 25 15:31 unexport
ubuntu@ubuntu:~$ ls -a /sys/class/gpio/
. .. export gpiochip0 gpiochip100 gpiochip504 unexport
と表示されます。GPIOの入出力ファイルが存在することが確認できました。
ラズパイマウスを操作する手続きについては、上田隆一著『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 510, 0 Feb 25 15:45 /dev/rtbuzzer0
crw-rw-rw- 1 root root 511, 0 Feb 25 15:45 /dev/rtled0
crw-rw-rw- 1 root root 511, 1 Feb 25 15:45 /dev/rtled1
crw-rw-rw- 1 root root 511, 2 Feb 25 15:45 /dev/rtled2
crw-rw-rw- 1 root root 511, 3 Feb 25 15:45 /dev/rtled3
crw-rw-rw- 1 root root 508, 0 Feb 25 15:45 /dev/rtlightsensor0
crw-rw-rw- 1 root root 504, 0 Feb 25 15:45 /dev/rtmotor0
crw-rw-rw- 1 root root 506, 0 Feb 25 15:45 /dev/rtmotor_raw_l0
crw-rw-rw- 1 root root 507, 0 Feb 25 15:45 /dev/rtmotor_raw_r0
crw-rw-rw- 1 root root 505, 0 Feb 25 15:45 /dev/rtmotoren0
crw-rw-rw- 1 root root 509, 0 Feb 25 15:45 /dev/rtswitch0
crw-rw-rw- 1 root root 509, 1 Feb 25 15:45 /dev/rtswitch1
crw-rw-rw- 1 root root 509, 2 Feb 25 15:45 /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 の簡単な操作
まず最初に、ロボット操作用の基本的なパッケージを作成します。パッケージの名前をpimouse_rosとします。
~/catkin_ws/src/にパッケージを作成するために、
$ cd ~/catkin_ws/src/
$ catkin_create_pkg pimouse_ros std_msgs rospy
-----結果
Created file pimouse_ros/package.xml
Created file pimouse_ros/CMakeLists.txt
Created folder pimouse_ros/src
Successfully created files in /home/ubuntu/catkin_ws/src/pimouse_ros. Please adjust the values in package.xml.
と入力します。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
---- 結果
Base path: /home/ubuntu/catkin_ws
Source space: /home/ubuntu/catkin_ws/src
Build space: /home/ubuntu/catkin_ws/build
Devel space: /home/ubuntu/catkin_ws/devel
Install space: /home/ubuntu/catkin_ws/install
####
#### Running command: "cmake /home/ubuntu/catkin_ws/src -DCATKIN_DEVEL_PREFIX=/home/ubuntu/catkin_ws/devel -DCMAKE_INSTALL_PREFIX=/home/ubuntu/catkin_ws/install -G Unix Makefiles" in "/home/ubuntu/catkin_ws/build"
####
-- Using CATKIN_DEVEL_PREFIX: /home/ubuntu/catkin_ws/devel
-- Using CMAKE_PREFIX_PATH: /home/ubuntu/catkin_ws/devel;/opt/ros/noetic
-- This workspace overlays: /home/ubuntu/catkin_ws/devel;/opt/ros/noetic
-- Found PythonInterp: /usr/bin/python3 (found suitable version "3.8.10", minimum required is "3")
-- Using PYTHON_EXECUTABLE: /usr/bin/python3
-- Using Debian Python package layout
-- Using empy: /usr/lib/python3/dist-packages/em.py
-- Using CATKIN_ENABLE_TESTING: ON
-- Call enable_testing()
-- Using CATKIN_TEST_RESULTS_DIR: /home/ubuntu/catkin_ws/build/test_results
-- Forcing gtest/gmock from source, though one was otherwise available.
-- Found gtest sources under '/usr/src/googletest': gtests will be built
-- Found gmock sources under '/usr/src/googletest': gmock will be built
-- Found PythonInterp: /usr/bin/python3 (found version "3.8.10")
-- Using Python nosetests: /usr/bin/nosetests3
-- catkin 0.8.10
-- BUILD_SHARED_LIBS is on
-- BUILD_SHARED_LIBS is on
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- ~~ traversing 2 packages in topological order:
-- ~~ - pimouse_ros
-- ~~ - ros_test
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- +++ processing catkin package: 'pimouse_ros'
-- ==> add_subdirectory(pimouse_ros)
-- +++ processing catkin package: 'ros_test'
-- ==> add_subdirectory(ros_test)
-- Configuring done
-- Generating done
-- Build files have been written to: /home/ubuntu/catkin_ws/build
####
#### Running command: "make -j4 -l4" in "/home/ubuntu/catkin_ws/build"
####
とします。
ROSのページ で説明した通り、ノード間での通信はTopicを介したメッセージの送信・受信としてプログラムされる。このロボットでのノードはデバイス・ファイルにある rtbuzzer、rtsensors、rtled、や rtmotors などである。
ブザーのノードを立ち上げ、他のノードからメッセージ（周波数）を受け取り、それをデバイス・ファイルに書き込むというプログラムを書きましょう。
#!/usr/bin/env python3
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/ディレクトリに保存します。
$ source ~/catkin_ws/devel/setup.bash
$ roscd pimouse_ros
$ mkdir scripts
$ cd scripts
$ nano buzzer.py
$ chmod +x buzzer.py
と入力します。chmod +x buzzer.pyで、python ファイルを実行可能な状態にしておきます。
このプログラムを実行するために、３つのターミナルを起動します。ssh 接続でラズパイのターミナルを３つ立ち上げておきます。最初のターミナルで、roscoreを立ち上げます。
$ source ~/catkin_ws/devel/setup.bash
$ roscore
２つ目のターミナルで、
$ source ~/catkin_ws/devel/setup.bash
$ rosrun pimouse_ros buzzer.py
とノードを立ち上げます。topicをパブリッシュするために、３つ目のターミナルから
$ source ~/catkin_ws/devel/setup.bash
$ rostopic pub -1 '/buzzer' std_msgs/UInt16 1000
と打ちます。'/buzzer'がトピック名で、メッセージの型がstd_msgs/UInt16であり、周波数がhz=1000という意味になります。すると、ブザーがピーとなります。停止するために
$ rostopic pub -1 '/buzzer' std_msgs/UInt16 0
と周波数をゼロとして入力します。音が止まります。1、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 python3
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
$ nano lightsensors1.py
$ 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: -->
<exec_depend>message_runtime</exec_depend>
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
-----
具体的には、
message_generation
message_runtime
の２行のコメントを外します。これは、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
## Message1.msg
## Message2.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を立ち上げて、別のターミナルから
$ source ~/catkin_ws/devel/setup.bash
$ rosrun pimouse_ros lightsensors1.py
と入力して、エラーが出なければ、成功です。また、LightSensorValuesから生成されたファイルは以下のように入力すると、
$ cd ~/catkin_ws/devel/
$ find . | grep LightSensorValues
---------
./include/pimouse_ros/LightSensorValues.h
./lib/python3/dist-packages/pimouse_ros/msg/_LightSensorValues.py
./lib/python3/dist-packages/pimouse_ros/msg/__pycache__/_LightSensorValues.cpython-38.pyc
./share/roseus/ros/pimouse_ros/msg/LightSensorValues.l
./share/common-lisp/ros/pimouse_ros/msg/LightSensorValues.lisp
./share/common-lisp/ros/pimouse_ros/msg/_package_LightSensorValues.lisp
./share/gennodejs/ros/pimouse_ros/msg/LightSensorValues.js
と表示されます。
距離センサーからの値を読めるようにしましょう。そこで、lightsensors1.pyを以下のように書き換えます。
#!/usr/bin/env python3
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/
$ nano lightsensors2.py
$ chmod +x lightsensors2.py
$ rosrun pimouse_ros lightsensors2.py
と入力します。別のターミナルから
$ 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 python3
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/
$ nano lightsensors.py
$ 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
ファイル名をMotorFreqs.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 python3
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を使用するときは、自動停止しませんので、停止のためのキー入力が必要です。
このファイルはこれ以降順次拡張されていきます。
このMotorFreqs.msgを組み込んだプログラムでモーターを操作するためには、catkin_makeをする必要があります。
$ cd ~/catkin_ws/
$ catkin_make
catkin_makeの後、roscoreが起動していないときは、roscoreを立ち上げ、二つ目のターミナルから
$ 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
$ nano 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>
（略）
<exec_depend>message_runtime</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>std_srvs</exec_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 python3
#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 python3
#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
以下のセクションでは、ラズパイ・マウスを壁の手前でストップさせたり、壁に沿って走らせることをします。このような操作を実現するパッケージを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>
<exec_depend>rospy</exec_depend>
<exec_depend>pimouse_ros</exec_depend>
pimouse_ros の１行を exec_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 python3
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>
このファイルをwall_around.launchという名前で、launchディレクトリに保存してください。
デバイス・ドライバーを読み込み、モーターの電源スイッチを入れて、ロボットを壁から離して、壁に平行に向けておいてください。そして、
$ roslaunch pimouse_corridor wall_around.launch
と入力します。すると、ロボットが走り出して、壁から10センチほど離れて、壁や柱の凹凸を避けながら走ります。
pimouseの壁に沿った自動走行の動画です
Your browser doesn't support HTML5 video. Here is a link to the video instead.
ページのトップに戻る
Raspberry Pi Mouse の実際の操作：Ubuntu 16.04のケース
トップページに戻る
