Python3ではじめるROS2（カスタムメッセージ編）

#はじめに
ROS2とPython3を用いて、カスタムメッセージを使った簡単なノードを作ってみます。

**Python3ではじめるROS2 関連記事**

|回数|サブタイトル|内容|
|:--|:--|:--|
|[第1回](https://qiita.com/myasu/items/3a026697f535183e12e8)（今回）|カスタムメッセージ編|・環境の準備<br>・カスタムメッセージを作る<br>・（おまけ）.bashrcのカスタマイズ|
|[第2回](https://qiita.com/myasu/items/87ee6a208f4af615e653)|ノード編|・Pub/Subのノードを作る|
|[第3回](https://qiita.com/myasu/items/39362b03780cb773b1ed)|自動起動編|・roslaunchから起動する<br>・systemdからroslaunchを起動する|

#１．環境の準備

ここでのハードウェア環境は、以下を想定しています。

 - Raspberry Pi 3+
 - Ubuntu Linux 18.04 (64bit版)

```sh:コマンドライン
~$ uname -a
Linux raspi3u 4.15.0-1048-raspi2 #52-Ubuntu SMP PREEMPT Wed Sep 18 08:57:33 UTC 2019 aarch64 aarch64 aarch64 GNU/Linux
```

##（１）インストール
下記を参考にして、ROS2本体のインストール、その他ツールのインストールをします。
（wiringPiは無理に入れなくても良いです）

 1. [Ubuntu Linux 18.04でのROS2のインストール](https://gbiggs.github.io/rosjp_ros2_intro/computer_prep_linux.html)
 1. [colconのインストール](https://qiita.com/seshimaru/items/66c64dfe1b807ce7c2b0)
 1. [その他ツール](https://qiita.com/NeK/items/d2bdd8798de7cdea1932)
 1. [WiringPi](https://qiita.com/myasu/items/e3f81b2826ed5797a040)


##（２）作業ディレクトリの準備

ホームディレクトリに`ros2_ws`と、さらにその下に`src`を作ります。

```sh:コマンドライン
~$ mkdir -p ./ros2_ws/src
~$ cd !$
cd ./ros2_ws/src
~/ros2_ws/src$ 
```

#２．カスタムmsgの作成

参考ページ

 - [Defining custom interfaces (msg/srv)](https://index.ros.org/doc/ros2/Tutorials/Defining-custom-interfaces-(msg-srv)/)
 - [Introduction to msg and srv interfaces](https://index.ros.org//doc/ros2/Tutorials/Rosidl-Tutorial/)

##（１）準備

ROS1ではあまり気にしませんでしたが、ROS2では必ず「パッケージ」を作る必要があります。
ここでは、カスタムメッセージのパッケージを準備します。

```sh:コマンドライン
#ワークスペースを作成
~/ros2_ws/src/$ ros2 pkg create pubsubpy_mes
~/ros2_ws/src$ cd pubsubpy_mes/
#オリジナルのバックアップ、不要なファイルの改名
~/ros2_ws/src/pubsubpy_mes$ cp -p package.xml package.xml.org
~/ros2_ws/src/pubsubpy_mes$ cp -p CMakeLists.txt CMakeLists.txt.org
~/ros2_ws/src/pubsubpy_mes$ mkdir msg srv 
~/ros2_ws/src/pubsubpy_mes$ ls
CMakeLists.txt  CMakeLists.txt.org  msg  package.xml  package.xml.org  src  srv
```

この時点で、下記のようなディレクトリ構成になっています。

```:ディレクトリ構成
~/ros2_ws
   + src
     + pubsubpy_mes
        + msg
        + src
        + srv
```

##（２）メッセージ・サービスのファイル

###メッセージ

```sh:コマンドライン
~/ros2_ws/src/pubsubpy_mes$ cd msg
~/ros2_ws/src/pubsubpy_mes/msg$ touch GpioMsg.msg
```

**注意：ファイル名の先頭文字は、必ず大文字にして下さい。**

```:GpioMsg.msg
int16 port
int16 value
```
扱うことの出来る型は、[About ROS 2 Interfaces](https://index.ros.org/doc/ros2/Concepts/About-ROS-Interfaces/)を参考願います。
こちらでの例は、RaspberryPiのGPIOを操作する事を想定したメッセージを作っています。

 - port : RaspberryPiのBCM番号
 - value : 0 → OFF(L)、1 → ON(H)

###サービス

※次の中編のサンプルプログラムでは、サービスは扱いません。（参考まで）

```sh:コマンドライン
~/ros2_ws/src/pubsubpy_mes$ cd srv 
~/ros2_ws/src/pubsubpy_mes/msg$ touch AdSrv.srv
```

**注意：ファイル名の先頭文字は、必ず大文字にして下さい。**

ROS1と同じく、上段は引数、下段は返り値です。

```:AdSrv.srv
int16 ch
---
int16 ch
int16 value
```

こちらでの例は、RaspberryPiにADコンバータICを接続して、指定のチャネルからAD値を読み取る事を想定したサービスを作っています。

 - ch : ADコンバータの入力チャネル番号
 - value : AD変換値



##（３）ビルド用のファイル

先に実行した`ros2 pkg create pubsubpy_mes`で作られたファイルを編集します。

ソース中に`###`でコメントしている所を、必要に応じて書き換えて下さい。
`CMakeLists.txt`と`package.xml`の間で、同じパッケージ名を指定して下さい。（ビルドエラーになります）


###CMakeLists.txt

```cmake:CMakeLists.txt
cmake_minimum_required(VERSION 3.5)

### ここにパッケージ名を記述 ###
project(pubsubpy_mes)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)


find_package(rosidl_default_generators REQUIRED)

### ここにメッセージやサービスのファイルを記述 ###
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/GpioMsg.msg"
  "srv/AdSrv.srv"
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_export_dependencies(rosidl_default_runtime)
ament_package()
```

###package.xml

```xml:package.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <!--### ここにパッケージ名を記述 ###-->
  <name>pubsubpy_mes</name>
  <version>0.0.0</version>
  <description>sample message</description>
  <maintainer email="ubuntu@todo.todo">ubuntu</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

<!-- Customized Message -->
  <buildtool_depend>rosidl_default_generators</buildtool_depend>
  <depend>action_msgs</depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

```


##（４）ビルド

```sh:コマンドライン
#ワーキングディレクトリに戻ってからビルド
#ビルドの際は、必ず~/ros2_wsに移動します。
~/ros2_ws/src/pubsubpy_mes$ cd ../..
~/ros2_ws$ colcon build --symlink-install
Starting >>> pubsubpy_mes
[Processing: pubsubpy_mes]                            
[Processing: pubsubpy_mes]                                 
[Processing: pubsubpy_mes]                                        
Finished <<< pubsubpy_mes [1min 42s]                                

Summary: 1 package finished [1min 44s]
~/ros2_ws$
```

RaspberryPi3+だと、メッセージのビルドにこのくらいの時間が掛かります。

cmakeのキャッシュをクリアするときは、オプション`--cmake-clean-cache`を付けます。

```
colcon build --symlink-install --cmake-clean-cache
```

colconの引数は[こちら](https://qiita.com/seshimaru/items/ed344530ead80ab1733f)を参考にして下さい。

##（５）パッケージのインストール確認

カスタムメッセージのパッケージがインストールできているか確認します。

```sh:コマンドライン
#ワークディレクトリの環境変数を読み込み
#これはビルドする度に実行して下さい
~/ros2_ws$ source ./install/setup.bash
```

#### Dashingまで

```sh:コマンドライン
#メッセージの確認
#（grepつけて関係あるメッセージだけ表示）
~/ros2_ws$ ros2 msg list | grep pubsubpy_mes
pubsubpy_mes/msg/GpioMsg

#サービスの確認
#（grepつけて関係あるサービスだけ表示）
~/ros2_ws/src$ ros2 srv list | grep pubsubpy_mes
pubsubpy_mes/srv/AdSrv
~/ros2_ws/src$ 
```

#### Eloquent以降

前述の通り実行すると、下記の様なメッセージが出ます。

```sh:コマンドライン
/opt/ros/eloquent/lib/python3.6/site-packages/ros2msg/verb/list.py:26: UserWarning: 'ros2 msg' is deprecated and will be removed in a future ROS release. Instead use: 'ros2 interface list -m'
  "'ros2 msg' is deprecated and will be removed in a future ROS release. "
```

Eloquent以降は引数が変わるようですので、下記の様に実行します。

```sh:コマンドライン
#メッセージの確認
#（grepつけて関係あるメッセージだけ表示）
$ ros2 interface list -m | grep pubsubpy_mes
    pubsubpy_mes/msg/GpioMsg

#サービスの確認
$ ros2 interface list -s | grep pubsubpy_mes
    pubsubpy_mes/srv/AdSrv
```


次回は、カスタムメッセージを使ったノードを作ります。


#３．おまけ

いろんなコマンド操作があるので、`.bashrc`などに色々小細工すると便利です。

ここでは、常に`~/ros2_ws/`を作業ディレクトリにすることを前提に、最低限必要なコマンドなどをaliasや関数にまとめてます。

```sh:~/.bashrc
・・・（省略）・・・
# my ROS2 settings
if [ -f ~/.bashrc.mine ]; then
  . ~/.bashrc.mine
fi

```

```sh:~/.bashrc.mine
# ----------------------------
# .bashrc for ROS2
# ----------------------------

# ssh経由で操作しているときに
# 本体のディスプレイでXのウィンドウを開く
export DISPLAY=:0.0

### ROS Development Environment
### for ROS2
echo -n "Loading ROS2 Env..."

# colconするときに必要な操作をまとめて実行する関数
# $@ には関数の引数が入っている
function mycolcon() {
  if [ -e $@ ]; then
    #現在のディレクトリ位置を保存
    CUR_DIR=`pwd`
    #引数に与えられたディレクトリに移動
    cd $@
    echo "Build... "
    #colconを実行
    colcon build --symlink-install
    if [ -e ./install/setup.bash ]; then
      #環境変数があれば読み込み
      source ./install/setup.bash
    fi
    #元居たディレクトリに戻る
    cd $CUR_DIR
  fi
}

# ログイン時に環境変数を読み込み
export CHOOSE_ROS_DISTRO=dashing
. /opt/ros/${CHOOSE_ROS_DISTRO}/setup.bash

### for ROS2 my settings
if [ -f ~/ros2_ws/install/setup.bash ]; then
  . ~/ros2_ws/install/setup.bash
fi

# 同一ネットワークにROS2が混在するときに、一意にする為の環境変数
# 必要なときにコメントを外します
#export ROS_DOMAIN_ID=168000

echo "Done."
```


上記の`.bashrc`で定義した、`mycolcon`を実行するときは、下記のように入力します。

```sh:コマンドライン
(どこのディレクトリに居てもOK)
 $ mycolcon ~/ros2_ws
```

引数に指定したディレクトリに自動的に移動して、ビルド、環境変数の読み込みまで完了させて、元いたディレクトリに帰ってきます。

#おわりに

ROS2の勉強を19年7月から始めたのですが、やはりPython3のまとまった作例が少なかったので、覚え書きを兼ねてまとめを作ってみました。
参考になりましたら幸いです。