Python3ではじめるROS2（自動起動編）

#はじめに
「ROS2 Advent Calendar 2019 14日」に参加しております。
（すでに投稿済みの内容ですが、これに合わせて若干手直ししました。）

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を起動する|


#１．roslaunchから起動

カスタムメッセージ編・ノード編で作ってきたPub / Subノードを、launchファイルからまとめて起動します。

##（１）準備

launchファイルを作ります。

```sh:コマンドライン
~/ros2_ws/src$ cd pubsubpy
~/ros2_ws/src/pubsubpy$ touch pubsubpy.launch.py
```

##（２）launchファイル

```python:pubsubpy.launch.py
# -----------------------------------------------
# ROS LAUNCH
#
# The MIT License (MIT)
# Copyright (C) 2019 myasu.
# -----------------------------------------------

"""Launch a add_two_ints_server and a (synchronous) add_two_ints_client."""

import launch
import launch_ros.actions


def generate_launch_description():

    ### ここにlaunchしたいノードを定義
    ### node_executableのところは、setup.pyのなかの
    ### entry_pointsで指定した （例）pub = node.Pub:main
    ### の左辺側の文字と合わせて下さい
    pub = launch_ros.actions.Node(
        package='pubsubpy', node_executable='pub', output='screen')
    sub = launch_ros.actions.Node(
        package='pubsubpy', node_executable='sub', output='screen')

    ### こちらにもlaunchしたいノードを記述
    ### 上記で定義した （例）pub = launch_ros.actions.Node
    ### の、左辺側の変数を列挙します。
    ### 記述しなかったら、そのノードは起動しません。
    ###
    ### target_action=sub に記述したノードが落ちたら、
    ### launchで起動したものが一式落ちます。
    return launch.LaunchDescription([
        pub,
        sub,
        # TODO(wjwwood): replace this with a `required=True|False` option on ExecuteProcess().
        # Shutdown launch when client exits.
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=sub,
                on_exit=[launch.actions.EmitEvent(
                    event=launch.events.Shutdown())],
            )),
    ])
```

最終的には、下記のようなディレクトリ構成・ファイル構成になります。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/145581/d21275c0-bd68-576c-8212-62d0d3750794.png)


##（３）ビルド

**注意：launchファイルは、` --symlink-install`が効かないので、編集する度に必ずビルドして下さい**

```sh:コマンドライン
~/ros2_ws/src/pubsubpy$ cd ../..
~/ros2_ws$ colcon build --symlink-install
Starting >>> pubsubpy_mes
Finished <<< pubsubpy_mes [8.83s]                        
Starting >>> pubsubpy
Finished <<< pubsubpy [15.8s]            
                      
Summary: 2 packages finished [26.7s]

#環境変数の読み込み
~/ros2_ws$ source ./install/setup.bash
```

##（４）実行

**eloquent以前**
実行中は、コンソール上のメッセージが出てきません。（たまに流れてくるときもあります・・・）
[Ctrl-C]すると、バッファに貯まっていたメッセージが一気に流れてきます。

**foxy以降**　(21/3/18追記)
実行中は、すぐにコンソール上にメッセージが出てきます。

```sh:コマンドライン
~/ros2_ws$ ros2 launch pubsubpy pubsubpy.launch.py
[INFO] [launch]: All log files can be found below /home/ubuntu/.ros/log/2019-10-14-19-46-14-616087-raspi3u-23637
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [pub-1]: process started with pid [23708]
[INFO] [sub-2]: process started with pid [23709]
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[sub-2] [INFO] [mysub]: mysub initializing...
[sub-2] [INFO] [mysub]: mysub do...
[sub-2] [INFO] [mysub]: Subscription > Port: 0 Value: 0
[sub-2] [INFO] [mysub]: Subscription > Port: 0 Value: 1
[sub-2] [INFO] [mysub]: mysub done.
[INFO] [pub-1]: process has finished cleanly [pid 23708]
[INFO] [sub-2]: process has finished cleanly [pid 23709]
[pub-1] [INFO] [mypub]: mypub initializing...
[pub-1] [INFO] [mypub]: mypub do...
[pub-1] [INFO] [mypub]: Publish [0]
[pub-1] [INFO] [mypub]: Publish [1]
[pub-1] [INFO] [mypub]: mypub done.
Task exception was never retrieved
future: <Task finished coro=<LaunchService._process_one_event() done, defined at /opt/ros/dashing/lib/python3.6/site-packages/launch/launch_service.py:175> exception=InvalidHandle('Asked to destroy handle, but it was already destroyed',)>
Traceback (most recent call last):
  File "/opt/ros/dashing/lib/python3.6/site-packages/launch/launch_service.py", line 177, in _process_one_event
    await self.__process_event(next_event)
  File "/opt/ros/dashing/lib/python3.6/site-packages/launch/launch_service.py", line 186, in __process_event
    entities = event_handler.handle(event, self.__context)
  File "/opt/ros/dashing/lib/python3.6/site-packages/launch/event_handlers/on_shutdown.py", line 67, in handle
    return self.__on_shutdown(cast(Shutdown, event), context)
  File "/opt/ros/dashing/lib/python3.6/site-packages/launch_ros/default_launch_description.py", line 39, in _shutdown
    self.__launch_ros_node.destroy_node()
  File "/opt/ros/dashing/lib/python3.6/site-packages/rclpy/node.py", line 1408, in destroy_node
    self.handle.destroy()
  File "/opt/ros/dashing/lib/python3.6/site-packages/rclpy/handle.py", line 92, in destroy
    raise InvalidHandle('Asked to destroy handle, but it was already destroyed')
rclpy.handle.InvalidHandle: Asked to destroy handle, but it was already destroyed
~/ros2_ws$ 
```

