背景

Robotec.ai社のros2-for-unityを使ってみて、自分でもスクラッチの状態からUnityやその他のアプリにROS2ノードを組み込んでROS2を直接喋らせるのをやってみたいと思いました。
いきなり共有オブジェクト(.so)をUnityに組み込むのも良いのですが、動作を理解したりデバッグしたりする上では、呼び出す側のプログラムはできるだけシンプルな方が良いです。
なので今回は手始めとして、ROS2ノードとして振る舞う共有オブジェクトと、その共有オブジェクトを呼び出すアプリも実装して動作を確認してみることにします。

今回つくったもの

ROS Shared Object
- ソースコード一式を含むプロジェクトです。
ros_shared_object.cpp
- 公式のデモでおなじみのTalker-listenerのTalkerと同等の機能を持つ共有オブジェクトのソースコードです。
- 公式のTalkerと同じトピック名を使っているので公式のListenerとの間でpub/sub通信が可能です。
test.cpp
- 今回作成する共有オブジェクトを呼び出すためのアプリのソースコードです。
CMakeLists.txt
- 共有オブジェクトとアプリをビルドするためのcmake用のファイルです。

環境

OS: Ubuntu 22.04
ROS2: Rolling Ridley (公式の手順に従ってインストールされたros-rolling-desktopを想定)
その他: git、cmake、build-essentialなど

解説

1 ROS2ノードとして振舞う共有オブジェクトのプログラム

公開しているリポジトリのros_shared_object.cpp が全てです。
このソースコードはgodot_rosのtalker.hppを参考にして実装しました。
短いので全文を掲載します。

ros_shared_object.cpp

#include <cstdlib>
#include <cstring>
#include <memory>
#include <chrono>

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

// Talkerクラスの定義と実装
class Talker
{
public:
    Talker()
    {
        // ノードとして機能するようにここでrclcppを初期化する。引数はダミー
        rclcpp::init(0, nullptr);

        // ノードを生成する
        node_ = std::make_shared<rclcpp::Node>("ros_shared_object_talker_node");
        rclcpp::QoS qos(rclcpp::KeepLast(7));

        // トピック名を"chatter"にすることで、ROS2のdemo_nodesのListenerで購読できるようになる
        pub_ = node_->create_publisher<std_msgs::msg::String>("chatter", qos);
    }

    Talker(const Talker&) = delete;
    Talker& operator = (const Talker&) = delete;

    ~Talker()
    {
        rclcpp::shutdown();
    }

    void spin_some()
    {
        // アプリケーション側から都度呼び出されることを想定
        rclcpp::spin_some(node_);
    }

    void talk(const int &count)
    {
        // 配信する
        msg_ = std::make_unique<std_msgs::msg::String>();
        msg_->data = "Hello from shared object: " + std::to_string(count);
        RCLCPP_INFO(node_->get_logger(), "Publishing: '%s'", msg_->data.c_str());
        pub_->publish(std::move(msg_));
    }

private:
    std::shared_ptr<rclcpp::Node> node_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
    std::unique_ptr<std_msgs::msg::String> msg_;
};

extern "C"
{
// アプリケーションからこの共有オブジェクトの機能にアクセスするためのエントリポイントの定義と実装
// manglingを避けるためにCリンケージにする

// Talkerのインスタンスを生成する
intptr_t create()
{
    return reinterpret_cast<intptr_t>(new Talker);
}

// Talkerのspin_someを呼び出す
void spin_some(intptr_t ptr)
{
    reinterpret_cast<Talker*>(ptr)->spin_some();
}

// Talkerのtalkを呼び出す
void talk(intptr_t ptr, int count)
{
    reinterpret_cast<Talker*>(ptr)->talk(count);
}

// Talkerのインスタンスを破棄する
void destroy(intptr_t ptr)
{
    delete reinterpret_cast<Talker*>(ptr);
}

}

一般的なTalkerの実装に加えて、rclcpp::init(0, nullptr)およびrclcpp::shutdown()によるrclcppの初期化と終了、Talker::spin_some()の呼び出し、そして各エントリポイントの定義と実装のあたりがポイントかなと思います。

2 共有オブジェクトを呼び出すアプリ

公開しているリポジトリのtest.cpp が全てです。
こちらも短いので全文を掲載します。

test.cpp

#include <dlfcn.h>
#include <cstdint>
#include <iostream>
#include <chrono>
#include <thread>
#include <csignal>

using namespace std;

namespace
{
    const string SO_FILE_PATH = "./libros_shared_object.so";
    volatile sig_atomic_t flag = 1;

    void handler(int signum)
    {
        flag = 0;
    }
}

int main(int argc, char* argv[])
{
    // Ctrl+Cが入力されたときに、きちんと終了処理をするためにSIGINTをハンドルする
    signal(SIGINT, handler);

    // 共有オブジェクトをロードする
    void* handle = dlopen(SO_FILE_PATH.c_str(), RTLD_NOW);
    cout << "dlopen: " << &handle << endl;

    // 各エントリポイントを取得する
    intptr_t (*create)() = (intptr_t (*)())dlsym(handle, "create");
    void (*spin_some)(intptr_t) = (void (*)(intptr_t))dlsym(handle, "spin_some");
    void (*talk)(intptr_t, int) = (void (*)(intptr_t, int))dlsym(handle, "talk");
    void (*destroy)(intptr_t) = (void (*)(intptr_t))dlsym(handle, "destroy");

    // インスタンスを生成する
    intptr_t instance = (*create)();

    while (flag != 0)
    {
        // 1秒刻みでtalkとspin_someを呼び出す
        (*talk)(instance, 0);
        (*spin_some)(instance);
        this_thread::sleep_for(chrono::seconds(1));
    }

    (*destroy)(instance);

    dlclose(handle);

    return 0;
}

