sykwer’s blog

力こそパワー

C++版ROS2のメッセージ型構造体にカスタムメモリアロケータを指定したい

この記事は, ROS2 Advent Calendar 2022 の12月16日分の記事です.

2022年12月16日時点のROS2エコシステムの現状を元に書いた記事ですので, 近い将来事情が変わっているかもしれません.

qiita.com

こんにちは, @sykwer です. 普段は自動運転システムに特化したOSやそのリアルタイム性についての研究開発をしています.

本記事は, C++版のROS2メッセージ型構造体に対して, カスタムメモリアロケータを指定することができるか検証するという内容になります.

ROS2公式ドキュメントに示される通り, publisherオブジェクト, subscriptionオブジェクト, executorオブジェクトなどに対してカスタムアロケータを渡すことができます (Implementing a custom memory allocator — ROS 2 Documentation: Humble documentation). しかし, rosidlによって生成されるROS2メッセージ型構造体のメモリ戦略を指定するためのカスタムメモリアロケータの渡し方はどこにも載っていません.

ROS2メッセージ型構造体にカスタムメモリアロケータを渡す方法が存在するか確かめるために, rosidlの中身や, rosidlによって自動生成されるコードの中身を調査します.

カスタムメモリアロケータを指定するモチベ

C++においては, std::vectorやstd::mapなどのコンテナ型に対して, テンプレート引数でアロケータ型を指定することができます.

template<class T, class Allocator = std::allocator<T>>
class vector;

template<class Key, class T, class Compare = std::less<Key>, class Allocator = std::allocator<std::pair<const Key, T>>>
class map;

このアロケータ型を明示的に指定しない場合, std::allocator<T> がデフォルトアロケータとして使用されます. 後の章で説明する通り, rosidlによって自動生成されるROS2メッセージ型構造体も同様で, アロケータには std::allocator<T> が使用されます.

C++におけるアロケータとは, C++ named requirements: Allocator - cppreference.com で説明される条件を満たしたクラスのことであり, ざっくり言うと allocate と deallocate をメソッドとして持つクラスです. std::allocator<T> は以下のように, allocate()呼び出しでは operator new() を, deallocate()呼び出しでは operator delete()を呼び出します.

namespace std {
  template <typename T>
  class allocator {
    public:
    using value_type = T;

    T* allocate(size_t n) {
      return ::operator new(n * sizeof(T));
    }

    void deallocate(T *p, size_t n) {
      ::operator delete(p, n * sizeof(T));
    }
  };
}

operator new() 等でメモリの確保をすると, brk(2) か mmap(2) どちらかのシステムコールが呼ばれることがあります. brk(2) は既存のheap領域にあたる vm_area_struct のサイズを拡張するシステムコールであり, mmap(2) は新規に vm_area_struct を作成するシステムコールです.

※ vm_area_structについて

以下の図のように, プロセスが持つ仮想アドレス空間は, vm_area_struct のリストとして管理されています. ユーザプログラムはこの vm_area_struct が示すアドレス範囲にのみアクセスすることができ, この範囲外にアクセスする命令を発行すると, Segmentation Fault が発生します.

この vm_area_struct のどれかが, いわゆる text領域, data領域, stack領域, heap領域 とか呼ばれたりするものです. mmapシステムコールによって, ユーザプログラムから自由に vm_area_structを作成することができます.

プロセスが作成された当初は一定のサイズの heap領域 (1MB?) が確保されています (下図左).

この領域はユーザ空間のmallocランタイムによって管理されています. mallocランタイムはこの領域をmemory chunkの集合として管理し, ユーザプログラムからの割り当て要求にしたがって, memory chunkを渡します. mallocランタイムの実装に依存しますが, memory chunkはくっついたり分割されたりして, 頭良くこの領域を使おうとします.

