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 インストール
- 他サイトにもいろいろと載っているので省略
- 参考↓

https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md

https://qiita.com/porizou1/items/be1eb78015828d43f9fb

# ROS melodicインストール
`$ cd && git clone https://github.com/karaage0703/jetson-nano-tools`
`$ cd ~/jetson-nano-tools `
`$ ./install-ros-melodic.sh `


https://www.amazon.co.jp/Jetson-Nano-%E6%94%B9%E8%A8%82%E7%AC%AC2%E7%89%88-Japan-Group/dp/4800712831/ref=sr_1_1?adgrpid=121202618772&dib=eyJ2IjoiMSJ9.DC5F5_bGAZ7EwxBrmr_0NF7xNwTgLtL_gZ7EaPB-kJm44JRCFteLRDQYQJXYxU3JXDwOGwDNRb3kZM1QqnqRbkIVjrgK5wCF1VJ35ujLqiwD2Bd13Q0_WVv7GSOuIJWJZYxiUbQzITOeHJmNNY3rhZjIk4QeOzuKhXu-Q0psHeVi93bqiUMROYJsilLFVYh81h9NqZc8EEpI5kgcNrCbg30vNAr8iCTNUa_DcoUHrcU9lZbwremFFMXqtEIVr3LIKo6RfgGnpKUOYSQ4xks2PCj83FgxTR92osTm5lVG2N4.qA-NR1Aso3BMOQuQkqCmBvZ3DWmJTnVJLVfTf1w4A-o&dib_tag=se&hvadid=665899714008&hvdev=c&hvqmt=e&hvtargid=kwd-847493099499&hydadcr=27489_14701809&jp-ad-ap=0&keywords=jetson+nano%E8%B6%85%E5%85%A5%E9%96%80&qid=1730101812&sr=8-1

P236より引用

# realsense-ros セットアップ
https://github.com/IntelRealSense/realsense-ros/tree/ros1-legacy
- git cloneする。ros1-legacyのブランチのモノを使用することに注意。


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

# 実行
roslaunchで`rs_camera.launch`、rosrunで`depth_subscriber.py`を実行する。
## 処理の様子
- 正方形(赤)内の深度の平均値を返す。
![3.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3805832/be023537-db7c-d768-3a7b-8a89bf683cfb.png)
- 取得できる深度がない場合(赤枠内が全て黒)はエラーを出力している。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3805832/0e2b68cc-a67f-079f-ab07-041b0274f50f.png)

# コード全文
```python: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

```

# 参考
https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md

https://qiita.com/porizou1/items/be1eb78015828d43f9fb

https://www.amazon.co.jp/Jetson-Nano-%E6%94%B9%E8%A8%82%E7%AC%AC2%E7%89%88-Japan-Group/dp/4800712831/ref=sr_1_1?adgrpid=121202618772&dib=eyJ2IjoiMSJ9.DC5F5_bGAZ7EwxBrmr_0NF7xNwTgLtL_gZ7EaPB-kJm44JRCFteLRDQYQJXYxU3JXDwOGwDNRb3kZM1QqnqRbkIVjrgK5wCF1VJ35ujLqiwD2Bd13Q0_WVv7GSOuIJWJZYxiUbQzITOeHJmNNY3rhZjIk4QeOzuKhXu-Q0psHeVi93bqiUMROYJsilLFVYh81h9NqZc8EEpI5kgcNrCbg30vNAr8iCTNUa_DcoUHrcU9lZbwremFFMXqtEIVr3LIKo6RfgGnpKUOYSQ4xks2PCj83FgxTR92osTm5lVG2N4.qA-NR1Aso3BMOQuQkqCmBvZ3DWmJTnVJLVfTf1w4A-o&dib_tag=se&hvadid=665899714008&hvdev=c&hvqmt=e&hvtargid=kwd-847493099499&hydadcr=27489_14701809&jp-ad-ap=0&keywords=jetson+nano%E8%B6%85%E5%85%A5%E9%96%80&qid=1730101812&sr=8-1