micro-ROSを使って、ROS2の世界とマイコンを繋いでみる

micro-ROSとは

　最近ROS2がだんだん広がってきていますが、ROS2はリアルタイム性が改善しているとかなんとかあって、自動車などの自動運転システムで使用できるようにBOSCHなどの自動車部品メーカーも参画して開発が進められています。
　ROSではマイコンとの通信を実現するためには、mROSというものがあり（https://qiita.com/takasehideki/items/7d783ecd605dcee29ee0）、使いたいところですが、まだ使えていません（すみません）。一方ROS2でマイコンと通信するためにはBOSCHなどが積極的に開発しているmicro-ROS（https://micro-ros.github.io/）があります。ただ、このmicro-ROSですが、現在（2019年12月）はまだCrystalまでの対応となっていますが、今回Dashingで無理やり動かして見ることにチャレンジしてみたいと思います。

準備

ハードウェア

• Ubuntu 18.04のPC
• STM32マイコンボード STM32-E407（https://strawberry-linux.com/catalog/items?code=15075）

ソフトウェア

• ROS2 Dashing

やってみる

　とりあえず、チュートリアルに従って作業を進めていきたいと思います。チュートリアルはこちら（https://github.com/micro-ROS/micro-ros-build/tree/dashing/micro_ros_setup）基本は書いてあるとおりに進めていきたいと思います。
　まず、1/4 Setting up the workspaceから作業を始めますが、早速注意点があります。今回はDashingで試すので、git cloneでDashingブランチを選択します。

source /opt/ros/dashing/setup.bash
sudo apt install python-rosdep
mkdir -p uros_ws/src
cd uros_ws
git clone --recursive -b dashing https://github.com/micro-ROS/micro-ros-build.git src/micro-ros-buildd

続いて、2/4 Building this packageを実行していきます。

colcon build --packages-select micro_ros_setup
source install/local_setup.bash

ここまで特に問題なく進みます。

続いて、3/4 Building the Micro-ROS agentを進めます。

ros2 run micro_ros_setup create_agent_ws.sh  # add agent packages
colcon build
source install/local_setup.sh

ここでも特に問題もなく順調です。
さて続いて4/4 Building the client (aka firmware)に進みます。

ros2 run micro_ros_setup create_firmware_ws.sh
cd firmware/NuttX
tools/configure.sh configs/olimex-stm32-e407/drive_base # FOR EXAMPLE!
cd ../..
ros2 run micro_ros_setup build_firmware.sh

はい、ここでつまづきました。

chip/stm32_usbhost.c:80:52: error: 'TRACE1_NSTRINGS' undeclared here (not in a function)
 static const struct stm32_usbhost_trace_s g_trace1[TRACE1_NSTRINGS] =
                                                    ^~~~~~~~~~~~~~~
chip/stm32_usbhost.c:150:52: error: 'TRACE2_NSTRINGS' undeclared here (not in a function)
 static const struct stm32_usbhost_trace_s g_trace2[TRACE2_NSTRINGS] =
                                                    ^~~~~~~~~~~~~~~
chip/stm32_usbhost.c: In function 'usbhost_trformat1':
chip/stm32_usbhost.c:235:13: warning: implicit declaration of function 'TRACE1_INDEX' [-Wimplicit-function-declaration]
   int ndx = TRACE1_INDEX(id);
             ^~~~~~~~~~~~
chip/stm32_usbhost.c: In function 'usbhost_trformat2':
chip/stm32_usbhost.c:247:13: warning: implicit declaration of function 'TRACE2_INDEX' [-Wimplicit-function-declaration]
   int ndx = TRACE2_INDEX(id);
             ^~~~~~~~~~~~
At top level:
chip/stm32_usbhost.c:150:43: warning: 'g_trace2' defined but not used [-Wunused-variable]
 static const struct stm32_usbhost_trace_s g_trace2[TRACE2_NSTRINGS] =
                                           ^~~~~~~~
chip/stm32_usbhost.c:80:43: warning: 'g_trace1' defined but not used [-Wunused-variable]
 static const struct stm32_usbhost_trace_s g_trace1[TRACE1_NSTRINGS] =
                                           ^~~~~~~~