- **（12/14追記）**
    - コメントに頂いたとおり、メッセージを即出力したい場合は、`print`関数を使って下さい。
    - `get_logger().warn`や、`get_logger().error`など緊急度の高いメッセージは、バッファされること無く即出力される事を確認しました




#２．systemdからroslaunchを起動

一通り試作が終わって本番に適用する際に、装置（RaspberryPi）の電源が入ってLinuxが立ち上がったら、自動的にROS2のノードも立ち上げが必要になります。
ここでは、systemdからlounchする方法を解説します。

##（１）考え方

 1. `pubsubpy.sh`に起動コマンド`ros2 launch pubsubpy pubsubpy.launch.py`を記述
 2. `systemd` から `pubsubpy.sh`を起動
 3. コンソールに出てくる情報は、`./log/pubsubpy.log`に出力
 4. `./log/pubsubpy.log`は定期的に`logrotate`する

##（２）ファイルの準備

```sh:コマンドライン
~/ros2_ws/$ cd src/pubsubpy
~/ros2_ws/src/pubsubpy$ touch pubsubpy.sh pubsubpy.service pubsubpy.logrotate
~/ros2_ws/src/pubsubpy$ mkdir log
```

###pubsubpy.sh

```sh:pubsubpy.sh
#!/bin/bash
#--------------------------------------------------------------------
#バックグラウンド実行用のスクリプト
#--------------------------------------------------------------------

#変数の設定
SCRIPTDIR=/home/ubuntu/ros2_ws/src/pubsubpy/
LOGDIR=$SCRIPTDIR/log
ENVFILE=/home/ubuntu/ros2_ws/install/setup.bash

#実行
if [ -f ${ENVFILE} ]; then
    #環境変数読み込み
    echo "Loading ROS2 Env..."
    source /home/ubuntu/ros2_ws/install/setup.bash
    if [ -d ${LOGDIR} ]; then
        echo "ROS2 Launching..."
        #スタンドアロンで起動する場合に必須の項目
        #ネットワーク上で一意にするための値
        export ROS_DOMAIN_ID=232
        #DDSの通信先ホストを限定する
        export ROS_ALLOWED_HOSTS="localhost.local:robot_1.local"
        #roslaunch実行
        exec ros2 launch pubsubpy pubsubpy.launch.py >> ${LOGDIR}/pubsubpy.log 2>&1
    else
        echo "There is no ${LOGDIR}"
    fi
else
    echo "There is no ${ENVFILE}"
fi
```


###pubsubpy.service

```ini:pubsubpy.service
[Unit]
Description=ROS2 launch test
After=local-fs.target
ConditionPathExists=/home/ubuntu/ros2_ws/src/pubsubpy

[Service]
ExecStart=/home/ubuntu/ros2_ws/src/pubsubpy/pubsubpy.sh
ExecStop=/bin/kill ${MAINPID}
Restart=on-failure
StartLimitInterval=60
StartLimitBurst=3
KillMode=mixed
Type=simple
User=pi
Group=pi

[Install]
WantedBy=multi-user.target
```

###systemdに登録する

```sh:コマンドライン
#serviceファイルをシンボリック（cpしてもOK）
$ sudo ln -s /home/ubuntu/ros2_ws/src/pubsubpy/pubsubpy.service /etc/systemd/system
#デーモンを有効化
$ sudo systemctl enable pubsubpy.service
#起動
$ sudo systemctl start pubsubpy.service
#ステータスを確認
$ systemctl status pubsubpy.service
```

上記の例では、`pubsubpy.service`ファイルをシンボリックリンクしてますが、コピーしても構いません。

