ros2 foxyでnode間通信を行う（C++)

はじめに

今回は簡単なros2のpubsub通信についての記録。

環境

os	ubuntu20.04
ros2	foxy

内容

今回はpublisher(情報を発信する側)とsubscriber(情報を受け取る側)のふたつのnodeを立ち上げて
topic（情報を流す通路のようなもの）を用いて通信しようと思います。

実際の動き↓

実装

ros2ではディレクトリのなかにパッケージを作成しそのパッケージのなかでコード等を書いていきます。

ワークスペースの準備

mkdirでワークスペースを作成する。（名前は自由）
cdでつくったワークスペースに入りましょう

mkdir ros2_ws/src
cd ros2_ws/src

パッケージを作成する

専用のコマンドを用いてパッケージを作成する

~/ros2_ws/src$ros2 pkg create --build-type [タイプを指定]　[パッケージ名]

今回はtest_pubsubという名前のパッケージを作成します

ros2 pkg create --build-type ament_cmake test_pubsub

するといろいろなファイル、フォルダが作成されます
コードはパッケージのなかにあるsrcというフォルダのなかに追加します

Publisher側

まずコード全体

publisher.cpp

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

#include <chrono>

using namespace std::chrono_literals;

class Publisher : public rclcpp::Node
{
    public:
    Publisher()
    : Node("test_publisher")
    {
      publisher = this->create_publisher<std_msgs::msg::String>("test_topic", 10);
      timer = this->create_wall_timer(
      500ms, std::bind(&Publisher::timer_callback, this));
    }
    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;
}

#includeの部分で必要なものを調達

rclcpp:ros2で必要
std_msgs/msg/string:string型のメッセージを使用するため必要
chrono:時間操作系

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

#include <chrono>

using namespace std::chrono_literals;

classで始まる｛　｝の中身でnodeを定義する

classの中のpublic部分

test_publisherがnode名です

Publisher()
    : Node("test_publisher")

publisherという名前のpublisherを作りました(ややこしくてごめんなさい)
thisはnodeの概念を引用しているのでnodeに能力を付与しているようなイメージです

publisher = this->create_publisher<メッセージ型>("topic名", １とか１０とか);

今回はtimerも作成し一定時間（今回だと0.5秒）ごとに関数を呼び出すことにしました。

private部分

timer_callback関数を定義。
以下が呼び出す

timer = this->create_wall_timer(
      500ms, std::bind(&Publisher::timer_callback, this));

String型のメッセージを宣言
そのなかに"Hello, world!"が入ってる
RCLCPP_INFOでこのコードが動いたときにターミナルに"Publishing: '%s'"が表示されます
最後にpublisherが情報を発信するよう命令しています

timer_callback関数

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);
    }

最後にtimerやpublisherなどを宣言してください（私は忘れがち）

rclcpp::TimerBase::SharedPtr timer;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher;

main関数

上から
ros2の初期化
nodeを動かす
ros2を終了します

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

subscriber側

コード全体

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("test_subscriber")
    {
      subscription = this->create_subscription<std_msgs::msg::String>(
      "test_topic", 10, std::bind(&Subscriber::sub_callback, this, _1));
    }

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

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

基本的にはpublisher側と同じです
sub_callback関数はsubscriptionが情報を受け取るたびに呼び出されます
ここで必ずpublisher側とsubscriber側でtopic名が同じになるようにしてください

package.xmlの編集

”ament_cmake”の下に以下を追加してください

  <depend>rclcpp</depend>
  <depend>std_msgs</depend>

もしgeometry_msgsを使った場合はそれも記入する

CMakeLists.txtの編集

まず”find_package(ament_cmake REQUIRED)”のしたに以下を追加

find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

ここからはpublisher側とsubscriber側で分けて書きます

publisher

add_executable(talker src/publisher.cpp)
ament_target_dependencies(talker rclcpp std_msgs)

install(TARGETS
  talker
  DESTINATION lib/${PROJECT_NAME})

talkerという名前でこのパッケージの中にアプリのようなかたちで
実装しています

subscriber

add_executable(listener src/subscriber.cpp)
ament_target_dependencies(listener rclcpp std_msgs)

install(TARGETS
  listener
  DESTINATION lib/${PROJECT_NAME})

publisherと同様です

実施

以上で準備が整いました
ros2_wsの中にいる状態でbuild（パッケージを立ち上げること）します

~/ros2_ws$ colcon build

ここで”Summary: 1 package finished”とでていればokです
続いて以下を行ってください

.  install/setup.bash

１つ目のターミナル

ros2 run test_pubsub talker

１つ目の引数がパッケージ名で2つ目が実行するアプリです。
２つ目のターミナル

ros2 run test_pubsub listener

結果

以下のように表示されるはずです

talker

[INFO] [1674725105.241128964] [test_publisher]: Publishing: 'Hello, world! '
[INFO] [1674725105.741356342] [test_publisher]: Publishing: 'Hello, world! '
[INFO] [1674725106.241327303] [test_publisher]: Publishing: 'Hello, world! '
[INFO] [1674725106.741119231] [test_publisher]: Publishing: 'Hello, world! '
[INFO] [1674725107.241057022] [test_publisher]: Publishing: 'Hello, world! '
[INFO] [1674725107.741096984] [test_publisher]: Publishing: 'Hello, world! '
[INFO] [1674725108.241035940] [test_publisher]: Publishing: 'Hello, world! '
[INFO] [1674725108.741051098] [test_publisher]: Publishing: 'Hello, world! '
...

listener

[INFO] [1674725156.172499793] [test_subscriber]: I heard: 'Hello, world! '
[INFO] [1674725156.672715761] [test_subscriber]: I heard: 'Hello, world! '
[INFO] [1674725157.172353140] [test_subscriber]: I heard: 'Hello, world! '
[INFO] [1674725157.672381771] [test_subscriber]: I heard: 'Hello, world! '
[INFO] [1674725158.172521599] [test_subscriber]: I heard: 'Hello, world! '
[INFO] [1674725158.672336151] [test_subscriber]: I heard: 'Hello, world! '
[INFO] [1674725159.172307888] [test_subscriber]: I heard: 'Hello, world! '
[INFO] [1674725159.672672949] [test_subscriber]: I heard: 'Hello, world! '
...

最後に

今回はc++でros2のpubsub通信をおこなってみました
書き方は公式に則っているので
わかりやすくかけたのではないかと思います