Jupyter Notebook から rclpy を読み出す実験 その７：SubしてPubする

すでにあるトピックを購読、加工して再配布というような処理を考えます。
今回はD435の値を一定の書式にしてmsgとして配布する方法を試してみました。

実際のところ

前提

• Ubuntu 22 LTS
• Jupyter

スクリプト

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from std_msgs.msg import String
from cv_bridge import CvBridge, CvBridgeError
import cv2

# ...

# Realsense SDK の深度パラメータを取得し、一番深い部分と浅い部分の座標をrclpyのtopicで送信する
class DepthInfoTopiPublisher(Node):
    # 初期化
    def __init__(self):
        super().__init__('depth_info_topic_publisher')
        self.publisher_ = self.create_publisher(String, '/depth_info', 10)
        self.subscription = self.create_subscription(
            Image,
            '/camera/camera/depth/image_rect_raw',
            self.depth_callback,
            10)
        self.bridge = CvBridge()
        self.depth_image = None

    # コールバック関数
    def depth_callback(self, data):
        try:
            self.depth_image = self.bridge.imgmsg_to_cv2(data, "passthrough")
            self.process_depth_image()
        except CvBridgeError as e:
            self.get_logger().error(str(e))

    def process_depth_image(self):
        min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(self.depth_image)
        print("I'm here")
        msg = String()
        msg.data = f'Nearest point is at: {min_loc} with a depth of: {min_val} units\nFurthest point is at: {max_loc} with a depth of: {max_val} units'
        self.get_logger().info(msg.data)
        self.publisher_.publish(msg)

depth_info_topic_publisher = DepthInfoTopiPublisher()

rclpy.spin_once(depth_info_topic_publisher)

I'm here
[INFO] [XXX.YYY] [depth_info_topic_publisher]: Nearest point is at: (0, 0) with a depth of: 0.0 units
Furthest point is at: (5, 17) with a depth of: 8066.0 units

さらにトピックの方でも

$ ros2 topic echo /depth_info
data: 'Nearest point is at: (0, 0) with a depth of: 0.0 units

  Furthest point is at: (5, 17) with a depth of: 8066.0 units'
---