アプリの方の実装は、共有オブジェクトを利用するための一般的な作法になっていると思います。
見ての通り、ROS2関連のAPIは一切呼び出していません。
あと、あえてポイントというと、signal(SIGINT, handler)でCtrl+Cをハンドルして、ループの終了とそのあとの終了処理が呼ばれるようにしているところでしょうか。こうしないと終了時に終了処理がおろそかになっているというような旨の警告が出てしまいます。

3 CMaleLists.txt

CMakeLists.txtが共有オブジェクトとテストアプリをビルドするためのcmake用のファイルです。

CMakeLists.txt

cmake_minimum_required(VERSION 3.13)
project(ros_shared_object CXX)

enable_language(CXX)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)

set (ROS_DISTRO "rolling")

set (ROS_INCLUDE_DIR "/opt/ros/${ROS_DISTRO}/include")
message(STATUS "ROS_INCLUDE_DIR = ${ROS_INCLUDE_DIR}")
set (ROS_LINK_DIR "/opt/ros/${ROS_DISTRO}/lib")
message(STATUS "ROS_LINK_DIR = ${ROS_LINK_DIR}")

add_library(ros_shared_object SHARED ros_shared_object.cpp)

target_include_directories(ros_shared_object
PRIVATE
    "${ROS_INCLUDE_DIR}"
    "${ROS_INCLUDE_DIR}/rclcpp"
    "${ROS_INCLUDE_DIR}/rcl"
    "${ROS_INCLUDE_DIR}/rcutils"
    "${ROS_INCLUDE_DIR}/rmw"
    "${ROS_INCLUDE_DIR}/rcl_yaml_param_parser"
    "${ROS_INCLUDE_DIR}/rosidl_runtime_c"
    "${ROS_INCLUDE_DIR}/rosidl_typesupport_interface"
    "${ROS_INCLUDE_DIR}/rcpputils"
    "${ROS_INCLUDE_DIR}/builtin_interfaces"
    "${ROS_INCLUDE_DIR}/rosidl_runtime_cpp"
    "${ROS_INCLUDE_DIR}/tracetools"
    "${ROS_INCLUDE_DIR}/rcl_interfaces"
    "${ROS_INCLUDE_DIR}/libstatistics_collector"
    "${ROS_INCLUDE_DIR}/statistics_msgs"
    "${ROS_INCLUDE_DIR}/std_msgs"
    "${ROS_INCLUDE_DIR}/sensor_msgs"
    "${ROS_INCLUDE_DIR}/geometry_msgs"
)

target_link_directories(ros_shared_object
PRIVATE
    ${ROS_LINK_DIR}
)

target_link_libraries(ros_shared_object
PRIVATE
    rclcpp
    rcl
    rcl_yaml_param_parser
    rcutils
    rcpputils
    rmw
    rmw_implementation
    tracetools
    ament_index_cpp
    std_msgs__rosidl_typesupport_cpp
    sensor_msgs__rosidl_typesupport_cpp
    rosidl_typesupport_cpp
    rosidl_typesupport_c
    rosidl_runtime_c
    libstatistics_collector
    statistics_msgs__rosidl_typesupport_cpp
    rosgraph_msgs__rosidl_typesupport_cpp
    rcl_interfaces__rosidl_typesupport_cpp
    rcl_interfaces__rosidl_typesupport_c
    rcl_interfaces__rosidl_generator_c
    rcl_logging_interface
    rcl_logging_spdlog
    builtin_interfaces__rosidl_generator_c
)

add_executable(test test.cpp)

ここのポイントはcolconを使わずにcmakeを直接呼び出してMakefileを生成するための工夫です。
特にtarget_include_directoriesの中で各モジュールのインクルードパスを設定しているところと、target_link_librariesに各ライブラリを列挙しているところは、colconに頼らずにビルドするために必要な記述です。こういうふうに依存関係を手書きしてみると、colconのありがたみが身に沁みます。

ビルドから実行まで

ビルド

共有オブジェクトlibros_shared_object.soと、アプリtestをビルドするには、ターミナルを開いて次のようにします。

git clone https://github.com/comoc/ros_shared_object.git
cd ros_shared_object
mkdir build && cd build
cmake ..
make

成功すれば、同じディレクトリ内にlibros_shared_object.soとtestが生成されます。
このターミナルは実行時も使うのでそのままにしておいてください。

実行

もう一つターミナルを開いてListerを起動します。Python版でもC++版でもどちらでも良いです。

source /opt/ros/rolling/setup.bash
ros2 run demo_nodes_py listener # Python版のListener
#または
ros2 run demo_nodes_cpp listener # C++番のListener

ビルドに使ったターミナルでtestアプリを起動します。

source /opt/ros/rolling/setup.bash
./test

実行結果

tmuxで左右に分割して表示してみました。
左がListener、右が今回作ったものです。

双方でpub/sub出来ている様子が分かると思います。

まとめ

今回アプリにネイティブでROS2を喋らせるようにするための第一歩として、共有オブジェクト化したROS2ノードを自作のアプリから呼び出して既存のノードとpub/sub通信するのをやってみました。
作り方さえ分かってしまえばそれほど難しいものでもない事が分かったので、アプリにROS2を喋せるためのハードルが下がった気がします。
こういうのは急がば回れで一回自分の手で一からやってみるのが良いですね。
作った共有オブジェクトをUnityに組み込めたら続編を書きたいと思います。
では。

参考

flynneva/godot_ros
- Godot用のROS2モジュール。自分でやってみようとしたときの指南役になってくれました。