Makefile:172: recipe for target 'stm32_usbhost.o' failed
make[1]: *** [stm32_usbhost.o] Error 1
make[1]: ディレクトリ '/home/tatsuyai/uros_ws/firmware/NuttX/arch/arm/src' から出ます
tools/LibTargets.mk:134: recipe for target 'arch/arm/src/libarch.a' failed
make: *** [arch/arm/src/libarch.a] Error 2

こちらが発生している原因ですが、stm32_usbhost.hを確認したところ、CONFIG_USBHOSTが定義されていない、つまりUSBHOSTで使用すると設定されていない状態で、stm32_usbhost.cをコンパイルしようとしているためのようです。

ですので、
stm32_usbhost.cのはじめに

#if defined(CONFIG_USBHOST)

を追加して、
最後に

#endif

を追加してください。

その後再度、

ros2 run micro_ros_setup build_firmware.sh

を実行します。

すると問題なくビルドが通ります。
バイナリは、
~/uros_ws/firmware/NuttX
に作成されます。

　とりあえず、チュートリアルに従って作業を進めただけなので、一体これが何者なのかよくわかりません（汗。もうちょっと親切にしていただきたいところですが、デフォルトのdefconfigを見てみたところ、

CONFIG_UROS_EXAMPLES_KOBUKI=y
CONFIG_UROS_EXAMPLES_PUBLISHER=y

という記述があったので、どうやら、KOBUKI用のサンプルと、PUBLISHERのサンプルがビルドされた模様です。

設定を変更して再度ビルドする場合は、

cd ./firmware/NuttX
make distclean
./tools/configure.sh configs/olimex-stm32-e407/drive_base # FOR EXAMPLE!
cd ../..
ros2 run micro_ros_setup build_firmware.sh

をして再度ビルドします。

　昔ならではの組み込み屋からすると、なぜ2つのappがビルドされているのにバイナリーが一個なのかわけがわからないと思いますが、このmicro-ROSはNuttXをベースにしています。NuttXは簡単に言うとUnixライクなターミナルを備えたRTOSなので、外からNSHと呼ばれるシェル環境に入ってアプリを実行することができるのです！

では、ボードに書き込みましょう。

cd firmware/NuttX
scripts/flash.sh olimex-stm32-e407

はい、書き込めません。。。
どうやら、このスクリプトはUSB経由で書き込めないようですので、
scripts/flash.shの中身を確認したところ、interfaceに/usr/share/openocd/interface/ftdi/olimex-arm-usb-ocd-h.cfgを使用しているためだめなようです。
もしチュートリアルどおりにすすめるならこちらを購入する必要があります。
https://strawberry-linux.com/catalog/items?code=15066

今回は手元にJTAG20Pinの書き込みツールがなかったので、DFUモードで書き込みできないか試してみます。
　まずBOOT0ピンをH、BOOT1ピンをLになるようにジャンパーピンをセットします。そして電源もUSB OTG1から供給されるようにしてUSB接続しましょう。

lsusb
Bus 001 Device 005: ID 0483:df11 STMicroelectronics STM Device in DFU Mode

と出てこればOKです。

DFUで書き込むために、dfu-utilをインストールします。

sudo apt install dfu-util

それでは書き込んでいきましょう。

dfu-util -a0 -d 0x0483:0xdf11 -s 0x08000000 -D ./nuttx.bin

書き込めたら、BOOTモードをもとに戻して、電源を入れ直します。
nshにアクセスするために、picocomを入れます。

sudo apt install picocom
lsusb
Bus 001 Device 007: ID 0525:a4a7 Netchip Technology, Inc. Linux-USB Serial Gadget (CDC ACM mode)

正しく認識されました。
それでは、nshにアクセスしてみましょう。

sudo picocom --b 115200 /dev/ttyACM0

接続完了後にEnterを何回か押すと、

NuttShell (NSH)
nsh>help
help usage:  help [-v] [<cmd>]

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

Builtin Apps:
  subscriber  myapp       renew       kobuki      cu          
  chat        publisher   ping        serialrx    
nsh>

ちゃんとアプリを認識していますね。
続いてIPアドレスを確認します。

