ROSでレゴマインドストームEV3を動かしてみた

ROS初心者が勉強のため[LEGO Mindstorms EV3/NXTのROS indigo対応](http://www.slideshare.net/yutakakondo/lego-mindstorms-ev3ros)を参考にして、レゴマインドストームEV3をROSで動かしてみたので手順をメモしておく。

# 事前に準備したこと

* [VirtualBoxでROSを動かしてみる - Qiita](http://qiita.com/oh_rusty_nail/items/b19f474b0d88eecddc1e)の手順でVM上にubuntu 14.04+ROS indigo環境を構築
* ワークスペースを~/catkin_ws/srcに作成

```
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ catkin_init_workspace
$ cd ~/catkin_ws
$ catkin_make
$ vi ~/.bashrc # setup.bashの読み込み先を~/catkin_ws/devel/setup.bashに変更する
$ tail -1 ~/.bashrc
source /opt/ros/indigo/setup.bash
```
* EV3にev3devインストール

# EV3側環境構築

[python-ev3のInstall the python-ev3 on EV3](https://github.com/topikachu/python-ev3)のやり方にそって進める。

```bash
# 現在の日時確認
$ date
# 日時がずれていたら以下の例のように修正(これをしておかないとeasy_installでSSL: CERTIFICATE_VERIFY_FAILEでエラーとなる)
date -s "2015/10/27 12:00:00"

$ apt-get update
$ apt-get install virtualenv  virtualenvwrapper python-setuptools python-smbus python-pil
$ source /etc/bash_completion.d/virtualenvwrapper
 
$ mkvirtualenv ev3_py27 --python=/usr/bin/python2.7 --system-site-packages
$ workon ev3_py27
$ easy_install python-ev3
$ easy_install rpyc
```

# EV3側センサ確認

* 簡単に試すため真面目に組まずにモーター2個とジャイロセンサとUS(UltraSonic)センサだけをEV3に接続
* センサのポート確認

```bash
$ for f in /sys/class/lego-sensor/sensor*; do echo -n "$f: "; cat $f/driver_name;  done
/sys/class/lego-sensor/sensor0: lego-ev3-touch
/sys/class/lego-sensor/sensor1: lego-ev3-gyro
/sys/class/lego-sensor/sensor2: lego-ev3-color
/sys/class/lego-sensor/sensor4: lego-ev3-us
# これだと 0:タッチセンサ、1:ジャイロセンサ、2:カラーセンサ、3:USセンサ
 
$ workon ev3_py27
$ python
>>> from ev3.lego import LegoSensor
>>> LegoSensor(0).mode
Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
  File "/root/.virtualenvs/ev3_py27/local/lib/python2.7/site-packages/python_ev3-0.0.3.0-py2.7.egg/ev3/ev3dev.py", line 251, in __init__
ev3.ev3dev.NoSuchSensorError: No such sensor port=0 name=None
>>> LegoSensor(1).mode
'TOUCH'
>>> LegoSensor(2).mode
'COL-REFLECT'
>>> LegoSensor(3).mode
'GYRO-ANG'
>>> LegoSensor(4).mode
'US-DIST-CM'
# これで確認すると1:タッチセンサ、2:カラーセンサ、3:ジャイロセンサ、4:USセンサ
# /sys/class/lego-sensor/sensorXの番号に対応するかと思ったが、python-ev3上ではこのポート番号扱いになるらしい
>>> exit()
```

# EV3側でrpyc_classic.py動作

```bash
$ rpyc_classic.py
```

# VM側でmindstorms_ros環境構築

* 最初はnavigationやjoyはgithubからcloneして配置してたが[問題](http://answers.ros.org/question/201630/orocos-bfl-not-found-while-installing-navigation-stack-ros-indigo-ubuntu-1404/)が発生したりしたためapt-getで入れるようにした。

```bash
$ sudo easy_install python-ev3
$ sudo easy_install rpyc
$ sudo easy_install nxt-python
 
$ sudo apt-get install ros-indigo-navigation
$ sudo apt-get install ros-indigo-joy
 
$ git clone https://github.com/youtalk/mindstorms_ros.git
$ cd mindstorms_ros
$ cp -R nxt/* ~/catkin_ws/src
$ cp -R nxt_apps/* ~/catkin_ws/src
$ cp -R nxt_robots/* ~/catkin_ws/src
$ cp -R ev3/* ~/catkin_ws/src
$ cp -R ev3_robots/* ~/catkin_ws/src
 
$ vi ~/catkin_ws/src/ev3_robot_gyro_car/robot.yaml
# => モータ、センサーのポートの設定を正しく対応させる
 
$ vi ~/catkin_ws/src/ev3_ros/scripts/ev3_ros.py
# => Motorオブジェクトにpulses_per_secondがないらしいので
#    js.velocity.append(math.radians(self.motor.pulses_per_second))をコメントアウトして js.velocity.append(1)に
 
$ cd ~/catkin_ws
$ catkin_make
```

# VM側でgyro car動作

```bash
$ roscore
 
# 別端末で
 
$ roslaunch ev3_robot_gyro_car robot.launch host:=10.42.0.3
 
# 別端末で
 
$ roslaunch nxt_teleop teleop_keyboard.launch
```

* roslaunch nxt_teleop、、、した端末でキーボード操作してみると反応は鈍いがモーターが操作に連動して動作することを確認

# gyro carについていろいろみてみる

* rqt_graph

![gyro_car.png](https://qiita-image-store.s3.amazonaws.com/0/16768/473cda1b-c691-bc64-9fc7-f8a1dfb9c1a9.png)

* urdf

![gyro_car_urdf.png](https://qiita-image-store.s3.amazonaws.com/0/16768/204f1c06-ef54-2196-1070-1bae22088a65.png)

```bash
# 出し方(http://wiki.ros.org/urdf参照)
$ sudo apt-get install liburdfdom-tools
$ urdf_to_graphiz ~/catkin_ws/src/ev3_robot_gyro_car/robot.urdf
```