ROSで画像処理ができるようになるまで[python編]

はじめに

ROSで画像をいじる系の記事で、WEBカメラを使えるようにやROSに入ってるエッジ検出を試すなどは結構あるんですが、自分でコード書くような記事が少ないので。まとめようと思い書いています。

SET UP

ROSがいろいろな環境を破壊しないように、Docker内で構築していきます。Ubuntu16.04のバージョンであるkineticを使用します。
--netとDISPLAYとX11-unixのオプションはDockerでGUI画面などを使用するためのオプション。
--deviceはWEBカメラをDocker内で使用するためのオプション。
~/dockerはホストからのデータ渡し用フォルダの用意です。

host

$ docker pull ros:kinetic
$ xhost +
$ mkdir ~/docker
$ docker run -it --net host -e DISPLAY=$DISPLAY \
 -v /tmp/.X11-unix:/tmp/.X11-unix \
 -v ~/docker:/data \
 --device /dev/video0:/dev/video0:rw \
 ros:kinetic

ROS関係

必要なものをインストールします。
特に複数ターミナルを使用するので、byobuやscreenなどを使用するのが楽です。あとはちょっとした編集をするためにエディタも入れておきました。
/workspaceは、作業用のフォルダとして作成しました。

docker

$ apt-get update && apt-get -y upgrade
$ apt-get install -y byobu vim tree wget
$ apt-get install -y ros-kinetic-uvc-camera
$ apt-get install -y ros-kinetic-rqt ros-kinetic-rqt-common-plugins

Python関係

docker

$ apt-get install -y build-essential libbz2-dev libdb-dev \
  libreadline-dev libffi-dev libgdbm-dev liblzma-dev \
  libncursesw5-dev libsqlite3-dev libssl-dev \
  zlib1g-dev uuid-dev tk-dev
$ apt-get install -y python-dev
$ apt-get install -y python-rospy
$ mkdir /workspace && cd /workspace
$ wget https://bootstrap.pypa.io/get-pip.py
$ python get-pip.py
$ rm get-pip.py
$ ldconfig

動作確認

WEBカメラが使えるか確認します。

docker

$ byobu
$ roscore
# Shift+F2で新しいターミナルを開く
# WEBカメラ用のノード起動
$ rosrun uvc_camera uvc_camera_node
# Shift+F2で新しいターミナルを開く
# 画像ビューワの起動
$ rqt_image_view image:=/image_raw

これで下のビューワーが開きます。
映像が表示されてない場合は、左上の/image_rawの箇所のプルダウンを切り替えてください。

では、どのような仕組みで動いてるか、rqt_graphでノードとトピックを描画してみます。

docker

# Shift+F2で新しいターミナルを開く
$ rqt_graph

上記のコマンドを打つと、ウィンドウが起動して、以下のような図が出ます。これが、起動中のノードとトピックの関係を表した図です。
楕円で囲まれているものがノードであり、四角は、トピックを表します。トピックが表示されていない場合、ウィンドウの左上のプルダウンから、Nodes/Topics(active)を選択すると、以下の図になります。

自分で書く

まずはプログラムを書くための準備をします。
以下のコマンドでROSのワークスペースができます。
catkin_create_pkgコマンドで使用するパッケージを読み込みます。
rospyがPythonでROSを使用する際に必要なパッケージです。
cv_bridgeがROSのImageトピックとOpenCVのcv::Mat(PythonならNumpy)の変換を行ってくれるパッケージです。

docker

$ mkdir -p ./catkin_ws/src && cd catkin_ws
$ catkin_make
$ cd src
$ catkin_create_pkg img_proc rospy cv_bridge
$ cd img_proc/
$ mkdir script
$ cd /workspace/catkin_wa
$ vi src/img_proc/script/image_proc.py

さて、WEBカメラから取得した画像を受け取って描画してみます。
他のノードからデータを受け取る際には、Subscriberを使用します。WEBカメラの映像を受け取るには、/image_rawのトピックを受け取る必要があるので、rospy.Subscriber("image_raw", Image, process_image)で受け取ります。
Image型で受け取ったメッセージは、process_image関数に渡され処理されます。
以下のプログラムは、参考サイト[6]のコードを参考にさせていただきました。

detect_pump.py

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image 
from cv_bridge import CvBridge
import cv2

