ROS 2 FoxyでRealSense D435の画像をsubscribeする(QoSの設定とPython、C++実装)

RealSenseをROS 2で利用するときに、少しつまずきがあったので、こちらに記録用にまとめておきます。

具体的には以下のrealsense-rosパッケージを使い、realsenseから得られるRGB画像及びDepth画像をImageトピックとしてpublishしたものを、自分でsubscribeするプログラムを作成しましたが、ROS 2ではQoS(Quality of Service)の設定を適切にしないと画像がうまくsubscribeできないということを知りました。重要なのはreliability settingをbest_effortに変更することなのですが、これを含めたrealsense画像をsubscribeするシンプルなプログラムをサンプルとして紹介します。

私の場合、普段はPythonを中心に使っていますが、画像などの重ためのデータを扱う際にノード間のデータのやり取りをプロセス内通信にして余計なメモリへのコピーをなくし、パフォーマンス低下を防げるcomponentの利用は現状C++が必須のようなのと、その他のパッケージでもサンプルがC++で記述されているものもありそうなので、念の為PythonとC++の両方の実装を行いました。なおパッケージ名や関数名などはできるだけ両言語で同じようになるようにしましたが、両言語のパッケージをそれぞれ試される場合は重複を避けるためパッケージ名を変更してください。

Componentに興味のある方はこちらを参考にしてください。 docs.ros.org

事前に以下のサイトの手順に従ってrealsense-rosパッケージをビルドしてください。

github.com

また今回つまずきのポイントになったQoSに関する説明はこちらにあります。やり取りすデータに欠損などが発生した際のデータの扱いに関するルールを事前に定義しておける機能と理解しましたが、今回はその中のReliabilityPolicyというものがRELIABLEになっていると、RealSenseの画像データがsubscribeされないという問題が発生したので設定を変更してBEST_EFFORTにしています。

docs.ros2.org

Python実装

パッケージを作成し、image_subscriber.pyを作成する。

$ cd ~/ros2_ws/src
$ ros2 pkg create --build-type ament_python realsense_subscriber
$ touch ~/ros2_ws/src/realsense_subscriber/src/realsense_subscriber/image_subscriber.py

image_subscriber.py

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2


class RealSenseSubscriber(Node):
    def __init__(self):
        super().__init__("realsense_subscriber_python")
        self.declare_parameter('image_topic_name', '/camera/color/image_raw')
        image_topic_name = self.get_parameter('image_topic_name').get_parameter_value().string_value

        video_qos = rclpy.qos.QoSProfile(depth=10)
        video_qos.reliability = rclpy.qos.QoSReliabilityPolicy.BEST_EFFORT

        self.sub_img = self.create_subscription(
            Image,
            image_topic_name,
            self.on_image_subscribed,
            video_qos
        )
        self.sub_img
    
    def on_image_subscribed(self, img):
        img_np = CvBridge().imgmsg_to_cv2(img)
        img_np = cv2.cvtColor(img_np, cv2.COLOR_BGR2RGB)
        cv2.imshow("Image", img_np)
        cv2.waitKey(1)


def main(args=None):
    try:
        rclpy.init(args=args)
        rclpy.spin(RealSenseSubscriber())
    
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()
        

if __name__ == "__main__":
    main()

setup.pyを編集し、entry_pointsにrealsense_subscriberを追記する。

from setuptools import setup

package_name = 'realsense_subscriber'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='root',
    maintainer_email='root@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            "realsense_subscriber = realsense_subscriber.image_subscriber:main"
        ],
    },
)

package.xmlを編集し、rclpyとsensor_msgsを追記する。

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>realsense_subscriber</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="root@todo.todo">root</maintainer>
  <license>TODO: License declaration</license>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>

  <export>
    <build_type>ament_python</build_type>
    <exec_depend>rclpy</exec_depend>
    <exec_depend>sensor_msgs</exec_depend>
  </export>
</package>

C++実装

パッケージを作成し、image_subscriber.cppを作成する。

$ cd ~/ros2_ws/src
$ ros2 pkg create --build-type ament_cmake realsense_subscriber
$ touch ~/ros2_ws/src/realsense_subscriber/src/image_subscriber.cpp

