ROS2 コンポーネントの練習

概要

ROS2のコンポーネントの練習として、usbカメラの画像をImageトピックとしてPublishするノードとSubscribeするノードをコンポーネントを使って実装し実行する。

プログラムの作成にあたっては以下の書籍の10章を参考にした

compositionの詳細については公式のドキュメントか参考にある記事を参考にしてください
https://docs.ros.org/en/foxy/Tutorials/Intermediate/Composition.html

作成するもの

画像は実行時のrqt_graph

• image_publisher_node : OpenCVでUSBカメラからRGB画像を取得し、画像をsensor_msg/ImageトピックをPublishするノード
• image_subscriber_node : sensor_msg/ImageトピックをSubscribeして画像を表示するノード

事前準備

ワークスペースの作成

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
colcon build 

パッケージの作成

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake usb_cam 

CMakeLists.txtに依存パッケージを追加

CMakeLists.txt

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(OpenCV REQUIRED COMPONENTS core highgui videoio)
find_package(rclcpp_components REQUIRED)

作成したプログラム

~/ros2_ws/src/usb_cam/src/ 以下に作成

ros2 componentから実行できるようにCMakeLists.txtに以下を追加

CMakeLists.txt

add_library(${PROJECT_NAME} SHARED
  src/usb_cam.cpp
  src/image_subscriber.cpp
)
ament_target_dependencies(${PROJECT_NAME} rclcpp OpenCV cv_bridge sensor_msgs rclcpp_components)
rclcpp_components_register_nodes(${PROJECT_NAME} "usb_cam_node::ImagePublisher")
rclcpp_components_register_nodes(${PROJECT_NAME} "usb_cam_node::ImageSubscriber")

install(TARGETS ${PROJECT_NAME} DESTINATION lib)
# Call ament build system
ament_package()

image_publisher_node

usb_cam.cpp

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

namespace usb_cam_node
{

class ImagePublisher : public rclcpp::Node
{
public:
  ImagePublisher(rclcpp::NodeOptions options);
  ~ImagePublisher();

private:
  void publishImage();
  rclcpp::TimerBase::SharedPtr timer_;
  cv::VideoCapture camera_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
};


ImagePublisher::ImagePublisher(rclcpp::NodeOptions options) : Node("image_publisher", options)
{
  timer_ = this->create_wall_timer(std::chrono::milliseconds(1000 / 30), std::bind(&ImagePublisher::publishImage, this));
  image_pub_ = create_publisher<sensor_msgs::msg::Image>("image_raw", 10);

  auto camera_path = this->declare_parameter<std::string>("camera_path", "/dev/video0");
  camera_.open(camera_path);
  if(!camera_.isOpened()){
    RCLCPP_ERROR(this->get_logger(), "Failed to open %s", camera_path.c_str());
    rclcpp::shutdown();
  }
}

ImagePublisher::~ImagePublisher()
{
}

void ImagePublisher::publishImage()
{
  sensor_msgs::msg::Image ros_img;
  cv_bridge::CvImage cv_img;
  cv_img.encoding = "bgr8";

  camera_ >> cv_img.image;
  cv_img.header.stamp = this->now();
  cv_img.toImageMsg(ros_img);
  image_pub_->publish(std::move(ros_img));
}

}

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(usb_cam_node::ImagePublisher)

image_subscriber_node

image_subscriber.cpp

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


namespace usb_cam_node
{

class ImageSubscriber : public rclcpp::Node
{
public:
  ImageSubscriber(rclcpp::NodeOptions options);
  ~ImageSubscriber();
private:
  void imageCallback(sensor_msgs::msg::Image::SharedPtr img);
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
};


ImageSubscriber::ImageSubscriber(rclcpp::NodeOptions options) : Node("image_subscriber", options)
{
  using std::placeholders::_1;
  image_sub_ = this->create_subscription<sensor_msgs::msg::Image>("image_raw", 10, std::bind(&ImageSubscriber::imageCallback, this, _1));
}

ImageSubscriber::~ImageSubscriber()
{
}

void ImageSubscriber::imageCallback(sensor_msgs::msg::Image::SharedPtr img)
{
  auto cv_img = cv_bridge::toCvShare(img, img->encoding);
  if(cv_img->image.empty()) return;
  cv::imshow("Image", cv_img->image);
  cv::waitKey(1);
}

}

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(usb_cam_node::ImageSubscriber)

実行

コンポーネント一覧を表示

source ros2_ws/install/setup.bash
ros2 component types

component_containerの実行

ros2 run rclcpp_components component_container

image_publisher_nodeの実行

ros2 component load /ComponentManager usb_cam usb_cam_node::ImagePublisher

image_subscriber_nodeの実行

ros2 component load /ComponentManager usb_cam usb_cam_node::ImageSubscriber 

launchファイルから実行

以下のようにpythonのlaunchファイルをlaunchディレクトリ以下に作成する

cd ~/ros2_ws/src/usb_cam/
mkdir launch
# launchファイル作成
cd ~/ros2_ws/
colcon build

usb_cam_node_components.launch.py

import launch
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode


def generate_launch_description():
    """Generate launch description with multiple components."""
    container = ComposableNodeContainer(
            name='image_container',
            namespace='',
            package='rclcpp_components',
            executable='component_container',
            composable_node_descriptions=[
                ComposableNode(
                    package='usb_cam',
                    plugin='usb_cam_node::ImagePublisher',
                    name='image_publisher_node',
                    extra_arguments=[{'use_intra_process_comms': True}]),
                ComposableNode(
                    package='usb_cam',
                    plugin='usb_cam_node::ImageSubscriber',
                    name='image_subscriber_node',
                    extra_arguments=[{'use_intra_process_comms': True}])
            ],
            output='both',
    )

    return launch.LaunchDescription([container])

実行　

ros2  launch usb_cam usb_cam_node_components.launch.py

同様にUSBカメラの画像が表示されれば成功

参考