**（覚え書き）**

デーモンを`sudo systemctl disable pubsubpy.service`で無効化したときに、

 - シンボリックリンク
    - pubsubpy.serviceのシンボリックリンクが消える（ので、enableする前にもう一度シンボリックリンクを作る必要がある）
 - コピー
    - pubsubpy.serviceファイルは残る

このような挙動の違いがあります。

##（３）logrotateの設定

###pubsubpy.logrotate

```sh:pubsubpy.logrotate
#---------------------------------------------------------------------
# pubsubpy.logrotate
#---------------------------------------------------------------------

/home/ubuntu/ros2_ws/src/pubsubpy/log/pubsubpy.log {
  daily
  missingok
  rotate 15
  compress
  delaycompress
  notifempty
  copytruncate
  su ubuntu ubuntu
}
```

|コマンド|意味|他のコマンド|
|:---|:---|:---|
|daily|毎日ローテート|weekly/monthly/yearly|
|missingok|ログファイルが無くてもエラーにしない|nomissingok|
|rotate15|ログの世代数(例は15ファイル、15日分)|0にすると残さない|
|compress|古いログファイルをgzipで圧縮|nocompress|
|delaycompress|古いログの1世代目だけは圧縮しない|-|
|notifempty|空ファイルはローテートしない|ifempty|
|copytruncate|更新中のログファイルを別名ファイルにcpしてから、元のログファイルを空にする|-|
|su ubuntu ubuntu|処理する時のユーザ/グループを指定|-|

`~/ros2_ws/src/pubsubpy/log/`ディレクトリは、この時点では`ubuntu:ubuntu`です。

###logrotateに登録する

```sh:コマンドライン
#logrotateの設定ディレクトリに移動
$ cd /etc/logrotate.d
#logrotateファイルをコピー（シンボリックリンクは不可）
$ sudo cp ~/ros2_ws/src/pubsubpy/pubsubpy.logrotate ./pubsubpy
#パーミッションを変更
$ sudo chmod 644 pubsubpy
#logrotateに登録
$ sudo logrotate -f ./pubsubpy
```

これでログが自動的にローテーションします。

動作チェックは、下記のようにします。

```sh:コマンドライン
$ sudo /usr/sbin/logrotate -f /etc/logrotate.d/uecsd 
$ ls -la /home/ubuntu/ros2_ws/src/pubsubpy/log/
合計 592
drwxrwxr-x 2 pi pi   4096  1月  6 09:03 .
drwxr-xr-x 5 pi pi   4096  1月  6 08:59 ..
-rw-r--r-- 1 pi pi      0  1月  6 09:03 pubsubpy.log
-rw-r--r-- 1 pi pi   8238  1月  6 09:03 pubsubpy.log.1
-rw-r--r-- 1 pi pi 583851  1月  6 09:03 pubsubpy.log.2.gz
```

pubsubpy.logに番号がついて、古いものはgz圧縮されています。


###エラー例

（１）下記のエラーが出た場合は、改行コードを`LF`に変更してください。

```
$ sudo logrotate -f ./pubsubpy
error: pubsubpy:10 lines must begin with a keyword or a filename (possibly in double quotes)
error: pubsubpy:11 lines must begin with a keyword or a filename (possibly in double quotes)
error: pubsubpy:19, unexpected text after }
```

（２）下記のエラーが出た場合は、設定ファイルのパーミッションを`644`にしてください。（シンボリックリンクを使うと、ファイルのパーミッションが`777`になるので、このエラーの回避が出来なくなります）

```
$ sudo logrotate -f ./pubsubpy
error: Ignoring poka because of bad file mode - must be 0644 or 0444.
```

（３）下記のエラーが出た場合は、`~/ros2_ws/src/pubsubpy/log/`のパーミッションが合っていません。設定ファイルの`su`項目で、該当ディレクトリのユーザ・グループを合わせてください。

```
error: skipping "/home/ubuntu/ros2_ws/src/pubsubpy/log/poka.log" because parent directory has insecure permissions (It's world writable or writable by group which is not "root") Set "su" directive in config file to tell logrotate which user/group should be used for rotation.
```

#３．その他のTips

##（１）ROS2をスタンドアロンで動かすとき

とある用途で、ネットワークの繋がらない現場で使う装置に、ROS2を使ったところ、正常に動かない現象が発生しました。