nsh>mount -t procfs /proc
nsh>ifconfig
eth0    Link encap:Ethernet HWaddr 00:e0:de:ad:be:ef at UP
    inet addr:192.168.8.247 DRaddr:192.168.8.1 Mask:255.255.255.0

だそうです。IPアドレスが適当に割り振られていそうなので、手動で割り振り直します。

nsh>ifconfig eth0 192.168.23.100
nsh>ifconfig
eth0    Link encap:Ethernet HWaddr 00:e0:de:ad:be:ef at UP
    inet addr:192.168.23.100 DRaddr:192.168.23.1 Mask:255.255.255.0

さて、それでは実行してみましょう。

nsh>publisher
UDP mode => ip: 127.0.0.1 - port: 8888

>>> [rcutils|error_handling.c:106] rcutils_set_error_state()
This error state is being overwritten:

  'failed to create node session on Micro ROS Agent., at /home/tatsuyai/uros_ws/firmware/mcu_ws/uros/rmw_microxrcedds/rmw_microxrcedds_c/src/rmw_node.c:216, at /home/tatsuyai/uros_ws/firmware/mcu_ws/ros2/rcl/rcl/src/rcl/node.c:326'

with this new error message:

  'rcl node's rmw handle is invalid, at /home/tatsuyai/uros_ws/firmware/mcu_ws/ros2/rcl/rcl/src/rcl/node.c:464'

rcutils_reset_error() should be called after error handling to avoid this.
<<<
[ERROR] [rcl]: Failed to fini publisher for node: 1
Node initialization error: rcl node's rmw handle is invalid, at /home/tatsuyai/uros_ws/firmware/mcu_ws/ros2/rcl/rcl/src/rcl/node.c:464

はい、だめです。全然だめです。
ここで2つほどやることがあります。
1つめは、そもそもmicro-ROSはPCにagentを立ててあげないと、動作することができません。
ですので、新しいターミナルを開いて、agentを立ち上げます。

cd ~/uros_ws/install/micro_ros_agent/lib/micro_ros_agent
cd ./micro_ros_agent udp --port 8888

それと同時にもう一つ、micro-ROSがアクセスしようとしているのが、127.0.0.1になっています。これではだめですので、

cd ~/uros_ws/firmware/mcu_ws/build/rmw_microxrcedds/include/rmw_microxrcedds_c

でここにある、config.hのUDP_IPをPCのIPにします。

config.h

#ifndef RMW_MICROXRCEDDS_CONFIG_H
#define RMW_MICROXRCEDDS_CONFIG_H

#include <uxr/client/config.h>

#define MICRO_XRCEDDS_UDP
/* #undef MICRO_XRCEDDS_SERIAL */
/* #undef MICRO_XRCEDDS_USE_REFS */
#define MICRO_XRCEDDS_USE_XML

#ifdef MICRO_XRCEDDS_UDP
    #define UDP_IP "192.168.23.68"
    #define UDP_PORT 8888
    #define MAX_TRANSPORT_MTU UXR_CONFIG_UDP_TRANSPORT_MTU
#elif defined(MICRO_XRCEDDS_SERIAL)
    #define SERIAL_DEVICE "/dev/ttyS0"
    #define MAX_TRANSPORT_MTU UXR_CONFIG_SERIAL_TRANSPORT_MTU
#endif

以下略

編集したら再度ビルドして、DFUモードで書き込みます。

nsh>publisher
...
...
..

Sent: '993'
Sent: '994'
Sent: '995'
Sent: '996'
Sent: '997'
Sent: '998'
Sent: '999'
TOTAL sent: 1000
<<<
nsh>

うまく行ったようです。

ただ1000回しか送らないので、確認するのが辛いですが、送信中に

ros2 topic echo /std_msgs_msg_Int32
...
...
...
data: 993
---
data: 994
---
data: 995
---
data: 996
---
data: 997
---
data: 998
---
data: 999
---

という感じに、ちゃんとPublishできています。

まとめ

　いろいろ苦労はしましたが、ROS2 Dashingとmicro-ROSでマイコンからROS2の世界と通信することができました。これを応用すれば、色々な用途に使えるでしょう。（ただHOST PCが必要なのが玉に瑕ですが。。。誰かHOST PC無しで動かす方法知りませんか？）HOST PCなしという意味ではmROSにも期待したいところです。