def process_image(msg):
    try:
        bridge = CvBridge()
        orig = bridge.imgmsg_to_cv2(msg, "bgr8")
        img = cv2.cvtColor(orig, cv2.COLOR_BGR2GRAY)
        cv2.imshow('image', img)
        cv2.waitKey(1)
    except Exception as err:
        print(err)

def start_node():
    rospy.init_node('img_proc')
    rospy.loginfo('img_proc node started')
    rospy.Subscriber("image_raw", Image, process_image)
    rospy.spin()

if __name__ == '__main__':
    try:
        start_node()
    except rospy.ROSInterruptException:
        pass

実行は、以下のようになります。

docker

$ byobu
$ roscore
# Shift+F2で新しいターミナルを開く
$ rosrun uvc_camera uvc_camera_node
# Shift+F2で新しいターミナルを開く
$ chmod u+x ./src/img_proc/script/image_proc.py 
$ source ./devel/setup.bash 
$ rosrun img_proc image_proc.py

以下のように、OpenCVのビューワが起動して、グレー画像が描画されます。

あとは、自分の好きなようにprocess_image関数内で画像処理をすれば良いだけです。

処理結果を別ノードに渡す

処理結果を別のノードに送りたいということがあると思います。
やっとやり方がわかったので書いていきます。

上記のdetect_pump.pyを以下のように改造します。
ビューワーのところを消し、Publisherを追加します。このPublisherで他のノードにデータを送ることができます。
今回は、グレー画像を送るので、bridge.cv2_to_imgmsg()の最後の変数をmono8（1ch画像）にs設定していますが、bgr8にすることで、カラー画像も送れます。

detect_pump.py

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image 
from cv_bridge import CvBridge
import cv2

def process_image(msg):
    try:
        bridge = CvBridge()
        orig = bridge.imgmsg_to_cv2(msg, "bgr8")
        img = cv2.cvtColor(orig, cv2.COLOR_BGR2GRAY)
        imgMsg = bridge.cv2_to_imgmsg(img, "mono8")
        pub = rospy.Publisher('image_gray', Image, queue_size=10)
        pub.publish(imgMsg)
    except Exception as err:
        print(err)

def start_node():
    rospy.init_node('img_proc')
    rospy.loginfo('img_proc node started')
    rospy.Subscriber("image_raw", Image, process_image)
    rospy.spin()

if __name__ == '__main__':
    try:
        start_node()
    except rospy.ROSInterruptException:
        pass

ビューワーコードを書いて確認してみます。コードの格納場所は、以下にしました。

docker

$ /workspace/catkin_ws/src/img_proc/script/viewer.py

viewer.py

#!/usr/bin/env python
import rospy
import cv2

from cv_bridge import CvBridge
from sensor_msgs.msg import Image

def process_image(msg):
    try:
        bridge = CvBridge()
        orig = bridge.imgmsg_to_cv2(msg, "mono8")
        cv2.imshow('img', orig)
        cv2.waitKey(10)
    except Exception as err:
        print(err)

def start_node():
    rospy.init_node('viewer')
    rospy.loginfo('viewer node started')
    rospy.Subscriber("image_gray", Image, process_image)
    rospy.spin()

if __name__ == '__main__':
    try:
        start_node()
    except rospy.ROSInterruptException:
        pass

実行してみます。

docker

$ byobu
$ roscore
# Shift+F2で新しいターミナルを開く
$ rosrun uvc_camera uvc_camera_node
# Shift+F2で新しいターミナルを開く 
$ source ./devel/setup.bash 
$ rosrun img_proc image_proc.py
# Shift+F2で新しいターミナルを開く
$ chmod u+x ./src/img_proc/script/viewer.py
$ rosrun img_proc viewer.py

実行結果は、OpenCVのビューワーが開いて、WEBカメラのモノクロ画像が表示されます。

処理の流れをrqt_graphを見てみるとちゃんと、画像は渡されています。

参考サイト

[1] dockerでカメラデバイスを共有する
[2] Dockerコンテナの中でGUIアプリケーションを起動させる
[3] 如何在docker image下使用rqt_graph #483
[4] How to install python-rospy on Ubuntu 16.04 (Xenial Xerus)
[5] ROS KineticでのUSBカメラ接続とキャリブレーションまで
[6] ROS-Industrial Website|Blog OpenCV 画像処理（ Pyhton ）
[7] Visual Studio CodeからDockerで作ったコンテナ内を操作する
[8] ROSでOpenCVにて取得した画像をPublish,Subscribeする