ROS2で道案内ロボットをつくる〜画像処理編（前編）〜

# はじめに
　本記事では、移動型ロボットが**事前に登録されたパスをたどりつつ**も**後方から追随するユーザーを置いてけぼりにしない**ような「道案内ロボット」の実装について紹介します。
　前半では、指標物体の検知とその結果をトピックとしてパブリッシュする方法を紹介し、後半では、得られたデータをサブスクライブし、ゴールを更新したりキャンセルしたりする方法について紹介します。
　なお、後半はこちら

https://qiita.com/sfc_nakanishi_lab/private/c98b3d91c3e98f6d234d


# 実行環境
### PC
|項目|スペック| 
|:---|:-----|
|CPU|Core i7-9750H|
|OS|Ubuntu 20.04LTS|
|ROS2|Foxy Fitzroy|
### Raspberry Pi 4
|項目|スペック| 
|:---|:-----|
|CPU|1.5GHz クアッドコア Cortex-A72(ARMv8、64bit L1=32KB、L2=1M)|
|OS|Ubuntu 20.04LTS|
|ROS2|Foxy Fitzroy|
|Turtlebot3	|Waffle Pi|

# 1.画像処理の結果を送信するメッセージを作成する
　今回は指標物体にピンポン玉を選択し、ウェブカメラの画像上のピンポン玉の位置と半径をパブリッシュするためにメッセージを作成しました。

https://github.com/Takagi-Waluigi/imgproc_msgs

 新しく`Imgproc.msg`を作成し3つのfloatで構成しました。
```imgproc.msg
float64 x
float64 y
float64 radius
```
　ビルドして`. install/setup.bash`してから、`ros2 interface list`で` imgproc_msgs/msg/Imgproc`があることを確認してください。


# 2.OpenCVで画像処理
　カラーボールの検知を行っていた[こちら](https://temari.co.jp/blog/2021/01/10/opencv-13/)のサイトに掲載されていたプログラムを参考に画像処理ノードをつくりました。
```imgProc.py
import rclpy
from rclpy.node import Node

from std_msgs.msg import String
from imgproc_msgs.msg import Imgproc

import cv2
import numpy as np

class ImageProcessor(Node):

    def __init__(self):
        super().__init__('imageProcessor')
        self.publisher_ = self.create_publisher(Imgproc, 'img_data', 10)
        timer_period = 0.125  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.cap = cv2.VideoCapture(0)

        self.marker_x = 0.0
        self.marker_y = 0.0
        self.marker_radius = 0.0

    def timer_callback(self):    

        self._, self.frame = self.cap.read()
        self.frame = cv2.resize(self.frame, (int(self.frame.shape[1]/2), int(self.frame.shape[0]/2)))

        self.res_orange = self.getMask([0,50,100], [25,255,255])
        self.contours_frame = self.getContours(self.res_orange, 50, 30)

        #cv2.imshow('video',self.contours_frame)

        msg = Imgproc()

        msg.x = float(self.marker_x)
        msg.y = float(self.marker_y)
        msg.radius = float(self.marker_radius)

        self.publisher_.publish(msg)
        self.get_logger().info('Publishing (radius): "%s"' % msg.radius)
        

    def getMask(self, l, u):
        hsv = cv2.cvtColor(self.frame, cv2.COLOR_BGR2HSV)

        lower = np.array(l)
        upper = np.array(u)

        if(lower[0] >= 0):
            mask = cv2.inRange(hsv, lower, upper)
        else:
            h = hsv[:, :, 0]
            s = hsv[:, :, 1]
            v = hsv[:, :, 2]
            mask = np.zeros(h.shape, dtype=np.uint8)
            mask[((h < lower[0]*-1) | h > upper[0]) & (s > lower[1]) & (s < upper[1]) & (v > lower[2]) & (v < upper[2])] = 255
        
        return cv2.bitwise_and(self.frame,self.frame, mask= mask)
    
    def getContours(self, img, t, r):
        gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
        ret, thresh = cv2.threshold(gray, t, 255, cv2.THRESH_BINARY)

        contours, _= cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

        largest_radius = 0.0
        largest_x = 0.0
        largest_y = 0.0
        if len(contours) > 0:
            for cnt in contours:
                (x,y), radius = cv2.minEnclosingCircle(cnt)
                center = (int(x),int(y))
                radius = int(radius)
                radius_frame = cv2.circle(self.frame,center,0,(0,255,0),0)                

                if radius > r:
                    if radius > largest_radius:
                        largest_radius = radius
                        largest_x = x
                        largest_y = y

                    radius_frame = cv2.circle(self.frame,center,radius,(0,255,0),2)
            
            self.marker_radius = float(largest_radius)
            self.marker_x = float(largest_x)
            self.marker_y = float(largest_y)

            return radius_frame
        else:
            self.marker_radius = 0.0
            return self.frame

    def __del__(self):
        self.cap.release()
        cv2.destroyAllWindows()
        self.get_logger().info('Closing camera')


def main(args=None):
    rclpy.init(args=args)

    imgproc = ImageProcessor()

    rclpy.spin(imgproc)

    imgproc.destroy_node()

    rclpy.shutdown()


if __name__ == '__main__':
    main()

```

　画像処理の中の内容については引用元を参照いただきたいですが、ざっくり説明すると、特定の色（HSV形式）でマスクをしてから輪郭抽出するという流れになります。
　また、`from imgproc_msgs.msg import Imgproc`で作成したImgproc型のimportをしています。publishする際のトピック名は`img_data`としています。TB3のラズパイで回すことを想定しているので`cv2.imshow`はコメントアウトにしています。

　なお、画像処理のノードを立てるパッケージはこちらにまとめてあります。

https://github.com/Takagi-Waluigi/image_processor

　ノードを立てるとこのようにデータがPublishされているのがわかると思います。
<iframe width="560" height="315" src="https://www.youtube.com/embed/v5Fct0FiYlU" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

# 次回
　次回後編では、画像データをサブスクライブしてナビゲーションの実行やキャンセルさせる方法について紹介します。

https://qiita.com/sfc_nakanishi_lab/private/c98b3d91c3e98f6d234d


# 参考資料
https://qiita.com/NeK/items/26751b8ed12976ff977d

https://temari.co.jp/blog/2021/01/10/opencv-13/