ROS2 コンポーネントの練習

# 概要

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


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

https://www.ohmsha.co.jp/book/9784274226809/

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

# 作成するもの

画像は実行時のrqt_graph

![rosgraph.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/5ac245c8-9c62-5f39-33ca-a8a0e43c76f5.png)

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

# 事前準備

ワークスペースの作成

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

パッケージの作成
```bash
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake usb_cam 
```

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

```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に以下を追加

```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

```cpp: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

```cpp: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)
```

# 実行

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

```bash
source ros2_ws/install/setup.bash
ros2 component types
```

component_containerの実行

```bash
ros2 run rclcpp_components component_container
```

image_publisher_nodeの実行

```bash
ros2 component load /ComponentManager usb_cam usb_cam_node::ImagePublisher
```

image_subscriber_nodeの実行

```bash
ros2 component load /ComponentManager usb_cam usb_cam_node::ImageSubscriber 
```

# launchファイルから実行

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

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




```py: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])

```

実行　

```bash
ros2  launch usb_cam usb_cam_node_components.launch.py
```

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


# 参考

https://qiita.com/NeK/items/72cc4fe44e563533b67b

https://www.moriken254.com/entry/2019/05/07/220454

https://zenn.dev/hakuturu583/articles/ros2_turtorial



https://docs.ros.org/en/galactic/How-To-Guides/Launching-composable-nodes.html