ユーザプログラムからメモリ割り当て要求が続くと, heap領域が足りなくなってしまいます. 足りなくなるとmallocランタイムは brk(2) を発行します. brk(2) でheap領域を拡張することによって, mallocランタイムは, 使用することができる仮想アドレス空間リソースを追加することができます (下図真ん中).

あるサイズより小さな malloc(size) 呼び出しの場合は brk(2) によるheap領域拡張を引き起こしますが, あるサイズより大きなメモリ割り当て要求をすると mmap(2) によってmallocランタイムが使用することができる仮想アドレス範囲リソースを追加しようとします (下図右)

さて, このようにmallocランタイムでは仮想アドレス空間リソースの確保が行われますが, 注意しなくてはならないことは, 仮想アドレス空間リソースが最初に確保された時点では, 対応するページテーブルのエントリに物理ページ番号が挿入されておらず, first touchでソフトページフォルトが発生するという点です.

このソフトページフォルトが実行時間に大きなスパイクを発生させるため, リアルタイムシステムにとっては致命的となります. 例として GitHub - ros2/common_interfaces: A set of packages which contain common interface files (.msg and .srv). で提供されている PointCloud2 型を挙げます. 例えば, 出力メッセージのメモリ領域初期化のコードは以下となります.

auto output_msg = std::make_unique<PointCloud2>();
output_msg->data.resize(POINTS_DATA_SIZE);

// fill output_msg