image_subscriber.cpp

#include <rclcpp/rclcpp.hpp>
#include <rclcpp/qos.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>


using sensor_msgs::msg::Image;

class RealSenseSubscriber: public rclcpp::Node
{
    public:
        RealSenseSubscriber(rclcpp::NodeOptions options = rclcpp::NodeOptions());
        ~RealSenseSubscriber(){}
    
    private:
        void onImageSubscribed(Image::SharedPtr img);
        rclcpp::Subscription<Image>::SharedPtr sub_img;
        std::string image_topic_name;
};

RealSenseSubscriber::RealSenseSubscriber(rclcpp::NodeOptions options) : Node("realsense_subscriber", options)
{
    image_topic_name = this->declare_parameter<std::string>("image_topic_name", "/camera/color/image_raw");
    rclcpp::QoS video_qos(10);
    video_qos.best_effort();
    video_qos.durability_volatile();
    sub_img = this->create_subscription<Image>(image_topic_name, video_qos, std::bind(&RealSenseSubscriber::onImageSubscribed, this, std::placeholders::_1));
}

void RealSenseSubscriber::onImageSubscribed(Image::SharedPtr img)
{
    auto cv_img = cv_bridge::toCvShare(img, img->encoding);
    cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2BGR);
    cv::imshow("Image", cv_img->image);
    cv::waitKey(1);
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<RealSenseSubscriber>());
    rclcpp::shutdown();
    return 0;
}

CMakeList.txtの記述を追加する。

cmake_minimum_required(VERSION 3.5)
project(realsense_subscriber)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

set(ROS_LIBS rclcpp;cv_bridge;sensor_msgs)
find_package(ament_cmake REQUIRED)
foreach(lib IN LISTS ROS_LIBS)
  find_package(${lib} REQUIRED)
endforeach()

set(target image_subscriber)
add_executable(${target} src/${target}.cpp)
ament_target_dependencies(${target} rclcpp OpenCV cv_bridge sensor_msgs)
install(TARGETS ${target} DESTINATION lib/${PROJECT_NAME})

ament_package()

package.xmlに依存ライブラリとして、rclcppとlibopencv-devを追加する。

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>realsense_subscriber</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="root@todo.todo">root</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>rclcpp</depend>
  <depend>libopencv-dev</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

PythonとC++共通

こちらをコンパイルする。

$ colcon build --symlink-install --packages-select realsense_subscriber
$ source install/setup.bash

コンパイル後にまずrealsense-rosパッケージにあるrs_launch.pyを立ち上げます。

$ ros2 launch realsense2_camera rs_launch.py

その後、別のターミナルで作成したノードを実行します。

$ ros2 run realsense_subscriber image_subscriber

正しく実行できれば、ウィンドウが表示され、RealSenseから取得した画像が表示されます。(筆者環境ではDocker使用のため、表示にXephyrを使用していますが、マシンにROS 2を直接インストールしている場合はWindowが新規に表示されると思います)

f:id:hygradme:20210925234738p:plain

なおsubscribeするトピック名をパラメータで定義しているので、ノード起動時に引数を指定することでDepth画像を表示したり、別のWebカメラの画像などをpublishするノードを立ててそのtopicを指定すれば、その画像を表示することも可能です。参考にdepth画像をsubscribeする際の引数の設定がこちらになります。

$ ros2 run realsense_subscriber image_subscriber  --ros-args -p image_topic_name:=/camera/depth/image_rect_raw

なおDepth画像のピクセル値はmmで計測された深度情報が入っているので、そのまま表示しても0~255の値の範囲しか通常の画像表示では行えないので、25.5mm程度の距離までしか適切に表示できません。ピクセル値をそれぞれ一定の値で割って(1000など）、0~255の範囲に計測範囲の距離が収まるようにするとわかりやすく画像を表示することができるようになります。

参考

ScamperとRaspberry Piで学ぶROS2プログラミング入門(オーム社)
ロボットプログラミングROS2の実装・実践(科学情報出版株式会社)