micro-ROSチュートリアル｜ ホストPC・マイコン間 pubsub

micro-ROS のチュートリアルを進め、ホスト PC と STM マイコン間で ping-pong をするまでの記事です。
なるべく docker を利用していますので、手っ取り早く sample を動かしたい方向けです。
詳細は公式のチュートリアル(参考[1])をご覧ください。

記事は 3 つに分けて投稿します。本記事は 2 記事目になります。

 1.  [[ping-pong] ホストPC内 ping-pong](https://qiita.com/hssay/items/322dfd28b4a8a4cbd34d)
 2. [publisher] ホストPC・マイコン間 pubsub
 3. [[ping-pong] ホストPC・マイコン間 ping-pong](https://qiita.com/hssay/items/8d3f1b3209e89593c63c)

## 環境
ホスト PC：Ubuntu 18.04.4 LTS
対象ボード：STM32 Nucleo-144 開発 ボード NUCLEO-F767ZI

docker を使うので、他の環境でも動くかと思います。
nucleo-144 は比較的お手頃で、ポーティングしている方がいた(参考[2])ので選んでいます。

## 設定
今回使うnucleo-144は公式でサポートされていないハードウェアなので(参考[3])、ポーティングが必要になります。
１つ１つ項目を確認しながら設定変更が必要となりますが、それも大変なので、フォークして来て、ポーティング済みのリポジトリを用意しました。
細かい部分は公式ドキュメント(参考[1])、maeharaさんの説明、リポジトリのコミットログをご覧ください。

### 作業ディレクトリ（ws)の用意
#### docker イメージの作成
ボード・RTOS毎に作業ディレクトリ(ws)を作っていきます。
今回はdockerでの環境構築も目指すので、DockerFileも環境ごとに用意が必要となります。

フォークして修正を加えたリポジトリを用意しましたので、そちらを使います。

```
$ mkdir -p ~/uros_ws
$ git clone https://github.com/hsgwa/micro-ROS-docker.git
$ cd micro-ROS-docker
$ docker image build -t uros_nucleo-144_f767-netnsh:dashing micro-ROS-Nucleo144-NuttX
```

#### マウント用のwsをホスト側へコピー
docker内にwsは作られるのですが、できればdocker内からホスト側のwsをマウントさせたいです。
マウントするとdocker内のファイルがディレクトリ毎上書きされてしまうので、少々雑ですが以下のようにします。

1. dockerコンテナを作成
2. dockerコンテナ内のwsをホスト側へコピー
3. dockerコンテナを削除  
(4. ホストのwsをマウントしてdockerコンテナを再作成)

```
$ docker run -it --rm --name uros_nuttx_nucleo-144_f767-netnsh uros_nucleo-144_f767-netnsh:dashing
$ # 別ターミナルで以下を実行。コピー完了後、コンテナは終了させる。

$ cd ~/uros_ws
$ docker cp uros_nuttx_nucleo-144_f767-netnsh:/uros_ws nuttx_nucleo-144_f767-netnsh

```
今後は、ホスト側に置いた ws をマウントして使います。

### カーネルの設定

#### コンテナの起動

```
$ docker run -it --rm --privileged -v ~/uros_ws/nuttx_nucleo-144_f767-netnsh:/uros_ws \
  --name uros_nuttx_nucleo-144_f767-netnsh uros_nucleo-144_f767-netnsh:dashing
```
書き込みも行うため、--privilegedを付けています。

#### nucleo-144へポーティング
NuttXは、目的に合わせてコンフィギュレーションを行う必要があります。
通常はNuttX/tools内のスクリプトを利用するようですが、
micro-ROSの場合、micro_ros_setupというツールがあるので、そちらを使います。

```
$ cd /uros_ws
$ ros2 run micro_ros_setup configure_firmware.sh f767-netnsh
$ cd /uros_ws/firmware/NuttX
$ ls .config
.config # NuttX/configs/nucleo-144/f767-netnshからconfigファイルが作られる。
```

この設定ファイルでデバイスの設定や、サンプルプログラムのビルドなどを行います。

ポーティングする際は、[Micro-ROS configuration for NuttX
](https://micro-ros.github.io/docs/tutorials/advanced/nuttx/add_microros_config/)に書かれている項目が必要になります。
今回はNuttXリポジトリもポーティング済みを用意しています。

```
$ cd /uros_ws/firmware/NuttX
$ git remote set-url origin https://github.com/hsgwa/micro-ROS-NuttX.git && git pull origin master
$ cd /uros_ws
$ ros2 run micro_ros_setup configure_firmware.sh f767-netnsh # 設定の反映
```

※ pull の際に`hint: Waiting for your editor to close the file... 293` と出るときは vim をインストールしてから実行してください。

```
$ apt install -y vim
$ git reset --hard HEAD && git pull origin master
```


### サンプルの有効化・ネットワーク設定

次に、静的に設定している IP アドレスの変更と、今回使用するpublisherサンプルの有効化をします。

デフォルトで設定している IP アドレスは以下の通りです。
- agent(ホストPC)  : `192.168.11.2`
- board  : `192.168.11.3 (HEX: 0xC0A80B03)`
- router : `192.168.11.1 (HEX: 0xC0A80B01)`

Macアドレスは`0x00e0deadbeef`です。適宜ルーターの設定を行い、IPを静的に割り振ってください。


```
$ make menuconfig
Application Configuration > Examples > microROS Publisher # サンプルの有効
Application Configuration > Examples > "Hello, World!" example # サンプルの有効

Application Configuration > NSH Library > Networking Configuration > IP Address Configuration > Target IPv4 address
Application Configuration > NSH Library > Networking Configuration > IP Address Configuration > Router IPv4 address
Application Configuration > NSH Library > Networking Configuration > Hardware has no MAC address

```
最後は.configに保存して終了です。

設定が反映されている確認をします。

```
$ cat .config | grep -i -e ipaddr -e agent_ip -e uros_examples_publisher
CONFIG_UROS_EXAMPLES_PUBLISHER=y
CONFIG_NSH_IPADDR=0xC0A80B03
CONFIG_NSH_DRIPADDR=0xC0A80B01
CONFIG_UROS_AGENT_IP="192.168.11.2"
```

このままだと、`configure_firmware.sh`を実行すると設定項目はリセットされてしまいます。
毎回設定し直すのは大変なので、現在の設定を保存しておきます。

```
$ make savedefconfig
$ cp defconfig configs/nucleo-144/f767-netnsh/
```

## ビルド

```
$ cd /uros_ws
$ ros2 run micro_ros_setup build_firmware.sh
~
CP: nuttx.bin

$ ls firmware/NuttX/nuttx.bin
firmware/NuttX/nuttx.bin # これを書き込みます
```

## 書き込み
出来上がったnuttx.binを書き込みます。
mbed対応なので、ストレージにドラッグ＆ドロップで良いのですが、せっかくなのでコマンドラインで書き込みます。

st-flashツールは私の環境ではうまくいかなかったので、openocdを使います。

```
$ openocd -f board/st_nucleo_f7.cfg -c "program firmware/NuttX/nuttx.bin exit 0x08000000"
** Programming Finished **
```

USBの抜き差しをするとホストPC内でのファイルデバイスが代わり、コンテナ内からは認識できなくなります。
一旦コンテナを終了し、再度runし直してください。

※ 公式のやり方としては、用意されている`flash_firmware.sh`を使うみたいです。
[NSH console over UART & USB | micro-ROS](https://micro-ros.github.io/docs/tutorials/advanced/nuttx/nsh/)

## 動作確認
いきなりpub-subでも良いですが、一つずつ動作確認してからpub-sub サンプルを実行します。

### nshへの接続。hello, worldサンプルの実行
ST-LINKが認識されており、nshの接続に使用するデバイスファイルの存在を確認します。

```
$ lsusb # ST-LINK/V.2.1の確認
Bus 003 Device 002: ID 0483:374b STMicroelectronics ST-LINK/V2.1 (Nucleo-F103RB)
$ ls /dev/ttyACM0
/dev/ttyACM0
```

いよいよnshへ接続です。

```
$ sudo minicom -D /dev/ttyACM0
minicom へようこそ 2.7.1

オプション: I18n
コンパイルされた日時は:  Aug 13 2017, 15:25:34.
ポート /dev/ttyACM0, 09:05:30

CTRL-A Z を押すと、説明画面になります。

NuttShell (NSH) # ボード上のリセットボタンを押下
nsh> ? # help
help usage:  help [-v] [<cmd>]

  [         cd        echo      ifconfig  mkrd      pwd       test      wget      
  ?         cp        exec      ifdown    mh        rm        time      xd        
  addroute  cmp       exit      ifup      mount     rmdir     true      
  arp       dirname   false     kill      mv        route     uname     
  basename  dd        free      ls        mw        set       umount    
  break     delroute  help      mb        nslookup  sh        unset     
  cat       df        hexdump   mkdir     ps        sleep     usleep    

Builtin Apps:
  publisher   renew       adc_simple  ping        hello      # 有効にしたサンプルがある
nsh> hello
Hello, World!!
```

### ping による導通確認
ルーターへのping, ホストPCへのpingで導通を確認します。

```
nsh> ping 192.168.11.1
PING 192.168.11.3 56 bytes of data
56 bytes from 192.168.11.3: icmp_seq=0 time=0 ms # router OK
nsh> ping 192.168.11.2
PING 192.168.11.2 56 bytes of data
56 bytes from 192.168.11.2: icmp_seq=0 time=0 ms # host OK
```

## 実行
Nucleo-144から publish し、ホスト側でsubscribeするサンプルを実行します。

エージェントの起動

```
$ docker run -it --rm --net=host --name uros_agent microros/micro-ros-agent:dashing udp4 --port 8888
```

ping_pongデモを別ターミナルでint32_subscriberを起動

```
$ docker run -it --rm --net=host --name uros_demos_1 microros/micro-ros-demos:dashing ros2 run micro_ros_demos_rcl int32_subscriber
```

nshからpublisherを起動

```
$ sudo minicom -D /dev/ttyACM0
nsh> publisher
```

## 実行結果

![host_sub_uc_pub.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/62023/a20984fe-53ed-f4e2-1293-01a1e703d487.gif)

左が agent、中央が ホスト subscriber、右が マイコンpublisher(nsh)です。
左から順に起動し、publisher の起動直後、中央の画面で受信ができています。

ホストsubでは `Failed status on line 37: 2. Continuing.`と出続けていますが、subscribeのタイムアウトが動作しているようです。

subscriberのソースコードのうち、メインルーチンのみ抜粋して載せます。

```
  // micro-ROS-demos/rcl/int32_subscriber/main.c 抜粋
  do {
    RCSOFTCHECK(rcl_wait_set_clear(&wait_set))
    size_t index;
    RCSOFTCHECK(rcl_wait_set_add_subscription(&wait_set, &subscription, &index))
    RCSOFTCHECK(rcl_wait(&wait_set, RCL_MS_TO_NS(1))) // line 37 is here!

    if (wait_set.subscriptions[index]) {
      std_msgs__msg__Int32 msg;

      rcl_ret_t rc = rcl_take(wait_set.subscriptions[index], &msg, NULL, NULL);
      if (RCL_RET_OK == rc) {
        printf("I received: [%i]\n", msg.data);
      }
    }
  } while ( true );
```

2回目にpublisherを起動するとダンプを吐いて死ぬことがあります。
これは、参考2 と同じ現象で、回避策が挙げられています。一旦バージョンを戻してからビルド＆書き込みをしてください。

```
$ cd firmware/mcu_ws/uros/rmw_microxrcedds
$ git reset --hard 34ed379e4a838201e1bfe36325074c1629db2372
```

## 参考

1. [micro-ROS 公式](https://micro-ros.github.io/)
2. [github | micro-ROS_Demo_on_STM32_nucleo-f767zi](https://github.com/maehara-keisuke/micro-ROS_Demo_on_STM32_nucleo-f767zi/blob/master/README.md)
3. [Supported Hardware | micro-ROS](https://micro-ros.github.io//docs/overview/hardware/)