ROS2 Pub/Subサンプル

ROSやROS2ではPub/Sub方式で他のノードと通信することができます。
PublisherノードとSubscriberノードを作成して，Pub/Sub通信をしてみます。

プロジェクトの作成

ROS2開発環境のDockerコンテナを起動します。
Dockerイメージの作成方法，使用方法は以下の記事にまとめています。

tshell.hatenablog.com

tshell.hatenablog.com

コンテナを起動したら以下のコマンドを実行し，pubsubプロジェクトを作成していきます。

$ source /opt/ros/dashing/setup.bash
$ cd /home
$ mkdir src
$ cd src
$ ros2 pkg create --build-type ament_cmake pubsub --dependencies rclcpp

作成されたpubsubディレクトリ内のsrc ディレクトリに移動し，publisher.cppとsubscriber.cppを作成していきます。

ソースの記述

publisher.cppの作成

以下の内容でpublisher.cppを作成します。

#include <memory>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class Publisher : public rclcpp::Node
{
    public:
    Publisher() : Node("publisher_node")
    {
        timer_ = create_wall_timer(1s, std::bind(&Publisher::timer_callback, this));
        publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);

    }

    private:
    void timer_callback()
    {
        auto message = std_msgs::msg::String();
        message.data = "Hello World";
        RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
        publisher_->publish(message);
    }
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
};

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

subscriber.cppの作成

以下の内容でsubscriber.cppを作成します。

#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using std::placeholders::_1;

class Subscriber : public rclcpp::Node
{
    public:
    Subscriber() : Node("subscriber_node")
    {
        subscriber_ = this->create_subscription<std_msgs::msg::String>("topic", 10, std::bind(&Subscriber::topic_callback, this, _1));
    }

    private:
    void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
    {
        RCLCPP_INFO(this->get_logger(), "Subscribing: '%s'", msg->data.c_str());
    }
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
};


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

CMakeLists.txtの編集

ros2 pkg createコマンドで自動生成されたCMakeLists.txtを編集し，以下のような内容にします。

cmake_minimum_required(VERSION 3.5)
project(pubsub)

# 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()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

add_executable(publisher src/publisher.cpp)
ament_target_dependencies(publisher rclcpp std_msgs)
install(TARGETS
publisher
DESTINATION lib/${PROJECT_NAME}
)

add_executable(subscriber src/subscriber.cpp)
ament_target_dependencies(subscriber rclcpp std_msgs)
install(TARGETS
subscriber
DESTINATION lib/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

ビルド

上記の３ファイルを編集したら以下のコマンドを実行してビルドします。

$ cd /home
$ colcon build

動作確認

Publisherの起動

ROS2開発環境のDockerコンテナ内で以下のコマンドを実行し，Publisherを起動します。

$ cd /home/src
$ source install/setup.bash
$ ros2 run pubsub publisher

以下のように１秒毎に文字列がPublishされます。

Subscriberの起動

ROS2開発環境のDockerコンテナをもう一つ起動し，以下のコマンドを実行してSubscriberを起動します。

$ source /opt/ros/dashing/setup.bash
$ cd /home/src
$ source install/setup.bash
$ ros2 run pubsub publisher

以下のようにSubscribeした文字列を表示します。
publisher.cppとsubscriber.cppで同じトピックを指定しているので上記で起動したPublisherが出力した文字がSubscriberに表示されます。