ROSを使って画像情報でロボットを動かす

#はじめに
ROSの初歩的なコードをまとめる第三段。
自分のメモ代わり。
…のつもりですが、かなり膨大なものとなってしまいました。
ROS始めたての人にはかなり勉強になるのではないでしょうか。
というか勉強になりました、ありがとうございます。

関連記事は以下のとおり。

| 記事タイトル  |  内容 |
|:----------------:|:-----------------:|
| [ROSでHello World](https://qiita.com/keinko/items/880ff30e5a4ae57c6315)    | ROSに慣れるために簡単な処理を行う|
| [ROSで簡単な通信を行う](https://qiita.com/keinko/items/7d0f26c19414291660d9)             | ROSの基本である通信の処理をテストする |
| [ROSを使って画像情報でロボットを動かす](https://qiita.com/keinko/items/c8f9390419cb1cbb6467)          | 少し実践的に複数のシステムを動かす |


###目標
以下のようなシステムを開発していきます。

- ROSでロボットのシミュレーションを表示
- パソコンに取り付けたwebカメラをROSで起動
- そのカメラで画像認識を行う
    - 青色を認識したらロボットを前進させる信号を送る
    - 赤色を認識したらロボットを後退させる信号を送る
- シミュレーション画面に表示されたロボットが動く

###開発環境
デュアルブートしたUbuntu 18.04、ROSのバージョンはmelodic。
アーキテクチャはx86_64。初心者の方は以下で確認できます。

```terminal
$ lscpu | grep Architecture
Architecture:        x86_64
```

##カメラの準備
###ドライバのインストール
今回使うのは研究室に落ちてたwebカメラ。USBでつなげるやつ。
以下のコマンドでなんかしら表示されればパソコンがカメラを認識している証拠。

```terminal
$ ls /dev/video*
/dev/video0  /dev/video1
```
はじめにROSでUSBカメラを動かすためのドライバをインストール。

```terminal
$ sudo apt-get install ros-melodic-install
```

###実行テスト
ターミナルを3つ立ち上げて以下のコマンドを入力。

```terminal:ターミナル1
$ roscore
```

```terminal:ターミナル2
$ rosrun usb_cam usb_cam_node
```

```terminal:ターミナル3
$ rosrun image_view image_view image:=/usb_cam/image_raw
```

最後の`image:=/usb_cam/image_raw`ってなんやねんって思った人。
これはimage_viewパッケージに渡すTopic名を指定している。
Topic名の一覧を出力するコマンドは以下の`rostopic list`。
すごい便利だから覚えておきましょう。

```terminal:ターミナル4
$ rostopic list
/rosout
/rosout_agg
/usb_cam/camera_info
/usb_cam/image_raw
/usb_cam/image_raw/compressed
/usb_cam/image_raw/compressed/parameter_descriptions
/usb_cam/image_raw/compressed/parameter_updates
/usb_cam/image_raw/compressedDepth
/usb_cam/image_raw/compressedDepth/parameter_descriptions
/usb_cam/image_raw/compressedDepth/parameter_updates
/usb_cam/image_raw/theora
/usb_cam/image_raw/theora/parameter_descriptions
/usb_cam/image_raw/theora/parameter_updates
```

「画像データどうやって受け渡せばいいんだ…」とか悩んだら、とりあえず`rostopic list`で表示される画像データっぽいTopicを上記のターミナル3のようにして表示させてみましょう。


###トラブルシューティング
自分の環境ではこんな序盤からトラブル発生…。

```terminal
$ rosrun image_view image_view image:=/usb_cam/image_raw
[ INFO] [1556884074.352272211]: Using transport "raw"
(image_raw:15539): GLib-GObject-CRITICAL **: 20:47:54.388: g_object_unref: assertion 'G_IS_OBJECT (object)' failed
(image_raw:15539): GLib-GObject-CRITICAL **: 20:47:54.451: g_object_unref: assertion 'G_IS_OBJECT (object)' failed
Attempt to unlock mutex that was not locked
Aborted (core dumped)
```
こんな感じのエラーを吐かれて`image_view`で画像表示されない…。
[こちらのエラー処理](https://github.com/ros-perception/image_pipeline/issues/201#issue-159741962)を参考にして以下のようにしたら正常に動作しました。

```terminal
$ cd ~/catkin_ws/src
$ git clone https://github.com/ros-perception/image_pipeline.git
$ git -C image_pipeline fetch origin pull/343/head:mutex_error_resolve
$ git -C image_pipeline checkout mutex_error_resolve
$ cd ~/catkin_ws
$ catkin_make
$ source ~/catkin_ws/devel/setup.bash
```

##画像認識
ここでは画像認識を行うpythonファイルを作成していきます。

###いつもの準備
ROS初心者の自分は、とりあえずパッケージを以下のように新規作成します。

```terminal
$ cd ~/catkin_ws/src
$ catkin_create_pkg img_recog rospy roscpp std_msgs
$ cd ~/catkin_ws
$ catkin_make
$ source ~/catkin_ws/devel/setup.bash
```

どこでもいいんですが、また`scripts`っていうディレクトリをつくってそこに`color_img_recog.py`というファイルをつくりましょうか。

```terminal
$ roscd img_recog
$ mkdir scripts
$ touch color_img_recog.py
```

###コーディング
好きなエディタを開いて`color_img_recog.py`の中身を記述していきましょ。
画像認識の中身については割愛します。
とりあえずコピペして動作確認を行い、シミュレーションまで動かしてから最後にコードをじっくり見るといいと思います。

```python:color_img_recog.py
#!/usr/bin/env python
## coding: UTF-8

import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
from cv_bridge import CvBridge, CvBridgeError

class ColorExtract(object):
    def __init__(self):
        self._vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
        self._blue_pub = rospy.Publisher('blue_image', Image, queue_size=1)
        self._red_pub = rospy.Publisher('red_image', Image, queue_size=1)
        self._image_sub = rospy.Subscriber('/usb_cam/image_raw', Image, self.callback)
        self._bridge = CvBridge()
        self._vel = Twist()

    def get_colored_area(self, cv_image, lower, upper):
        hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
        mask_image = cv2.inRange(hsv_image, lower, upper)
        extracted_image = cv2.bitwise_and(cv_image, cv_image, mask=mask_image)
        area = cv2.countNonZero(mask_image)
        return (area, extracted_image)

    def callback(self, data):
        try:
            cv_image = self._bridge.imgmsg_to_cv2(data, 'bgr8')
        except CvBridgeError, e:
            print e
        blue_area, blue_image = self.get_colored_area(cv_image, np.array([50,100,100]), np.array([150,255,255]))
        red_area, red_image = self.get_colored_area(cv_image, np.array([150,100,150]), np.array([180,255,255]))

        try:
            self._blue_pub.publish(self._bridge.cv2_to_imgmsg(blue_image, 'bgr8'))
            self._red_pub.publish(self._bridge.cv2_to_imgmsg(red_image, 'bgr8'))
        except CvBridgeError, e:
            print e
        rospy.loginfo('blue=%d, red=%d' % (blue_area, red_area))
        if blue_area > 1000:
            self._vel.linear.x = 10
            self._vel_pub.publish(self._vel)
        if red_area > 1000:
            self._vel.linear.x = -10
            self._vel_pub.publish(self._vel)       

if __name__ == '__main__':
    rospy.init_node('color_extract')
    color = ColorExtract()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        pass
```

その後、`rosrun`で動くように以下を実行。

```
$ roscd img_recog
$ cd scripts
$ chmod 755 color_img_recog.py
```

###実行テスト
ターミナルを6つ開いて動作を確認。

```terminal:ターミナル1
$ roscore
```

```terminal:ターミナル2
$ rosrun usb_cam usb_cam_node
```

```terminal:ターミナル3
$ rosrun img_recog color_img_recog.py
```

```terminal:ターミナル4
$ rosrun image_view image_view image:=/usb_cam/image_raw
```

```terminal:ターミナル5
$ rosrun image_view image_view image:=/blue_image
```

```terminal:ターミナル6
$ rosrun image_view image_view image:=/red_image
```

以下のような3つの画像が取得されたでしょうか？
![Screenshot from 2019-06-28 20-50-42.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/208979/26f1b829-03c9-8f36-72a7-e96781b01162.png)
一つが生データ、それに加えて青だけ表示するものと、赤だけ表示するものです。

また、認識結果がターミナル3に表示されると思います。
今回はこの値が1000以上のとき、ロボットを前進させたり、後退させたりたいと思います。

```terminal:ターミナル3
$ rosrun img_recog color_img_recog.py
[INFO] [1561722870.013762]: blue=87268, red=3312
[INFO] [1561722870.050350]: blue=87156, red=3533
[INFO] [1561722870.087287]: blue=87080, red=3339
[INFO] [1561722870.128682]: blue=87312, red=3539
[INFO] [1561722870.175794]: blue=87049, red=3445
[INFO] [1561722870.207746]: blue=87114, red=3381
[INFO] [1561722870.252081]: blue=87145, red=3240
[INFO] [1561722870.290471]: blue=87131, red=3417
[INFO] [1561722870.336222]: blue=87181, red=3481
[INFO] [1561722870.376246]: blue=87109, red=3378
[INFO] [1561722870.421075]: blue=87202, red=3449
```

次にシミュレーション環境の準備です。

##シミュレーション環境の構築
ここらへんは自分も完全に初心者なので@protocol1964さんの記事を参考にmelodicで動くロボットをシミュレーション画面上に表示させます。
→[ROS MelodicでTurtlebot3をGazeboで動かしてついでにSLAMする](https://qiita.com/protocol1964/items/1e63aebddd7d5bfd0d1b)

###インストール
まず依存パッケージをインストールします。
たぶん今回の実装にはこんな必要ないと思います。
まあいちいち一つ一つ調べるのもめんどくさいので、自分はまとめてインストールしてしまいました。

```
$ sudo apt-get install ros-melodic-joy ros-melodic-teleop-twist-joy ros-melodic-teleop-twist-keyboard ros-melodic-laser-pro
![Screenshot from 2019-06-28 21-02-57.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/208979/d0db45bc-109c-b903-8a8e-5a01660ab70d.png)c ros-melodic-rgbd-launch ros-melodic-depthimage-to-laserscan ros-melodic-rosserial-arduino ros-melodic-rosserial-python ros-melodic-rosserial-server ros-melodic-rosserial-client ros-melodic-rosserial-msgs ros-melodic-amcl ros-melodic-map-server ros-melodic-move-base ros-melodic-urdf ros-melodic-xacro ros-melodic-compressed-image-transport ros-melodic-rqt-image-view ros-melodic-navigation
```

次にturtle-bot関連のコードをダウンロード。
turtle-bot君が今回、シミュレーションを行うロボットの名前です。
ダウンロードしたらビルドもしてしまいましょう。

```
$ cd ~/catkin_ws/src
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
![Screenshot from 2019-06-28 21-02-57.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/208979/d0db45bc-109c-b903-8a8e-5a01660ab70d.png)
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_gazebo_plugin.git
$ cd ~/catkin_ws
$ catkin_make
$ source ~/catkin_ws/devel/setup.bash
```

最後にturtle-botの形を決めないとエラーになるので、そちらを記述。

```
$ echo "export TURTLEBOT3_MODEL=burger" >> ~/.bashrc
$ source ~/.bashrc
```

###実行テスト
ターミナルを２つ開いて動作確認。

```terminal:ターミナル1
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch
```

```terminal:ターミナル2
$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
```

上記を打ち込むとシミュレーションのROSアプリケーション、gazeboが立ち上がる。
「shift」＋ドラッグ とかで視点変更できるので遊んでみてください。
ズームは右クリックでドラッグです。
![Screenshot from 2019-06-28 19-37-59.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/208979/2e755519-3555-4f8f-7c4d-707a539faf0e.png)

また、ターミナル2に「w」「a」「x」「d」のキーを打ち込むとロボットが動き出します。
長押しすると分かりやすいです。
「s」キーを押すと止まります。

実装するシステムの流れとしては以下のように考えると分かりやすいかもしれません。

- 青がカメラに映ったら「w」キーを押したような感じにする
- 赤がカメラに映ったら「x」キーを押したような感じにする

##システムの実行
以上、全てのプログラムを同時に起動します。
簡単に同時で起動できるのがROSのすごいところ。
ターミナルを7つ用意して以下を入力。

```terminal:ターミナル1(gazeboの起動)
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch
```

```terminal:ターミナル2(キーボードで制御)
$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
```

```terminal:ターミナル3(カメラのドライバ)
$ rosrun usb_cam usb_cam_node
```

```terminal:ターミナル4(画像認識による制御)
$ rosrun img_recog color_img_recog.py
```

```terminal:ターミナル5(カメラの画像を表示)
$ rosrun image_view image_view image:=/usb_cam/image_raw
```

```terminal:ターミナル6(カメラの画像のうち青と判定したものだけ表示)
$ rosrun image_view image_view image:=/blue_image
```

```terminal:ターミナル7(カメラの画像のうち赤と判定したものだけ表示)
$ rosrun image_view image_view image:=/red_image
```

どうでしょうか。
カメラで青を見たらロボットが進み、赤を見たら後退したでしょうか？？

最後に`rqt_graph`というコマンドを入力しましょう。
システム全てが図示されます。これもとっても重要なコマンドです。

```terminal:ターミナル8
$ rqt_graph
```

メニュー上部の「Nodes only」を「Nodes/Topics(all)」に変更した画像が以下です。
![Screenshot from 2019-06-28 21-05-02.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/208979/af1174ce-0974-2945-016a-eda3f8c6581c.png)
ノード名、Topic名等の確認や、正常に通信が行えているか、また人が書いたROSのコードの確認等に活用できます。

##おまけ
いつもこんなにターミナル立ち上げたくないですよね。
このように複数の`rosrun`を行う場合に、まとめて処理する方法が`roslaunch`です。
turtle-botはどなたが作ってくれたコードなので`roslaunch`でまとめて実行できるようになっていたんですね。
みなさんも余裕があれば`roslaunch`で今回のコードをまとめて動かせうようにしてみてください。

## 参考

- [Attempt to unlock mutex that was not locked #201](https://github.com/ros-perception/image_pipeline/issues/201#issue-159741962)
- [ROS MelodicでTurtlebot3をGazeboで動かしてついでにSLAMする](https://qiita.com/protocol1964/items/1e63aebddd7d5bfd0d1b)