ROS_melodic で深度を取得する

はじめに

• Realsenseを用いた深度情報を取得する方法として、直接取得する方法はネット上にあるがROSを用いて取得する方法はあまり紹介されていなかったので、備忘録、引継ぎもかねて投稿します。
• 今回の実装方法では、画像中央付近における平均深度を取得しています。なお、深度取得できていない部分(Colorized_depth_image上の黒い部分)は除外する仕様になっています。
• 今時、melodic？と思う方もいると思いますが、何かの参考になれば。

環境

• Jetson Nano
• ROS Melodic(Ubuntu 18.04)
• Realsense D415

忙しい人のために

• /camera/depth/image_rect_raw に深度情報が入っているのでそれをSubすればいい。

手順

• Intel Realsense SDK インストール
• ROS melodic インストール
• realsense-ros セットアップ
• Subscriberの作成
• 確認

Intel Realsense SDK インストール

• 他サイトにもいろいろと載っているので省略
• 参考↓

ROS melodicインストール

$ cd && git clone https://github.com/karaage0703/jetson-nano-tools
$ cd ~/jetson-nano-tools
$ ./install-ros-melodic.sh

P236より引用

realsense-ros セットアップ

• git cloneする。ros1-legacyのブランチのモノを使用することに注意。

Subscriberの作成

処理概要

• rs_camera.launchのノードからPubされる/camera/depth/image_rect_rawをSubする。
• 中央付近の深度情報を抽出する。
• その内、深度情報があるデータのみで平均値を算出する。

実行

roslaunchでrs_camera.launch、rosrunでdepth_subscriber.pyを実行する。

処理の様子

• 正方形(赤)内の深度の平均値を返す。
  
• 取得できる深度がない場合(赤枠内が全て黒)はエラーを出力している。
  

コード全文

depth_subscriber.py

#!/usr/bin/env python
## coding: UTF-8

import rospy
import cv2 as cv
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from std_msgs.msg import Float32,Int16
import numpy as np
import math

class CentralDepthMeasurement:
    def __init__(self):
        rospy.init_node('drive_forward_by_depth')
        rospy.Subscriber('/camera/depth/image_rect_raw', Image, self.depth_callback)
        # ROS特有のメッセージ形式をOpencvの画像形式に変換するもの。
        self.bridge = CvBridge()

    def depth_callback(self, data):
        depth_image = self.bridge.imgmsg_to_cv2(data, desired_encoding='passthrough')
        #お好みで画像サイズを変更してください。
        depth_resize_image = cv.resize(depth_image,(640,360))
        # 深度画像を正規化し、uint8型に変換している。
        normalized_depth = cv.normalize(depth_resize_image, None, 0, 255, cv.NORM_MINMAX)
        normalized_depth = np.uint8(normalized_depth)

        image_height, image_width = depth_resize_image.shape
        # 取得する深度画像の範囲(１辺のpx数)を設定する。
        measure_range_px = 20
        center_depth_array = []        

        for i in range(measure_range_px):
            for j in range(measure_range_px):
                measure_pixel_height = (image_height/2 - measure_range_px / 2) + i
                measure_pixel_width = (image_width/2 - measure_range_px / 2) + j
                # resize後の画像を指定すること。
                measure_pixel_depth = depth_resize_image[measure_pixel_height, measure_pixel_width]
                # 取得できなかった深度情報は平均値算出時に用いない。
                if measure_pixel_depth > 0:
                    center_depth_array.append(measure_pixel_depth)
        average_center_depth = np.mean(center_depth_array)

        if math.isnan(average_center_depth):
            rospy.logerr("ERROR: 深度を取得できません。")
        else:
            rospy.loginfo("average_center_depth '{}'".format(average_center_depth))

        # 深度画像を白黒画像からカラー画像に変換する。
        colored_depth = cv.applyColorMap(normalized_depth, cv.COLORMAP_JET)
        # 深度測定範囲を描画する。
        top_left_coordinate = ((image_width/2 - measure_range_px / 2),(image_height/2 - measure_range_px / 2))
        bottom_right_coordinate = ((image_width/2 + measure_range_px / 2),(image_height/2 + measure_range_px / 2))
        cv.rectangle(colored_depth, top_left_coordinate, bottom_right_coordinate, (0, 0, 255), 2)
        cv.imshow("colored_depth_data", colored_depth)
        cv.waitKey(1)

if __name__ == '__main__':
    try:
        system = CentralDepthMeasurement()
        rospy.spin()
    except rospy.ROSInterruptException:pass

参考