ROS2とrealsenseD435を使って物体との距離を計測する

はじめに

今回はrealsenseD435のdepth画像とrgb画像を使って赤色の物体を認識してその物体との距離を計測します。

実行環境

• ubuntu20.04
• ROS2 foxy
• kernal 5.11
• realsense-ros 4.51.1
• realsense SDK 2.51.1

独自メッセージ

今回自分でSearchBall.msgファイルを作ってその中に、距離,画像中心からの角度入れています。

float32 l
float32 arg

実際のコード

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from rclpy.qos import QoSProfile,QoSReliabilityPolicy
import cv2
from cv_bridge import CvBridge,CvBridgeError
import numpy as np
from gakurobo_msgs.msg import SearchBall

class DepthImg(Node):
    def __init__(self):
        super().__init__('depth_node')
        pos = QoSProfile(
            depth=10,
            reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
        )
        self.img_dep_sub = self.create_subscription(Image,'/camera/depth/image_rect_raw',self.depth_cb,pos)
        self.img_bgr_sub = self.create_subscription(Image,'/camera/color/image_raw',self.bgr_cb,pos)
        self.info_pub = self.create_publisher(SearchBall,'search_ball_info',10)
        self.br=CvBridge()
        self.row,self.col=240,424
        self.mx =240
        self.my =424
        self.info = SearchBall()
        timer_preiod = 0.5
        self.timer = self.create_timer(timer_preiod,self.timer_cb)

    def timer_cb(self):
        self.info_pub.publish(self.info)
            
        
    
    def depth_cb(self,img):
        try:
            cv_depth_img=self.br.imgmsg_to_cv2(img,desired_encoding='passthrough')
        except CvBridgeError as e:
            print(e)
        self.row = self.my
        self.col = self.mx
        depth_value = cv_depth_img[self.row,self.col]
        self.get_logger().info(f"Depth value at pixel ({self.row}, {self.col}): {depth_value*0.001} meters\n")
        self.info.l=depth_value*0.001
        cv2.imshow("depth",cv_depth_img)
        cv2.waitKey(1)
    
    def bgr_cb(self,img):
        try:
            cv_bgr_img = self.br.imgmsg_to_cv2(img,"bgr8")
        except CvBridgeError as e:
            print(e)
        hsv_image = cv2.cvtColor(cv_bgr_img,cv2.COLOR_BGR2HSV)
        color_min1 = np.array([0,50,50])
        color_max1 = np.array([6,255,255])
        color_min2 = np.array([174,50,50])
        color_max2 = np.array([180,255,255])
        mask1 = cv2.inRange(hsv_image,color_min1,color_max1)
        mask2 = cv2.inRange(hsv_image,color_min2,color_max2)
        color_mask = mask1+mask2
        masked_img =cv2.bitwise_and(cv_bgr_img,cv_bgr_img,mask= color_mask)
        out_img = masked_img
        num_labels, label_img, stats, centroids = cv2.connectedComponentsWithStats(color_mask) 
        # 背景のラベルを削除
        num_labels = num_labels - 1
        stats = np.delete(stats, 0, 0)
        centroids = np.delete(centroids, 0, 0)
        if num_labels>=1:
            max_index=np.argmax(stats[:,4])
            x = stats[max_index][0]
            y = stats[max_index][1]
            w = stats[max_index][2]
            h = stats[max_index][3]
            s = stats[max_index][4]
            self.mx = int(centroids[max_index][0]) # 重心のX座標
            self.my = int(centroids[max_index][1]) # 重心のY座標
            if s>2000:
                print(f'赤のボールを発見:({self.mx},{self.my})')
                self.info.arg = float(self.mx-320)
                cv2.rectangle(out_img, (x, y), (x+w, y+h), (255, 0, 255)) # ラベルを四角で囲む
                cv2.putText(out_img, "%d,%d"%(self.mx, self.my), (x-15, y+h+15), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 0)) # 重心を表示
                cv2.putText(out_img, "%d"%(s), (x, y+h+30), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 0)) # 面積を表示
                color = (0,255,0)
                center =(self.mx,self.my)
                radius = 20
                cv2.circle(out_img,center,radius,color)
            else:
                print("赤のボールは認識されませんでした")    
        else:
            print("赤色のボールは認識されませんでした")
        cv2.imshow('result',out_img)
        cv2.waitKey(1)
                        

