mROSを仮想環境上で動作させる！

#概要
mROSは組込みデバイス向けのROSフレームワークとして開発されたものです．mROSがないと，組込みデバイス上で動作するアプリケーションと汎用OS上で動作するROSノード間の通信は面倒です．(自前でプロキシROSノードを作成しなければなりません)

mROSは以前から使ってみたいと思っていましたが，実機の準備が難しく，なかなか手を出せないでいました．今回，mROSを仮想環境上で動作させることができましたので，その手順を紹介します．

※mROSについて詳しく知りたい方は，本家サイトを参照ください．
※https://qiita.com/takasehideki/items/7d783ecd605dcee29ee0
※https://qiita.com/m_ksg/items/0060a8738e6a34a0f237

#仮想環境
仮想環境としてはTOPPERS/athrillを使用しました．


#開発環境
mROSおよびathrillのビルド環境は以下の通りです．

* OS
 * Windows 10
* ビルド/実行環境
 * WSL
* エディタ
 * vscode
* コンパイラ
 * v850-elf-gcc
* 仮想環境
 * athrill
 
上記環境の構築手順は，以下で詳しく説明されています．

https://qiita.com/Maeda-Shoota/items/4b1be75a6c4993522ab1


#実行環境

* ROSバージョン
 * kinetic
* ubuntu
 * Windows10/WSL
* X window
 * Xmingを使用しています．以下が参考になります．
 * https://www.n-novice.com/entry/2018/02/09/012854

#ソース配置場所
mROSを利用するには，以下のライブラリ/OSが必要です．

* TCP/IP通信スタック
 * mbed
* OS
 * asp

mROSおよび上記を仮想環境上でビルド/実行できるようにしたソースを以下で公開しています．

* mROS
 * https://github.com/tmori/mROS_athrill
* mbed/OS
 * https://github.com/tmori/asp-athrill-mbed
  * aspは「asp-athrill」フォルダに配置されています．
  * mbedは「mbed-lib」フォルダに配置されています．

#gccライブラリ
ROSおよびmROSはC++が前提となりますので，動的メモリ用のライブラリ(malloc/free)が必要となります．
一方，gccのライブラリは汎用OSを前提とした実装になっていますので，組み込み向けではありません．
そのため，gccのライブラリから動的メモリ用ライブラリのオブジェクトファイル群を削除し，
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 \
             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
```

#フォルダ構成
mROS および mbed/OSのソースを以下の構成でチェックアウトします．

```
.
├── asp-athrill-mbed
└── mROS_athrill
    ├── app
    ├── design
    ├── mbed_build
    ├── mros_build
    ├── mros-lib
    ├── README.md
    ├── sample_build
    └── sample_src
