mROSをathrillに移植するまで

#概要
今，話題の[mROS](https://qiita.com/m_ksg/items/0060a8738e6a34a0f237)を仮想環境(athrill)上で動作させたいと考えています．

#ゴール設定
最終ゴールは，
　・[1] 既存のmROS実装をathrill向けに移植し，
　・[2] サンプル動作確認(既存のROSノードと結合)させる
ことです．

ですが，上記ゴールに良く着くまでに様々な壁があるので，サブゴールを以下のように設定します．

##[1] mROS移植のサブゴール

1. mROSのmbed依存インタフェースを特定する**【完了:2018/12/22】**
* athrillをC++対応する**【完了:2018/12/23】**
  1. athrill用コンパイラ(g++)を準備する**【完了:2018/12/23】**
  * 組込み向けC++のライブラリをそろえる(libstdc++等)**【完了:2018/12/23】**
  * 簡単なサンプルアプリでC++ライブラリの動作確認する**【完了:2018/12/23】**
* athrill向けのmbedデバイスを設計・実装する**【完了:2018/12/20】**
* athrill向けのmbed実装を作る**【完了:2018/12/24】**
* mROSのビルド環境を作る**【完了：2018/12/27】**

##[2] サンプル動作確認のサブゴール

1. aspのビルド環境を作る**【完了：2018/12/27】**
* サンプルプログラムを作る**【完了：2018/12/27】**
* サンプル動作確認する**【完了：2018/12/30】**

#進捗状況

なんとか，年越し前に全ゴール達成しました！

* [1] mROS移植のサブゴール
  * 進捗率 100%
* [2] サンプル動作確認のサブゴール
  * 進捗率 100%(subscribe/publishできました！)

以下，進捗状況詳細


##mROSのmbed依存APIを特定する
ゴール達成．
以下が，依存インターフェースであることが判明．
　　- Ethernet Interface
　　- TCPSocketConnection
　　- TCPSocketServer
　　- std::vector
　　- std::string
　　- std::stringstream
　　- std::cin

##athrillをC++対応する
###athrill用コンパイラ(g++)を準備する
まだ公開できていませんが，準備はできました．
[gcc作成時](https://qiita.com/kanetugu2018/items/b3918e3dc001b78da4b0)に，以下のようにc++を追加するだけでした．

```
--enable-languages=c,c++
```

###組込み向けC++のライブラリをそろえる(libstdc++等)
newlibをビルドすると，C++向けのライブラリとして libstdc++.a ができます．

C++固有の処理はこのライブラリで実現されており，以下のコードをビルド＆リンクしました．

```
TestClass *obj = new TestClass();
```

リンクした結果，幾つか問題が発生しました．

* 以下の警告が大量に出力される
  * warning: could not locate special linker symbol __gp
  * 原因はまだよくわかっていない…とりあえず，リンクは通ったので調査優先度は低設定にしておきます．
* 動的メモリ獲得(malloc)が動作しない
  * mallocはlibc.aで実装されているのですが，このmallocは汎用OS向けなのでうまく動作しません．
  * 対処方法として，動的メモリ獲得周りのライブラリファイルを libc.a から削除し，
  * athrill向けの動的メモリ獲得ライブラリと差し替える方向で検討しています．
  * 削除対象とするライブラリファイルは以下の通りです．

```
ar -d libc.a lib_a-malloc.o lib_a-mallocr.o lib_a-freer.o lib_a-signal.o  
             lib_a-realloc.o lib_a-reallocr.o lib_a-calloc.o lib_a-callocr.o
```

###簡単なサンプルアプリでC++ライブラリの動作確認する
ゴール達成．
サンプル動作確認済み．以下にサンプルプログラムを公開済み．

https://github.com/tmori/athrill/blob/master/sample/baremetal_c%2B%2B/step1/main.cpp

##athrill向けのmbedデバイスを設計・実装する
ゴール達成．
mbed向けの通信API実装を，athrillとして提供しました．
以下で，APIを公開済み．

https://github.com/tmori/athrill/blob/master/trunk/apl/include/athrill_syscall.h

##athrill向けのmbed実装を作る
ゴール達成．
以下で，mbded実装を公開しました(サンプル動作確認済み)．

https://github.com/tmori/asp-athrill-mbed

以下が移植したソース配置場所です．

https://github.com/tmori/asp-athrill-mbed/tree/master/mbed-lib


##mROSのビルド環境を作る
以下で公開しています．

https://github.com/tmori/mROS_athrill

以下の順番でフォルダ移動し，make clean;make を実行します．

①mbed_build
②mros_build
③app

##aspのビルド環境を作る

上記の③でaspのビルドがかかります．
ただ，実行順番が若干まずいです．
mrosは，kernel_cfg.hをインクルードするのですが，
kernel_cfg.hはaspをビルドしないといけない．

なので，一旦，appでカーネルビルドして，そこでできたkernel_cfg.hを
mros側にコピーしてmrosをビルドしています・・・．

##サンプルプログラムを作る

とりあえず，単純な pub/sub だけをするようなプログラムにしました．

https://github.com/tmori/mROS_athrill/tree/master/app

ここで新たな問題が発生しました(リンクエラーです)．

```
/usr/local/athrill-gcc/lib/gcc/v850-elf/4.9-GNUV850_v14.01/../../../../v850-elf/lib/libc.a(lib_a-exit.o): In function `_exit':
exit.c:(.text+0x14): relocation truncated to fit: R_V810_GPWLO_1 against symbol `__global_impure_ptr' defined in .rosdata section in /usr/local/athrill-gcc/lib/gcc/v850-elf/4.9-GNUV850_v14.01/../../../../v850-elf/lib/libc.a(lib_a-impure.o)
/usr/local/athrill-gcc/lib/gcc/v850-elf/4.9-GNUV850_v14.01/../../../../v850-elf/lib/libc.a(lib_a-fflush.o): In function `_fflush':
fflush.c:(.text+0x1fc): relocation truncated to fit: R_V810_GPWLO_1 against symbol `__global_impure_ptr' defined in .rosdata section in /usr/local/athrill-gcc/lib/gcc/v850-elf/4.9-GNUV850_v14.01/../../../../v850-elf/lib/libc.a(lib_a-impure.o)
/usr/local/athrill-gcc/lib/gcc/v850-elf/4.9-GNUV850_v14.01/../../../../v850-elf/lib/libc.a(lib_a-findfp.o): In function `__cleanup':
findfp.c:(.text+0x72): relocation truncated to fit: R_V810_GPWLO_1 against symbol `__global_impure_ptr' defined in .rosdata section in /usr/local/athrill-gcc/lib/gcc/v850-elf/4.9-GNUV850_v14.01/../../../../v850-elf/lib/libc.a(lib_a-impure.o)
/usr/local/athrill-gcc/lib/gcc/v850-elf/4.9-GNUV850_v14.01/../../../../v850-elf/lib/libc.a(lib_a-findfp.o): In function `___sfp':
findfp.c:(.text+0x1fa): relocation truncated to fit: R_V810_GPWLO_1 against symbol `__global_impure_ptr' defined in .rosdata section in /usr/local/athrill-gcc/lib/gcc/v850-elf/4.9-GNUV850_v14.01/../../../../v850-elf/lib/libc.a(lib_a-impure.o)
/usr/local/athrill-gcc/lib/gcc/v850-elf/4.9-GNUV850_v14.01/../../../../v850-elf/lib/libc.a(lib_a-refill.o): In function `___srefill_r':
refill.c:(.text+0x12e): relocation truncated to fit: R_V810_GPWLO_1 against symbol `__global_impure_ptr' defined in .rosdata section in /usr/local/athrill-gcc/lib/gcc/v850-elf/4.9-GNUV850_v14.01/../../../../v850-elf/lib/libc.a(lib_a-impure.o)
/usr/local/athrill-gcc/lib/gcc/v850-elf/4.9-GNUV850_v14.01/../../../../v850-elf/lib/libc.a(lib_a-__atexit.o): In function `___register_exitproc':
__atexit.c:(.text+0xa): relocation truncated to fit: R_V810_GPWLO_1 against symbol `__global_impure_ptr' defined in .rosdata section in /usr/local/athrill-gcc/lib/gcc/v850-elf/4.9-GNUV850_v14.01/../../../../v850-elf/lib/libc.a(lib_a-impure.o)
/usr/local/athrill-gcc/lib/gcc/v850-elf/4.9-GNUV850_v14.01/../../../../v850-elf/lib/libc.a(lib_a-__call_atexit.o): In function `___call_exitprocs':
__call_atexit.c:(.text+0xe): relocation truncated to fit: R_V810_GPWLO_1 against symbol `__global_impure_ptr' defined in .rosdata section in /usr/local/athrill-gcc/lib/gcc/v850-elf/4.9-GNUV850_v14.01/../../../../v850-elf/lib/libc.a(lib_a-impure.o)
collect2: error: ld returned 1 exit status
Makefile:336: recipe for target 'asp' failed
make: *** [asp] Error 1
```

色々調べましたが，メモリ配置が規定サイズを超えたときに出力されるようです．
ライブラリ内(libc.a)で問題の変数があるようで，外部から手の出しようがない．

一方，組み込み向けには exit とか fflushとか不要なので，発想を変えて呼び出し元のライブラリ群を削除することにしました．

```
ar -d libc.a  lib_a-exit.o lib_a-__atexit.o lib_a-__call_atexit.o lib_a-fflush.o lib_a-findfp.o lib_a-refill.o
```

それで，呼ばれることのない(と思われる)関数たちをスタブ関数として定義してリンクを通しました．
まだこわくて動作確認していません．

気力が戻ったら動作確認をやってみます！

* 2018/12/29
  * いくつか問題があったので，以下を修正．
     * 大容量メモリ消費変数を bss_noclr に配置(起動時間が恐ろしく遅かったもので・・)
     * stringstream の数値⇒文字列変換がなぜか動作しなかったので sprintfで応急処置
     * mROS側の問題？と思われる個所をいくつか修正
     * IPアドレスはローカルでやっているので，"0.0.0.0"に変更
  * 上記修正を実施したところ，mROSおよび上位アプリの起動成功しました(ROSはkineticを使っています)．
  * mROS起動後，ROSコマンドを実行した結果は以下の通り．
    * ROSマスタがROSノードを認識したらしい(mros_node, mros_node2)
    * ROSトピックも認識したらしい(/mros_msg, /test_string)
    * しかぁし，mROS内の内部ログでエラーログが出ているので，その解析を実施中…．

```
$ rosnode list
/mros_node
/mros_node2
/rosout
```

```
$ rostopic list
/mros_msg
/rosout
/rosout_agg
/test_string
```
* 2018/12/29　16:49
  * とりあえず，subscribe 動きました！
  * 以下の修正とオペレーションを追加で実施
     * 不具合修正：mROS内の符号拡張問題の類似修正を実施
     * 機能拡張：mROS内のget_port2のXMLタグ判定をKinetic向けに拡張
     * OP追加：ROSトピック配信していなかったので mROS内でsubscribeできなかったことが判明(配信すると成功した)

* 2018/12/30　8:22

* ROSマスタ起動
  * publish 動きました！
  * 以下の修正とオペレーションを追加で実施
     * EthernetInterface修正：IPアドレスを外部から設定できるようにした
     * 機能拡張：mROS内のpub_gen_img_msgのヘッダをKinetic向けに拡張
     * 機能拡張：mROS内のtest_requestResponse/req_topic_nameのXMLタグ判定をKinetic向けに拡張
     * 以下，テスト実施結果です．

```
roscore
... logging to /home/chagall/.ros/log/38eb392c-0bc1-11e9-a7ce-54ee75b43c96/roslaunch-Chagall-15806.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://Chagall:53567/
ros_comm version 1.12.14


SUMMARY
========

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.14

NODES

auto-starting new master
process[master]: started with pid [15816]
ROS_MASTER_URI=http://Chagall:11311/

setting /run_id to 38eb392c-0bc1-11e9-a7ce-54ee75b43c96
process[rosout-1]: started with pid [15829]
started core service [/rosout]
```

* ROSトピック配信

```
$ rostopic pub -r 1 test_string std_msgs/String "This is a test data!"
```

* athrillでmROSサンプルプログラム起動

```
$ athrill-run
OK: found device_config.txt
OK: found memory.txt
OK: found asp
cfg1_out
core id num=1
ROM : START=0x0 SIZE=512
RAM : START=0x6ff7000 SIZE=8192
RAM : START=0xdead0000 SIZE=1
MALLOC : START=0x10000000 SIZE=20480
Elf loading was succeeded:0x0 - 0x6c558 : 433.344 KB
Elf loading was succeeded:0x6c558 - 0x6fee1 : 1.404 KB
ELF SYMBOL SECTION LOADED:index=1380
ELF SYMBOL SECTION LOADED:sym_num=4196
ELF STRING TABLE SECTION LOADED:index=1381
Not supported:unknown typeref(*) debug_offset=0x350f
　：
Not supported:unknown typeref(*) debug_offset=0x34886
athrill_device_func_call=0xdead0000
[DBG>[NEXT> pc=0x0 kernel_cfg_asm.S 24
```

* athrillでmROSサンプルプログラム実行

```
[DBG>[NEXT> pc=0x0 kernel_cfg_asm.S 24
c
[CPU>System logging task is started on port 1.
5 messages are lost.
========Activate mROS PUBLISH========
========Activate mROS SUBSCRIBE========
========Activate mROS XML-RPC Slave========
========Activate mROS XML-RPC Master========
XML_MAS_TASK: enter loop
XML_SLV_TASK: Listen
**********mROS Main task finish**********
========Activate user task1========
usr task ID [7]
Change state [2]
pub connect ip=0.0.0.0 port=11311
lwip_connect connect ip=0 port=11311
XML_MAS_TASK: Sleep SUB_TASK
XML_MAS_TASK: regiester Publisher ID:[1]
HTTP/1.0 200 OK
Server: BaseHTTP/0.3 Python/2.7.12
Date: Sat, 29 Dec 2018 23:28:42 GMT
Content-type: text/xml
Content-length: 326

<?xml version='1.0'?>
<methodResponse>
<params>
<param>
<value><array><data>
<value><int>1</int></value>
<value><string>Subscribed to [/test_string]</string></value>
<value><array><data>
<value><string>http://Chagall:53593/</string></value>
</data></array></value>
</data></array></value>
</param>
</params>
</methodResponse>

response = HTTP/1.0 200 OK
Server: BaseHTTP/0.3 Python/2.7.12
Date: Sat, 29 Dec 2018 23:28:42 GMT
Content-type: text/xml
Content-length: 326

<?xml version='1.0'?>
<methodResponse>
<params>
<param>
<value><array><data>
<value><int>1</int></value>
<value><string>Subscribed to [/test_string]</string></value>
<value><array><data>
<value><string>http://Chagall:53593/</string></value>
</data></array></value>
</data></array></value>
</param>
</params>
</methodResponse>

XML_MAS_TASK: enter loop
PUB_TASK:publisher initialization Node ID[1]
wake up user task [7]
Change state [3]
Change state [3]

Data Publish Start
========Activate user task2========
usr task ID [8]
Change state [1]
sub connect ip=0.0.0.0 port=11311
lwip_connect connect ip=0 port=11311
sus user task [7]
sus user task [8]
XML_MAS_TASK: regiester Subscriber ID:[2]
XML_MAS_TASK: ip[0.0.0.0],port[53593]
XML_MAS_TASK: enter loop
SUB_TASK: subscriber initialization　node ID:[2] index:[0]
SUB_TASK:IP [0.0.0.0][127.0.0.1]
XML_MAS_TASK: send request topic
XML_MAS_TASK: node num [1]
XML_MAS_TASK: request node [ID:2, topic:/test_string]
XML_MAS_TASK: ip[0.0.0.0],port[53593]
lwip_connect connect ip=0 port=53593
COMPLETE SEND REQUEST [601]
XML_MAS_TASK: 512 response=HTTP/1.0 200 OK
Server: BaseHTTP/0.3 Python/2.7.12
Date: Sat, 29 Dec 2018 23:28:44 GMT
Content-type: text/xml
Content-length: 377

<?xml version='1.0'?>
<methodResponse>
<params>
<param>
<value><array><data>
<value><int>1</int></value>
<value><string>ready on Chagall:53594</string></value>
<value><array><data>
<value><string>TCPROS</string></value>
<value><string>Chagall</string></value>
<value><int>53594</int></value>
</data></array></value>
</data></array></value>
</param>
</params>
</methodResponse
2 0 2 0
sdata=131074
XML_MAS_TASK: enter loop
SUB_TASK:port [53594]
sub_gen_header:len=134
connect:ip=0.0.0.0 port=53594
sub_task: size=134 send_buf=
lwip_connect connect ip=0 port=53594
SUB_TASK: TCPROS HEADER CONNECT
sub_task: send_buf=
SUB_TASK: SEND TCPROS HEADER
sub_task: rcv_buf=
SUB_TASK: subscriber connected
wake up user task [7]
wake up user task [8]
Change state [3]
SUB_TASK:data length [56]
I heard [This is a test data!]
SUB_TASK:data length [28]
I heard [This is a test data!]
SUB_TASK:data length [28]
I heard [This is a test data!]
SUB_TASK:data length [28]
I heard [This is a test data!]
SUB_TASK:data length [28]
I heard [This is a test data!]
SUB_TASK:data length [28]
I heard [This is a test data!]
SUB_TASK:data length [28]
I heard [This is a test data!]
SUB_TASK:data length [28]
I heard [This is a test data!]
SUB_TASK:data length [28]
I heard [publish test data(0)]
PUB_TASK: PUBLISHING ERROR ! [-1]
SUB_TASK:data length [28]
I heard [This is a test data!]
SUB_TASK:data length [28]
I heard [This is a test data!]
SUB_TASK:data length [28]
I heard [This is a test data!]
SUB_TASK:data length [28]
I heard [This is a test data!]
XML_SLV_TASK: ACCEPT err [0]
XML_SLV_TASK: Connected
XML_SLV_TASK: methodName [requestTopic]
sus user task [7]
sus user task [8]
Change state [2]
XML_SLV_TASK: request topic [callerid=/mros_node]
Change state [4]
XML_SLV_TASK: Listen
PUB_TASK:request Topic node[1]
PUB_TASK:Listening
PUB_TASK:Connected
PUB_TASK: TCPROS connection received
PUB_TASK: publisher connected
wake up user task [7]
wake up user task [8]
Change state [3]
SUB_TASK:data length [28]
I heard [This is a test data!]
SUB_TASK:data length [28]
I heard [This is a test data!]
SUB_TASK:data length [28]
I heard [This is a test data!]
SUB_TASK:data length [28]
 ：
```

* PC側のROSトピック(/mros_msg) 購読実行結果

```
$ rostopic echo /mros_msg
data: "publish test data(1)"
---
data: "publish test data(2)"
---
data: "publish test data(3)"
---
```

* rqt_graph 実行結果

![image.png](https://qiita-image-store.s3.amazonaws.com/0/244147/b2005d58-4fb4-f78a-dbf0-5d26df81e1a3.png)