publisher_->publish(std::move(output_msg);

この POINTS_DATA_SIZE が10MB強であるときの, output_msg->data.resize() にかかるターンアラウンドタイムの時系列推移 (横軸: 呼び出し回数インデックス, 縦軸: ターンアラウンドタイム (us)) とそのヒストグラムは以下のようになります. スパイクが多数発生していることが分かります.

このスパイクが発生している箇所がソフトページフォルト由来であることを明らかにするために, output_msg->resize() の処理区間における Performance Counterの minor-faults の数を横軸に, ターンアラウンドタイムを縦軸にプロットして散布図を作りました. すると以下のように, 綺麗な比例関係にあることが分かり, ソフトページフォルトがスパイクの原因になっていることがはっきりしました. ソフトページフォルトが発生したりしなかったりするのは, operator delete()に伴って munmap(2) で仮想アドレス空間リソースを解放するかどうかは, mallocランタイムの実装依存によるタイミングに依存しているからです.

このようなソフトページフォルトが発生することを防ぐには, あらかじめfirst touchした領域からのみメモリ割り当てをするようなカスタムメモリアロケータが必要になります.

また, リアルタイムシステム向けに推奨されたメモリ割り当てアルゴリズムというのも存在しており, 組み込み環境でリアルタイムシステムを構築するためには, 全てのheap割り当てにカスタムメモリアロケータを用意するのは必須と言えるでしょう.

rosidlで生成されるROS2メッセージ型構造体の定義

さて, ROS2メッセージ型構造体にカスタムメモリアロケータを指定する必要性が分かったので, まずカスタムメモリアロケータを適用する先の構造体の定義を見ていくこととします. 今回は先ほどの例にも出した sensor_msgs パッケージの PointCloud2 メッセージ型を例とします.

rosidlは, .msg ファイルに独自フォーマットで記述されたROS2メッセージのフィールド情報を元に, 様々なヘッダファイルやソースファイルを自動生成します. その中でも, オリジナルROS2メッセージの構造体を定義した .hpp ファイルは install/パッケージ名/include/パッケージ名/msg/detail/型名__struct.hpp に生成されます.

sensor_msgs パッケージは rosdep でインストールすると, もちろん /opt/ros/{ros2 version name}/ 以下にインストールされています. sensor_msgs::msg::PointCloud2 の構造体を定義したヘッダファイルは, /opt/ros/{ros2 version name}/include/sensor_msgs/msg/detail/point_cloud2__struct.hpp に配置されているので, 見に行ってみましょう. 大部分を省略したコードを以下に示します.

namespace sensor_msgs {
namespace msg {

template<class ContainerAllocator>
struct PointCloud2_ {
  explicit PointCloud2_(
    rosidl_runtime_cpp::MessageInitialization _init = rosidl_runtime_cpp::MessageInitialization::ALL
  ) { }

  explicit PointCloud2_(
    const ContainerAllocator &_alloc,
    rosidl_runtime_cpp::MessageInitialization _init = rosidl_runtime_cpp::MessageInitialization::ALL,
  ) { }
};

using PointCloud2 = sensor_msgs::msg::PointCloud2_<std::allocator<void>>;

}
}

このように, PointCloud2 というのは template<class ContainerAllocator> struct PointCloud2_ に対してアロケータを std::allocator<void> に指定したものであるということが分かります.

ナイーブなアロケータ指定と symbol lookup error

前章の内容を踏まえると, ROS2アプリケーション側から以下のように, カスタムメモリアロケータをROS2メッセージ構造体に対して設定できそうです.

template<typename T>
class MyAllocator {
   ...
};

MyAllocator<void> my_allocator;
auto output_msg = std::make_unique<sensor_msgs::msg::PointCloud2_<MyAllocator<void>>(my_allocator);
...

しかし, この一見うまくいくナイーブなアプローチでは, 実行時の symbol lookup error: undefined symbol というエラーメッセージと共に落ちてしまいます.

具体的には, rclcpp 内のヘッダファイルから呼ばれている以下の関数で symbol lookup error: undefined symbol が発生していました (ソースコード該当箇所:rclcpp/rclcpp/include/rclcpp/get_message_type_support_handle.hpp at 33dae5d679751b603205008fcb31755986bcee1c · ros2/rclcpp · GitHub ). この関数は例えば rclcpp::Node::create_publisher() の呼び出しに従って呼ばれます.

namespace rclcpp
{

get_message_type_support_handle()
{
  auto handle = rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>();
  if (!handle) {
    throw std::runtime_error("Type support handle unexpectedly nullptr");
  }
  return *handle;
}

}

この rosidl_typesupport_cpp::get_message_type_support_handle という関数は, ヘッダファイルrosidl/rosidl_runtime_cpp/include/rosidl_runtime_cpp/message_type_support_decl.hpp at galactic · ros2/rosidl · GitHub で以下のように宣言されています.

template<typename T>
const rosidl_message_type_support_t * get_message_type_support_handle();

今回の場合は, Tにsensor_msgs::msg::PointCloud2_<MyAllocator<void>> が渡されて, ROS2アプリケーション側バイナリのシンボルテーブルには, カスタムアロケータ版メッセージ型でインスタンス化された関数名シンボルが入っていることが nm -D で確認できます.

結論から述べると, 今回生じた問題の理由は, rosidl_typesupport_cpp::get_message_type_support_handle という関数の実装側に, その「カスタムアロケータ版メッセージ型でインスタンス化された関数名シンボル」が存在していないことです.

rosidl_typesupport_cpp::get_message_type_support_handle という関数の実装は, rosidl が .msg ファイルから自動生成したソースファイルの中に含まれています. PointCloud2 の場合は /opt/ros/{ros2 version name}/include/sensor_msgs/msg/detail/point_cloud2__type_support.cpp に該当ソースファイルが配置されていました. そこには以下のように rosidl_typesupport_cpp::get_message_type_support_handle という関数が定義されていることが確認できました.

#include <sensor_msgs/msg/detail/point_cloud2__struct.hpp>
#include <rosidl_typesupport_introspection_cpp/message_type_support_decl.hpp>

template<>
ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_PUBLIC
const rosidl_message_type_support_t *
get_message_type_support_handle<sensor_msgs::msg::PointCloud2>() {
  return &::sensor_msgs::msg::rosidl_typesupport_introspection_cpp::PointCloud2_message_type_support_handle;
}

get_message_type_support_handleのテンプレート引数の部分に sensor_msgs::msg::PointCloud2と書かれています. これはすなわち sensor_msgs::msg::PointCloud2_<std::allocator<void>> ということであるので, メッセージ型のアロケータが std::allocator<void> の場合に対してのみ, この関数は実装を用意しているということになります. もちろん MyAllocator<void> に対する実装は存在しないので, symbol lookup error: undefined symbol になるというわけです.

ちなみに, このアロケータを std::allocator<void> とベタ書きしている empy テンプレートファイルの箇所は rosidl/rosidl_typesupport_introspection_cpp/resource/msg__type_support.cpp.em at b8381d955a111cdf8a52a0f1891cc55de5f19db1 · ros2/rosidl · GitHub です.

C++のコンテナ型が抱える負債と多相アロケータ

今回発生した問題の根本的な原因は, C++のコンテナ型にアロケータ型が含まれてしまっているという点です. そもそもアロケータの違い (メモリ割り当て戦略の違い) がコンテナ型に現れるべきではありません.

この議題に関しては, C++17から導入された 多相アロケータが解決を図ろうとしています. 多相アロケータは std::pmr 名前空間に実装されています. ドキュメントは std::pmr::polymorphic_allocator - cppreference.com です.

C++の多相アロケータは, いままでのアロケータ型を含んでしまっているコンテナ型との連続性を保ちつつ, アロケータを差し替えても, コンテナ型に変化がないようにする工夫をしました. すなわち, コンテナ型に常に同じアロケータ型 (std::pmr::polymorphic_allocator) を渡し, メモリ割り当て戦略の実装は外部のオブジェクト(std::pmr::memory_resource) に任せて, std::pmr::polymorphic_allocator オブジェクトは std::pmr::memory_resource オブジェクトへのポインタを持つだけという方法です (Strategy Patternですね).

これならば, メモリ割り当て戦略である std::pmr::memory_resource オブジェクトの差し替えをおこなったとしても, コンテナ型 (例えば std::vector<int, std::pmr::polymorphic_allocator>) は変化することはありません. ただ唯一の欠点としては, 多相アロケータ型を含むコンテナ型同士でメモリ割り当て戦略が型に現れないというだけで, 既存のメモリアロケータ (例えば std::allocator) を含むコンテナ型とは型の違いが出てしまうという点です. つまり, 明日朝起きたら全てのコンテナ型のアロケータ型が std::pmr::polymorphic_allocator に置き換わっていると人類が幸せになれるということです.

次章では, rosidl を中心としたROS2メッセージのエコシステムに, 多相アロケータを導入することによって, ROS2メッセージ構造体にカスタムアロケータを渡せるようにならないかを検証します.

rosidl改造による多相アロケータの導入

ROS2メッセージ構造体のアロケータ型として, 多相アロケータを導入することが可能であるかを検証します. その手順は以下の通りとなります.

  • セルフビルドのROS2環境を作る

  • ROS2メッセージ構造体の定義にて, using PointCloud2 = sensor_msgs::msg::PointCloud2_<std::allocator<void>>; というようになっている箇所が using PointCloud2 = sensor_msgs::msg::PointCloud2_<std::pmr::polymorphic_allocator<void>>; となるように, ros2/rosidl repositoryで管理している empy テンプレートファイルを書き換えて, ビルドし直す.

  • 全てのROS2メッセージ構造体のアロケータが std;;pmr::polymorphic_allocator<void> に変更されたら, ROS2環境全体をビルドしなおし, ビルドが通らない箇所を地道に直していく (修正箇所の規模を把握する).

include/sensor_msgs/msg/detail/point_cloud2__struct.hpp のような, ROS2メッセージ構造体を定義するヘッダファイル等は, (galacticの場合は) common_interfaces/sensor_msgs/CMakeLists.txt at galactic · ros2/common_interfaces · GitHub 　のように rosidl_generate_interfaces という rosidl で定義されたCMakeマクロをトリガーとして生成されます. 生成元となるファイルは, ros2/rosidl repository内にて .em という拡張子の empyテンプレートファイルとして管理されています.

.em ファイルで, using PointCloud2 = sensor_msgs::msg::PointCloud2_<std::allocator<void>>; のようなコードを生成する箇所は rosidl/rosidl_generator_cpp/resource/msg__struct.hpp.em at galactic · ros2/rosidl · GitHub であるので, ここの std::allocator<void> を std::pmr::polymorphic_allocator<void> に書き換えて, ファイルの冒頭に #include <memory_resource> と追記し, ROS2環境全体をビルドし直せばよいです.

※ 2022/12/16現在, ROS2エコシステム全体はC++14でビルドされるように各 CMakeLists.txt が設定されており, C++17からのfeatureである std::pmr 名前空間の機能を使うことができません. とりあえず std::experimental::pmr に生えているAPIを使えばC++14でも多相アロケータを使用することができるのでそちらで実験します.

ROS2エコシステム全体の修正の必要性

rosidl/rosidl_generator_cpp/resource/msg__struct.hpp.em にて std::allocator<void> を std::pmr::polymorphic_allocator<void> に書き換えて, ROS2エコシステム全体をビルドしなおすと, 様々な種類のビルドエラーが生じました.

ビルドエラーの原因としては, ROS2エコシステム各repositoryに記述されているソースコード中で, std::allocator 型を含むコンテナ型と, ROS2メッセージ型の内部のコンテナプロパティ (std::pmr::polymorphic_allocator 型を含む) 間で型の不整合が起きていることが中心でした.

例えば, common_interfaces/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.hpp at galactic · ros2/common_interfaces · GitHub では以下のように文字列比較をしているために型の不整合が発生しています.

while ((field_iter != field_end) && (field_iter->name != field_name)) {
  ...
}

libstatistics_collector/src/libstatistics_collector/collector/generate_statistics_message.cpp at galactic · ros-tooling/libstatistics_collector · GitHubのように, 文字列代入を行おうとしている箇所でも型の不整合が起きます.

msg.metrics_source = metric_name;

rosidl_typesupport_fastrtps/rosidl_typesupport_fastrtps_cpp/resource/msg__type_support.cpp.em at galactic · ros2/rosidl_typesupport_fastrtps · GitHub のように, std::wstring ( std::basic_string<wchar_t, std::char_traits<wchar_t>, std::allocator<wchar_t>) と .em ファイルの中にベタ書きしてしまっている事例も見受けられます.

他にもたくさんの型不整合が発生する箇所があります. つまり, 多相アロケータを導入するアプローチを成功させようとすると, ROS2エコシステムの膨大なrepository群に一つ一つ修正のPRを出さなくてはならず, また今後のコミュニティの開発にてこのような型不整合を引き起こすコードが入り込まないように監視する必要が出てきてしまいます.

まとめ

2022/12/16現在から観測して, C++ ROS2のメッセージ型構造体にカスタムメモリアロケータを指定するのはかなりの困難を伴うことになります. 取れる選択肢としては,

  • ROS2エコシステム全体 (というか全世界) の std::vector や std::string のようなコンテナ型が, 明日朝起きたら全て std::pmr::vector や std::pmr::string に書き変わっている (無理)

  • ROS2エコシステム全体の std::allocator<void> となっている由来のビルドエラー箇所を1つずつ修正してPRを出す (しんどすぎる)

  • ROS2エコシステムを脱出する (???)

ただ, メッセージ型構造体に直接カスタムメモリアロケータを指定するという綺麗なアプローチを諦めて, もう全てのheapメモリ割り当て要求を LD_PRELOAD で引っ掛けて, まるごとカスタムメモリアロケータを適用してしまうという多少汚いアプローチを取ってしまうのが丸いかもしれません.

→ つくりました https://github.com/tier4/heaphook