|
Jetson nanoはアメリカの半導体メーカーであるNVIDIA社が製造・販売をするシングルボードコンピューターです。NVIDIA社のJetbotホームページでは、Jetson Nano を搭載したロボットをDIYで制作する手順が紹介されてます。このJetbotホームページでは、Jetson Nano を搭載したロボット群 JetBot AI Robotの完成品も紹介されており、実際に注文取り寄せもできます。日本国内で、「Jetson Nano」を使ったAI自律走行車「JetBot」を実際に作成して、操作した実例が 「JetBotを動かしてみよう 第1回 部品の調達から作成したモデルによる走行」に掲載されています。JetBot ロボットを自作することに興味のある方には参考になります。
このページでは、完成したロボットを前提として、Jetson Nano Developer Kit B01 を搭載したRobotの操作について説明します。使用するRobotはRT社製の Jetson Nano Mouse です（価格7万7千円）。Jetson Nano Mouse は、NVIDIA社の Jetson Nano を搭載したロボット群 JetBot AI Robot の中で紹介されているロボットの一つです。
RT Corporation社の製品紹介では以下のように説明されています。「Jetson Nano MouseはAI関連技術とロボット制御技術を同時に学ぶことができる小型二輪移動ロボットです。広角カメラ2台を前方に搭載しており、Jetson Nanoの性能を活かして機械学習や画像処理技術の研究開発に利用できます。ロボット制御用のソフトウェアをGitHubで公開しているため、AGVや自動運転で用いる技術開発、研究のプロトタイピングにも活用できます。」Jetson Nano Mouse は 同社製の Raspberry Pi Mouse ロボットと類似の２輪走行ロボットです。
Jetson Nanoは、Tegra X1のSoCに4GBのメモリを搭載したコンピューティングモジュールで、ピーク性能は472GFLOPSを実現しています。Maxwell世代のGPUを内蔵しているため、CUDAを使った推論を利用して、画像認識などの機能を実装できます。Jetson Nano自体は多種類のインターフェイス機器を内蔵していませんが、開発キットと呼ばれる追加の機能を持つベース基板に接続されています。この開発キットにはJetson Nanoのモジュールそのものと、I/Oを内蔵した基板が組み合わさって提供されており、Jetson Nano単体では用意できない各種I/O(USBやHDMIなど)や電源回路が搭載されています。
Jetson Nano開発者キットの仕様は以下の通りです。
別途用意が必要なものは
Jetson Nano Mouse の仕様は以下の通りです。
このページでは、リモートPCとして Mac を使用した手順を説明しています。
Jetson Nanoを搭載したRT社製 Jetson Nano Mouse ロボット
Last updated: 2022.3.10
Jetson Nano へのOS等のインストールとセットアップ |
Jetson Nano へのOS等のインストール方法には２通りの方法があります。
一つの方法は、RT社が用意したイメージファイルをダウンロードする方法です。こちらの手順の方が簡単です。このイメージファイルはカスタム版JetPackで、公式のJetPackのイメージファイルをベースとしてJetson Nano Mouse向けに機械学習ライブラリのPyTorchやTensorflow、 及び、プログラム実行環境であるJupyterLab等があらかじめインストールされています。 その他、Jetson Nano MouseのLEDやモータを駆動するために必要な デバイスドライバがあらかじめインストールされています。解説はこのサイトにありますので、そちらを参考にして下さい。Jetpackのバージョンは4.5、PytorchとTensorflowのバージョンはTorch 1.6.0、Tensorflow 1.15.5 です。
別の方法は、NVIDIA社の公式サイトの手順に沿ってJetPackのイメージファイルをダウンロードすることから始める方法です。この手順は、Jetson Nano のセットアップと機械学習入門のページに説明がありますので、それを参考にして下さい。JetPack の圧縮ファイル（jetson-nano-jp46-sd-card-image.zip）をNVIDIAの公式サイトからダウンロードします。7GB程度の圧縮ファイルです。これを解凍すると、sd-blob-b01.imgが作成されます。約14GBのイメージファイルです。このイメージファイルのブート後、Pytorch 及び Jupyter Notebook をインストールします。このページではこの方法でOSなどをインストールするケースについて説明します。
イメージファイルを microSD カードに書き込む方法は以下の通りです。balenaEtcherを使用します。
$ diskutil list
と入力すると、micro SD のデバイス名がわかります。多分、/dev/disk2/ となっていると思います。balenaEtcher を起動して、Flash from file でsd-blob-b01.imgファイルを選択し、select target を microSD名(/dev/disk2/) にして、書き込み（Flash）します。書き込みが終わったら、microSDカードを取り出します。「接続したディスクは、このコンピュータで読み取れないディスクでした」という警告が出ますが無視します。
microSD カードを用いて、以下の手順でJetson Nano での最初の Boot & インストールをおこないます。
Jetson Nanoに電源を入れると、最初にインストールのためのデスクトップ画面が表示されます。無線LANアダプタはTP-Link社のTL-WN725Nを用いました。ライセンス確認の画面が表示されるので、Continueを押します。言語選択(system cofiguration)の画面で言語を日本語に、キーボード選択で日本語、地域でTokyoを選択します。WiFiネットワークの選択画面が出るので使用しているWiFiを選択、タイムゾーン選択画面が出るので、Tokyoを選択します。ユーザー名とパスワードを入力します。次の画面で、APP partition sizeはデフォルトのサイズ（最大値）で良いと思います。スワップ領域とはPyTorch等でAIのモデルを読み込む際に、メモリの不足分を補うための領域です。基本的にはデフォルトの「作成する（Create SWAP File）」を選びます。初期設定に使った余分な領域を削除するかの確認が出ますが、基本的には「続ける（continue）」を押して先に進みます。節電設定の選択はデフォルトで良いと思います。（RT社が準備したカスタム版Jetpackをダウンロードした時は、言語等の設定はすべて済んでおり、既にユーザー名がjetson 、パスワードがjetson に設定されています。ただし、WiFiの設定は自分で行う必要があります。)
JetPack のデスクトップ画面
インストールが完了したら、デスクトップ画面が表示されます。ログインして、ターミナル（LXTerminal）をクリックして
$ sudo apt update $ sudo apt upgrade
とパッケージをアップデートします。JetPackには、デフォルトのエディタとしてvimが入っていますが、vimの扱いは初心者には煩雑ですので、nanoをインストールします。
$ sudo apt install nano gedit
この後に、Jetson Nano Mouse のデバイスドライバ、及び、セットアップスクリプトをJetson Nanoにダウンロードして、インストールすることになります。
最初に、デバイスドライバをインストールします。
$ git clone https://github.com/rt-net/JetsonNanoMouse.git $ cd JetsonNanoMouse $ make build $ sudo make install
次に、セットアップスクリプトをダウンロードします。
$ git clone https://github.com/rt-net/jnmouse_utils.git
機械学習ライブラリのPyTorchやTensorflow、プログラム実行環境であるJupyterLab等をインストールする手順については、Jetson Nano で Pytorch を用いた推論のページを参考にして下さい。
IPアドレスを知るために、ターミナルから
$ ip a
と打ちます。このIPアドレスの番号を記録してください。
ここまで終了したら、Jetson Nano を終了して、電源を切ります。Jetson Nano を Jetson Nano Mouse ロボットに搭載して、電池のコネクターが接続されていることを確認してください。WiFi 経由で Raspberry Pi への ssh 接続をするために必要な IP アドレスがわかっているとします。
Jetson Nano Mouse のメイン電源スイッチをオンにしてください。数分後、ロボットの Ubuntu が起動したと思われたら、WiFi 経由で接続します。リモート PC から ssh 接続をします。Jetson Nano に設定したユーザー名とIPアドレスを使って、PCのターミナルから
$ ssh {ユーザー名}@192.168.**.**
と入力します。パスワードを入力すれば、以下のように表示されて、ログインできます。
Welcome to Ubuntu 18.04.5 LTS (GNU/Linux 4.9.201-tegra aarch64)
* Documentation: https://help.ubuntu.com
* Management: https://landscape.canonical.com
* Support: https://ubuntu.com/advantage
This system has been minimized by removing packages and content that are
not required on a system that users do not log into.
To restore this content, you can run the 'unminimize' command.
---
RT社のサイトにあるJetson Nano用OSの書き込みと初期設定に従い、Jetson Nanoのパフォーマンス設定とブートローダ更新を以下のように行います。次のコマンドを実行し、Jetsonのパフォーマンス設定を行います
$ cd ~/jnmouse_utils/scripts $ ./configure-jetson.sh
次のコマンドを実行し、ブートローダを更新します
$ cd ~/jnmouse_utils/scripts $ ./update-qspi.sh
SPI通信の有効化を行います。Jetson NanoのGPIOを設定するためのツールであるJetson-IOを使って、SPI1を有効にし、 Jetson NanoとJetson Nano Mouseの基板が通信できるようにします。 これによりJetson Nano Mouse前方の距離センサが使えるようになります。次のコマンドを実行し、Jetson-IOを起動します。ターミナルウィンドウのサイズが小さいと、Jetson-IOのメニュー表示が途中で切れてしまいます。ターミナルウィンドウは縦に長くしてから実行すること。
$ sudo /opt/nvidia/jetson-io/jetson-io.py
「Configure 40-pin expansion header」を選択し、「Configure header pins manually」を選択すると、GPIO画面になります。spi1をクリックして、「＊」と有効にします。この選択後はBackを選び、メニューに戻ります。 「Select one of the following options:」で、「Save and reboot to reconfigure pins」を選択して、任意のキーを叩いて再起動します。この設定でライトセンサからの信号を読めることになります。これは Jetpack 4.6 をインストールした時の手順です。が、RT社が用意したイメージファイルをインストールするケースでは、上の説明と若干異なります。なお、NVIDIA社の公式ページに新しい説明があります。
ロボットのデバイスとGIOPピン番号との関係は「取扱説明書」に書いてある通りです。GPIOの状態を見てみましょう。/sys/class/gpio/というディレクトリがあることを確認します。ルート権限でlsを使います。
$ ls -l /sys/class/gpio/ $ ls -l /dev/rt* --------- 結果 crw-rw-rw- 1 root root 497, 0 3月 8 16:26 /dev/rtbuzzer0 lrwxrwxrwx 1 root root 4 3月 8 16:26 /dev/rtc -> rtc1 crw------- 1 root root 252, 0 3月 8 16:26 /dev/rtc0 crw------- 1 root root 252, 1 3月 8 16:26 /dev/rtc1 crw-rw-rw- 1 root root 500, 0 3月 8 16:26 /dev/rtcounter_l0 crw-rw-rw- 1 root root 499, 0 3月 8 16:26 /dev/rtcounter_r0 crw-rw-rw- 1 root root 505, 0 3月 8 16:26 /dev/rtled0 crw-rw-rw- 1 root root 505, 1 3月 8 16:26 /dev/rtled1 crw-rw-rw- 1 root root 505, 2 3月 8 16:26 /dev/rtled2 crw-rw-rw- 1 root root 505, 3 3月 8 16:26 /dev/rtled3 crw-rw-rw- 1 root root 501, 0 3月 8 16:26 /dev/rtlightsensor0 crw-rw-rw- 1 root root 498, 0 3月 8 16:26 /dev/rtmotor_raw_l0 crw-rw-rw- 1 root root 496, 0 3月 8 16:26 /dev/rtmotor_raw_r0 crw-rw-rw- 1 root root 502, 0 3月 8 16:26 /dev/rtmotoren0 crw-rw-rw- 1 root root 504, 0 3月 8 16:26 /dev/rtswitch0 crw-rw-rw- 1 root root 504, 1 3月 8 16:26 /dev/rtswitch1 crw-rw-rw- 1 root root 504, 2 3月 8 16:26 /dev/rtswitch2 crw-rw-rw- 1 root root 503, 0 3月 8 16:26 /dev/rtswitches0
rtled0を点灯したいときは、
$ echo 1 > /dev/rtled0
と入力します。消灯するときは
$ echo 0 > /dev/rtled0
と入力します。正常に作動することを確認して下さい。
RT社が準備したサンプルコードを実行してみましょう。ダウンロードした JetsonNanoMouse ファイルの中の/samplesに5種類のサンプルコードがあります。これを使用します。
$ cd ~/JetsonNanoMouse/samples $ bash step1.sh
と入力すると、LED0〜LED3が点滅します。キーボード入力でブザーを鳴らすサンプルコードを実行します。
$ bash step2.sh
このコマンドを実行した後、キーボードのキーを任意に打つと音が出ます。「０」を打って終わります。音が止まったら、「ctrl + c」で抜けます。残りの3種類のコードを同様に実行します。
ここまでできれば先程紹介したサンプルプログラムを動かすためのJetson Nanoのセットアップは概ね完了です。
Jetson Nano Mouse の操作: Jupyter Notebook |
RT社が用意した rt-net/jnm_jupyternotebook@GitHub をダウンロードし、そのディレクトリに移動します。
$ git clone https://github.com/rt-net/jnm_jupyternotebook.git $ cd jnm_jupyternotebook $ sudo python3 setup.py install
つぎにJetson Nano Mouse制御用のPythonパッケージ、「jnmouse」をインストールします。このパッケージは「jetbot」とある程度互換性が保たれるように実装されています。なお、このJupyter Notebookは NVIDIA社のJetBotのものをベースとしており、JetBotの解説を読みながら理解をすすめることもできます。NVIDIA社のJetBotのコードは このGitHubにあります。
次にダウンロードしてきたJupyter Notebookを~/Notebooksディレクトリにコピーします。
$ mkdir Notebooks $ mv jnm_jupyternotebook Notebooks/ $ cd Notebooks/jnm_jupyternotebook $ ./scripts/copy_notebook.sh
jupyter notebook を起動して、リモートPCのWebブラウザから操作します。「http://{Jetson Nano のIPアドレス}:8888/jetson にアクセスします。
（RT社が用意したJetBotでは、.bashrc ファイルに設定がありますので、Jupyter Lab のサーバが8888番ポートで立ち上がるようになっています。ターミナルから起動することなく、リモートPCのWebブラウザでhttp://{jnmouseのIPアドレス}:8888を開き、パスワード"jnmouse"を入力してJupyterLabへログインします。）
/jnm_jupyternotebook/notebooks にロボットを操作するサンプルコードがあります。例えば、basic_motion フォルダの中に、basic_motion.ipynb というノートブックがあります。これを用いてロボットの移動を操作できます。最初のセルを以下のように追加修正して下さい。
%cd /home/koichi/jnm_jupyternotebook/ from jnmouse import Robot
この修正をしてから、上から順番に実行して下さい。ロボットが移動しますので、空間を確保して下さい。「# display buttons」とあるセルで、ロボットを移動させるボタンが表示されます。このボタンをクリックすると自由に移動させることができます。
jetbotの例もみて下さい。
ROSのインストールとセットアップ |
ROSのインストールを行います。OSのubuntu 18.04に対応した ROS Melodic Morenia をインストールします。ROSのUbuntu install of ROS Melodicにしたがいます
以下、ROS Melodic Morenia のインストールの手順を説明します。「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-melodic-desktopをインストールする。GUIを含めた基本セットをインストールしたい場合は、ros-melodic-desktop-fullを選択して、インストールしてください。(GUIを用いたシミュレーションをしない場合は、ros-melodic-desktopでも十分です。)
$ sudo apt install ros-melodic-desktop-full
サイズが約470MBありますので、ダウンロードとインストールに若干の時間がかかります。ROS環境変数を設定するために、
$ echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc $ source ~/.bashrc
と入力して、bashファイルをシェルに組み込む。更に、PythonのROSパッケージなどを組み込むために
$ sudo apt install python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential
とコマンドを入力する。
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 Skip end-of-life distro "ardent" Skip end-of-life distro "bouncy" Skip end-of-life distro "crystal" Skip end-of-life distro "dashing" Skip end-of-life distro "eloquent" Add distro "foxy" Add distro "galactic" Skip end-of-life distro "groovy" Skip end-of-life distro "hydro" Skip end-of-life distro "indigo" Skip end-of-life distro "jade" Skip end-of-life distro "kinetic" Skip end-of-life distro "lunar" Add distro "melodic" Add distro "noetic" Add distro "rolling" updated cache in /home/koichi/.ros/rosdep/sources.cache
これで、一応のインストールは終了します。インストールが成功していれば、
$ printenv | grep ROS $ roscore
とコマンド入力すると、ROSの本体が作動します。以下の通りに、ROSが起動されたことが表示されます。
started roslaunch server http://Jetson:43183/ ros_comm version 1.14.12 SUMMARY ======== PARAMETERS * /rosdistro: melodic * /rosversion: 1.14.12 NODES auto-starting new master process[master]: started with pid [17687] ROS_MASTER_URI=http://Jetson:11311/ setting /run_id to ac872fda-a00b-11ec-83a6-5ca6e68574bd process[rosout-1]: started with pid [17703] started core service [/rosout]
「Cntl + c」 で ROS から出ます。
ROSを用いたJetson Nano Mouse の操作 |
ロボットに一連の動作をさせるためのプログラム群を納める作業スペースをcatkinワークスペースと言います。ROSを使用するにあたって最初にすることはこのcatkinワークスペースを作成することです。正確に言い換えると、例えば「catkin_ws」というわかりやすい名称のディレクトリを作成することです。このディレクトリの中にコードや設定ファイルを納めると、ロボットを動かす時に必要なもの一式が「catkin_make」というコマンド一つで生成できます。詳しい説明は
以下のコマンドを入力して、作業用のワークスペースを作成します。
$ source /opt/ros/melodic/setup.bash $ sudo apt install python3-empy # if pytho3-empy has not been installed $ mkdir -p ~/catkin_ws/src $ cd ~/catkin_ws/ $ catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3
このコマンド「catkin_make」はC言語のビルド・システムのmakeに対応していて、C++でのコードのコンパイルに関係する仕事をしてます。この結果、「source」ディレクトリの中に「CMakeLists.txt」が作成され、「catkin_ws」ディレクトリの下に「devel」と「build」という名称の二つのディレクトリが生成されます。いくつかの「setup.*sh」ファイルも生成されます。なお、このcatkin_make は システム Pythoh3 を使用しますので、このPython へのパスが通っている必要があります。（RT社のカスタム版JetPack を使用している場合、システムPython2を使用していますので、以下のpythonスクリプトの記述での Python3 はすべて Pythonにして下さい。）
このワークスペース関連の設定をシェルに読み込ませるために、
$ source ~/catkin_ws/devel/setup.bash
と入力する必要があります。
$ echo $ROS_PACKAGE_PATH
と打って、Terminalの標示からcatkin_ws/srcへのパスが確認できれば成功です。
まず最初に、ロボット操作用の基本的なパッケージを作成します。パッケージの名前をjnmouse_rosとします。 ~/catkin_ws/src/にパッケージを作成するために、以下のコマンドを入力します。
$ cd ~/catkin_ws/src/ $ catkin_create_pkg jnmouse_ros std_msgs rospy --rosdistro "melodic" -----結果 Created file jnmouse_ros/package.xml Created file jnmouse_ros/CMakeLists.txt Created folder jnmouse_ros/src Successfully created files in /home/koichi/catkin_ws/src/jnmouse_ros. Please adjust the values in package.xml.
catkin_create_pkgはパッケージを作成させるコマンドです。パッケージの名称の後に、ビルドに必要なライブラリのリストを書きます。rospyを明記しているのは、Pythonでコードを書くためです。（ROSのAPIのほとんどはC++とPythonで書かれています。）jnmouse_rosというディレクトリができて、package.xmlとCMakelists.txt が自動的に作成されます。
$ source ~/catkin_ws/devel/setup.bash $ cd jnmouse_ros $ ls
と入力すると、生成されたファイルが表示されます。
pimoue_ros をワークスペースに組み込むために、以下のコマンドを打ちます。
$ cd ~/catkin_ws $ catkin_make ---- result 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: "cmake /home/koichi/catkin_ws/src -DCATKIN_DEVEL_PREFIX=/home/koichi/catkin_ws/devel -DCMAKE_INSTALL_PREFIX=/home/koichi/catkin_ws/install -G Unix Makefiles" 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/melodic -- This workspace overlays: /home/koichi/catkin_ws/devel;/opt/ros/melodic -- Found PythonInterp: /usr/bin/python2 (found suitable version "2.7.17", minimum required is "2") -- Using PYTHON_EXECUTABLE: /usr/bin/python2 -- 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/googletest': gtests will be built -- Found gmock sources under '/usr/src/googletest': gmock will be built -- Found PythonInterp: /usr/bin/python2 (found version "2.7.17") -- Using Python nosetests: /usr/bin/nosetests-2.7 -- catkin 0.7.29 -- BUILD_SHARED_LIBS is on -- BUILD_SHARED_LIBS is on -- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -- ~~ traversing 1 packages in topological order: -- ~~ - jnmouse_ros -- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -- +++ processing catkin package: 'jnmouse_ros' -- ==> add_subdirectory(jnmouse_ros) -- Configuring done -- Generating done -- Build files have been written to: /home/koichi/catkin_ws/build #### #### Running command: "make -j4 -l4" in "/home/koichi/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 としてjnmouse_ros/scripts/ディレクトリに保存します。
$ source ~/catkin_ws/devel/setup.bash $ roscd jnmouse_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 jnmouse_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 jnmouse_ros $ mkdir msg $ cd msg $ nano LightSensorValues.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 jnmouse_ros.msg import LightSensorValues
rospy.init_node('lightsensors')
このスクリプトはノード'lightsensors'を立ち上げるだけの機能しかありません。from jnmouse_ros.msg import LightSensorValuesは'lightsensors'からのメッセージの型を組み込むためのコマンドです。このファイルを実行可能なファイルにするために、以下のように打ち込んでください。
$ roscd jnmouse_ros/scripts $ nano lightsensors1.py $ chmod +x lightsensors1.py
roscoreを立ち上げて、
$ rosrun jnmouse_ros lightsensors1.py
と打つと、エラーが出ます。なぜなら、jnmouse_ros.msg というモジュールがまだ組み込まれていないからです。このモジュールを組み込むためには、catkin_makeが必要となります。
まず、package.xmlとCMakeLists.txtを編集します。package.xmlを開いて、以下のように二つの行のコメント を外して、上書き保存します。
$ roscd jnmouse_ros $ nano 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言語の知識がない人はそうなんだと理解して進んでください。ディレクトリ jnmouse_ros にある CMakeLists.txtを開いてください。
$ roscd jnmouse_ros $ nano 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 jnmouse_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 jnmouse_ros lightsensors1.py
と入力して、エラーが出なければ、成功です。また、LightSensorValuesから生成されたファイルは以下のように入力すると、
$ cd ~/catkin_ws/devel/ $ find . | grep LightSensorValues --------- ./include/jnmouse_ros/LightSensorValues.h ./lib/python3/dist-packages/jnmouse_ros/msg/_LightSensorValues.py ./lib/python3/dist-packages/jnmouse_ros/msg/__pycache__/_LightSensorValues.cpython-38.pyc ./share/roseus/ros/jnmouse_ros/msg/LightSensorValues.l ./share/common-lisp/ros/jnmouse_ros/msg/LightSensorValues.lisp ./share/common-lisp/ros/jnmouse_ros/msg/_package_LightSensorValues.lisp ./share/gennodejs/ros/jnmouse_ros/msg/LightSensorValues.js
と表示されます。
距離センサーからの値を読めるようにしましょう。そこで、lightsensors1.pyを以下のように書き換えます。
#!/usr/bin/env python3
import sys, rospy
from jnmouse_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という名前で保存します。
$ roscd jnmouse_ros/scripts/ $ nano lightsensors2.py $ chmod +x lightsensors2.py
roscoreを立ち上げ、別のターミナルから
$ source ~/catkin_ws/devel/setup.bash $ rosrun jnmouse_ros lightsensors2.py
と入力します。3つ目のターミナルから
$ source ~/catkin_ws/devel/setup.bash $ 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 jnmouse_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ディレクトリに保存してください。
$ roscd jnmouse_ros/scripts/ $ nano lightsensors.py $ chmod +x lightsensors.py
パラメータ値の設定がないときは、初期値(hz=10)のままで作動します。上と同じく、roscoreを立ち上げ、別のターミナルから
$ rosparam set lightsensors_freq 1 $ rosrun jnmouse_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ディレクトリに保存します。
$ roscd jnmouse_ros/msg $ nano MotorFreqs.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 jnmouse_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ディレクトリに保存してください。
$ roscd jnmouse_ros/scripts $ nano motors1.py $ chmod +x motors1.py
このスクリプトの説明を簡単にします。from jnmouse_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 ~/catkin_ws/devel/setup.bash $ rosrun jnmouse_ros motors1.py
と入力して、motors1.pyを実行させます。別のターミナルを開いて、
$ rostopic pub /motor_raw jnmouse_ros/MotorFreqs "left_hz: 100 (tabを打つ） right_hz:100"
と入力すると、hz=100でモーターが回転します。
止めるときは
$ source ~/catkin_ws/devel/setup.bash $ rostopic pub /motor_raw jnmouse_ros/MotorFreqs "left_hz: 0 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 jnmouse_ros $ mkdir launch $ cd launch $ nano jnmouse.launch
と入力して、以下のようなjnmouse.launchファイルを作成します。
<launch>
<node pkg="jnmouse_ros" name="lightsensors" type="lightsensors.py" required="true" />
<node pkg="jnmouse_ros" name="buzzer" type="buzzer.py" required="true" />
<node pkg="jnmouse_ros" name="motors" type="motors.py" required="true" />
</launch>このlaunchファイルに対応させて、スクリプトを以下のようにシンボリック・リンクを貼って統一します。
$ roscd jnmouse_ros/scripts $ ln -s motors1.py motors.py
このlaunchファイルを起動すれば、
$ roslaunch jnmouse_ros jnmouse.launch
roscoreが立ち上がり、launchファイルに記述されたスクリプトが実行されます。一つのターミナルで済みます。
RaspberryPi Mouse ロボットの自律走行 |
トピックを介した通信はメッセージを送受するノードが互いに待つことはなく、非同期で動作します。サービスを利用すると、相手のノードの処理を待ち、その結果を受け取ることができます。サービスを実装するプログラムを作成します。特に、モーターのオンオフに関するサービスを実装します。std_msgsに対応するサービス版std_srvsパッケージを利用します。
jnmouse_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 jnmouse_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 jnmouse_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として/jnmouse_ros/scriptsディレクトリに保存してください。
動作を確認してみます。ロボットのモーターの電源スイッチを入れて、jetsonのターミナルを立ち上げて、
$ roscore
と roscore を立ち上げて、２つ目のターミナルを立ち上げて、以下のように打ち込んでください。
$ roscd jnmouse_ros/scripts $ chmod +x motors2.py $ rosrun jnmouse_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で定義します。
jnmouse_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 jnmouse_ros.msg import MotorFreqs
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse
from jnmouse_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.
# modified by mashyk
動作確認をしましょう。モーターを浮かせておいて、モーターの電源スイッチを入れて、jnmouseのターミナルで roscore を立ち上げます。そして、２つ目のターミナルから以下のように順番にコマンドを入力します。
$ rosrun jnmouse_ros motors.py $ rosservice call /motor_on $ rosservice call /timedMotion "left_hz: 400 \ right_hz:400 \ duraion_ms:500" #enterキーを打つ
「#tabキーを打つ」などのコメントは入力しないでください。左右の車輪が400Hzで回転して、500ミリ秒後に停止します。回転数を変えたり、動作時間を変化させて、実行することを試みてください。モーターへの通電を停止するためには
$ rosservice call /motor_off
と入力してください。うまくいったら、成功です。
このセクションで作成したjnmouse_rosパッケージの構造は以下のようになっています。
~/catkin_ws/src/jnmouse_ros$ tree . ├── CMakeLists.txt ├── launch │ ├── jnmouse.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
以下のセクションでは、ラズパイ・マウスを壁の手前でストップさせたり、壁に沿って走らせることをします。このような操作を実現するパッケージをjnmouse_corridorという名称で作成します。また、jnmouse_rosパッケージをこれに組み込んで利用します。jnmouse_corridorパッケージを作成するために
$ cd ~/catkin_ws/src/ $ catkin_create_pkg jnmouse_corridor
と入力します。jnmouse_ros パッケージを利用することを jnmouse_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>jnmouse_ros</exec_depend>
jnmouse_ros の１行を exec_depend の一覧の最後に追加します。ここで、一度、catkin_make しておきましょう。
$ cd ~/catkin_ws $ catkin_make $ source ~/.bashrc
前のセクションと同様に、jnmouse_corriodr/scripts/ディレクトリを作ります。
$ roscd jnmouse_corridor $ mkdir scripts $ cd scripts
このディレクトリ（/scripts/）の中にjetson nano マウスを走らせるプログラムを配置します。最初に、壁の手前で停止させるwall_stop.pyを作成します。以下のファイルを書いて、保存してください。
#!/usr/bin/env python3
import rospy,copy
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse
from jnmouse_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ファイルを作成します。そのために、jnmouse_corridor の下に launch ディレクトリを作ります。
$ roscd jnmouse_corridor $ mkdir launch $ cd launch
以下のような、wall_stop.launchファイルを作成します。
<launch>
<include file="$(find jnmouse_ros)/launch/jnmouse.launch" />
<node pkg="jnmouse_corridor" name="wall_stop" type="wall_stop.py" />
</launch>
このファイルをlaunchディレクトリに保存してください。
最後に、ロボットを動かしてみましょう。デバイス・ドライバーを読み込み、モーターの電源スイッチを入れて、ロボットを壁から１メートルほど離して、壁に向けておいてください。そこで、ラズパイのターミナルで
$ roslaunch jnmouse_corridor wall_stop.launch
と入力します。すると、ロボットが走り出して、壁の10センチ手前で停止します。
次に、ロボットを壁沿に走らせることを試みましょう。このために、wall_around.pyというスクリプト作成します。以下のファイルをコピペして、
#!/usr/bin/env python3
import rospy,copy,math
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse
from jnmouse_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ディレクトリに保存してください。
$ roscd jnmouse_corridor/scripts $ nano wall_around.py $ chmod +x wall_around.py
さらに、launchファイルを作成します。
<launch>
<include file="$(find jnmouse_ros)/launch/jnmouse.launch" />
<node pkg="jnmouse_corridor" name="wall_around" type="wall_around.py" />
</launch>
このファイルをwall_around.launchという名前で、launchディレクトリに保存してください。
デバイス・ドライバーを読み込み、モーターの電源スイッチを入れて、ロボットを壁から離して、壁に平行に向けておいてください。そして、
$ source ~/catkin_ws/devel/setup.bash $ roslaunch jnmouse_corridor wall_around.launch
と入力します。すると、ロボットが走り出して、壁から10センチほど離れて、壁や柱の凹凸を避けながら走ります。
以下の動画はpimouse ですが、jnmouseでも同じように壁に沿って自動走行します。pimouseに比べてjnmouseは力強い走りになります。