TOPPERS箱庭/ROS対応版・単体ロボット向けシミュレータ

#概要
TOPPERSプロジェクトで公開されている[単体ロボット向けシミュレータ](https://toppers.github.io/hakoniwa/prototypes/single-robot/)をROS対応することに成功しましたので，その概要とインストール手順をご紹介します！また，本内容は，[SWEST22の『IoT/自動運転システム時代のシミュレーション環境『箱庭』について語り合おう』](https://swest.toppers.jp/SWEST22/program/s1b.html#s1) でもデモ予定ですので，興味のある方はぜひ参加ください！

#うれしさ
単体ロボット向けシミュレータは，１個のロボットをマイコンシミュレータとUnityを連携させたシミュレーション環境です．これを ROS 対応するとどうなるか？

- うれしさ１
  - EV3RTのロボット制御のセンシングデータをROSトピックとして配信できます．
- うれしさ２
  - ROSツール群と連携できるようになります．
- うれしさ３
  - ROSトピックを通して，ロボット間の連携が可能になります．

#どうやって実現したか
単体ロボット向けシミュレータのROS化するために，箱庭アセットの１つである [mROS](https://qiita.com/takasehideki/items/7d783ecd605dcee29ee0)を利用させて頂きました．

mROSはマイコン上で動作するプログラムから ROS 機能を利用できるようにしてくれる重要な箱庭要素技術になっており，今回の単体ロボット向けシミュレータをROS化する上で欠かせないものです．

##スタック構成
スタック構成は，こんな感じで，EV3RT とおなじ並びで mROSを配置しました．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/0c69ab52-a869-f74f-4d8a-86a3a816cab4.png)

単体ロボット向けシミュレータは，V850版とARM版がありますが，今回は ARM版で対応しました．
(V850版はご要望あれば別途対応検討します)

#うれしさ１(センシングデータROSトピック化)
単体ロボット向けシミュレータのデモとして，ライントレースされているものがありますが，この場合ですと，EV3RT APIで取得したカラーセンサのセンシング情報を ROSトピックで配信できると便利です．リアルタイムにシミュレーション中のセンシングデータを観測できるようになります．また，ROSトピックで配信できるデータは何でもありなので，制御用変数等のデータもあわせて配信できます．

以下，トピック配信のアプリケーション実装例です．

```C
/*
 * トピック配信用のバッファ(今回は文字列データを使用)
 */
static char str_buf[1024];

/*
 * ROSトピック配信用の共通関数部分
 */
static void topic_publish(ros::Publisher &pub, int value)
{
	std_msgs::String str;
	sprintf(str_buf, "v:%d", value);
	str.data = string(str_buf);
	pub.publish(str);
	return;
}

/*
 * EV3RT API で取得したセンシングデータを ROSトピック配信する専用タスク
 */
void usr_task2(intptr_t unused)
{
	syslog(LOG_NOTICE,"========Activate user task2========");
	int argc = 0;
	char *argv = NULL;

	/*
	 * ROSノード登録
	 */
	ros::init(argc,argv,"robo_sensor");
	ros::NodeHandle n;
	ros::Rate loop_rate(10); //100msec

	/*
	 * ROS出版トピック登録
	 */
	ros::Publisher pub1 = n.advertise<std_msgs::String>("robo_sensor", 1);
	ros::Publisher pub2 = n.advertise<std_msgs::String>("robo_color", 1);

	while (1) {
		/*
		 * カラーセンサ値をROSトピックで配信
		 */
		float sensor_data = ev3_color_sensor_get_reflect(EV3_PORT_1);
		int sensor_data_100 = (int) (sensor_data * 100.0);
		topic_publish(pub1, sensor_data_100);

		/*
		 * カラー値をROSトピックで配信
		 */
		color_id = ev3_color_sensor_get_color(EV3_PORT_1);
		topic_publish(pub2, color_id);
		loop_rate.sleep();
	}
	return;
}
```

#うれしさ２(ROSツールとの連携)
ROSトピック配信されたデータは，ROSツールで参照できます．
今回の場合ですと，rostopic コマンドを使うことで，センシングデータの値をリアルタイムに参照できます．

以下，実行例です．上記のsensor_data_100 の値を ROSトピックで配信しています．ロボットの蛇行走行にともない，センサの値が上下していることがわかりますね．

![rosEV3RT.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/899d646a-6d80-9719-a3bf-3238aba28dfb.gif)


#うれしさ３(マルチロボット連携)
ROS化することで，単体ロボット向けシミュレータをマルチロボット化がやりやすくなります．

たとえば，以下の２つのロボットを考えてみます．

* 踏切ロボット
  * EV3RTのAPIを使用して踏切用のロボットを作ってみました．
  * https://twitter.com/i/status/1294465584130744321
  * ロボットARM用モータを使って，遮断機を上げ下げしてます．

* 自動走行ロボット
  * 単体ロボット向けシミュレータが標準で提供している HackEVのロボットです．
  * センサ
     * カラーセンサ，ジャイロセンサ，超音波センサ，タッチセンサ(2個)
  * モータ
     * 右モータ，左モータ，ARM用モータ

ロボット間の連携デモとしては，踏切の遮断機の状態をROSトピックで配信します．そのトピックを HackEV側で購読し，遮断機が下りている状態であるときは停止させ，上げている状態であるときは走行させるという制御がわかりやすいと思います．

以下，その実行例です．

* https://twitter.com/i/status/1294536745044701184

#インストール手順
まずは，ARM版の[単体ロボット向けシミュレータを箱庭Webサイト](https://toppers.github.io/hakoniwa/single-robot-setup/single-robot-setup-index/)を参照して頂きインストールください．

##アプリケーション
ROS対応版のアプリケーションは，以下で公開しています(インストール手順に従っていただければ自動的に配置されています)．

* https://github.com/toppers/ev3rt-athrill-ARMv7-A/tree/master/sdk/mros-obj

###コンフィグファイルの設定
MMAP版をご利用になられる場合は，memory_mmap.txtのmmapファイルの指定を絶対パスにしてください．
以下設定例です．

```
MMAP, 0x40000000, /mnt/c/project/toppers/ev3rt-athrill-ARMv7/sdk/mros-obj/athrill_mmap.bin
MMAP, 0x40010000, /mnt/c/project/toppers/ev3rt-athrill-ARMv7/sdk/mros-obj/unity_mmap.bin
```

###mROSのインストールおよび設定
まずは，[mROS](https://github.com/tlk-emb/mROS)と [asp-athrill-mbed](https://github.com/toppers/asp-athrill-mbed) を以下の構成で githubから clone しましょう．

```
$ tree . -L 1
.
├── asp-athrill-mbed
├── athrill
├── athrill-target-ARMv7-A
├── ev3rt-athrill-ARMv7
└── mROS
```

mROSのclone方法

```
$ git clone https://github.com/tlk-emb/mROS.git
```

asp-athrill-mbed のclone方法

```
$ git clone https://github.com/toppers/asp-athrill-mbed.git
```

次に，mROSの設定が必要です．
設定対象ファイルは，以下です．

* ev3rt-athrill-ARMv7/sdk/mros-obj/mros_config/mros_sys_config.h

設定内容は以下の２パラメータです．

1. MROS_MASTER_IPADDR
  * ROSマスタ配置PCのIPアドレスを指定します．ロボット数が１個の場合は変更不要です．
2. MROS_NODE_IPADDR
  * シミュレーション用PCのIPアドレスを指定します．ロボット数が１個の場合は変更不要です．

###ビルド方法
ARM版のEV3RTのアプリビルド手順と同じです．以下を参照ください．

* https://toppers.github.io/hakoniwa/single-robot-usage/01_usage_arm_v2.0/


##Unityパッケージ
Unityパッケージは，以下で公開しています．

* https://github.com/toppers/hakoniwa-Unity-HackEV/releases/tag/v2.1

Unityのプロジェクトを新規に作成いただき，上記パッケージをインポートしてください．インポート手順は，従来と同じ手順です．

なお，本パッケージからは，Unityのパラメータ設定は，config.jsonというファイルで一元管理するようになりました．詳細は，箱庭Webサイト上で解説予定ですので少々お待ちください．そんなに待てない！という方は，個別にご相談ください．

##シミュレーション実行方法
以下を参照ください．


* https://toppers.github.io/hakoniwa/single-robot-usage/01_usage_arm_v2.0/

※athrillの起動場所は，今回の環境にあわせてください．