```

#ビルド手順

ビルド順番は以下の通りです．

1. mbed
* mros
* OSおよびサンプルプログラム

##mbedのビルド
mbedのビルド方法は以下の通りです．

```
$ cd mROS_athrill/mbed_build
$ make clean
$ make
:
v850-elf-ar -rcv  libmbed.a EthernetInterface.o Endpoint.o Socket.o TCPSocketConnection.o TCPSocketServer.o lwip-athrill.o
a - EthernetInterface.o
a - Endpoint.o
a - Socket.o
a - TCPSocketConnection.o
a - TCPSocketServer.o
a - lwip-athrill.o
```

##mROSのビルド
mROSのビルド方法は以下の通りです．

```
$ cd mROS_athrill/mros_build
$ make clean
$ make
:
v850-elf-ar -rcv  libmros.a xmlparser.o tcp_ros.o xmlcall.o ros.o mros.o
a - xmlparser.o
a - tcp_ros.o
a - xmlcall.o
a - ros.o
a - mros.o
```

##OSおよびサンプルプログラム
OSおよびサンプルプログラムのビルド方法は以下の通りです．

```
$ cd mROS_athrill/app
$ make clean
$ make
:
v850-elf-gcc -c  -mv850e2v3 -mdisable-callt -g  -Wall -O2  -DLABEL_ASM  -I. -I../../asp-athrill-mbed/asp-athrill/include -I../../asp-athrill-mbed/asp-athrill/arch -I../../asp-athrill-mbed/asp-athrill  -I../../asp-athrill-mbed/asp-athrill/target/v850_gcc -I../../asp-athrill-mbed/asp-athrill/arch/v850_gcc -DALLFUNC  -I../../asp-athrill-mbed/asp-athrill/kernel ../../asp-athrill-mbed/asp-athrill/kernel/exception.c
rm -f libkernel.a
v850-elf-ar -rcs libkernel.a  target_support.o v850es_fk3.o prc_support.o prc_sil.o  target_config.o target_timer.o target_serial.o prc_config.o startup.o task.o wait.o time_event.o task_manage.o task_refer.o task_sync.o task_except.o semaphore.o eventflag.o dataqueue.o pridataq.o mailbox.o mempfix.o time_manage.o cyclic.o alarm.o sys_manage.o interrupt.o exception.o
v850-elf-ranlib libkernel.a
v850-elf-gcc -c  -mv850e2v3 -mdisable-callt -g  -Wall -O2  -DLABEL_ASM  -I. -I../../asp-athrill-mbed/asp-athrill/include -I../../asp-athrill-mbed/asp-athrill/arch -I../../asp-athrill-mbed/asp-athrill  -I../../asp-athrill-mbed/asp-athrill/target/v850_gcc -I../../asp-athrill-mbed/asp-athrill/arch/v850_gcc  ../../asp-athrill-mbed/asp-athrill/syssvc/banner.c
v850-elf-g++  -mv850e2v3 -mdisable-callt -g  -Wall -O2  -DLABEL_ASM  -I. -I../../asp-athrill-mbed/asp-athrill/include -I../../asp-athrill-mbed/asp-athrill/arch -I../../asp-athrill-mbed/asp-athrill  -I../../asp-athrill-mbed/asp-athrill/target/v850_gcc -I../../asp-athrill-mbed/asp-athrill/arch/v850_gcc -nostartfiles -lgcc -lc   -Wl,-Map=v850es_fk3.map  -T ../../asp-athrill-mbed/asp-athrill/arch/v850_gcc/v850es_fk3.ld -o asp start.o \
                  log_output.o vasyslog.o t_perror.o strerror.o app.o    banner.o syslog.o serial.o logtask.o  kernel_cfg.o  kernel_cfg_asm.o  -Wl,--gc-sections -nostartfiles -L../mros_build -L../mbed_build -lmros -lmbed -lnosys  libkernel.a
v850-elf-nm -n asp > asp.syms
v850-elf-objcopy -O srec -S asp asp.srec
../../asp-athrill-mbed/asp-athrill/cfg/cfg/cfg --pass 3 --kernel asp -I. -I../../asp-athrill-mbed/asp-athrill/include -I../../asp-athrill-mbed/asp-athrill/arch -I../../asp-athrill-mbed/asp-athrill  -I../../asp-athrill-mbed/asp-athrill/target/v850_gcc -I../../asp-athrill-mbed/asp-athrill/arch/v850_gcc \
                        --rom-image asp.srec --symbol-table asp.syms \
                        -T ../../asp-athrill-mbed/asp-athrill/target/v850_gcc/target_check.tf --api-table ../../asp-athrill-mbed/asp-athrill/kernel/kernel_api.csv --cfg1-def-table ../../asp-athrill-mbed/asp-athrill/kernel/kernel_def.csv --cfg1-def-table ../../asp-athrill-mbed/asp-athrill/arch/v850_gcc/prc_def.csv  app.cfg
check complete
```



#デモ
##サンプルプログラム
仮想環境上で動作するサンプルプログラムは，mROS_athrill/app/app.cpp です．
本サンプルでは，以下のROSトピックを配信・購読します．

* mros_nodeノード
 * 配信トピック：mros_msg
* mros_node2ノード
 * 購読トピック：test_string 


##デモ実行手順
サンプルプログラムの動作確認手順は以下の通りです．

1. ROSマスタ起動
* ROSトピック配信
* athrillでmROSサンプルプログラム起動
* athrillでmROSサンプルプログラム実行
* PC側のROSトピック(/mros_msg) 購読実行結果
* rqt_graph 実行結果

###ROSマスタ起動
まずは，WSL上でROSマスタを起動します．

```
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トピック配信
mROSを起動する前に，まずはmROS上で購読するROSトピックを配信します．

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

###athrillでmROSサンプルプログラム起動
仮想環境上(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サンプルプログラム実行
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) 購読実行結果
mROSから配信されているトピックをWSL上で以下のように購読できています！

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

###rqt_graph 実行結果
さらに，rqt_graphでROSトピックおよびROSノードの配信・購読関係がグラフ化されています！

![image.png](https://qiita-image-store.s3.amazonaws.com/0/244147/d04abe3e-6382-5e63-e3ec-50f0711e4c7d.png)