###ハマった状況

 - 環境
    - ROS2 Dashing
    - Ubuntu 18.04
    - Raspberry Pi 3+
 - 状況
    - ネットワーク（LAN / Wifi）に繋がっていないとき
        1. `ros2 lainch ...`で複数のノードを立ち上げた　→　`ros2 node list`や`ros2 topic list`で検索しても、一部のノードやトピックが見えない。
        - 再度`ros2 lainch ...`で立ち上げ直しても、やはり見えない。見えなくなってるノードの再現性も無い。（別のノードが見えたり、消えたりしている）
        - ros2 run ...でで個別に立ち上げても同様。
    - ネットワーク（LAN / Wifi）に繋がっているとき
        1. `ros2 lainch ...`で複数のノードを立ち上げた　→　`ros2 node list`や`ros2 topic list`で検索すると、必要なノード・トピックが全て見える

###スタンドアロンで動かすための処置

~~下記の環境変数を設定します。~~
このようにしても、正常に立ち上がらない例がありました。いまのところ、完全な回避作は見つかってません。（19/11/12追記）

```
#ネットワーク上で一意にするための値
export ROS_DOMAIN_ID=<任意の数字 0～232>
#DDSの通信先ホストを限定する
export ROS_ALLOWED_HOSTS="localhost.local:robot_1.local"
```

`ROS_DOMAIN_ID`は、同じ値の環境変数から起動したノード間だけで通信出来るようにします。

`ROS_ALLOWED_HOSTS`は、値に指定したホストとだけ通信できるようにします。

同一のLANネットワーク上でROS2の環境が存在するときに、混信を防ぐための仕組みですが、スタンドアロン環境でも設定が必須です。

（`ROS_ALLOWED_HOSTS`に言及している資料が大変少ないので、こちらがどんな効果が有るのかはは良く解りません・・・）


###参考
 - https://index.ros.org/doc/ros2/Concepts/Overview-of-ROS-2-concepts/#discovery
 - https://github.com/ros2/ros2/issues/798
 - https://answers.ros.org/question/319723/use-ros2-fastrtps-with-standalone-fastrtps-programs/


##（２）ROS_DOMAIN_IDの設定範囲

同じネットワークに、複数のROS2環境が混在すると混信が発生します。
（ハンズオンなどをすると、その際中に経験します・・・）
0～65535までの任意の値を設定出来るようですが・・・本当にその範囲に割り当て出来るかを確認してみました。

###結論

 - 0～232

###範囲の確認方法

環境変数に適当な数字を入れると範囲を教えてくれます。最終的に、RTPS のほうが範囲の指定をしてきます。

```sh:コマンドライン
~$ export ROS_DOMAIN_ID=65500
~$ ros2 node list
Traceback (most recent call last):
  File "/opt/ros/dashing/bin/ros2", line 11, in <module>
・・・（省略）・・・
OverflowError: getsockaddrarg: port must be 0-65535.
~$ export ROS_DOMAIN_ID=233
~$ ros2 node list
Calculated port number is too high. Probably the domainId is over 232 or portBase is too high.
2019-11-06 12:28:17.342 [RTPS Error] Calculated port number is too high. Probably the domainId is over 232 or portBase is too high. -> Function getMulticastPort
~$ export ROS_DOMAIN_ID=232
~$ ros2 node list
~$ 
```
###参考
 - https://github.com/ros2/rmw_fastrtps/issues/261
 - https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#get-a-personal-ros-domain-id


#おわりに

ROS2の勉強を19年7月から始めたのですが、やはりPython3のまとまった作例が少なかったので、覚え書きを兼ねてまとめを作ってみました。（特にlounchファイルのサンプルが少ないこととか、スタンドアロン環境でROS_DOMAIN_IDが必須なこととか・・・）
参考になりましたら幸いです。


#参考資料

 - https://index.ros.org/doc/ros2/Installation/Eloquent/Linux-Development-Setup/
 - https://www.theconstructsim.com/ros2-tutorials-5-how-to-create-a-ros2-package-for-python-update/
 - https://qiita.com/NeK/items/c9ba8aa3a005087762e2
 - https://qiita.com/l1sum/items/6acabc94b040f8b0f7cd
 - https://gbiggs.github.io/rosjp_ros2_intro/
 - https://github.com/ros2/tutorials/
   - https://github.com/ros2/tutorials/tree/rosidl_tutorials/rclpy_tutorials
   - https://github.com/ros2/tutorials/tree/rosidl_tutorials/rosidl_tutorials/rosidl_tutorials_msgs
 - https://index.ros.org/doc/ros2/Tutorials/
   - https://index.ros.org/doc/ros2/Tutorials/Defining-custom-interfaces-(msg-srv)/
   - https://index.ros.org//doc/ros2/Tutorials/Python-Programming/
   - https://index.ros.org//doc/ros2/Tutorials/Launch-system/
- https://milestone-of-se.nesuke.com/sv-basic/linux-basic/logrotate/