def main():
    rclpy.init()
    node = DepthImg()
    print('depth_node is started')
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    rclpy.shutdown()

if __name__=='__main__':
    main()
            

解説

• depth_cbではcv_bridgeを使って画像をopenCVで扱えるように変換しています。

        try:
            cv_depth_img=self.br.imgmsg_to_cv2(img,desired_encoding='passthrough')
        except CvBridgeError as e:
            print(e)

そのあと指定されたピクセルのdepthの値を持ってきて、SearchBall型のmsgに入れています。（距離の単位はmです。）

        self.row = self.my
        self.col = self.mx
        depth_value = cv_depth_img[self.row,self.col]
        self.get_logger().info(f"Depth value at pixel ({self.row}, {self.col}): {depth_value*0.001} meters\n")
        self.info.l=depth_value*0.001

• bgr_cbではcv_bridgeで変換したあとにhsv空間に変換しています。

        try:
            cv_bgr_img = self.br.imgmsg_to_cv2(img,"bgr8")
        except CvBridgeError as e:
            print(e)
        hsv_image = cv2.cvtColor(cv_bgr_img,cv2.COLOR_BGR2HSV)

赤の領域は２つに分かれているのでマスクは２つ用意して足し合わせています。

        color_min1 = np.array([0,50,50])
        color_max1 = np.array([6,255,255])
        color_min2 = np.array([174,50,50])
        color_max2 = np.array([180,255,255])
        mask1 = cv2.inRange(hsv_image,color_min1,color_max1)
        mask2 = cv2.inRange(hsv_image,color_min2,color_max2)
        color_mask = mask1+mask2

そのマスクを使って赤色の領域だけを取り出し色がつながっている領域でラベルを取ります。
背景が一番大きなラベルとなりますが、今回背景には興味がないので削除しています。

        masked_img =cv2.bitwise_and(cv_bgr_img,cv_bgr_img,mask= color_mask)
        out_img = masked_img
        num_labels, label_img, stats, centroids = cv2.connectedComponentsWithStats(color_mask) 
        # 背景のラベルを削除
        num_labels = num_labels - 1
        stats = np.delete(stats, 0, 0)
        centroids = np.delete(centroids, 0, 0)

色の面積が2000pix以下のラベルはノイズとして無視しています。
色面積の重心位置を円で可視化しています。

        if num_labels>=1:
            max_index=np.argmax(stats[:,4])
            x = stats[max_index][0]
            y = stats[max_index][1]
            w = stats[max_index][2]
            h = stats[max_index][3]
            s = stats[max_index][4]
            self.mx = int(centroids[max_index][0]) # 重心のX座標
            self.my = int(centroids[max_index][1]) # 重心のY座標
            if s>2000:
                print(f'赤のボールを発見:({self.mx},{self.my})')
                self.info.arg = float(self.mx-320)
                cv2.rectangle(out_img, (x, y), (x+w, y+h), (255, 0, 255)) # ラベルを四角で囲む
                cv2.putText(out_img, "%d,%d"%(self.mx, self.my), (x-15, y+h+15), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 0)) # 重心を表示
                cv2.putText(out_img, "%d"%(s), (x, y+h+30), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 0)) # 面積を表示
                color = (0,255,0)
                center =(self.mx,self.my)
                radius = 20
                cv2.circle(out_img,center,radius,color)

実行結果

今回のコードを実行すると以下の画像のようになります。

端末の様子

最後に

今回はhsv変換を使って色認識をしました。想像よりも色認識は光の影響を受けやすく認識精度を上げるのは難しかったです。次回にはyolo.v8を使った画像認識を紹介したいと思います。