LoginSignup
16
16

More than 3 years have passed since last update.

@keinko(Kein Kosugi)

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

Last updated at Posted at 2019-06-28

はじめに

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

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

記事タイトル 内容
ROSでHello World ROSに慣れるために簡単な処理を行う
ROSで簡単な通信を行う ROSの基本である通信の処理をテストする
ROSを使って画像情報でロボットを動かす 少し実践的に複数のシステムを動かす

目標

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

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

開発環境

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

$ lscpu | grep Architecture
Architecture:        x86_64

カメラの準備

ドライバのインストール

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

$ ls /dev/video*
/dev/video0  /dev/video1

はじめにROSでUSBカメラを動かすためのドライバをインストール。

$ sudo apt-get install ros-melodic-install

実行テスト

ターミナルを3つ立ち上げて以下のコマンドを入力。

ターミナル1
$ roscore
ターミナル2
$ rosrun usb_cam usb_cam_node
ターミナル3
$ rosrun image_view image_view image:=/usb_cam/image_raw

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

ターミナル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のようにして表示させてみましょう。

トラブルシューティング

自分の環境ではこんな序盤からトラブル発生…。

$ 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で画像表示されない…。
こちらのエラー処理を参考にして以下のようにしたら正常に動作しました。

$ 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初心者の自分は、とりあえずパッケージを以下のように新規作成します。

$ 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というファイルをつくりましょうか。

$ roscd img_recog
$ mkdir scripts
$ touch color_img_recog.py

コーディング

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

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つ開いて動作を確認。

ターミナル1
$ roscore
ターミナル2
$ rosrun usb_cam usb_cam_node
ターミナル3
$ rosrun img_recog color_img_recog.py
ターミナル4
$ rosrun image_view image_view image:=/usb_cam/image_raw
ターミナル5
$ rosrun image_view image_view image:=/blue_image
ターミナル6
$ rosrun image_view image_view image:=/red_image

以下のような3つの画像が取得されたでしょうか？
Screenshot from 2019-06-28 20-50-42.png
一つが生データ、それに加えて青だけ表示するものと、赤だけ表示するものです。

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

ターミナル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する

インストール

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

$ 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

実行テスト

ターミナルを２つ開いて動作確認。

ターミナル1
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch
ターミナル2
$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

上記を打ち込むとシミュレーションのROSアプリケーション、gazeboが立ち上がる。
「shift」＋ドラッグ とかで視点変更できるので遊んでみてください。
ズームは右クリックでドラッグです。
Screenshot from 2019-06-28 19-37-59.png

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

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

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

システムの実行

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

ターミナル1(gazeboの起動)
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch
ターミナル2(キーボードで制御)
$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
ターミナル3(カメラのドライバ)
$ rosrun usb_cam usb_cam_node
ターミナル4(画像認識による制御)
$ rosrun img_recog color_img_recog.py
ターミナル5(カメラの画像を表示)
$ rosrun image_view image_view image:=/usb_cam/image_raw
ターミナル6(カメラの画像のうち青と判定したものだけ表示)
$ rosrun image_view image_view image:=/blue_image
ターミナル7(カメラの画像のうち赤と判定したものだけ表示)
$ rosrun image_view image_view image:=/red_image

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

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

ターミナル8
$ rqt_graph

メニュー上部の「Nodes only」を「Nodes/Topics(all)」に変更した画像が以下です。
Screenshot from 2019-06-28 21-05-02.png
ノード名、Topic名等の確認や、正常に通信が行えているか、また人が書いたROSのコードの確認等に活用できます。

おまけ

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

参考

16
16
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
16
16